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 #include "btMLCPSolverInterface.h"
21 #include "btLemkeAlgorithm.h"
22 
27 {
28 protected:
29 public:
34 
36  : m_maxValue(100000),
37  m_debugLevel(0),
38  m_maxLoops(1000),
39  m_useLoHighBounds(true)
40  {
41  }
42  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)
43  {
45  {
46  BT_PROFILE("btLemkeSolver::solveMLCP");
47  int n = A.rows();
48  if (0 == n)
49  return true;
50 
51  bool fail = false;
52 
53  btVectorXu solution(n);
54  btVectorXu q1;
55  q1.resize(n);
56  for (int row = 0; row < n; row++)
57  {
58  q1[row] = -b[row];
59  }
60 
61  // cout << "A" << endl;
62  // cout << A << endl;
63 
65 
66  //slow matrix inversion, replace with LU decomposition
67  btMatrixXu A1;
68  btMatrixXu B(n, n);
69  {
70  //BT_PROFILE("inverse(slow)");
71  A1.resize(A.rows(), A.cols());
72  for (int row = 0; row < A.rows(); row++)
73  {
74  for (int col = 0; col < A.cols(); col++)
75  {
76  A1.setElem(row, col, A(row, col));
77  }
78  }
79 
80  btMatrixXu matrix;
81  matrix.resize(n, 2 * n);
82  for (int row = 0; row < n; row++)
83  {
84  for (int col = 0; col < n; col++)
85  {
86  matrix.setElem(row, col, A1(row, col));
87  }
88  }
89 
90  btScalar ratio, a;
91  int i, j, k;
92  for (i = 0; i < n; i++)
93  {
94  for (j = n; j < 2 * n; j++)
95  {
96  if (i == (j - n))
97  matrix.setElem(i, j, 1.0);
98  else
99  matrix.setElem(i, j, 0.0);
100  }
101  }
102  for (i = 0; i < n; i++)
103  {
104  for (j = 0; j < n; j++)
105  {
106  if (i != j)
107  {
108  btScalar v = matrix(i, i);
109  if (btFuzzyZero(v))
110  {
111  a = 0.000001f;
112  }
113  ratio = matrix(j, i) / matrix(i, i);
114  for (k = 0; k < 2 * n; k++)
115  {
116  matrix.addElem(j, k, -ratio * matrix(i, k));
117  }
118  }
119  }
120  }
121  for (i = 0; i < n; i++)
122  {
123  a = matrix(i, i);
124  if (btFuzzyZero(a))
125  {
126  a = 0.000001f;
127  }
128  btScalar invA = 1.f / a;
129  for (j = 0; j < 2 * n; j++)
130  {
131  matrix.mulElem(i, j, invA);
132  }
133  }
134 
135  for (int row = 0; row < n; row++)
136  {
137  for (int col = 0; col < n; col++)
138  {
139  B.setElem(row, col, matrix(row, n + col));
140  }
141  }
142  }
143 
144  btMatrixXu b1(n, 1);
145 
146  btMatrixXu M(n * 2, n * 2);
147  for (int row = 0; row < n; row++)
148  {
149  b1.setElem(row, 0, -b[row]);
150  for (int col = 0; col < n; col++)
151  {
152  btScalar v = B(row, col);
153  M.setElem(row, col, v);
154  M.setElem(n + row, n + col, v);
155  M.setElem(n + row, col, -v);
156  M.setElem(row, n + col, -v);
157  }
158  }
159 
160  btMatrixXu Bb1 = B * b1;
161  // q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
162 
163  btVectorXu qq;
164  qq.resize(n * 2);
165  for (int row = 0; row < n; row++)
166  {
167  qq[row] = -Bb1(row, 0) - lo[row];
168  qq[n + row] = Bb1(row, 0) + hi[row];
169  }
170 
171  btVectorXu z1;
172 
173  btMatrixXu y1;
174  y1.resize(n, 1);
175  btLemkeAlgorithm lemke(M, qq, m_debugLevel);
176  {
177  //BT_PROFILE("lemke.solve");
178  lemke.setSystem(M, qq);
179  z1 = lemke.solve(m_maxLoops);
180  }
181  for (int row = 0; row < n; row++)
182  {
183  y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]);
184  }
185  btMatrixXu y1_b1(n, 1);
186  for (int i = 0; i < n; i++)
187  {
188  y1_b1.setElem(i, 0, y1(i, 0) - b1(i, 0));
189  }
190 
191  btMatrixXu x1;
192 
193  x1 = B * (y1_b1);
194 
195  for (int row = 0; row < n; row++)
196  {
197  solution[row] = x1(row, 0); //n];
198  }
199 
200  int errorIndexMax = -1;
201  int errorIndexMin = -1;
202  float errorValueMax = -1e30;
203  float errorValueMin = 1e30;
204 
205  for (int i = 0; i < n; i++)
206  {
207  x[i] = solution[i];
208  volatile btScalar check = x[i];
209  if (x[i] != check)
210  {
211  //printf("Lemke result is #NAN\n");
212  x.setZero();
213  return false;
214  }
215 
216  //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
217  //we need to figure out why it happens, and fix it, or detect it properly)
218  if (x[i] > m_maxValue)
219  {
220  if (x[i] > errorValueMax)
221  {
222  fail = true;
223  errorIndexMax = i;
224  errorValueMax = x[i];
225  }
227  }
228  if (x[i] < -m_maxValue)
229  {
230  if (x[i] < errorValueMin)
231  {
232  errorIndexMin = i;
233  errorValueMin = x[i];
234  fail = true;
235  //printf("x[i] = %f,",x[i]);
236  }
237  }
238  }
239  if (fail)
240  {
241  int m_errorCountTimes = 0;
242  if (errorIndexMin < 0)
243  errorValueMin = 0.f;
244  if (errorIndexMax < 0)
245  errorValueMax = 0.f;
246  m_errorCountTimes++;
247  // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
248  for (int i = 0; i < n; i++)
249  {
250  x[i] = 0.f;
251  }
252  }
253  return !fail;
254  }
255  else
256 
257  {
258  int dimension = A.rows();
259  if (0 == dimension)
260  return true;
261 
262  // printf("================ solving using Lemke/Newton/Fixpoint\n");
263 
264  btVectorXu q;
265  q.resize(dimension);
266  for (int row = 0; row < dimension; row++)
267  {
268  q[row] = -b[row];
269  }
270 
271  btLemkeAlgorithm lemke(A, q, m_debugLevel);
272 
273  lemke.setSystem(A, q);
274 
275  btVectorXu solution = lemke.solve(m_maxLoops);
276 
277  //check solution
278 
279  bool fail = false;
280  int errorIndexMax = -1;
281  int errorIndexMin = -1;
282  float errorValueMax = -1e30;
283  float errorValueMin = 1e30;
284 
285  for (int i = 0; i < dimension; i++)
286  {
287  x[i] = solution[i + dimension];
288  volatile btScalar check = x[i];
289  if (x[i] != check)
290  {
291  x.setZero();
292  return false;
293  }
294 
295  //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
296  //we need to figure out why it happens, and fix it, or detect it properly)
297  if (x[i] > m_maxValue)
298  {
299  if (x[i] > errorValueMax)
300  {
301  fail = true;
302  errorIndexMax = i;
303  errorValueMax = x[i];
304  }
306  }
307  if (x[i] < -m_maxValue)
308  {
309  if (x[i] < errorValueMin)
310  {
311  errorIndexMin = i;
312  errorValueMin = x[i];
313  fail = true;
314  //printf("x[i] = %f,",x[i]);
315  }
316  }
317  }
318  if (fail)
319  {
320  static int errorCountTimes = 0;
321  if (errorIndexMin < 0)
322  errorValueMin = 0.f;
323  if (errorIndexMax < 0)
324  errorValueMax = 0.f;
325  printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin, errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
326  for (int i = 0; i < dimension; i++)
327  {
328  x[i] = 0.f;
329  }
330  }
331 
332  return !fail;
333  }
334  return true;
335  }
336 };
337 
338 #endif //BT_LEMKE_SOLVER_H
#define btMatrixXu
Definition: btMatrixX.h:529
#define btVectorXu
Definition: btMatrixX.h:528
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simula...
void setSystem(const btMatrixXu &M_, const btVectorXu &q_)
set system with Matrix M and vector q
original version written by Erwin Coumans, October 2013
Definition: btLemkeSolver.h:27
btScalar m_maxValue
Definition: btLemkeSolver.h:30
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:42
bool m_useLoHighBounds
Definition: btLemkeSolver.h:33
original version written by Erwin Coumans, October 2013