2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
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:
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.
15 ///original version written by Erwin Coumans, October 2013
17 #ifndef BT_LEMKE_SOLVER_H
18 #define BT_LEMKE_SOLVER_H
20 #include "btMLCPSolverInterface.h"
21 #include "btLemkeAlgorithm.h"
23 ///The btLemkeSolver is based on "Fast Implementation of Lemkeās Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
24 ///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
25 ///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
26 class btLemkeSolver : public btMLCPSolverInterface
33 bool m_useLoHighBounds;
39 m_useLoHighBounds(true)
42 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)
44 if (m_useLoHighBounds)
46 BT_PROFILE("btLemkeSolver::solveMLCP");
53 btVectorXu solution(n);
56 for (int row = 0; row < n; row++)
61 // cout << "A" << endl;
64 /////////////////////////////////////
66 //slow matrix inversion, replace with LU decomposition
70 //BT_PROFILE("inverse(slow)");
71 A1.resize(A.rows(), A.cols());
72 for (int row = 0; row < A.rows(); row++)
74 for (int col = 0; col < A.cols(); col++)
76 A1.setElem(row, col, A(row, col));
81 matrix.resize(n, 2 * n);
82 for (int row = 0; row < n; row++)
84 for (int col = 0; col < n; col++)
86 matrix.setElem(row, col, A1(row, col));
92 for (i = 0; i < n; i++)
94 for (j = n; j < 2 * n; j++)
97 matrix.setElem(i, j, 1.0);
99 matrix.setElem(i, j, 0.0);
102 for (i = 0; i < n; i++)
104 for (j = 0; j < n; j++)
108 btScalar v = matrix(i, i);
113 ratio = matrix(j, i) / matrix(i, i);
114 for (k = 0; k < 2 * n; k++)
116 matrix.addElem(j, k, -ratio * matrix(i, k));
121 for (i = 0; i < n; i++)
128 btScalar invA = 1.f / a;
129 for (j = 0; j < 2 * n; j++)
131 matrix.mulElem(i, j, invA);
135 for (int row = 0; row < n; row++)
137 for (int col = 0; col < n; col++)
139 B.setElem(row, col, matrix(row, n + col));
146 btMatrixXu M(n * 2, n * 2);
147 for (int row = 0; row < n; row++)
149 b1.setElem(row, 0, -b[row]);
150 for (int col = 0; col < n; col++)
152 btScalar v = B(row, col);
153 M.setElem(row, col, v);
154 M.setElem(n + row, n + col, v);
155 M.setElem(n + row, col, -v);
156 M.setElem(row, n + col, -v);
160 btMatrixXu Bb1 = B * b1;
161 // q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
165 for (int row = 0; row < n; row++)
167 qq[row] = -Bb1(row, 0) - lo[row];
168 qq[n + row] = Bb1(row, 0) + hi[row];
175 btLemkeAlgorithm lemke(M, qq, m_debugLevel);
177 //BT_PROFILE("lemke.solve");
178 lemke.setSystem(M, qq);
179 z1 = lemke.solve(m_maxLoops);
181 for (int row = 0; row < n; row++)
183 y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]);
185 btMatrixXu y1_b1(n, 1);
186 for (int i = 0; i < n; i++)
188 y1_b1.setElem(i, 0, y1(i, 0) - b1(i, 0));
195 for (int row = 0; row < n; row++)
197 solution[row] = x1(row, 0); //n];
200 int errorIndexMax = -1;
201 int errorIndexMin = -1;
202 float errorValueMax = -1e30;
203 float errorValueMin = 1e30;
205 for (int i = 0; i < n; i++)
208 volatile btScalar check = x[i];
211 //printf("Lemke result is #NAN\n");
216 //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
217 //we need to figure out why it happens, and fix it, or detect it properly)
218 if (x[i] > m_maxValue)
220 if (x[i] > errorValueMax)
224 errorValueMax = x[i];
226 ////printf("x[i] = %f,",x[i]);
228 if (x[i] < -m_maxValue)
230 if (x[i] < errorValueMin)
233 errorValueMin = x[i];
235 //printf("x[i] = %f,",x[i]);
241 int m_errorCountTimes = 0;
242 if (errorIndexMin < 0)
244 if (errorIndexMax < 0)
247 // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
248 for (int i = 0; i < n; i++)
258 int dimension = A.rows();
262 // printf("================ solving using Lemke/Newton/Fixpoint\n");
266 for (int row = 0; row < dimension; row++)
271 btLemkeAlgorithm lemke(A, q, m_debugLevel);
273 lemke.setSystem(A, q);
275 btVectorXu solution = lemke.solve(m_maxLoops);
280 int errorIndexMax = -1;
281 int errorIndexMin = -1;
282 float errorValueMax = -1e30;
283 float errorValueMin = 1e30;
285 for (int i = 0; i < dimension; i++)
287 x[i] = solution[i + dimension];
288 volatile btScalar check = x[i];
295 //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
296 //we need to figure out why it happens, and fix it, or detect it properly)
297 if (x[i] > m_maxValue)
299 if (x[i] > errorValueMax)
303 errorValueMax = x[i];
305 ////printf("x[i] = %f,",x[i]);
307 if (x[i] < -m_maxValue)
309 if (x[i] < errorValueMin)
312 errorValueMin = x[i];
314 //printf("x[i] = %f,",x[i]);
320 static int errorCountTimes = 0;
321 if (errorIndexMin < 0)
323 if (errorIndexMax < 0)
325 printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin, errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
326 for (int i = 0; i < dimension; i++)
338 #endif //BT_LEMKE_SOLVER_H