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
21 #include "btMLCPSolverInterface.h"
22 #include "btLemkeAlgorithm.h"
23
24
25
26
30 class btLemkeSolver : public btMLCPSolverInterface
31 {
32 protected:
33
34 public:
35
36 btScalar m_maxValue;
37 int m_debugLevel;
38 int m_maxLoops;
39 bool m_useLoHighBounds;
40
41
42
43 btLemkeSolver()
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
btAlignedObjectArray< int >
btMLCPSolverInterface.h
btLemkeAlgorithm
Definition: btLemkeAlgorithm.h:32
btMatrixXu
#define btMatrixXu
Definition: btMatrixX.h:549
btLemkeSolver
original version written by Erwin Coumans, October 2013
Definition: btLemkeSolver.h:30
btLemkeSolver::m_debugLevel
int m_debugLevel
Definition: btLemkeSolver.h:37
btMLCPSolverInterface
original version written by Erwin Coumans, October 2013
Definition: btMLCPSolverInterface.h:22
btVectorXu
#define btVectorXu
Definition: btMatrixX.h:548
btLemkeSolver::btLemkeSolver
btLemkeSolver()
Definition: btLemkeSolver.h:43
btLemkeAlgorithm::solve
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simu...
Definition: btLemkeAlgorithm.cpp:63
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:50
btLemkeSolver::m_maxValue
btScalar m_maxValue
Definition: btLemkeSolver.h:36
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
btFuzzyZero
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:550
btLemkeSolver::m_maxLoops
int m_maxLoops
Definition: btLemkeSolver.h:38
btLemkeAlgorithm::setSystem
void setSystem(const btMatrixXu &M_, const btVectorXu &q_)
set system with Matrix M and vector q
Definition: btLemkeAlgorithm.h:63
btLemkeSolver::m_useLoHighBounds
bool m_useLoHighBounds
Definition: btLemkeSolver.h:39
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btLemkeAlgorithm.h