Bullet Collision Detection & Physics Library
btLemkeSolver.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
16 
17 #ifndef BT_LEMKE_SOLVER_H
18 #define BT_LEMKE_SOLVER_H
19 
20 
21 #include "btMLCPSolverInterface.h"
22 #include "btLemkeAlgorithm.h"
23 
24 
25 
26 
31 {
32 protected:
33 
34 public:
35 
40 
41 
42 
44  :m_maxValue(100000),
45  m_debugLevel(0),
46  m_maxLoops(1000),
47  m_useLoHighBounds(true)
48  {
49  }
50  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)
51  {
52 
53  if (m_useLoHighBounds)
54  {
55 
56  BT_PROFILE("btLemkeSolver::solveMLCP");
57  int n = A.rows();
58  if (0==n)
59  return true;
60 
61  bool fail = false;
62 
63  btVectorXu solution(n);
64  btVectorXu q1;
65  q1.resize(n);
66  for (int row=0;row<n;row++)
67  {
68  q1[row] = -b[row];
69  }
70 
71  // cout << "A" << endl;
72  // cout << A << endl;
73 
75 
76  //slow matrix inversion, replace with LU decomposition
77  btMatrixXu A1;
78  btMatrixXu B(n,n);
79  {
80  BT_PROFILE("inverse(slow)");
81  A1.resize(A.rows(),A.cols());
82  for (int row=0;row<A.rows();row++)
83  {
84  for (int col=0;col<A.cols();col++)
85  {
86  A1.setElem(row,col,A(row,col));
87  }
88  }
89 
90  btMatrixXu matrix;
91  matrix.resize(n,2*n);
92  for (int row=0;row<n;row++)
93  {
94  for (int col=0;col<n;col++)
95  {
96  matrix.setElem(row,col,A1(row,col));
97  }
98  }
99 
100 
101  btScalar ratio,a;
102  int i,j,k;
103  for(i = 0; i < n; i++){
104  for(j = n; j < 2*n; j++){
105  if(i==(j-n))
106  matrix.setElem(i,j,1.0);
107  else
108  matrix.setElem(i,j,0.0);
109  }
110  }
111  for(i = 0; i < n; i++){
112  for(j = 0; j < n; j++){
113  if(i!=j)
114  {
115  btScalar v = matrix(i,i);
116  if (btFuzzyZero(v))
117  {
118  a = 0.000001f;
119  }
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));
123  }
124  }
125  }
126  }
127  for(i = 0; i < n; i++){
128  a = matrix(i,i);
129  if (btFuzzyZero(a))
130  {
131  a = 0.000001f;
132  }
133  btScalar invA = 1.f/a;
134  for(j = 0; j < 2*n; j++){
135  matrix.mulElem(i,j,invA);
136  }
137  }
138 
139 
140 
141 
142 
143  for (int row=0;row<n;row++)
144  {
145  for (int col=0;col<n;col++)
146  {
147  B.setElem(row,col,matrix(row,n+col));
148  }
149  }
150  }
151 
152  btMatrixXu b1(n,1);
153 
154  btMatrixXu M(n*2,n*2);
155  for (int row=0;row<n;row++)
156  {
157  b1.setElem(row,0,-b[row]);
158  for (int col=0;col<n;col++)
159  {
160  btScalar v =B(row,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);
165 
166  }
167  }
168 
169  btMatrixXu Bb1 = B*b1;
170 // q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
171 
172  btVectorXu qq;
173  qq.resize(n*2);
174  for (int row=0;row<n;row++)
175  {
176  qq[row] = -Bb1(row,0)-lo[row];
177  qq[n+row] = Bb1(row,0)+hi[row];
178  }
179 
180  btVectorXu z1;
181 
182  btMatrixXu y1;
183  y1.resize(n,1);
184  btLemkeAlgorithm lemke(M,qq,m_debugLevel);
185  {
186  BT_PROFILE("lemke.solve");
187  lemke.setSystem(M,qq);
188  z1 = lemke.solve(m_maxLoops);
189  }
190  for (int row=0;row<n;row++)
191  {
192  y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
193  }
194  btMatrixXu y1_b1(n,1);
195  for (int i=0;i<n;i++)
196  {
197  y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
198  }
199 
200  btMatrixXu x1;
201 
202  x1 = B*(y1_b1);
203 
204  for (int row=0;row<n;row++)
205  {
206  solution[row] = x1(row,0);//n];
207  }
208 
209  int errorIndexMax = -1;
210  int errorIndexMin = -1;
211  float errorValueMax = -1e30;
212  float errorValueMin = 1e30;
213 
214  for (int i=0;i<n;i++)
215  {
216  x[i] = solution[i];
217  volatile btScalar check = x[i];
218  if (x[i] != check)
219  {
220  //printf("Lemke result is #NAN\n");
221  x.setZero();
222  return false;
223  }
224 
225  //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
226  //we need to figure out why it happens, and fix it, or detect it properly)
227  if (x[i]>m_maxValue)
228  {
229  if (x[i]> errorValueMax)
230  {
231  fail = true;
232  errorIndexMax = i;
233  errorValueMax = x[i];
234  }
236  }
237  if (x[i]<-m_maxValue)
238  {
239  if (x[i]<errorValueMin)
240  {
241  errorIndexMin = i;
242  errorValueMin = x[i];
243  fail = true;
244  //printf("x[i] = %f,",x[i]);
245  }
246  }
247  }
248  if (fail)
249  {
250  int m_errorCountTimes = 0;
251  if (errorIndexMin<0)
252  errorValueMin = 0.f;
253  if (errorIndexMax<0)
254  errorValueMax = 0.f;
255  m_errorCountTimes++;
256  // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
257  for (int i=0;i<n;i++)
258  {
259  x[i]=0.f;
260  }
261  }
262  return !fail;
263  } else
264 
265  {
266  int dimension = A.rows();
267  if (0==dimension)
268  return true;
269 
270 // printf("================ solving using Lemke/Newton/Fixpoint\n");
271 
272  btVectorXu q;
273  q.resize(dimension);
274  for (int row=0;row<dimension;row++)
275  {
276  q[row] = -b[row];
277  }
278 
279  btLemkeAlgorithm lemke(A,q,m_debugLevel);
280 
281 
282  lemke.setSystem(A,q);
283 
284  btVectorXu solution = lemke.solve(m_maxLoops);
285 
286  //check solution
287 
288  bool fail = false;
289  int errorIndexMax = -1;
290  int errorIndexMin = -1;
291  float errorValueMax = -1e30;
292  float errorValueMin = 1e30;
293 
294  for (int i=0;i<dimension;i++)
295  {
296  x[i] = solution[i+dimension];
297  volatile btScalar check = x[i];
298  if (x[i] != check)
299  {
300  x.setZero();
301  return false;
302  }
303 
304  //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
305  //we need to figure out why it happens, and fix it, or detect it properly)
306  if (x[i]>m_maxValue)
307  {
308  if (x[i]> errorValueMax)
309  {
310  fail = true;
311  errorIndexMax = i;
312  errorValueMax = x[i];
313  }
315  }
316  if (x[i]<-m_maxValue)
317  {
318  if (x[i]<errorValueMin)
319  {
320  errorIndexMin = i;
321  errorValueMin = x[i];
322  fail = true;
323  //printf("x[i] = %f,",x[i]);
324  }
325  }
326  }
327  if (fail)
328  {
329  static int errorCountTimes = 0;
330  if (errorIndexMin<0)
331  errorValueMin = 0.f;
332  if (errorIndexMax<0)
333  errorValueMax = 0.f;
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++)
336  {
337  x[i]=0.f;
338  }
339  }
340 
341 
342  return !fail;
343  }
344  return true;
345 
346  }
347 
348 };
349 
350 #endif //BT_LEMKE_SOLVER_H
#define btMatrixXu
Definition: btMatrixX.h:549
original version written by Erwin Coumans, October 2013
Definition: btLemkeSolver.h:30
original version written by Erwin Coumans, October 2013
#define btVectorXu
Definition: btMatrixX.h:548
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)
Definition: btLemkeSolver.h:50
btScalar m_maxValue
Definition: btLemkeSolver.h:36
#define BT_PROFILE(name)
Definition: btQuickprof.h:216
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:550
void setSystem(const btMatrixXu &M_, const btVectorXu &q_)
set system with Matrix M and vector q
bool m_useLoHighBounds
Definition: btLemkeSolver.h:39
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292