Bullet Collision Detection & Physics Library
btLemkeSolver.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
22#include "btLemkeAlgorithm.h"
23
24
25
26
31{
32protected:
33
34public:
35
40
41
42
44 :m_maxValue(100000),
45 m_debugLevel(0),
46 m_maxLoops(1000),
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
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
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
#define btVectorXu
Definition: btMatrixX.h:548
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:550
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
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:31
btScalar m_maxValue
Definition: btLemkeSolver.h:36
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
bool m_useLoHighBounds
Definition: btLemkeSolver.h:39
original version written by Erwin Coumans, October 2013