Blender  V3.3
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
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble y1
#define A1
Definition: RandGen.cpp:23
ATTR_WARN_UNUSED_RESULT const BMVert * v
#define A
#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
SIMD_FORCE_INLINE 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
uint col
#define M
#define B
static unsigned a[3]
Definition: RandGen.cpp:78
static const pxr::TfToken b("b", pxr::TfToken::Immortal)