Home | History | Annotate | Download | only in CollisionDispatch
      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