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 */ 16 17 #include "btCompoundCompoundCollisionAlgorithm.h" 18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h" 19 #include "BulletCollision/CollisionShapes/btCompoundShape.h" 20 #include "BulletCollision/BroadphaseCollision/btDbvt.h" 21 #include "LinearMath/btIDebugDraw.h" 22 #include "LinearMath/btAabbUtil2.h" 23 #include "BulletCollision/CollisionDispatch/btManifoldResult.h" 24 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" 25 26 27 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; 28 29 btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) 30 :btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped) 31 { 32 33 void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); 34 m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); 35 36 const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; 37 btAssert (col0ObjWrap->getCollisionShape()->isCompound()); 38 39 const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; 40 btAssert (col1ObjWrap->getCollisionShape()->isCompound()); 41 42 const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); 43 m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); 44 45 const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); 46 m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); 47 48 49 } 50 51 52 btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() 53 { 54 removeChildAlgorithms(); 55 m_childCollisionAlgorithmCache->~btHashedSimplePairCache(); 56 btAlignedFree(m_childCollisionAlgorithmCache); 57 } 58 59 void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) 60 { 61 int i; 62 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); 63 for (i=0;i<pairs.size();i++) 64 { 65 if (pairs[i].m_userPointer) 66 { 67 68 ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray); 69 } 70 } 71 } 72 73 74 void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() 75 { 76 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); 77 78 int numChildren = pairs.size(); 79 int i; 80 for (i=0;i<numChildren;i++) 81 { 82 if (pairs[i].m_userPointer) 83 { 84 btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; 85 algo->~btCollisionAlgorithm(); 86 m_dispatcher->freeCollisionAlgorithm(algo); 87 } 88 } 89 m_childCollisionAlgorithmCache->removeAllPairs(); 90 } 91 92 struct btCompoundCompoundLeafCallback : btDbvt::ICollide 93 { 94 int m_numOverlapPairs; 95 96 97 const btCollisionObjectWrapper* m_compound0ColObjWrap; 98 const btCollisionObjectWrapper* m_compound1ColObjWrap; 99 btDispatcher* m_dispatcher; 100 const btDispatcherInfo& m_dispatchInfo; 101 btManifoldResult* m_resultOut; 102 103 104 class btHashedSimplePairCache* m_childCollisionAlgorithmCache; 105 106 btPersistentManifold* m_sharedManifold; 107 108 btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, 109 const btCollisionObjectWrapper* compound0ObjWrap, 110 btDispatcher* dispatcher, 111 const btDispatcherInfo& dispatchInfo, 112 btManifoldResult* resultOut, 113 btHashedSimplePairCache* childAlgorithmsCache, 114 btPersistentManifold* sharedManifold) 115 :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), 116 m_childCollisionAlgorithmCache(childAlgorithmsCache), 117 m_sharedManifold(sharedManifold) 118 { 119 120 } 121 122 123 124 125 void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) 126 { 127 m_numOverlapPairs++; 128 129 130 int childIndex0 = leaf0->dataAsInt; 131 int childIndex1 = leaf1->dataAsInt; 132 133 134 btAssert(childIndex0>=0); 135 btAssert(childIndex1>=0); 136 137 138 const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape()); 139 btAssert(childIndex0<compoundShape0->getNumChildShapes()); 140 141 const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape()); 142 btAssert(childIndex1<compoundShape1->getNumChildShapes()); 143 144 const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); 145 const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); 146 147 //backup 148 btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); 149 const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); 150 btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; 151 152 btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); 153 const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); 154 btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; 155 156 157 //perform an AABB check first 158 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; 159 childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); 160 childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); 161 162 if (gCompoundCompoundChildShapePairCallback) 163 { 164 if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) 165 return; 166 } 167 168 if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) 169 { 170 btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); 171 btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); 172 173 174 btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); 175 176 btCollisionAlgorithm* colAlgo = 0; 177 178 if (pair) 179 { 180 colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; 181 182 } else 183 { 184 colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); 185 pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); 186 btAssert(pair); 187 pair->m_userPointer = colAlgo; 188 } 189 190 btAssert(colAlgo); 191 192 const btCollisionObjectWrapper* tmpWrap0 = 0; 193 const btCollisionObjectWrapper* tmpWrap1 = 0; 194 195 tmpWrap0 = m_resultOut->getBody0Wrap(); 196 tmpWrap1 = m_resultOut->getBody1Wrap(); 197 198 m_resultOut->setBody0Wrap(&compoundWrap0); 199 m_resultOut->setBody1Wrap(&compoundWrap1); 200 201 m_resultOut->setShapeIdentifiersA(-1,childIndex0); 202 m_resultOut->setShapeIdentifiersB(-1,childIndex1); 203 204 205 colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); 206 207 m_resultOut->setBody0Wrap(tmpWrap0); 208 m_resultOut->setBody1Wrap(tmpWrap1); 209 210 211 212 } 213 } 214 }; 215 216 217 static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, 218 const btDbvtAabbMm& b, const btTransform& xform) 219 { 220 btVector3 newmin,newmax; 221 btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); 222 btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); 223 return Intersect(a,newb); 224 } 225 226 227 static inline void MycollideTT( const btDbvtNode* root0, 228 const btDbvtNode* root1, 229 const btTransform& xform, 230 btCompoundCompoundLeafCallback* callback) 231 { 232 233 if(root0&&root1) 234 { 235 int depth=1; 236 int treshold=btDbvt::DOUBLE_STACKSIZE-4; 237 btAlignedObjectArray<btDbvt::sStkNN> stkStack; 238 stkStack.resize(btDbvt::DOUBLE_STACKSIZE); 239 stkStack[0]=btDbvt::sStkNN(root0,root1); 240 do { 241 btDbvt::sStkNN p=stkStack[--depth]; 242 if(MyIntersect(p.a->volume,p.b->volume,xform)) 243 { 244 if(depth>treshold) 245 { 246 stkStack.resize(stkStack.size()*2); 247 treshold=stkStack.size()-4; 248 } 249 if(p.a->isinternal()) 250 { 251 if(p.b->isinternal()) 252 { 253 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); 254 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); 255 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); 256 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); 257 } 258 else 259 { 260 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); 261 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); 262 } 263 } 264 else 265 { 266 if(p.b->isinternal()) 267 { 268 stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); 269 stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); 270 } 271 else 272 { 273 callback->Process(p.a,p.b); 274 } 275 } 276 } 277 } while(depth); 278 } 279 } 280 281 void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) 282 { 283 284 const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; 285 const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; 286 287 btAssert (col0ObjWrap->getCollisionShape()->isCompound()); 288 btAssert (col1ObjWrap->getCollisionShape()->isCompound()); 289 const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); 290 const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); 291 292 const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); 293 const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); 294 if (!tree0 || !tree1) 295 { 296 return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut); 297 } 298 ///btCompoundShape might have changed: 299 ////make sure the internal child collision algorithm caches are still valid 300 if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1)) 301 { 302 ///clear all 303 removeChildAlgorithms(); 304 m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); 305 m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); 306 307 } 308 309 310 ///we need to refresh all contact manifolds 311 ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep 312 ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm 313 { 314 int i; 315 btManifoldArray manifoldArray; 316 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); 317 for (i=0;i<pairs.size();i++) 318 { 319 if (pairs[i].m_userPointer) 320 { 321 btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; 322 algo->getAllContactManifolds(manifoldArray); 323 for (int m=0;m<manifoldArray.size();m++) 324 { 325 if (manifoldArray[m]->getNumContacts()) 326 { 327 resultOut->setPersistentManifold(manifoldArray[m]); 328 resultOut->refreshContactPoints(); 329 resultOut->setPersistentManifold(0); 330 } 331 } 332 manifoldArray.resize(0); 333 } 334 } 335 } 336 337 338 339 340 btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); 341 342 343 const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); 344 MycollideTT(tree0->m_root,tree1->m_root,xform,&callback); 345 346 //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); 347 348 //remove non-overlapping child pairs 349 350 { 351 btAssert(m_removePairs.size()==0); 352 353 //iterate over all children, perform an AABB check inside ProcessChildShape 354 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); 355 356 int i; 357 btManifoldArray manifoldArray; 358 359 360 361 362 363 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; 364 365 for (i=0;i<pairs.size();i++) 366 { 367 if (pairs[i].m_userPointer) 368 { 369 btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; 370 371 { 372 btTransform orgTrans0; 373 const btCollisionShape* childShape0 = 0; 374 375 btTransform newChildWorldTrans0; 376 btTransform orgInterpolationTrans0; 377 childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); 378 orgTrans0 = col0ObjWrap->getWorldTransform(); 379 orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); 380 const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); 381 newChildWorldTrans0 = orgTrans0*childTrans0 ; 382 childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); 383 } 384 385 { 386 btTransform orgInterpolationTrans1; 387 const btCollisionShape* childShape1 = 0; 388 btTransform orgTrans1; 389 btTransform newChildWorldTrans1; 390 391 childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); 392 orgTrans1 = col1ObjWrap->getWorldTransform(); 393 orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); 394 const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); 395 newChildWorldTrans1 = orgTrans1*childTrans1 ; 396 childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); 397 } 398 399 400 401 if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) 402 { 403 algo->~btCollisionAlgorithm(); 404 m_dispatcher->freeCollisionAlgorithm(algo); 405 m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); 406 } 407 } 408 } 409 for (int i=0;i<m_removePairs.size();i++) 410 { 411 m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); 412 } 413 m_removePairs.clear(); 414 } 415 416 } 417 418 btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) 419 { 420 btAssert(0); 421 return 0.f; 422 423 } 424 425 426 427
