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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btLemkeAlgorithm.cpp
Go to the documentation of this file.
1/* Copyright (C) 2004-2013 MBSim Development Team
2
3Code was converted for the Bullet Continuous Collision Detection and Physics Library
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*/
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
26using 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 bool isRayTermination = false;
173 pivotRowIndex = findLexicographicMinimum(A, pivotColIndex, z0Row, isRayTermination);
174 if (isRayTermination)
175 {
176 break; // ray termination
177 }
178 if (z0Row == pivotRowIndex)
179 { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
180 GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
181 basis[pivotRowIndex] = pivotColIndex; //update basis
182 break;
183 }
184 }
185#ifdef BT_DEBUG_OSTREAM
186 if (DEBUGLEVEL >= 1)
187 {
188 cout << "Number of loops: " << steps << endl;
189 cout << "Number of maximal loops: " << maxloops << endl;
190 }
191#endif //BT_DEBUG_OSTREAM
192
193 if (!validBasis(basis))
194 {
195 info = -1;
196#ifdef BT_DEBUG_OSTREAM
197 if (DEBUGLEVEL >= 1)
198 cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
199#endif //BT_DEBUG_OSTREAM
200
201 return solutionVector;
202 }
203 }
204#ifdef BT_DEBUG_OSTREAM
205 if (DEBUGLEVEL >= 2)
206 {
207 // cout << "A: " << A << endl;
208 cout << "pivotRowIndex " << pivotRowIndex << endl;
209 cout << "pivotColIndex " << pivotColIndex << endl;
210 }
211#endif //BT_DEBUG_OSTREAM
212
213 for (int i = 0; i < basis.size(); i++)
214 {
215 solutionVector[basis[i]] = A(i, 2 * dim + 1); //q_[i];
216 }
217
218 info = 0;
219
220 return solutionVector;
221}
222
223 int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex, const int& z0Row, bool& isRayTermination)
224{
225 isRayTermination = false;
226 btAlignedObjectArray<int> activeRows;
227
228 bool firstRow = true;
229 btScalar currentMin = 0.0;
230
231 int dim = A.rows();
232
233 for (int row = 0; row < dim; row++)
234 {
235 const btScalar denom = A(row, pivotColIndex);
236
237 if (denom > btMachEps())
238 {
239 const btScalar q = A(row, dim + dim + 1) / denom;
240 if (firstRow)
241 {
242 currentMin = q;
243 activeRows.push_back(row);
244 firstRow = false;
245 }
246 else if (fabs(currentMin - q) < btMachEps())
247 {
248 activeRows.push_back(row);
249 }
250 else if (currentMin > q)
251 {
252 currentMin = q;
253 activeRows.clear();
254 activeRows.push_back(row);
255 }
256 }
257 }
258
259 if (activeRows.size() == 0)
260 {
261 isRayTermination = true;
262 return 0;
263 }
264 else if (activeRows.size() == 1)
265 {
266 return activeRows[0];
267 }
268
269 // if there are multiple rows, check if they contain the row for z_0.
270 for (int i = 0; i < activeRows.size(); i++)
271 {
272 if (activeRows[i] == z0Row)
273 {
274 return z0Row;
275 }
276 }
277
278 // look through the columns of the inverse of the basic matrix from left to right until the tie is broken.
279 for (int col = 0; col < dim ; col++)
280 {
281 btAlignedObjectArray<int> activeRowsCopy(activeRows);
282 activeRows.clear();
283 firstRow = true;
284 for (int i = 0; i<activeRowsCopy.size();i++)
285 {
286 const int row = activeRowsCopy[i];
287
288 // denom is positive here as an invariant.
289 const btScalar denom = A(row, pivotColIndex);
290 const btScalar ratio = A(row, col) / denom;
291 if (firstRow)
292 {
293 currentMin = ratio;
294 activeRows.push_back(row);
295 firstRow = false;
296 }
297 else if (fabs(currentMin - ratio) < btMachEps())
298 {
299 activeRows.push_back(row);
300 }
301 else if (currentMin > ratio)
302 {
303 currentMin = ratio;
304 activeRows.clear();
305 activeRows.push_back(row);
306 }
307 }
308
309 if (activeRows.size() == 1)
310 {
311 return activeRows[0];
312 }
313 }
314 // must not reach here.
315 isRayTermination = true;
316 return 0;
317}
318
319 void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
320{
321 btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
322#ifdef BT_DEBUG_OSTREAM
323 cout << A << std::endl;
324#endif
325
326 for (int i = 0; i < A.rows(); i++)
327 {
328 if (i != pivotRowIndex)
329 {
330 for (int j = 0; j < A.cols(); j++)
331 {
332 if (j != pivotColumnIndex)
333 {
334 btScalar v = A(i, j);
335 v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
336 A.setElem(i, j, v);
337 }
338 }
339 }
340 }
341
342#ifdef BT_DEBUG_OSTREAM
343 cout << A << std::endl;
344#endif //BT_DEBUG_OSTREAM
345 for (int i = 0; i < A.cols(); i++)
346 {
347 A.mulElem(pivotRowIndex, i, -a);
348 }
349#ifdef BT_DEBUG_OSTREAM
350 cout << A << std::endl;
351#endif //#ifdef BT_DEBUG_OSTREAM
352
353 for (int i = 0; i < A.rows(); i++)
354 {
355 if (i != pivotRowIndex)
356 {
357 A.setElem(i, pivotColumnIndex, 0);
358 }
359 }
360#ifdef BT_DEBUG_OSTREAM
361 cout << A << std::endl;
362#endif //#ifdef BT_DEBUG_OSTREAM
363}
364
365 bool btLemkeAlgorithm::greaterZero(const btVectorXu& vector)
366{
367 bool isGreater = true;
368 for (int i = 0; i < vector.size(); i++)
369 {
370 if (vector[i] < 0)
371 {
372 isGreater = false;
373 break;
374 }
375 }
376
377 return isGreater;
378}
379
380 bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis)
381{
382 bool isValid = true;
383 for (int i = 0; i < basis.size(); i++)
384 {
385 if (basis[i] >= basis.size() * 2)
386 { //then z0 is in the base
387 isValid = false;
388 break;
389 }
390 }
391
392 return isValid;
393}
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
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:46
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::clear
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
Definition: btAlignedObjectArray.h:176
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btLemkeAlgorithm::m_M
btMatrixXu m_M
Definition: btLemkeAlgorithm.h:79
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:380
btLemkeAlgorithm::info
int info
did the algorithm find a solution
Definition: btLemkeAlgorithm.h:98
btLemkeAlgorithm::findLexicographicMinimum
int findLexicographicMinimum(const btMatrixXu &A, const int &pivotColIndex, const int &z0Row, bool &isRayTermination)
Definition: btLemkeAlgorithm.cpp:223
btLemkeAlgorithm::greaterZero
bool greaterZero(const btVectorXu &vector)
Definition: btLemkeAlgorithm.cpp:365
btLemkeAlgorithm::DEBUGLEVEL
int DEBUGLEVEL
define level of debug output
Definition: btLemkeAlgorithm.h:90
btLemkeAlgorithm::steps
unsigned int steps
number of steps until the Lemke algorithm found a solution
Definition: btLemkeAlgorithm.h:85
btLemkeAlgorithm::GaussJordanEliminationStep
void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray< int > &basis)
Definition: btLemkeAlgorithm.cpp:319
btLemkeAlgorithm::m_q
btVectorXu m_q
Definition: btLemkeAlgorithm.h:80