1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 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 #include "btMinkowskiPenetrationDepthSolver.h" 17 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" 18 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" 19 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" 20 #include "BulletCollision/CollisionShapes/btConvexShape.h" 21 22 #define NUM_UNITSPHERE_POINTS 42 23 24 25 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, 26 const btConvexShape* convexA,const btConvexShape* convexB, 27 const btTransform& transA,const btTransform& transB, 28 btVector3& v, btVector3& pa, btVector3& pb, 29 class btIDebugDraw* debugDraw 30 ) 31 { 32 33 (void)v; 34 35 bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); 36 37 struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result 38 { 39 40 btIntermediateResult():m_hasResult(false) 41 { 42 } 43 44 btVector3 m_normalOnBInWorld; 45 btVector3 m_pointInWorld; 46 btScalar m_depth; 47 bool m_hasResult; 48 49 virtual void setShapeIdentifiersA(int partId0,int index0) 50 { 51 (void)partId0; 52 (void)index0; 53 } 54 virtual void setShapeIdentifiersB(int partId1,int index1) 55 { 56 (void)partId1; 57 (void)index1; 58 } 59 void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) 60 { 61 m_normalOnBInWorld = normalOnBInWorld; 62 m_pointInWorld = pointInWorld; 63 m_depth = depth; 64 m_hasResult = true; 65 } 66 }; 67 68 //just take fixed number of orientation, and sample the penetration depth in that direction 69 btScalar minProj = btScalar(BT_LARGE_FLOAT); 70 btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.)); 71 btVector3 minA,minB; 72 btVector3 seperatingAxisInA,seperatingAxisInB; 73 btVector3 pInA,qInB,pWorld,qWorld,w; 74 75 #ifndef __SPU__ 76 #define USE_BATCHED_SUPPORT 1 77 #endif 78 #ifdef USE_BATCHED_SUPPORT 79 80 btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; 81 btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; 82 btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; 83 btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; 84 int i; 85 86 int numSampleDirections = NUM_UNITSPHERE_POINTS; 87 88 for (i=0;i<numSampleDirections;i++) 89 { 90 btVector3 norm = getPenetrationDirections()[i]; 91 seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; 92 seperatingAxisInBBatch[i] = norm * transB.getBasis() ; 93 } 94 95 { 96 int numPDA = convexA->getNumPreferredPenetrationDirections(); 97 if (numPDA) 98 { 99 for (int i=0;i<numPDA;i++) 100 { 101 btVector3 norm; 102 convexA->getPreferredPenetrationDirection(i,norm); 103 norm = transA.getBasis() * norm; 104 getPenetrationDirections()[numSampleDirections] = norm; 105 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); 106 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); 107 numSampleDirections++; 108 } 109 } 110 } 111 112 { 113 int numPDB = convexB->getNumPreferredPenetrationDirections(); 114 if (numPDB) 115 { 116 for (int i=0;i<numPDB;i++) 117 { 118 btVector3 norm; 119 convexB->getPreferredPenetrationDirection(i,norm); 120 norm = transB.getBasis() * norm; 121 getPenetrationDirections()[numSampleDirections] = norm; 122 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); 123 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); 124 numSampleDirections++; 125 } 126 } 127 } 128 129 130 131 132 convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); 133 convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); 134 135 for (i=0;i<numSampleDirections;i++) 136 { 137 btVector3 norm = getPenetrationDirections()[i]; 138 if (check2d) 139 { 140 norm[2] = 0.f; 141 } 142 if (norm.length2()>0.01) 143 { 144 145 seperatingAxisInA = seperatingAxisInABatch[i]; 146 seperatingAxisInB = seperatingAxisInBBatch[i]; 147 148 pInA = supportVerticesABatch[i]; 149 qInB = supportVerticesBBatch[i]; 150 151 pWorld = transA(pInA); 152 qWorld = transB(qInB); 153 if (check2d) 154 { 155 pWorld[2] = 0.f; 156 qWorld[2] = 0.f; 157 } 158 159 w = qWorld - pWorld; 160 btScalar delta = norm.dot(w); 161 //find smallest delta 162 if (delta < minProj) 163 { 164 minProj = delta; 165 minNorm = norm; 166 minA = pWorld; 167 minB = qWorld; 168 } 169 } 170 } 171 #else 172 173 int numSampleDirections = NUM_UNITSPHERE_POINTS; 174 175 #ifndef __SPU__ 176 { 177 int numPDA = convexA->getNumPreferredPenetrationDirections(); 178 if (numPDA) 179 { 180 for (int i=0;i<numPDA;i++) 181 { 182 btVector3 norm; 183 convexA->getPreferredPenetrationDirection(i,norm); 184 norm = transA.getBasis() * norm; 185 getPenetrationDirections()[numSampleDirections] = norm; 186 numSampleDirections++; 187 } 188 } 189 } 190 191 { 192 int numPDB = convexB->getNumPreferredPenetrationDirections(); 193 if (numPDB) 194 { 195 for (int i=0;i<numPDB;i++) 196 { 197 btVector3 norm; 198 convexB->getPreferredPenetrationDirection(i,norm); 199 norm = transB.getBasis() * norm; 200 getPenetrationDirections()[numSampleDirections] = norm; 201 numSampleDirections++; 202 } 203 } 204 } 205 #endif // __SPU__ 206 207 for (int i=0;i<numSampleDirections;i++) 208 { 209 const btVector3& norm = getPenetrationDirections()[i]; 210 seperatingAxisInA = (-norm)* transA.getBasis(); 211 seperatingAxisInB = norm* transB.getBasis(); 212 pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); 213 qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); 214 pWorld = transA(pInA); 215 qWorld = transB(qInB); 216 w = qWorld - pWorld; 217 btScalar delta = norm.dot(w); 218 //find smallest delta 219 if (delta < minProj) 220 { 221 minProj = delta; 222 minNorm = norm; 223 minA = pWorld; 224 minB = qWorld; 225 } 226 } 227 #endif //USE_BATCHED_SUPPORT 228 229 //add the margins 230 231 minA += minNorm*convexA->getMarginNonVirtual(); 232 minB -= minNorm*convexB->getMarginNonVirtual(); 233 //no penetration 234 if (minProj < btScalar(0.)) 235 return false; 236 237 btScalar extraSeparation = 0.5f;///scale dependent 238 minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); 239 240 241 242 243 244 //#define DEBUG_DRAW 1 245 #ifdef DEBUG_DRAW 246 if (debugDraw) 247 { 248 btVector3 color(0,1,0); 249 debugDraw->drawLine(minA,minB,color); 250 color = btVector3 (1,1,1); 251 btVector3 vec = minB-minA; 252 btScalar prj2 = minNorm.dot(vec); 253 debugDraw->drawLine(minA,minA+(minNorm*minProj),color); 254 255 } 256 #endif //DEBUG_DRAW 257 258 259 260 btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); 261 262 btScalar offsetDist = minProj; 263 btVector3 offset = minNorm * offsetDist; 264 265 266 267 btGjkPairDetector::ClosestPointInput input; 268 269 btVector3 newOrg = transA.getOrigin() + offset; 270 271 btTransform displacedTrans = transA; 272 displacedTrans.setOrigin(newOrg); 273 274 input.m_transformA = displacedTrans; 275 input.m_transformB = transB; 276 input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; 277 278 btIntermediateResult res; 279 gjkdet.setCachedSeperatingAxis(-minNorm); 280 gjkdet.getClosestPoints(input,res,debugDraw); 281 282 btScalar correctedMinNorm = minProj - res.m_depth; 283 284 285 //the penetration depth is over-estimated, relax it 286 btScalar penetration_relaxation= btScalar(1.); 287 minNorm*=penetration_relaxation; 288 289 290 if (res.m_hasResult) 291 { 292 293 pa = res.m_pointInWorld - minNorm * correctedMinNorm; 294 pb = res.m_pointInWorld; 295 v = minNorm; 296 297 #ifdef DEBUG_DRAW 298 if (debugDraw) 299 { 300 btVector3 color(1,0,0); 301 debugDraw->drawLine(pa,pb,color); 302 } 303 #endif//DEBUG_DRAW 304 305 306 } 307 return res.m_hasResult; 308 } 309 310 btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() 311 { 312 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 313 { 314 btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), 315 btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), 316 btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), 317 btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), 318 btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), 319 btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), 320 btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), 321 btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), 322 btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), 323 btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), 324 btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), 325 btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), 326 btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), 327 btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), 328 btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), 329 btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), 330 btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), 331 btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), 332 btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), 333 btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), 334 btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), 335 btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), 336 btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), 337 btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), 338 btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), 339 btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), 340 btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), 341 btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), 342 btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), 343 btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), 344 btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), 345 btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), 346 btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), 347 btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), 348 btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), 349 btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), 350 btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), 351 btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), 352 btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), 353 btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), 354 btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), 355 btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) 356 }; 357 358 return sPenetrationDirections; 359 } 360 361 362
