• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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