1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org 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 ///original version written by Erwin Coumans, October 2013 16 17 #ifndef BT_LEMKE_SOLVER_H 18 #define BT_LEMKE_SOLVER_H 19 20 #include "btMLCPSolverInterface.h" 21 #include "btLemkeAlgorithm.h" 22 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 27 { 28 protected: 29 public: 30 btScalar m_maxValue; 31 int m_debugLevel; 32 int m_maxLoops; 33 bool m_useLoHighBounds; 34 btLemkeSolver()35 btLemkeSolver() 36 : m_maxValue(100000), 37 m_debugLevel(0), 38 m_maxLoops(1000), 39 m_useLoHighBounds(true) 40 { 41 } 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) 43 { 44 if (m_useLoHighBounds) 45 { 46 BT_PROFILE("btLemkeSolver::solveMLCP"); 47 int n = A.rows(); 48 if (0 == n) 49 return true; 50 51 bool fail = false; 52 53 btVectorXu solution(n); 54 btVectorXu q1; 55 q1.resize(n); 56 for (int row = 0; row < n; row++) 57 { 58 q1[row] = -b[row]; 59 } 60 61 // cout << "A" << endl; 62 // cout << A << endl; 63 64 ///////////////////////////////////// 65 66 //slow matrix inversion, replace with LU decomposition 67 btMatrixXu A1; 68 btMatrixXu B(n, n); 69 { 70 //BT_PROFILE("inverse(slow)"); 71 A1.resize(A.rows(), A.cols()); 72 for (int row = 0; row < A.rows(); row++) 73 { 74 for (int col = 0; col < A.cols(); col++) 75 { 76 A1.setElem(row, col, A(row, col)); 77 } 78 } 79 80 btMatrixXu matrix; 81 matrix.resize(n, 2 * n); 82 for (int row = 0; row < n; row++) 83 { 84 for (int col = 0; col < n; col++) 85 { 86 matrix.setElem(row, col, A1(row, col)); 87 } 88 } 89 90 btScalar ratio, a; 91 int i, j, k; 92 for (i = 0; i < n; i++) 93 { 94 for (j = n; j < 2 * n; j++) 95 { 96 if (i == (j - n)) 97 matrix.setElem(i, j, 1.0); 98 else 99 matrix.setElem(i, j, 0.0); 100 } 101 } 102 for (i = 0; i < n; i++) 103 { 104 for (j = 0; j < n; j++) 105 { 106 if (i != j) 107 { 108 btScalar v = matrix(i, i); 109 if (btFuzzyZero(v)) 110 { 111 a = 0.000001f; 112 } 113 ratio = matrix(j, i) / matrix(i, i); 114 for (k = 0; k < 2 * n; k++) 115 { 116 matrix.addElem(j, k, -ratio * matrix(i, k)); 117 } 118 } 119 } 120 } 121 for (i = 0; i < n; i++) 122 { 123 a = matrix(i, i); 124 if (btFuzzyZero(a)) 125 { 126 a = 0.000001f; 127 } 128 btScalar invA = 1.f / a; 129 for (j = 0; j < 2 * n; j++) 130 { 131 matrix.mulElem(i, j, invA); 132 } 133 } 134 135 for (int row = 0; row < n; row++) 136 { 137 for (int col = 0; col < n; col++) 138 { 139 B.setElem(row, col, matrix(row, n + col)); 140 } 141 } 142 } 143 144 btMatrixXu b1(n, 1); 145 146 btMatrixXu M(n * 2, n * 2); 147 for (int row = 0; row < n; row++) 148 { 149 b1.setElem(row, 0, -b[row]); 150 for (int col = 0; col < n; col++) 151 { 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); 157 } 158 } 159 160 btMatrixXu Bb1 = B * b1; 161 // q = [ (-B*b1 - lo)' (hi + B*b1)' ]' 162 163 btVectorXu qq; 164 qq.resize(n * 2); 165 for (int row = 0; row < n; row++) 166 { 167 qq[row] = -Bb1(row, 0) - lo[row]; 168 qq[n + row] = Bb1(row, 0) + hi[row]; 169 } 170 171 btVectorXu z1; 172 173 btMatrixXu y1; 174 y1.resize(n, 1); 175 btLemkeAlgorithm lemke(M, qq, m_debugLevel); 176 { 177 //BT_PROFILE("lemke.solve"); 178 lemke.setSystem(M, qq); 179 z1 = lemke.solve(m_maxLoops); 180 } 181 for (int row = 0; row < n; row++) 182 { 183 y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]); 184 } 185 btMatrixXu y1_b1(n, 1); 186 for (int i = 0; i < n; i++) 187 { 188 y1_b1.setElem(i, 0, y1(i, 0) - b1(i, 0)); 189 } 190 191 btMatrixXu x1; 192 193 x1 = B * (y1_b1); 194 195 for (int row = 0; row < n; row++) 196 { 197 solution[row] = x1(row, 0); //n]; 198 } 199 200 int errorIndexMax = -1; 201 int errorIndexMin = -1; 202 float errorValueMax = -1e30; 203 float errorValueMin = 1e30; 204 205 for (int i = 0; i < n; i++) 206 { 207 x[i] = solution[i]; 208 volatile btScalar check = x[i]; 209 if (x[i] != check) 210 { 211 //printf("Lemke result is #NAN\n"); 212 x.setZero(); 213 return false; 214 } 215 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) 219 { 220 if (x[i] > errorValueMax) 221 { 222 fail = true; 223 errorIndexMax = i; 224 errorValueMax = x[i]; 225 } 226 ////printf("x[i] = %f,",x[i]); 227 } 228 if (x[i] < -m_maxValue) 229 { 230 if (x[i] < errorValueMin) 231 { 232 errorIndexMin = i; 233 errorValueMin = x[i]; 234 fail = true; 235 //printf("x[i] = %f,",x[i]); 236 } 237 } 238 } 239 if (fail) 240 { 241 int m_errorCountTimes = 0; 242 if (errorIndexMin < 0) 243 errorValueMin = 0.f; 244 if (errorIndexMax < 0) 245 errorValueMax = 0.f; 246 m_errorCountTimes++; 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++) 249 { 250 x[i] = 0.f; 251 } 252 } 253 return !fail; 254 } 255 else 256 257 { 258 int dimension = A.rows(); 259 if (0 == dimension) 260 return true; 261 262 // printf("================ solving using Lemke/Newton/Fixpoint\n"); 263 264 btVectorXu q; 265 q.resize(dimension); 266 for (int row = 0; row < dimension; row++) 267 { 268 q[row] = -b[row]; 269 } 270 271 btLemkeAlgorithm lemke(A, q, m_debugLevel); 272 273 lemke.setSystem(A, q); 274 275 btVectorXu solution = lemke.solve(m_maxLoops); 276 277 //check solution 278 279 bool fail = false; 280 int errorIndexMax = -1; 281 int errorIndexMin = -1; 282 float errorValueMax = -1e30; 283 float errorValueMin = 1e30; 284 285 for (int i = 0; i < dimension; i++) 286 { 287 x[i] = solution[i + dimension]; 288 volatile btScalar check = x[i]; 289 if (x[i] != check) 290 { 291 x.setZero(); 292 return false; 293 } 294 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) 298 { 299 if (x[i] > errorValueMax) 300 { 301 fail = true; 302 errorIndexMax = i; 303 errorValueMax = x[i]; 304 } 305 ////printf("x[i] = %f,",x[i]); 306 } 307 if (x[i] < -m_maxValue) 308 { 309 if (x[i] < errorValueMin) 310 { 311 errorIndexMin = i; 312 errorValueMin = x[i]; 313 fail = true; 314 //printf("x[i] = %f,",x[i]); 315 } 316 } 317 } 318 if (fail) 319 { 320 static int errorCountTimes = 0; 321 if (errorIndexMin < 0) 322 errorValueMin = 0.f; 323 if (errorIndexMax < 0) 324 errorValueMax = 0.f; 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++) 327 { 328 x[i] = 0.f; 329 } 330 } 331 332 return !fail; 333 } 334 return true; 335 } 336 }; 337 338 #endif //BT_LEMKE_SOLVER_H 339