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 17 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.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 "btManifoldResult.h" 24 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" 25 26 btShapePairCallback gCompoundChildShapePairCallback = 0; 27 28 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) 29 :btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), 30 m_isSwapped(isSwapped), 31 m_sharedManifold(ci.m_manifold) 32 { 33 m_ownsManifold = false; 34 35 const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; 36 btAssert (colObjWrap->getCollisionShape()->isCompound()); 37 38 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); 39 m_compoundShapeRevision = compoundShape->getUpdateRevision(); 40 41 42 preallocateChildAlgorithms(body0Wrap,body1Wrap); 43 } 44 45 void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) 46 { 47 const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; 48 const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; 49 btAssert (colObjWrap->getCollisionShape()->isCompound()); 50 51 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); 52 53 int numChildren = compoundShape->getNumChildShapes(); 54 int i; 55 56 m_childCollisionAlgorithms.resize(numChildren); 57 for (i=0;i<numChildren;i++) 58 { 59 if (compoundShape->getDynamicAabbTree()) 60 { 61 m_childCollisionAlgorithms[i] = 0; 62 } else 63 { 64 65 const btCollisionShape* childShape = compoundShape->getChildShape(i); 66 67 btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully) 68 m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold); 69 } 70 } 71 } 72 73 void btCompoundCollisionAlgorithm::removeChildAlgorithms() 74 { 75 int numChildren = m_childCollisionAlgorithms.size(); 76 int i; 77 for (i=0;i<numChildren;i++) 78 { 79 if (m_childCollisionAlgorithms[i]) 80 { 81 m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); 82 m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); 83 } 84 } 85 } 86 87 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() 88 { 89 removeChildAlgorithms(); 90 } 91 92 93 94 95 struct btCompoundLeafCallback : btDbvt::ICollide 96 { 97 98 public: 99 100 const btCollisionObjectWrapper* m_compoundColObjWrap; 101 const btCollisionObjectWrapper* m_otherObjWrap; 102 btDispatcher* m_dispatcher; 103 const btDispatcherInfo& m_dispatchInfo; 104 btManifoldResult* m_resultOut; 105 btCollisionAlgorithm** m_childCollisionAlgorithms; 106 btPersistentManifold* m_sharedManifold; 107 108 btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold) 109 :m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), 110 m_childCollisionAlgorithms(childCollisionAlgorithms), 111 m_sharedManifold(sharedManifold) 112 { 113 114 } 115 116 117 void ProcessChildShape(const btCollisionShape* childShape,int index) 118 { 119 btAssert(index>=0); 120 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape()); 121 btAssert(index<compoundShape->getNumChildShapes()); 122 123 124 //backup 125 btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); 126 127 const btTransform& childTrans = compoundShape->getChildTransform(index); 128 btTransform newChildWorldTrans = orgTrans*childTrans ; 129 130 //perform an AABB check first 131 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; 132 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); 133 m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); 134 135 if (gCompoundChildShapePairCallback) 136 { 137 if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) 138 return; 139 } 140 141 if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) 142 { 143 144 btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); 145 146 147 //the contactpoint is still projected back using the original inverted worldtrans 148 if (!m_childCollisionAlgorithms[index]) 149 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold); 150 151 152 const btCollisionObjectWrapper* tmpWrap = 0; 153 154 ///detect swapping case 155 if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) 156 { 157 tmpWrap = m_resultOut->getBody0Wrap(); 158 m_resultOut->setBody0Wrap(&compoundWrap); 159 m_resultOut->setShapeIdentifiersA(-1,index); 160 } else 161 { 162 tmpWrap = m_resultOut->getBody1Wrap(); 163 m_resultOut->setBody1Wrap(&compoundWrap); 164 m_resultOut->setShapeIdentifiersB(-1,index); 165 } 166 167 168 m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); 169 170 #if 0 171 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) 172 { 173 btVector3 worldAabbMin,worldAabbMax; 174 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1)); 175 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1)); 176 } 177 #endif 178 179 if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) 180 { 181 m_resultOut->setBody0Wrap(tmpWrap); 182 } else 183 { 184 m_resultOut->setBody1Wrap(tmpWrap); 185 } 186 187 } 188 } 189 void Process(const btDbvtNode* leaf) 190 { 191 int index = leaf->dataAsInt; 192 193 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape()); 194 const btCollisionShape* childShape = compoundShape->getChildShape(index); 195 196 #if 0 197 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) 198 { 199 btVector3 worldAabbMin,worldAabbMax; 200 btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); 201 btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax); 202 m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0)); 203 } 204 #endif 205 206 ProcessChildShape(childShape,index); 207 208 } 209 }; 210 211 212 213 214 215 216 void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) 217 { 218 const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; 219 const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; 220 221 btAssert (colObjWrap->getCollisionShape()->isCompound()); 222 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); 223 224 ///btCompoundShape might have changed: 225 ////make sure the internal child collision algorithm caches are still valid 226 if (compoundShape->getUpdateRevision() != m_compoundShapeRevision) 227 { 228 ///clear and update all 229 removeChildAlgorithms(); 230 231 preallocateChildAlgorithms(body0Wrap,body1Wrap); 232 m_compoundShapeRevision = compoundShape->getUpdateRevision(); 233 } 234 235 if (m_childCollisionAlgorithms.size()==0) 236 return; 237 238 const btDbvt* tree = compoundShape->getDynamicAabbTree(); 239 //use a dynamic aabb tree to cull potential child-overlaps 240 btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); 241 242 ///we need to refresh all contact manifolds 243 ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep 244 ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm 245 { 246 int i; 247 btManifoldArray manifoldArray; 248 for (i=0;i<m_childCollisionAlgorithms.size();i++) 249 { 250 if (m_childCollisionAlgorithms[i]) 251 { 252 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); 253 for (int m=0;m<manifoldArray.size();m++) 254 { 255 if (manifoldArray[m]->getNumContacts()) 256 { 257 resultOut->setPersistentManifold(manifoldArray[m]); 258 resultOut->refreshContactPoints(); 259 resultOut->setPersistentManifold(0);//??necessary? 260 } 261 } 262 manifoldArray.resize(0); 263 } 264 } 265 } 266 267 if (tree) 268 { 269 270 btVector3 localAabbMin,localAabbMax; 271 btTransform otherInCompoundSpace; 272 otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); 273 otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); 274 275 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); 276 //process all children, that overlap with the given AABB bounds 277 tree->collideTV(tree->m_root,bounds,callback); 278 279 } else 280 { 281 //iterate over all children, perform an AABB check inside ProcessChildShape 282 int numChildren = m_childCollisionAlgorithms.size(); 283 int i; 284 for (i=0;i<numChildren;i++) 285 { 286 callback.ProcessChildShape(compoundShape->getChildShape(i),i); 287 } 288 } 289 290 { 291 //iterate over all children, perform an AABB check inside ProcessChildShape 292 int numChildren = m_childCollisionAlgorithms.size(); 293 int i; 294 btManifoldArray manifoldArray; 295 const btCollisionShape* childShape = 0; 296 btTransform orgTrans; 297 298 btTransform newChildWorldTrans; 299 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; 300 301 for (i=0;i<numChildren;i++) 302 { 303 if (m_childCollisionAlgorithms[i]) 304 { 305 childShape = compoundShape->getChildShape(i); 306 //if not longer overlapping, remove the algorithm 307 orgTrans = colObjWrap->getWorldTransform(); 308 309 const btTransform& childTrans = compoundShape->getChildTransform(i); 310 newChildWorldTrans = orgTrans*childTrans ; 311 312 //perform an AABB check first 313 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); 314 otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); 315 316 if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) 317 { 318 m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); 319 m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); 320 m_childCollisionAlgorithms[i] = 0; 321 } 322 } 323 } 324 } 325 } 326 327 btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) 328 { 329 btAssert(0); 330 //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures 331 btCollisionObject* colObj = m_isSwapped? body1 : body0; 332 btCollisionObject* otherObj = m_isSwapped? body0 : body1; 333 334 btAssert (colObj->getCollisionShape()->isCompound()); 335 336 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); 337 338 //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps 339 //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals 340 //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: 341 //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 342 //then use each overlapping node AABB against Tree0 343 //and vise versa. 344 345 btScalar hitFraction = btScalar(1.); 346 347 int numChildren = m_childCollisionAlgorithms.size(); 348 int i; 349 btTransform orgTrans; 350 btScalar frac; 351 for (i=0;i<numChildren;i++) 352 { 353 //btCollisionShape* childShape = compoundShape->getChildShape(i); 354 355 //backup 356 orgTrans = colObj->getWorldTransform(); 357 358 const btTransform& childTrans = compoundShape->getChildTransform(i); 359 //btTransform newChildWorldTrans = orgTrans*childTrans ; 360 colObj->setWorldTransform( orgTrans*childTrans ); 361 362 //btCollisionShape* tmpShape = colObj->getCollisionShape(); 363 //colObj->internalSetTemporaryCollisionShape( childShape ); 364 frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); 365 if (frac<hitFraction) 366 { 367 hitFraction = frac; 368 } 369 //revert back 370 //colObj->internalSetTemporaryCollisionShape( tmpShape); 371 colObj->setWorldTransform( orgTrans); 372 } 373 return hitFraction; 374 375 } 376 377 378 379