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