1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2014 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 16 #ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H 17 #define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H 18 19 #include "LinearMath/btTransform.h" // Note that btVector3 might be double precision... 20 #include "btGjkEpa3.h" 21 #include "btGjkCollisionDescription.h" 22 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" 23 24 25 26 27 28 29 template <typename btConvexTemplate> 30 bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b, 31 const btGjkCollisionDescription& colDesc, 32 btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB) 33 { 34 (void)v; 35 36 // const btScalar radialmargin(btScalar(0.)); 37 38 btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input? 39 40 btGjkEpaSolver3::sResults results; 41 42 43 if(btGjkEpaSolver3_Penetration(a,b,guessVector,results)) 44 45 { 46 // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); 47 //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); 48 wWitnessOnA = results.witnesses[0]; 49 wWitnessOnB = results.witnesses[1]; 50 v = results.normal; 51 return true; 52 } else 53 { 54 if(btGjkEpaSolver3_Distance(a,b,guessVector,results)) 55 { 56 wWitnessOnA = results.witnesses[0]; 57 wWitnessOnB = results.witnesses[1]; 58 v = results.normal; 59 return false; 60 } 61 } 62 return false; 63 } 64 65 template <typename btConvexTemplate, typename btGjkDistanceTemplate> 66 int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo) 67 { 68 69 bool m_catchDegeneracies = true; 70 btScalar m_cachedSeparatingDistance = 0.f; 71 72 btScalar distance=btScalar(0.); 73 btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); 74 75 btVector3 pointOnA,pointOnB; 76 btTransform localTransA = a.getWorldTransform(); 77 btTransform localTransB = b.getWorldTransform(); 78 79 btScalar marginA = a.getMargin(); 80 btScalar marginB = b.getMargin(); 81 82 int m_curIter = 0; 83 int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN? 84 btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir; 85 86 bool isValid = false; 87 bool checkSimplex = false; 88 bool checkPenetration = true; 89 int m_degenerateSimplex = 0; 90 91 int m_lastUsedMethod = -1; 92 93 { 94 btScalar squaredDistance = BT_LARGE_FLOAT; 95 btScalar delta = btScalar(0.); 96 97 btScalar margin = marginA + marginB; 98 99 100 101 simplexSolver.reset(); 102 103 for ( ; ; ) 104 //while (true) 105 { 106 107 btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis(); 108 btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis(); 109 110 btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA); 111 btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB); 112 113 btVector3 pWorld = localTransA(pInA); 114 btVector3 qWorld = localTransB(qInB); 115 116 117 118 btVector3 w = pWorld - qWorld; 119 delta = m_cachedSeparatingAxis.dot(w); 120 121 // potential exit, they don't overlap 122 if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared)) 123 { 124 m_degenerateSimplex = 10; 125 checkSimplex=true; 126 //checkPenetration = false; 127 break; 128 } 129 130 //exit 0: the new point is already in the simplex, or we didn't come any closer 131 if (simplexSolver.inSimplex(w)) 132 { 133 m_degenerateSimplex = 1; 134 checkSimplex = true; 135 break; 136 } 137 // are we getting any closer ? 138 btScalar f0 = squaredDistance - delta; 139 btScalar f1 = squaredDistance * colDesc.m_gjkRelError2; 140 141 if (f0 <= f1) 142 { 143 if (f0 <= btScalar(0.)) 144 { 145 m_degenerateSimplex = 2; 146 } else 147 { 148 m_degenerateSimplex = 11; 149 } 150 checkSimplex = true; 151 break; 152 } 153 154 //add current vertex to simplex 155 simplexSolver.addVertex(w, pWorld, qWorld); 156 btVector3 newCachedSeparatingAxis; 157 158 //calculate the closest point to the origin (update vector v) 159 if (!simplexSolver.closest(newCachedSeparatingAxis)) 160 { 161 m_degenerateSimplex = 3; 162 checkSimplex = true; 163 break; 164 } 165 166 if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2) 167 { 168 m_cachedSeparatingAxis = newCachedSeparatingAxis; 169 m_degenerateSimplex = 6; 170 checkSimplex = true; 171 break; 172 } 173 174 btScalar previousSquaredDistance = squaredDistance; 175 squaredDistance = newCachedSeparatingAxis.length2(); 176 #if 0 177 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo 178 if (squaredDistance>previousSquaredDistance) 179 { 180 m_degenerateSimplex = 7; 181 squaredDistance = previousSquaredDistance; 182 checkSimplex = false; 183 break; 184 } 185 #endif // 186 187 188 //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); 189 190 //are we getting any closer ? 191 if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 192 { 193 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); 194 checkSimplex = true; 195 m_degenerateSimplex = 12; 196 197 break; 198 } 199 200 m_cachedSeparatingAxis = newCachedSeparatingAxis; 201 202 //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject 203 if (m_curIter++ > gGjkMaxIter) 204 { 205 #if defined(DEBUG) || defined (_DEBUG) 206 207 printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); 208 printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n", 209 m_cachedSeparatingAxis.getX(), 210 m_cachedSeparatingAxis.getY(), 211 m_cachedSeparatingAxis.getZ(), 212 squaredDistance); 213 #endif 214 215 break; 216 217 } 218 219 220 bool check = (!simplexSolver.fullSimplex()); 221 //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); 222 223 if (!check) 224 { 225 //do we need this backup_closest here ? 226 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); 227 m_degenerateSimplex = 13; 228 break; 229 } 230 } 231 232 if (checkSimplex) 233 { 234 simplexSolver.compute_points(pointOnA, pointOnB); 235 normalInB = m_cachedSeparatingAxis; 236 237 btScalar lenSqr =m_cachedSeparatingAxis.length2(); 238 239 //valid normal 240 if (lenSqr < 0.0001) 241 { 242 m_degenerateSimplex = 5; 243 } 244 if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) 245 { 246 btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); 247 normalInB *= rlen; //normalize 248 249 btScalar s = btSqrt(squaredDistance); 250 251 btAssert(s > btScalar(0.0)); 252 pointOnA -= m_cachedSeparatingAxis * (marginA / s); 253 pointOnB += m_cachedSeparatingAxis * (marginB / s); 254 distance = ((btScalar(1.)/rlen) - margin); 255 isValid = true; 256 257 m_lastUsedMethod = 1; 258 } else 259 { 260 m_lastUsedMethod = 2; 261 } 262 } 263 264 bool catchDegeneratePenetrationCase = 265 (m_catchDegeneracies && m_degenerateSimplex && ((distance+margin) < 0.01)); 266 267 //if (checkPenetration && !isValid) 268 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) 269 { 270 //penetration case 271 272 //if there is no way to handle penetrations, bail out 273 274 // Penetration depth case. 275 btVector3 tmpPointOnA,tmpPointOnB; 276 277 m_cachedSeparatingAxis.setZero(); 278 279 bool isValid2 = btGjkEpaCalcPenDepth(a,b, 280 colDesc, 281 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB); 282 283 if (isValid2) 284 { 285 btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; 286 btScalar lenSqr = tmpNormalInB.length2(); 287 if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) 288 { 289 tmpNormalInB = m_cachedSeparatingAxis; 290 lenSqr = m_cachedSeparatingAxis.length2(); 291 } 292 293 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) 294 { 295 tmpNormalInB /= btSqrt(lenSqr); 296 btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); 297 //only replace valid penetrations when the result is deeper (check) 298 if (!isValid || (distance2 < distance)) 299 { 300 distance = distance2; 301 pointOnA = tmpPointOnA; 302 pointOnB = tmpPointOnB; 303 normalInB = tmpNormalInB; 304 305 isValid = true; 306 m_lastUsedMethod = 3; 307 } else 308 { 309 m_lastUsedMethod = 8; 310 } 311 } else 312 { 313 m_lastUsedMethod = 9; 314 } 315 } else 316 317 { 318 ///this is another degenerate case, where the initial GJK calculation reports a degenerate case 319 ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) 320 ///reports a valid positive distance. Use the results of the second GJK instead of failing. 321 ///thanks to Jacob.Langford for the reproduction case 322 ///http://code.google.com/p/bullet/issues/detail?id=250 323 324 325 if (m_cachedSeparatingAxis.length2() > btScalar(0.)) 326 { 327 btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; 328 //only replace valid distances when the distance is less 329 if (!isValid || (distance2 < distance)) 330 { 331 distance = distance2; 332 pointOnA = tmpPointOnA; 333 pointOnB = tmpPointOnB; 334 pointOnA -= m_cachedSeparatingAxis * marginA ; 335 pointOnB += m_cachedSeparatingAxis * marginB ; 336 normalInB = m_cachedSeparatingAxis; 337 normalInB.normalize(); 338 339 isValid = true; 340 m_lastUsedMethod = 6; 341 } else 342 { 343 m_lastUsedMethod = 5; 344 } 345 } 346 } 347 } 348 } 349 350 351 352 if (isValid && ((distance < 0) || (distance*distance < colDesc.m_maximumDistanceSquared))) 353 { 354 355 m_cachedSeparatingAxis = normalInB; 356 m_cachedSeparatingDistance = distance; 357 distInfo->m_distance = distance; 358 distInfo->m_normalBtoA = normalInB; 359 distInfo->m_pointOnB = pointOnB; 360 distInfo->m_pointOnA = pointOnB+normalInB*distance; 361 return 0; 362 } 363 return -m_lastUsedMethod; 364 } 365 366 367 368 369 #endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H 370
