Bullet Collision Detection & Physics Library: btLemkeAlgorithm.cpp Source File

Bullet Collision Detection & Physics Library

btLemkeAlgorithm.cpp
Go to the documentation of this file.
1 /* Copyright (C) 2004-2013 MBSim Development Team
2
3 Code was converted for the Bullet Continuous Collision Detection and Physics Library
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 */
15
16 //The original version is here
17 //https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
18 //This file is re-distributed under the ZLib license, with permission of the original author
19 //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
20 //STL/std::vector replaced by btAlignedObjectArray
21
22 #include "btLemkeAlgorithm.h"
23
24 #undef BT_DEBUG_OSTREAM
25 #ifdef BT_DEBUG_OSTREAM
26 using namespace std;
27 #endif //BT_DEBUG_OSTREAM
28
29 btScalar btMachEps()
30 {
31 static bool calculated = false;
32 static btScalar machEps = btScalar(1.);
33 if (!calculated)
34 {
35 do
36 {
37 machEps /= btScalar(2.0);
38 // If next epsilon yields 1, then break, because current
39 // epsilon is the machine epsilon.
40 } while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0));
41 // printf( "\nCalculated Machine epsilon: %G\n", machEps );
42 calculated = true;
43 }
44 return machEps;
45 }
46
47 btScalar btEpsRoot()
48 {
49 static btScalar epsroot = 0.;
50 static bool alreadyCalculated = false;
51
52 if (!alreadyCalculated)
53 {
54 epsroot = btSqrt(btMachEps());
55 alreadyCalculated = true;
56 }
57 return epsroot;
58 }
59
60 btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
61 {
62 steps = 0;
63
64 int dim = m_q.size();
65 #ifdef BT_DEBUG_OSTREAM
66 if (DEBUGLEVEL >= 1)
67 {
68 cout << "Dimension = " << dim << endl;
69 }
70 #endif //BT_DEBUG_OSTREAM
71
72 btVectorXu solutionVector(2 * dim);
73 solutionVector.setZero();
74
75 //, INIT, 0.);
76
77 btMatrixXu ident(dim, dim);
78 ident.setIdentity();
79 #ifdef BT_DEBUG_OSTREAM
80 cout << m_M << std::endl;
81 #endif
82
83 btMatrixXu mNeg = m_M.negative();
84
85 btMatrixXu A(dim, 2 * dim + 2);
86 //
87 A.setSubMatrix(0, 0, dim - 1, dim - 1, ident);
88 A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1, mNeg);
89 A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
90 A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1, m_q);
91
92 #ifdef BT_DEBUG_OSTREAM
93 cout << A << std::endl;
94 #endif //BT_DEBUG_OSTREAM
95
96 // btVectorXu q_;
97 // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
98
99 btAlignedObjectArray<int> basis;
100 //At first, all w-values are in the basis
101 for (int i = 0; i < dim; i++)
102 basis.push_back(i);
103
104 int pivotRowIndex = -1;
105 btScalar minValue = 1e30f;
106 bool greaterZero = true;
107 for (int i = 0; i < dim; i++)
108 {
109 btScalar v = A(i, 2 * dim + 1);
110 if (v < minValue)
111 {
112 minValue = v;
113 pivotRowIndex = i;
114 }
115 if (v < 0)
116 greaterZero = false;
117 }
118
119 // int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value
120 int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards
121 int pivotColIndex = 2 * dim; // first col is that of z0
122
123 #ifdef BT_DEBUG_OSTREAM
124 if (DEBUGLEVEL >= 3)
125 {
126 // cout << "A: " << A << endl;
127 cout << "pivotRowIndex " << pivotRowIndex << endl;
128 cout << "pivotColIndex " << pivotColIndex << endl;
129 cout << "Basis: ";
130 for (int i = 0; i < basis.size(); i++)
131 cout << basis[i] << " ";
132 cout << endl;
133 }
134 #endif //BT_DEBUG_OSTREAM
135
136 if (!greaterZero)
137 {
138 if (maxloops == 0)
139 {
140 maxloops = 100;
141 // maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
142 }
143
144 /*start looping*/
145 for (steps = 0; steps < maxloops; steps++)
146 {
147 GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
148 #ifdef BT_DEBUG_OSTREAM
149 if (DEBUGLEVEL >= 3)
150 {
151 // cout << "A: " << A << endl;
152 cout << "pivotRowIndex " << pivotRowIndex << endl;
153 cout << "pivotColIndex " << pivotColIndex << endl;
154 cout << "Basis: ";
155 for (int i = 0; i < basis.size(); i++)
156 cout << basis[i] << " ";
157 cout << endl;
158 }
159 #endif //BT_DEBUG_OSTREAM
160
161 int pivotColIndexOld = pivotColIndex;
162
163 /*find new column index */
164 if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
165 pivotColIndex = basis[pivotRowIndex] + dim;
166 else
167 //else do it the other way round and get in the corresponding w-value
168 pivotColIndex = basis[pivotRowIndex] - dim;
169
170 /*the column becomes part of the basis*/
171 basis[pivotRowIndex] = pivotColIndexOld;
172
173 pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
174
175 if (z0Row == pivotRowIndex)
176 { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
177 GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
178 basis[pivotRowIndex] = pivotColIndex; //update basis
179 break;
180 }
181 }
182 #ifdef BT_DEBUG_OSTREAM
183 if (DEBUGLEVEL >= 1)
184 {
185 cout << "Number of loops: " << steps << endl;
186 cout << "Number of maximal loops: " << maxloops << endl;
187 }
188 #endif //BT_DEBUG_OSTREAM
189
190 if (!validBasis(basis))
191 {
192 info = -1;
193 #ifdef BT_DEBUG_OSTREAM
194 if (DEBUGLEVEL >= 1)
195 cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
196 #endif //BT_DEBUG_OSTREAM
197
198 return solutionVector;
199 }
200 }
201 #ifdef BT_DEBUG_OSTREAM
202 if (DEBUGLEVEL >= 2)
203 {
204 // cout << "A: " << A << endl;
205 cout << "pivotRowIndex " << pivotRowIndex << endl;
206 cout << "pivotColIndex " << pivotColIndex << endl;
207 }
208 #endif //BT_DEBUG_OSTREAM
209
210 for (int i = 0; i < basis.size(); i++)
211 {
212 solutionVector[basis[i]] = A(i, 2 * dim + 1); //q_[i];
213 }
214
215 info = 0;
216
217 return solutionVector;
218 }
219
220 int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex)
221 {
222 int RowIndex = 0;
223 int dim = A.rows();
224 btAlignedObjectArray<btVectorXu> Rows;
225 for (int row = 0; row < dim; row++)
226 {
227 btVectorXu vec(dim + 1);
228 vec.setZero(); //, INIT, 0.)
229 Rows.push_back(vec);
230 btScalar a = A(row, pivotColIndex);
231 if (a > 0)
232 {
233 Rows[row][0] = A(row, 2 * dim + 1) / a;
234 Rows[row][1] = A(row, 2 * dim) / a;
235 for (int j = 2; j < dim + 1; j++)
236 Rows[row][j] = A(row, j - 1) / a;
237
238 #ifdef BT_DEBUG_OSTREAM
239 // if (DEBUGLEVEL) {
240 // cout << "Rows(" << row << ") = " << Rows[row] << endl;
241 // }
242 #endif
243 }
244 }
245
246 for (int i = 0; i < Rows.size(); i++)
247 {
248 if (Rows[i].nrm2() > 0.)
249 {
250 int j = 0;
251 for (; j < Rows.size(); j++)
252 {
253 if (i != j)
254 {
255 if (Rows[j].nrm2() > 0.)
256 {
257 btVectorXu test(dim + 1);
258 for (int ii = 0; ii < dim + 1; ii++)
259 {
260 test[ii] = Rows[j][ii] - Rows[i][ii];
261 }
262
263 //=Rows[j] - Rows[i]
264 if (!LexicographicPositive(test))
265 break;
266 }
267 }
268 }
269
270 if (j == Rows.size())
271 {
272 RowIndex += i;
273 break;
274 }
275 }
276 }
277
278 return RowIndex;
279 }
280
281 bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v)
282 {
283 int i = 0;
284 // if (DEBUGLEVEL)
285 // cout << "v " << v << endl;
286
287 while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
288 i++;
289 if (v[i] > 0)
290 return true;
291
292 return false;
293 }
294
295 void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
296 {
297 btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
298 #ifdef BT_DEBUG_OSTREAM
299 cout << A << std::endl;
300 #endif
301
302 for (int i = 0; i < A.rows(); i++)
303 {
304 if (i != pivotRowIndex)
305 {
306 for (int j = 0; j < A.cols(); j++)
307 {
308 if (j != pivotColumnIndex)
309 {
310 btScalar v = A(i, j);
311 v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
312 A.setElem(i, j, v);
313 }
314 }
315 }
316 }
317
318 #ifdef BT_DEBUG_OSTREAM
319 cout << A << std::endl;
320 #endif //BT_DEBUG_OSTREAM
321 for (int i = 0; i < A.cols(); i++)
322 {
323 A.mulElem(pivotRowIndex, i, -a);
324 }
325 #ifdef BT_DEBUG_OSTREAM
326 cout << A << std::endl;
327 #endif //#ifdef BT_DEBUG_OSTREAM
328
329 for (int i = 0; i < A.rows(); i++)
330 {
331 if (i != pivotRowIndex)
332 {
333 A.setElem(i, pivotColumnIndex, 0);
334 }
335 }
336 #ifdef BT_DEBUG_OSTREAM
337 cout << A << std::endl;
338 #endif //#ifdef BT_DEBUG_OSTREAM
339 }
340
341 bool btLemkeAlgorithm::greaterZero(const btVectorXu& vector)
342 {
343 bool isGreater = true;
344 for (int i = 0; i < vector.size(); i++)
345 {
346 if (vector[i] < 0)
347 {
348 isGreater = false;
349 break;
350 }
351 }
352
353 return isGreater;
354 }
355
356 bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis)
357 {
358 bool isValid = true;
359 for (int i = 0; i < basis.size(); i++)
360 {
361 if (basis[i] >= basis.size() * 2)
362 { //then z0 is in the base
363 isValid = false;
364 break;
365 }
366 }
367
368 return isValid;
369 }
btMachEps
btScalar btMachEps()
Definition: btLemkeAlgorithm.cpp:29
btEpsRoot
btScalar btEpsRoot()
Definition: btLemkeAlgorithm.cpp:47
btLemkeAlgorithm.h
btMatrixXu
#define btMatrixXu
Definition: btMatrixX.h:529
btVectorXu
#define btVectorXu
Definition: btMatrixX.h:528
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btAlignedObjectArray< int >
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btLemkeAlgorithm::findLexicographicMinimum
int findLexicographicMinimum(const btMatrixXu &A, const int &pivotColIndex)
Definition: btLemkeAlgorithm.cpp:220
btLemkeAlgorithm::solve
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simula...
Definition: btLemkeAlgorithm.cpp:60
btLemkeAlgorithm::validBasis
bool validBasis(const btAlignedObjectArray< int > &basis)
Definition: btLemkeAlgorithm.cpp:356
btLemkeAlgorithm::greaterZero
bool greaterZero(const btVectorXu &vector)
Definition: btLemkeAlgorithm.cpp:341
btLemkeAlgorithm::LexicographicPositive
bool LexicographicPositive(const btVectorXu &v)
Definition: btLemkeAlgorithm.cpp:281
btLemkeAlgorithm::GaussJordanEliminationStep
void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray< int > &basis)
Definition: btLemkeAlgorithm.cpp:295