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