17 #ifndef BT_LEMKE_SOLVER_H    18 #define BT_LEMKE_SOLVER_H    47                 m_useLoHighBounds(true)
    53                 if (m_useLoHighBounds)
    66                 for (
int row=0;row<n;row++)
    81                                 A1.resize(A.rows(),A.cols());
    82                                 for (
int row=0;row<A.rows();row++)
    84                                         for (
int col=0;col<A.cols();col++)
    86                                                 A1.setElem(row,col,A(row,col));
    92                                 for (
int row=0;row<n;row++)
    94                                         for (
int col=0;col<n;col++)
    96                                                 matrix.setElem(row,col,A1(row,col));
   103                                 for(i = 0; i < n; i++){
   104                                 for(j = n; j < 2*n; j++){
   106                                                 matrix.setElem(i,j,1.0);
   108                                                 matrix.setElem(i,j,0.0);
   111                         for(i = 0; i < n; i++){
   112                                 for(j = 0; j < n; j++){
   120                                                 ratio = matrix(j,i)/matrix(i,i);
   121                                                 for(k = 0; k < 2*n; k++){
   122                                                         matrix.addElem(j,k,- ratio * matrix(i,k));
   127                         for(i = 0; i < n; i++){
   134                                 for(j = 0; j < 2*n; j++){
   135                                         matrix.mulElem(i,j,invA);
   143                         for (
int row=0;row<n;row++)
   145                                         for (
int col=0;col<n;col++)
   147                                                 B.setElem(row,col,matrix(row,n+col));
   155                 for (
int row=0;row<n;row++)
   157                         b1.setElem(row,0,-b[row]);
   158                         for (
int col=0;col<n;col++)
   161                                 M.setElem(row,col,v);
   162                                 M.setElem(n+row,n+col,v);
   163                                 M.setElem(n+row,col,-v);
   164                                 M.setElem(row,n+col,-v);
   174                 for (
int row=0;row<n;row++)
   176                         qq[row] = -Bb1(row,0)-lo[row];
   177                         qq[n+row] = Bb1(row,0)+hi[row];
   188                         z1  = lemke.
solve(m_maxLoops);
   190                 for (
int row=0;row<n;row++)
   192                         y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
   195                 for (
int i=0;i<n;i++)
   197                         y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
   204                 for (
int row=0;row<n;row++)
   206                         solution[row] = x1(row,0);
   209                 int errorIndexMax = -1;
   210                 int errorIndexMin = -1;
   211                 float errorValueMax = -1e30;
   212                 float errorValueMin = 1e30;
   214                 for (
int i=0;i<n;i++)
   229                                 if (x[i]> errorValueMax)
   233                                         errorValueMax = x[i];
   237                         if (x[i]<-m_maxValue)
   239                                 if (x[i]<errorValueMin)
   242                                         errorValueMin = x[i];
   250                         int m_errorCountTimes = 0;
   257                         for (
int i=0;i<n;i++)
   266                         int dimension = A.rows();
   274                 for (
int row=0;row<dimension;row++)
   289                 int errorIndexMax = -1;
   290                 int errorIndexMin = -1;
   291                 float errorValueMax = -1e30;
   292                 float errorValueMin = 1e30;
   294                 for (
int i=0;i<dimension;i++)
   296                         x[i] = solution[i+dimension];
   308                                 if (x[i]> errorValueMax)
   312                                         errorValueMax = x[i];
   316                         if (x[i]<-m_maxValue)
   318                                 if (x[i]<errorValueMin)
   321                                         errorValueMin = x[i];
   329                         static int errorCountTimes = 0;
   334                         printf(
"Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
   335                         for (
int i=0;i<dimension;i++)
   350 #endif //BT_LEMKE_SOLVER_H 
original version written by Erwin Coumans, October 2013 
original version written by Erwin Coumans, October 2013 
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemkeās Algorithm for Rigid Body Contact Simu...
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)
bool btFuzzyZero(btScalar x)
void setSystem(const btMatrixXu &M_, const btVectorXu &q_)
set system with Matrix M and vector q 
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...