Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h

  • This file uses an unknown character encoding.
Show All 11 Lines
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
///original version written by Erwin Coumans, October 2013 ///original version written by Erwin Coumans, October 2013
#ifndef BT_LEMKE_SOLVER_H #ifndef BT_LEMKE_SOLVER_H
#define BT_LEMKE_SOLVER_H #define BT_LEMKE_SOLVER_H
#include "btMLCPSolverInterface.h" #include "btMLCPSolverInterface.h"
#include "btLemkeAlgorithm.h" #include "btLemkeAlgorithm.h"
///The btLemkeSolver is based on "Fast Implementation of Lemke窶冱 Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
///The btLemkeSolver is based on "Fast Implementation of Lemke痴 Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time. ///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team ///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
class btLemkeSolver : public btMLCPSolverInterface class btLemkeSolver : public btMLCPSolverInterface
{ {
protected: protected:
public: public:
btScalar m_maxValue; btScalar m_maxValue;
int m_debugLevel; int m_debugLevel;
int m_maxLoops; int m_maxLoops;
bool m_useLoHighBounds; bool m_useLoHighBounds;
btLemkeSolver() btLemkeSolver()
:m_maxValue(100000), : m_maxValue(100000),
m_debugLevel(0), m_debugLevel(0),
m_maxLoops(1000), m_maxLoops(1000),
m_useLoHighBounds(true) m_useLoHighBounds(true)
{ {
} }
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{ {
if (m_useLoHighBounds) if (m_useLoHighBounds)
{ {
BT_PROFILE("btLemkeSolver::solveMLCP"); BT_PROFILE("btLemkeSolver::solveMLCP");
int n = A.rows(); int n = A.rows();
if (0==n) if (0 == n)
return true; return true;
bool fail = false; bool fail = false;
btVectorXu solution(n); btVectorXu solution(n);
btVectorXu q1; btVectorXu q1;
q1.resize(n); q1.resize(n);
for (int row=0;row<n;row++) for (int row = 0; row < n; row++)
{ {
q1[row] = -b[row]; q1[row] = -b[row];
} }
// cout << "A" << endl; // cout << "A" << endl;
// cout << A << endl; // cout << A << endl;
///////////////////////////////////// /////////////////////////////////////
//slow matrix inversion, replace with LU decomposition //slow matrix inversion, replace with LU decomposition
btMatrixXu A1; btMatrixXu A1;
btMatrixXu B(n,n); btMatrixXu B(n, n);
{ {
BT_PROFILE("inverse(slow)"); //BT_PROFILE("inverse(slow)");
A1.resize(A.rows(),A.cols()); A1.resize(A.rows(), A.cols());
for (int row=0;row<A.rows();row++) for (int row = 0; row < A.rows(); row++)
{ {
for (int col=0;col<A.cols();col++) for (int col = 0; col < A.cols(); col++)
{ {
A1.setElem(row,col,A(row,col)); A1.setElem(row, col, A(row, col));
} }
} }
btMatrixXu matrix; btMatrixXu matrix;
matrix.resize(n,2*n); matrix.resize(n, 2 * n);
for (int row=0;row<n;row++) for (int row = 0; row < n; row++)
{ {
for (int col=0;col<n;col++) for (int col = 0; col < n; col++)
{ {
matrix.setElem(row,col,A1(row,col)); matrix.setElem(row, col, A1(row, col));
} }
} }
btScalar ratio,a; btScalar ratio, a;
int i,j,k; int i, j, k;
for(i = 0; i < n; i++){ for (i = 0; i < n; i++)
for(j = n; j < 2*n; j++){ {
for (j = n; j < 2 * n; j++)
{
if(i==(j-n)) if (i == (j - n))
matrix.setElem(i,j,1.0); matrix.setElem(i, j, 1.0);
else else
matrix.setElem(i,j,0.0); matrix.setElem(i, j, 0.0);
} }
} }
for(i = 0; i < n; i++){ for (i = 0; i < n; i++)
for(j = 0; j < n; j++){ {
for (j = 0; j < n; j++)
{
if(i!=j) if (i != j)
{ {
btScalar v = matrix(i,i); btScalar v = matrix(i, i);
if (btFuzzyZero(v)) if (btFuzzyZero(v))
{ {
a = 0.000001f; a = 0.000001f;
} }
ratio = matrix(j,i)/matrix(i,i); ratio = matrix(j, i) / matrix(i, i);
for(k = 0; k < 2*n; k++){ for (k = 0; k < 2 * n; k++)
{
matrix.addElem(j,k,- ratio * matrix(i,k)); matrix.addElem(j, k, -ratio * matrix(i, k));
} }
} }
} }
} }
for(i = 0; i < n; i++){ for (i = 0; i < n; i++)
{
a = matrix(i,i); a = matrix(i, i);
if (btFuzzyZero(a)) if (btFuzzyZero(a))
{ {
a = 0.000001f; a = 0.000001f;
} }
btScalar invA = 1.f/a; btScalar invA = 1.f / a;
for(j = 0; j < 2*n; j++){ for (j = 0; j < 2 * n; j++)
{
matrix.mulElem(i,j,invA); matrix.mulElem(i, j, invA);
} }
} }
for (int row=0;row<n;row++) for (int row = 0; row < n; row++)
{ {
for (int col=0;col<n;col++) for (int col = 0; col < n; col++)
{ {
B.setElem(row,col,matrix(row,n+col)); B.setElem(row, col, matrix(row, n + col));
} }
} }
} }
btMatrixXu b1(n,1); btMatrixXu b1(n, 1);
btMatrixXu M(n*2,n*2); btMatrixXu M(n * 2, n * 2);
for (int row=0;row<n;row++) for (int row = 0; row < n; row++)
{ {
b1.setElem(row,0,-b[row]); b1.setElem(row, 0, -b[row]);
for (int col=0;col<n;col++) for (int col = 0; col < n; col++)
{ {
btScalar v =B(row,col); btScalar v = B(row, col);
M.setElem(row,col,v); M.setElem(row, col, v);
M.setElem(n+row,n+col,v); M.setElem(n + row, n + col, v);
M.setElem(n+row,col,-v); M.setElem(n + row, col, -v);
M.setElem(row,n+col,-v); M.setElem(row, n + col, -v);
} }
} }
btMatrixXu Bb1 = B*b1; btMatrixXu Bb1 = B * b1;
// q = [ (-B*b1 - lo)' (hi + B*b1)' ]' // q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
btVectorXu qq; btVectorXu qq;
qq.resize(n*2); qq.resize(n * 2);
for (int row=0;row<n;row++) for (int row = 0; row < n; row++)
{ {
qq[row] = -Bb1(row,0)-lo[row]; qq[row] = -Bb1(row, 0) - lo[row];
qq[n+row] = Bb1(row,0)+hi[row]; qq[n + row] = Bb1(row, 0) + hi[row];
} }
btVectorXu z1; btVectorXu z1;
btMatrixXu y1; btMatrixXu y1;
y1.resize(n,1); y1.resize(n, 1);
btLemkeAlgorithm lemke(M,qq,m_debugLevel); btLemkeAlgorithm lemke(M, qq, m_debugLevel);
{ {
BT_PROFILE("lemke.solve"); //BT_PROFILE("lemke.solve");
lemke.setSystem(M,qq); lemke.setSystem(M, qq);
z1 = lemke.solve(m_maxLoops); z1 = lemke.solve(m_maxLoops);
} }
for (int row=0;row<n;row++) for (int row = 0; row < n; row++)
{ {
y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]); y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]);
} }
btMatrixXu y1_b1(n,1); btMatrixXu y1_b1(n, 1);
for (int i=0;i<n;i++) for (int i = 0; i < n; i++)
{ {
y1_b1.setElem(i,0,y1(i,0)-b1(i,0)); y1_b1.setElem(i, 0, y1(i, 0) - b1(i, 0));
} }
btMatrixXu x1; btMatrixXu x1;
x1 = B*(y1_b1); x1 = B * (y1_b1);
for (int row=0;row<n;row++) for (int row = 0; row < n; row++)
{ {
solution[row] = x1(row,0);//n]; solution[row] = x1(row, 0); //n];
} }
int errorIndexMax = -1; int errorIndexMax = -1;
int errorIndexMin = -1; int errorIndexMin = -1;
float errorValueMax = -1e30; float errorValueMax = -1e30;
float errorValueMin = 1e30; float errorValueMin = 1e30;
for (int i=0;i<n;i++) for (int i = 0; i < n; i++)
{ {
x[i] = solution[i]; x[i] = solution[i];
volatile btScalar check = x[i]; volatile btScalar check = x[i];
if (x[i] != check) if (x[i] != check)
{ {
//printf("Lemke result is #NAN\n"); //printf("Lemke result is #NAN\n");
x.setZero(); x.setZero();
return false; return false;
} }
//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
//we need to figure out why it happens, and fix it, or detect it properly) //we need to figure out why it happens, and fix it, or detect it properly)
if (x[i]>m_maxValue) if (x[i] > m_maxValue)
{ {
if (x[i]> errorValueMax) if (x[i] > errorValueMax)
{ {
fail = true; fail = true;
errorIndexMax = i; errorIndexMax = i;
errorValueMax = x[i]; errorValueMax = x[i];
} }
////printf("x[i] = %f,",x[i]); ////printf("x[i] = %f,",x[i]);
} }
if (x[i]<-m_maxValue) if (x[i] < -m_maxValue)
{ {
if (x[i]<errorValueMin) if (x[i] < errorValueMin)
{ {
errorIndexMin = i; errorIndexMin = i;
errorValueMin = x[i]; errorValueMin = x[i];
fail = true; fail = true;
//printf("x[i] = %f,",x[i]); //printf("x[i] = %f,",x[i]);
} }
} }
} }
if (fail) if (fail)
{ {
int m_errorCountTimes = 0; int m_errorCountTimes = 0;
if (errorIndexMin<0) if (errorIndexMin < 0)
errorValueMin = 0.f; errorValueMin = 0.f;
if (errorIndexMax<0) if (errorIndexMax < 0)
errorValueMax = 0.f; errorValueMax = 0.f;
m_errorCountTimes++; m_errorCountTimes++;
// printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
for (int i=0;i<n;i++) for (int i = 0; i < n; i++)
{ {
x[i]=0.f; x[i] = 0.f;
} }
} }
return !fail; return !fail;
} else }
else
{ {
int dimension = A.rows(); int dimension = A.rows();
if (0==dimension) if (0 == dimension)
return true; return true;
// printf("================ solving using Lemke/Newton/Fixpoint\n"); // printf("================ solving using Lemke/Newton/Fixpoint\n");
btVectorXu q; btVectorXu q;
q.resize(dimension); q.resize(dimension);
for (int row=0;row<dimension;row++) for (int row = 0; row < dimension; row++)
{ {
q[row] = -b[row]; q[row] = -b[row];
} }
btLemkeAlgorithm lemke(A,q,m_debugLevel); btLemkeAlgorithm lemke(A, q, m_debugLevel);
lemke.setSystem(A,q); lemke.setSystem(A, q);
btVectorXu solution = lemke.solve(m_maxLoops); btVectorXu solution = lemke.solve(m_maxLoops);
//check solution //check solution
bool fail = false; bool fail = false;
int errorIndexMax = -1; int errorIndexMax = -1;
int errorIndexMin = -1; int errorIndexMin = -1;
float errorValueMax = -1e30; float errorValueMax = -1e30;
float errorValueMin = 1e30; float errorValueMin = 1e30;
for (int i=0;i<dimension;i++) for (int i = 0; i < dimension; i++)
{ {
x[i] = solution[i+dimension]; x[i] = solution[i + dimension];
volatile btScalar check = x[i]; volatile btScalar check = x[i];
if (x[i] != check) if (x[i] != check)
{ {
x.setZero(); x.setZero();
return false; return false;
} }
//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
//we need to figure out why it happens, and fix it, or detect it properly) //we need to figure out why it happens, and fix it, or detect it properly)
if (x[i]>m_maxValue) if (x[i] > m_maxValue)
{ {
if (x[i]> errorValueMax) if (x[i] > errorValueMax)
{ {
fail = true; fail = true;
errorIndexMax = i; errorIndexMax = i;
errorValueMax = x[i]; errorValueMax = x[i];
} }
////printf("x[i] = %f,",x[i]); ////printf("x[i] = %f,",x[i]);
} }
if (x[i]<-m_maxValue) if (x[i] < -m_maxValue)
{ {
if (x[i]<errorValueMin) if (x[i] < errorValueMin)
{ {
errorIndexMin = i; errorIndexMin = i;
errorValueMin = x[i]; errorValueMin = x[i];
fail = true; fail = true;
//printf("x[i] = %f,",x[i]); //printf("x[i] = %f,",x[i]);
} }
} }
} }
if (fail) if (fail)
{ {
static int errorCountTimes = 0; static int errorCountTimes = 0;
if (errorIndexMin<0) if (errorIndexMin < 0)
errorValueMin = 0.f; errorValueMin = 0.f;
if (errorIndexMax<0) if (errorIndexMax < 0)
errorValueMax = 0.f; errorValueMax = 0.f;
printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin, errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
for (int i=0;i<dimension;i++) for (int i = 0; i < dimension; i++)
{ {
x[i]=0.f; x[i] = 0.f;
} }
} }
return !fail; return !fail;
} }
return true; return true;
} }
}; };
#endif //BT_LEMKE_SOLVER_H #endif //BT_LEMKE_SOLVER_H