Bullet Collision Detection & Physics Library: btLemkeSolver.h Source File

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
26 class btLemkeSolver : public btMLCPSolverInterface
27 {
28 protected:
29 public:
30 btScalar m_maxValue;
31 int m_debugLevel;
32 int m_maxLoops;
33 bool m_useLoHighBounds;
34
35 btLemkeSolver()
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 {
44 if (m_useLoHighBounds)
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
btAlignedObjectArray< int >
btMLCPSolverInterface.h
btLemkeAlgorithm
Definition: btLemkeAlgorithm.h:29
btMatrixXu
#define btMatrixXu
Definition: btMatrixX.h:528
btLemkeSolver
original version written by Erwin Coumans, October 2013
Definition: btLemkeSolver.h:26
btLemkeSolver::m_debugLevel
int m_debugLevel
Definition: btLemkeSolver.h:31
btMLCPSolverInterface
original version written by Erwin Coumans, October 2013
Definition: btMLCPSolverInterface.h:22
btVectorXu
#define btVectorXu
Definition: btMatrixX.h:527
btLemkeSolver::btLemkeSolver
btLemkeSolver()
Definition: btLemkeSolver.h:35
btLemkeAlgorithm::solve
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Sim...
Definition: btLemkeAlgorithm.cpp:60
btLemkeSolver::solveMLCP
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
btLemkeSolver::m_maxValue
btScalar m_maxValue
Definition: btLemkeSolver.h:30
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
btFuzzyZero
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:552
btLemkeSolver::m_maxLoops
int m_maxLoops
Definition: btLemkeSolver.h:32
btLemkeAlgorithm::setSystem
void setSystem(const btMatrixXu &M_, const btVectorXu &q_)
set system with Matrix M and vector q
Definition: btLemkeAlgorithm.h:57
btLemkeSolver::m_useLoHighBounds
bool m_useLoHighBounds
Definition: btLemkeSolver.h:33
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btLemkeAlgorithm.h