src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

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