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 21 #include "btMLCPSolverInterface.h" 22 #include "btLemkeAlgorithm.h" 23 24 25 26 27 ///The btLemkeSolver is based on "Fast Implementation of Lemkes Algorithm for Rigid Body Contact Simulation (John E. Lloyd) " 28 ///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time. 29 ///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team 30 class btLemkeSolver : public btMLCPSolverInterface 31 { 32 protected: 33 34 public: 35 36 btScalar m_maxValue; 37 int m_debugLevel; 38 int m_maxLoops; 39 bool m_useLoHighBounds; 40 41 42 43 btLemkeSolver() 44 :m_maxValue(100000), 45 m_debugLevel(0), 46 m_maxLoops(1000), 47 m_useLoHighBounds(true) 48 { 49 } 50 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) 51 { 52 53 if (m_useLoHighBounds) 54 { 55 56 BT_PROFILE("btLemkeSolver::solveMLCP"); 57 int n = A.rows(); 58 if (0==n) 59 return true; 60 61 bool fail = false; 62 63 btVectorXu solution(n); 64 btVectorXu q1; 65 q1.resize(n); 66 for (int row=0;row<n;row++) 67 { 68 q1[row] = -b[row]; 69 } 70 71 // cout << "A" << endl; 72 // cout << A << endl; 73 74 ///////////////////////////////////// 75 76 //slow matrix inversion, replace with LU decomposition 77 btMatrixXu A1; 78 btMatrixXu B(n,n); 79 { 80 BT_PROFILE("inverse(slow)"); 81 A1.resize(A.rows(),A.cols()); 82 for (int row=0;row<A.rows();row++) 83 { 84 for (int col=0;col<A.cols();col++) 85 { 86 A1.setElem(row,col,A(row,col)); 87 } 88 } 89 90 btMatrixXu matrix; 91 matrix.resize(n,2*n); 92 for (int row=0;row<n;row++) 93 { 94 for (int col=0;col<n;col++) 95 { 96 matrix.setElem(row,col,A1(row,col)); 97 } 98 } 99 100 101 btScalar ratio,a; 102 int i,j,k; 103 for(i = 0; i < n; i++){ 104 for(j = n; j < 2*n; j++){ 105 if(i==(j-n)) 106 matrix.setElem(i,j,1.0); 107 else 108 matrix.setElem(i,j,0.0); 109 } 110 } 111 for(i = 0; i < n; i++){ 112 for(j = 0; j < n; j++){ 113 if(i!=j) 114 { 115 btScalar v = matrix(i,i); 116 if (btFuzzyZero(v)) 117 { 118 a = 0.000001f; 119 } 120 ratio = matrix(j,i)/matrix(i,i); 121 for(k = 0; k < 2*n; k++){ 122 matrix.addElem(j,k,- ratio * matrix(i,k)); 123 } 124 } 125 } 126 } 127 for(i = 0; i < n; i++){ 128 a = matrix(i,i); 129 if (btFuzzyZero(a)) 130 { 131 a = 0.000001f; 132 } 133 btScalar invA = 1.f/a; 134 for(j = 0; j < 2*n; j++){ 135 matrix.mulElem(i,j,invA); 136 } 137 } 138 139 140 141 142 143 for (int row=0;row<n;row++) 144 { 145 for (int col=0;col<n;col++) 146 { 147 B.setElem(row,col,matrix(row,n+col)); 148 } 149 } 150 } 151 152 btMatrixXu b1(n,1); 153 154 btMatrixXu M(n*2,n*2); 155 for (int row=0;row<n;row++) 156 { 157 b1.setElem(row,0,-b[row]); 158 for (int col=0;col<n;col++) 159 { 160 btScalar v =B(row,col); 161 M.setElem(row,col,v); 162 M.setElem(n+row,n+col,v); 163 M.setElem(n+row,col,-v); 164 M.setElem(row,n+col,-v); 165 166 } 167 } 168 169 btMatrixXu Bb1 = B*b1; 170 // q = [ (-B*b1 - lo)' (hi + B*b1)' ]' 171 172 btVectorXu qq; 173 qq.resize(n*2); 174 for (int row=0;row<n;row++) 175 { 176 qq[row] = -Bb1(row,0)-lo[row]; 177 qq[n+row] = Bb1(row,0)+hi[row]; 178 } 179 180 btVectorXu z1; 181 182 btMatrixXu y1; 183 y1.resize(n,1); 184 btLemkeAlgorithm lemke(M,qq,m_debugLevel); 185 { 186 BT_PROFILE("lemke.solve"); 187 lemke.setSystem(M,qq); 188 z1 = lemke.solve(m_maxLoops); 189 } 190 for (int row=0;row<n;row++) 191 { 192 y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]); 193 } 194 btMatrixXu y1_b1(n,1); 195 for (int i=0;i<n;i++) 196 { 197 y1_b1.setElem(i,0,y1(i,0)-b1(i,0)); 198 } 199 200 btMatrixXu x1; 201 202 x1 = B*(y1_b1); 203 204 for (int row=0;row<n;row++) 205 { 206 solution[row] = x1(row,0);//n]; 207 } 208 209 int errorIndexMax = -1; 210 int errorIndexMin = -1; 211 float errorValueMax = -1e30; 212 float errorValueMin = 1e30; 213 214 for (int i=0;i<n;i++) 215 { 216 x[i] = solution[i]; 217 volatile btScalar check = x[i]; 218 if (x[i] != check) 219 { 220 //printf("Lemke result is #NAN\n"); 221 x.setZero(); 222 return false; 223 } 224 225 //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 226 //we need to figure out why it happens, and fix it, or detect it properly) 227 if (x[i]>m_maxValue) 228 { 229 if (x[i]> errorValueMax) 230 { 231 fail = true; 232 errorIndexMax = i; 233 errorValueMax = x[i]; 234 } 235 ////printf("x[i] = %f,",x[i]); 236 } 237 if (x[i]<-m_maxValue) 238 { 239 if (x[i]<errorValueMin) 240 { 241 errorIndexMin = i; 242 errorValueMin = x[i]; 243 fail = true; 244 //printf("x[i] = %f,",x[i]); 245 } 246 } 247 } 248 if (fail) 249 { 250 int m_errorCountTimes = 0; 251 if (errorIndexMin<0) 252 errorValueMin = 0.f; 253 if (errorIndexMax<0) 254 errorValueMax = 0.f; 255 m_errorCountTimes++; 256 // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); 257 for (int i=0;i<n;i++) 258 { 259 x[i]=0.f; 260 } 261 } 262 return !fail; 263 } else 264 265 { 266 int dimension = A.rows(); 267 if (0==dimension) 268 return true; 269 270 // printf("================ solving using Lemke/Newton/Fixpoint\n"); 271 272 btVectorXu q; 273 q.resize(dimension); 274 for (int row=0;row<dimension;row++) 275 { 276 q[row] = -b[row]; 277 } 278 279 btLemkeAlgorithm lemke(A,q,m_debugLevel); 280 281 282 lemke.setSystem(A,q); 283 284 btVectorXu solution = lemke.solve(m_maxLoops); 285 286 //check solution 287 288 bool fail = false; 289 int errorIndexMax = -1; 290 int errorIndexMin = -1; 291 float errorValueMax = -1e30; 292 float errorValueMin = 1e30; 293 294 for (int i=0;i<dimension;i++) 295 { 296 x[i] = solution[i+dimension]; 297 volatile btScalar check = x[i]; 298 if (x[i] != check) 299 { 300 x.setZero(); 301 return false; 302 } 303 304 //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 305 //we need to figure out why it happens, and fix it, or detect it properly) 306 if (x[i]>m_maxValue) 307 { 308 if (x[i]> errorValueMax) 309 { 310 fail = true; 311 errorIndexMax = i; 312 errorValueMax = x[i]; 313 } 314 ////printf("x[i] = %f,",x[i]); 315 } 316 if (x[i]<-m_maxValue) 317 { 318 if (x[i]<errorValueMin) 319 { 320 errorIndexMin = i; 321 errorValueMin = x[i]; 322 fail = true; 323 //printf("x[i] = %f,",x[i]); 324 } 325 } 326 } 327 if (fail) 328 { 329 static int errorCountTimes = 0; 330 if (errorIndexMin<0) 331 errorValueMin = 0.f; 332 if (errorIndexMax<0) 333 errorValueMax = 0.f; 334 printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); 335 for (int i=0;i<dimension;i++) 336 { 337 x[i]=0.f; 338 } 339 } 340 341 342 return !fail; 343 } 344 return true; 345 346 } 347 348 }; 349 350 #endif //BT_LEMKE_SOLVER_H 351
