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 Simul...
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...