btCompoundCollisionAlgorithm.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
00017 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00018 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
00019 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
00020 #include "LinearMath/btIDebugDraw.h"
00021 #include "LinearMath/btAabbUtil2.h"
00022 #include "btManifoldResult.h"
00023 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00024 
00025 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
00026 :btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
00027 m_isSwapped(isSwapped),
00028 m_sharedManifold(ci.m_manifold)
00029 {
00030         m_ownsManifold = false;
00031 
00032         const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00033         btAssert (colObjWrap->getCollisionShape()->isCompound());
00034         
00035         const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
00036         m_compoundShapeRevision = compoundShape->getUpdateRevision();
00037         
00038         
00039         preallocateChildAlgorithms(body0Wrap,body1Wrap);
00040 }
00041 
00042 void    btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
00043 {
00044         const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00045         const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
00046         btAssert (colObjWrap->getCollisionShape()->isCompound());
00047         
00048         const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
00049 
00050         int numChildren = compoundShape->getNumChildShapes();
00051         int i;
00052         
00053         m_childCollisionAlgorithms.resize(numChildren);
00054         for (i=0;i<numChildren;i++)
00055         {
00056                 if (compoundShape->getDynamicAabbTree())
00057                 {
00058                         m_childCollisionAlgorithms[i] = 0;
00059                 } else
00060                 {
00061                         
00062                         const btCollisionShape* childShape = compoundShape->getChildShape(i);
00063 
00064                         btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
00065                         m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold);
00066                 }
00067         }
00068 }
00069 
00070 void    btCompoundCollisionAlgorithm::removeChildAlgorithms()
00071 {
00072         int numChildren = m_childCollisionAlgorithms.size();
00073         int i;
00074         for (i=0;i<numChildren;i++)
00075         {
00076                 if (m_childCollisionAlgorithms[i])
00077                 {
00078                         m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
00079                         m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
00080                 }
00081         }
00082 }
00083 
00084 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
00085 {
00086         removeChildAlgorithms();
00087 }
00088 
00089 
00090 
00091 
00092 struct  btCompoundLeafCallback : btDbvt::ICollide
00093 {
00094 
00095 public:
00096 
00097         const btCollisionObjectWrapper* m_compoundColObjWrap;
00098         const btCollisionObjectWrapper* m_otherObjWrap;
00099         btDispatcher* m_dispatcher;
00100         const btDispatcherInfo& m_dispatchInfo;
00101         btManifoldResult*       m_resultOut;
00102         btCollisionAlgorithm**  m_childCollisionAlgorithms;
00103         btPersistentManifold*   m_sharedManifold;
00104         
00105         btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*    resultOut,btCollisionAlgorithm**        childCollisionAlgorithms,btPersistentManifold*  sharedManifold)
00106                 :m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
00107                 m_childCollisionAlgorithms(childCollisionAlgorithms),
00108                 m_sharedManifold(sharedManifold)
00109         {
00110 
00111         }
00112 
00113 
00114         void    ProcessChildShape(const btCollisionShape* childShape,int index)
00115         {
00116                 btAssert(index>=0);
00117                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
00118                 btAssert(index<compoundShape->getNumChildShapes());
00119 
00120 
00121                 //backup
00122                 btTransform     orgTrans = m_compoundColObjWrap->getWorldTransform();
00123                 btTransform     orgInterpolationTrans = m_compoundColObjWrap->getWorldTransform();
00124                 const btTransform& childTrans = compoundShape->getChildTransform(index);
00125                 btTransform     newChildWorldTrans = orgTrans*childTrans ;
00126 
00127                 //perform an AABB check first
00128                 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
00129                 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
00130                 m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
00131 
00132                 if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
00133                 {
00134 
00135                         btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
00136 
00137 
00138                         //the contactpoint is still projected back using the original inverted worldtrans
00139                         if (!m_childCollisionAlgorithms[index])
00140                                 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold);
00141 
00142                         
00143                         const btCollisionObjectWrapper* tmpWrap = 0;
00144 
00146                         if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
00147                         {
00148                                 tmpWrap = m_resultOut->getBody0Wrap();
00149                                 m_resultOut->setBody0Wrap(&compoundWrap);
00150                                 m_resultOut->setShapeIdentifiersA(-1,index);
00151                         } else
00152                         {
00153                                 tmpWrap = m_resultOut->getBody1Wrap();
00154                                 m_resultOut->setBody1Wrap(&compoundWrap);
00155                                 m_resultOut->setShapeIdentifiersB(-1,index);
00156                         }
00157 
00158 
00159                         m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
00160 
00161 #if 0
00162                         if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
00163                         {
00164                                 btVector3 worldAabbMin,worldAabbMax;
00165                                 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
00166                                 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
00167                         }
00168 #endif
00169 
00170                         if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
00171                         {
00172                                 m_resultOut->setBody0Wrap(tmpWrap);
00173                         } else
00174                         {
00175                                 m_resultOut->setBody1Wrap(tmpWrap);
00176                         }
00177                         
00178                 }
00179         }
00180         void            Process(const btDbvtNode* leaf)
00181         {
00182                 int index = leaf->dataAsInt;
00183 
00184                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
00185                 const btCollisionShape* childShape = compoundShape->getChildShape(index);
00186 
00187 #if 0
00188                 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
00189                 {
00190                         btVector3 worldAabbMin,worldAabbMax;
00191                         btTransform     orgTrans = m_compoundColObjWrap->getWorldTransform();
00192                         btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
00193                         m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
00194                 }
00195 #endif
00196 
00197                 ProcessChildShape(childShape,index);
00198 
00199         }
00200 };
00201 
00202 
00203 
00204 
00205 
00206 
00207 void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00208 {
00209         const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00210         const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
00211 
00212         btAssert (colObjWrap->getCollisionShape()->isCompound());
00213         const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
00214 
00217         if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
00218         {
00220                 removeChildAlgorithms();
00221                 
00222                 preallocateChildAlgorithms(body0Wrap,body1Wrap);
00223         }
00224 
00225 
00226         const btDbvt* tree = compoundShape->getDynamicAabbTree();
00227         //use a dynamic aabb tree to cull potential child-overlaps
00228         btCompoundLeafCallback  callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
00229 
00233         {
00234                 int i;
00235                 btManifoldArray manifoldArray;
00236                 for (i=0;i<m_childCollisionAlgorithms.size();i++)
00237                 {
00238                         if (m_childCollisionAlgorithms[i])
00239                         {
00240                                 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
00241                                 for (int m=0;m<manifoldArray.size();m++)
00242                                 {
00243                                         if (manifoldArray[m]->getNumContacts())
00244                                         {
00245                                                 resultOut->setPersistentManifold(manifoldArray[m]);
00246                                                 resultOut->refreshContactPoints();
00247                                                 resultOut->setPersistentManifold(0);//??necessary?
00248                                         }
00249                                 }
00250                                 manifoldArray.resize(0);
00251                         }
00252                 }
00253         }
00254 
00255         if (tree)
00256         {
00257 
00258                 btVector3 localAabbMin,localAabbMax;
00259                 btTransform otherInCompoundSpace;
00260                 otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
00261                 otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
00262 
00263                 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
00264                 //process all children, that overlap with  the given AABB bounds
00265                 tree->collideTV(tree->m_root,bounds,callback);
00266 
00267         } else
00268         {
00269                 //iterate over all children, perform an AABB check inside ProcessChildShape
00270                 int numChildren = m_childCollisionAlgorithms.size();
00271                 int i;
00272                 for (i=0;i<numChildren;i++)
00273                 {
00274                         callback.ProcessChildShape(compoundShape->getChildShape(i),i);
00275                 }
00276         }
00277 
00278         {
00279                                 //iterate over all children, perform an AABB check inside ProcessChildShape
00280                 int numChildren = m_childCollisionAlgorithms.size();
00281                 int i;
00282                 btManifoldArray manifoldArray;
00283         const btCollisionShape* childShape = 0;
00284         btTransform     orgTrans;
00285         btTransform     orgInterpolationTrans;
00286         btTransform     newChildWorldTrans;
00287         btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
00288         
00289                 for (i=0;i<numChildren;i++)
00290                 {
00291                         if (m_childCollisionAlgorithms[i])
00292                         {
00293                                 childShape = compoundShape->getChildShape(i);
00294                         //if not longer overlapping, remove the algorithm
00295                 orgTrans = colObjWrap->getWorldTransform();
00296                 orgInterpolationTrans = colObjWrap->getWorldTransform();
00297                                 const btTransform& childTrans = compoundShape->getChildTransform(i);
00298                 newChildWorldTrans = orgTrans*childTrans ;
00299 
00300                                 //perform an AABB check first
00301                                 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
00302                                 otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
00303 
00304                                 if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
00305                                 {
00306                                         m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
00307                                         m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
00308                                         m_childCollisionAlgorithms[i] = 0;
00309                                 }
00310                         }
00311                 }
00312         }
00313 }
00314 
00315 btScalar        btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00316 {
00317         btAssert(0);
00318         //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
00319         btCollisionObject* colObj = m_isSwapped? body1 : body0;
00320         btCollisionObject* otherObj = m_isSwapped? body0 : body1;
00321 
00322         btAssert (colObj->getCollisionShape()->isCompound());
00323         
00324         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
00325 
00326         //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
00327         //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
00328         //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
00329         //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
00330         //then use each overlapping node AABB against Tree0
00331         //and vise versa.
00332 
00333         btScalar hitFraction = btScalar(1.);
00334 
00335         int numChildren = m_childCollisionAlgorithms.size();
00336         int i;
00337     btTransform orgTrans;
00338     btScalar frac;
00339         for (i=0;i<numChildren;i++)
00340         {
00341                 //btCollisionShape* childShape = compoundShape->getChildShape(i);
00342 
00343                 //backup
00344         orgTrans = colObj->getWorldTransform();
00345         
00346                 const btTransform& childTrans = compoundShape->getChildTransform(i);
00347                 //btTransform   newChildWorldTrans = orgTrans*childTrans ;
00348                 colObj->setWorldTransform( orgTrans*childTrans );
00349 
00350                 //btCollisionShape* tmpShape = colObj->getCollisionShape();
00351                 //colObj->internalSetTemporaryCollisionShape( childShape );
00352         frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
00353                 if (frac<hitFraction)
00354                 {
00355                         hitFraction = frac;
00356                 }
00357                 //revert back
00358                 //colObj->internalSetTemporaryCollisionShape( tmpShape);
00359                 colObj->setWorldTransform( orgTrans);
00360         }
00361         return hitFraction;
00362 
00363 }
00364 
00365 
00366