Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/output/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 8805

Last change on this file since 8805 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

Property svn:eol-style set to native

File size: 51.6 KB

Line
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 "btCollisionWorld.h"
17	#include "btCollisionDispatcher.h"
18	#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19	#include "BulletCollision/CollisionShapes/btCollisionShape.h"
20	#include "BulletCollision/CollisionShapes/btConvexShape.h"
21	#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
22	#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
23	#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
24	#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
25	#include "BulletCollision/CollisionShapes/btCompoundShape.h"
26	#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
27	#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
28	#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
29	#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
30	#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
31	#include "BulletCollision/BroadphaseCollision/btDbvt.h"
32	#include "LinearMath/btAabbUtil2.h"
33	#include "LinearMath/btQuickprof.h"
34	#include "LinearMath/btStackAlloc.h"
35	#include "LinearMath/btSerializer.h"
36	#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
37
38	//#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
39
40
41	//#define USE_BRUTEFORCE_RAYBROADPHASE 1
42	//RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest
43	//#define RECALCULATE_AABB_RAYCAST 1
44
45	//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
46	#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
47	#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
48	#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
49
50
51	///for debug drawing
52
53	//for debug rendering
54	#include "BulletCollision/CollisionShapes/btBoxShape.h"
55	#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
56	#include "BulletCollision/CollisionShapes/btCompoundShape.h"
57	#include "BulletCollision/CollisionShapes/btConeShape.h"
58	#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
59	#include "BulletCollision/CollisionShapes/btCylinderShape.h"
60	#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
61	#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
62	#include "BulletCollision/CollisionShapes/btSphereShape.h"
63	#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
64	#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
65	#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
66
67
68
69	btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
70	:m_dispatcher1(dispatcher),
71	m_broadphasePairCache(pairCache),
72	m_debugDrawer(0),
73	m_forceUpdateAllAabbs(true)
74	{
75	m_stackAlloc = collisionConfiguration->getStackAllocator();
76	m_dispatchInfo.m_stackAllocator = m_stackAlloc;
77	}
78
79
80	btCollisionWorld::~btCollisionWorld()
81	{
82
83	//clean up remaining objects
84	int i;
85	for (i=0;i<m_collisionObjects.size();i++)
86	{
87	btCollisionObject* collisionObject= m_collisionObjects[i];
88
89	btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
90	if (bp)
91	{
92	//
93	// only clear the cached algorithms
94	//
95	getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
96	getBroadphase()->destroyProxy(bp,m_dispatcher1);
97	collisionObject->setBroadphaseHandle(0);
98	}
99	}
100
101
102	}
103
104
105
106
107
108
109
110
111
112
113	void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
114	{
115
116	btAssert(collisionObject);
117
118	//check that the object isn't already added
119	btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
120
121	m_collisionObjects.push_back(collisionObject);
122
123	//calculate new AABB
124	btTransform trans = collisionObject->getWorldTransform();
125
126	btVector3 minAabb;
127	btVector3 maxAabb;
128	collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
129
130	int type = collisionObject->getCollisionShape()->getShapeType();
131	collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
132	minAabb,
133	maxAabb,
134	type,
135	collisionObject,
136	collisionFilterGroup,
137	collisionFilterMask,
138	m_dispatcher1,0
139	)) ;
140
141
142
143
144
145	}
146
147
148
149	void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
150	{
151	btVector3 minAabb,maxAabb;
152	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
153	//need to increase the aabb for contact thresholds
154	btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
155	minAabb -= contactThreshold;
156	maxAabb += contactThreshold;
157
158	if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
159	{
160	btVector3 minAabb2,maxAabb2;
161	colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
162	minAabb2 -= contactThreshold;
163	maxAabb2 += contactThreshold;
164	minAabb.setMin(minAabb2);
165	maxAabb.setMax(maxAabb2);
166	}
167
168	btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
169
170	//moving objects should be moderately sized, probably something wrong if not
171	if ( colObj->isStaticObject() \|\| ((maxAabb-minAabb).length2() < btScalar(1e12)))
172	{
173	bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
174	} else
175	{
176	//something went wrong, investigate
177	//this assert is unwanted in 3D modelers (danger of loosing work)
178	colObj->setActivationState(DISABLE_SIMULATION);
179
180	static bool reportMe = true;
181	if (reportMe && m_debugDrawer)
182	{
183	reportMe = false;
184	m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
185	m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
186	m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
187	m_debugDrawer->reportErrorWarning("Thanks.\n");
188	}
189	}
190	}
191
192	void btCollisionWorld::updateAabbs()
193	{
194	BT_PROFILE("updateAabbs");
195
196	btTransform predictedTrans;
197	for ( int i=0;i<m_collisionObjects.size();i++)
198	{
199	btCollisionObject* colObj = m_collisionObjects[i];
200
201	//only update aabb of active objects
202	if (m_forceUpdateAllAabbs \|\| colObj->isActive())
203	{
204	updateSingleAabb(colObj);
205	}
206	}
207	}
208
209
210
211	void btCollisionWorld::performDiscreteCollisionDetection()
212	{
213	BT_PROFILE("performDiscreteCollisionDetection");
214
215	btDispatcherInfo& dispatchInfo = getDispatchInfo();
216
217	updateAabbs();
218
219	{
220	BT_PROFILE("calculateOverlappingPairs");
221	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
222	}
223
224
225	btDispatcher* dispatcher = getDispatcher();
226	{
227	BT_PROFILE("dispatchAllCollisionPairs");
228	if (dispatcher)
229	dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
230	}
231
232	}
233
234
235
236	void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
237	{
238
239
240	//bool removeFromBroadphase = false;
241
242	{
243
244	btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
245	if (bp)
246	{
247	//
248	// only clear the cached algorithms
249	//
250	getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
251	getBroadphase()->destroyProxy(bp,m_dispatcher1);
252	collisionObject->setBroadphaseHandle(0);
253	}
254	}
255
256
257	//swapremove
258	m_collisionObjects.remove(collisionObject);
259
260	}
261
262
263
264	void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
265	btCollisionObject* collisionObject,
266	const btCollisionShape* collisionShape,
267	const btTransform& colObjWorldTransform,
268	RayResultCallback& resultCallback)
269	{
270	btSphereShape pointShape(btScalar(0.0));
271	pointShape.setMargin(0.f);
272	const btConvexShape* castShape = &pointShape;
273
274	if (collisionShape->isConvex())
275	{
276	// BT_PROFILE("rayTestConvex");
277	btConvexCast::CastResult castResult;
278	castResult.m_fraction = resultCallback.m_closestHitFraction;
279
280	btConvexShape* convexShape = (btConvexShape*) collisionShape;
281	btVoronoiSimplexSolver simplexSolver;
282	#define USE_SUBSIMPLEX_CONVEX_CAST 1
283	#ifdef USE_SUBSIMPLEX_CONVEX_CAST
284	btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
285	#else
286	//btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
287	//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
288	#endif //#USE_SUBSIMPLEX_CONVEX_CAST
289
290	if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
291	{
292	//add hit
293	if (castResult.m_normal.length2() > btScalar(0.0001))
294	{
295	if (castResult.m_fraction < resultCallback.m_closestHitFraction)
296	{
297	#ifdef USE_SUBSIMPLEX_CONVEX_CAST
298	//rotate normal into worldspace
299	castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
300	#endif //USE_SUBSIMPLEX_CONVEX_CAST
301
302	castResult.m_normal.normalize();
303	btCollisionWorld::LocalRayResult localRayResult
304	(
305	collisionObject,
306	0,
307	castResult.m_normal,
308	castResult.m_fraction
309	);
310
311	bool normalInWorldSpace = true;
312	resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
313
314	}
315	}
316	}
317	} else {
318	if (collisionShape->isConcave())
319	{
320	// BT_PROFILE("rayTestConcave");
321	if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
322	{
323	///optimized version for btBvhTriangleMeshShape
324	btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
325	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
326	btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
327	btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
328
329	//ConvexCast::CastResult
330	struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
331	{
332	btCollisionWorld::RayResultCallback* m_resultCallback;
333	btCollisionObject* m_collisionObject;
334	btTriangleMeshShape* m_triangleMesh;
335
336	btTransform m_colObjWorldTransform;
337
338	BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
339	btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform):
340	//@BP Mod
341	btTriangleRaycastCallback(from,to, resultCallback->m_flags),
342	m_resultCallback(resultCallback),
343	m_collisionObject(collisionObject),
344	m_triangleMesh(triangleMesh),
345	m_colObjWorldTransform(colObjWorldTransform)
346	{
347	}
348
349
350	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
351	{
352	btCollisionWorld::LocalShapeInfo shapeInfo;
353	shapeInfo.m_shapePart = partId;
354	shapeInfo.m_triangleIndex = triangleIndex;
355
356	btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
357
358	btCollisionWorld::LocalRayResult rayResult
359	(m_collisionObject,
360	&shapeInfo,
361	hitNormalWorld,
362	hitFraction);
363
364	bool normalInWorldSpace = true;
365	return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
366	}
367
368	};
369
370	BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
371	rcb.m_hitFraction = resultCallback.m_closestHitFraction;
372	triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
373	} else
374	{
375	//generic (slower) case
376	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
377
378	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
379
380	btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
381	btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
382
383	//ConvexCast::CastResult
384
385	struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
386	{
387	btCollisionWorld::RayResultCallback* m_resultCallback;
388	btCollisionObject* m_collisionObject;
389	btConcaveShape* m_triangleMesh;
390
391	btTransform m_colObjWorldTransform;
392
393	BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
394	btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
395	//@BP Mod
396	btTriangleRaycastCallback(from,to, resultCallback->m_flags),
397	m_resultCallback(resultCallback),
398	m_collisionObject(collisionObject),
399	m_triangleMesh(triangleMesh),
400	m_colObjWorldTransform(colObjWorldTransform)
401	{
402	}
403
404
405	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
406	{
407	btCollisionWorld::LocalShapeInfo shapeInfo;
408	shapeInfo.m_shapePart = partId;
409	shapeInfo.m_triangleIndex = triangleIndex;
410
411	btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
412
413	btCollisionWorld::LocalRayResult rayResult
414	(m_collisionObject,
415	&shapeInfo,
416	hitNormalWorld,
417	hitFraction);
418
419	bool normalInWorldSpace = true;
420	return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
421	}
422
423	};
424
425
426	BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
427	rcb.m_hitFraction = resultCallback.m_closestHitFraction;
428
429	btVector3 rayAabbMinLocal = rayFromLocal;
430	rayAabbMinLocal.setMin(rayToLocal);
431	btVector3 rayAabbMaxLocal = rayFromLocal;
432	rayAabbMaxLocal.setMax(rayToLocal);
433
434	concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
435	}
436	} else {
437	// BT_PROFILE("rayTestCompound");
438	if (collisionShape->isCompound())
439	{
440	struct LocalInfoAdder2 : public RayResultCallback
441	{
442	RayResultCallback* m_userCallback;
443	int m_i;
444
445	LocalInfoAdder2 (int i, RayResultCallback *user)
446	: m_userCallback(user), m_i(i)
447	{
448	m_closestHitFraction = m_userCallback->m_closestHitFraction;
449	}
450	virtual bool needsCollision(btBroadphaseProxy* p) const
451	{
452	return m_userCallback->needsCollision(p);
453	}
454
455	virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
456	{
457	btCollisionWorld::LocalShapeInfo shapeInfo;
458	shapeInfo.m_shapePart = -1;
459	shapeInfo.m_triangleIndex = m_i;
460	if (r.m_localShapeInfo == NULL)
461	r.m_localShapeInfo = &shapeInfo;
462
463	const btScalar result = m_userCallback->addSingleResult(r, b);
464	m_closestHitFraction = m_userCallback->m_closestHitFraction;
465	return result;
466	}
467	};
468
469	struct RayTester : btDbvt::ICollide
470	{
471	btCollisionObject* m_collisionObject;
472	const btCompoundShape* m_compoundShape;
473	const btTransform& m_colObjWorldTransform;
474	const btTransform& m_rayFromTrans;
475	const btTransform& m_rayToTrans;
476	RayResultCallback& m_resultCallback;
477
478	RayTester(btCollisionObject* collisionObject,
479	const btCompoundShape* compoundShape,
480	const btTransform& colObjWorldTransform,
481	const btTransform& rayFromTrans,
482	const btTransform& rayToTrans,
483	RayResultCallback& resultCallback):
484	m_collisionObject(collisionObject),
485	m_compoundShape(compoundShape),
486	m_colObjWorldTransform(colObjWorldTransform),
487	m_rayFromTrans(rayFromTrans),
488	m_rayToTrans(rayToTrans),
489	m_resultCallback(resultCallback)
490	{
491
492	}
493
494	void Process(int i)
495	{
496	const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
497	const btTransform& childTrans = m_compoundShape->getChildTransform(i);
498	btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
499
500	// replace collision shape so that callback can determine the triangle
501	btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape();
502	m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
503
504	LocalInfoAdder2 my_cb(i, &m_resultCallback);
505
506	rayTestSingle(
507	m_rayFromTrans,
508	m_rayToTrans,
509	m_collisionObject,
510	childCollisionShape,
511	childWorldTrans,
512	my_cb);
513
514	// restore
515	m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
516	}
517
518	void Process(const btDbvtNode* leaf)
519	{
520	Process(leaf->dataAsInt);
521	}
522	};
523
524	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
525	const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
526
527
528	RayTester rayCB(
529	collisionObject,
530	compoundShape,
531	colObjWorldTransform,
532	rayFromTrans,
533	rayToTrans,
534	resultCallback);
535	#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
536	if (dbvt)
537	{
538	btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
539	btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
540	btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
541	}
542	else
543	#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
544	{
545	for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
546	{
547	rayCB.Process(i);
548	}
549	}
550	}
551	}
552	}
553	}
554
555	void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
556	btCollisionObject* collisionObject,
557	const btCollisionShape* collisionShape,
558	const btTransform& colObjWorldTransform,
559	ConvexResultCallback& resultCallback, btScalar allowedPenetration)
560	{
561	if (collisionShape->isConvex())
562	{
563	//BT_PROFILE("convexSweepConvex");
564	btConvexCast::CastResult castResult;
565	castResult.m_allowedPenetration = allowedPenetration;
566	castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
567
568	btConvexShape* convexShape = (btConvexShape*) collisionShape;
569	btVoronoiSimplexSolver simplexSolver;
570	btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
571
572	btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
573	//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
574	//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
575
576	btConvexCast* castPtr = &convexCaster1;
577
578
579
580	if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
581	{
582	//add hit
583	if (castResult.m_normal.length2() > btScalar(0.0001))
584	{
585	if (castResult.m_fraction < resultCallback.m_closestHitFraction)
586	{
587	castResult.m_normal.normalize();
588	btCollisionWorld::LocalConvexResult localConvexResult
589	(
590	collisionObject,
591	0,
592	castResult.m_normal,
593	castResult.m_hitPoint,
594	castResult.m_fraction
595	);
596
597	bool normalInWorldSpace = true;
598	resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
599
600	}
601	}
602	}
603	} else {
604	if (collisionShape->isConcave())
605	{
606	if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
607	{
608	//BT_PROFILE("convexSweepbtBvhTriangleMesh");
609	btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
610	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
611	btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
612	btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
613	// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
614	btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
615
616	//ConvexCast::CastResult
617	struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
618	{
619	btCollisionWorld::ConvexResultCallback* m_resultCallback;
620	btCollisionObject* m_collisionObject;
621	btTriangleMeshShape* m_triangleMesh;
622
623	BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
624	btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
625	btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
626	m_resultCallback(resultCallback),
627	m_collisionObject(collisionObject),
628	m_triangleMesh(triangleMesh)
629	{
630	}
631
632
633	virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
634	{
635	btCollisionWorld::LocalShapeInfo shapeInfo;
636	shapeInfo.m_shapePart = partId;
637	shapeInfo.m_triangleIndex = triangleIndex;
638	if (hitFraction <= m_resultCallback->m_closestHitFraction)
639	{
640
641	btCollisionWorld::LocalConvexResult convexResult
642	(m_collisionObject,
643	&shapeInfo,
644	hitNormalLocal,
645	hitPointLocal,
646	hitFraction);
647
648	bool normalInWorldSpace = true;
649
650
651	return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
652	}
653	return hitFraction;
654	}
655
656	};
657
658	BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
659	tccb.m_hitFraction = resultCallback.m_closestHitFraction;
660	tccb.m_allowedPenetration = allowedPenetration;
661	btVector3 boxMinLocal, boxMaxLocal;
662	castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
663	triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
664	} else
665	{
666	if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
667	{
668	btConvexCast::CastResult castResult;
669	castResult.m_allowedPenetration = allowedPenetration;
670	castResult.m_fraction = resultCallback.m_closestHitFraction;
671	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape;
672	btContinuousConvexCollision convexCaster1(castShape,planeShape);
673	btConvexCast* castPtr = &convexCaster1;
674
675	if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
676	{
677	//add hit
678	if (castResult.m_normal.length2() > btScalar(0.0001))
679	{
680	if (castResult.m_fraction < resultCallback.m_closestHitFraction)
681	{
682	castResult.m_normal.normalize();
683	btCollisionWorld::LocalConvexResult localConvexResult
684	(
685	collisionObject,
686	0,
687	castResult.m_normal,
688	castResult.m_hitPoint,
689	castResult.m_fraction
690	);
691
692	bool normalInWorldSpace = true;
693	resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
694	}
695	}
696	}
697
698	} else
699	{
700	//BT_PROFILE("convexSweepConcave");
701	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
702	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
703	btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
704	btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
705	// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
706	btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
707
708	//ConvexCast::CastResult
709	struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
710	{
711	btCollisionWorld::ConvexResultCallback* m_resultCallback;
712	btCollisionObject* m_collisionObject;
713	btConcaveShape* m_triangleMesh;
714
715	BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
716	btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
717	btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
718	m_resultCallback(resultCallback),
719	m_collisionObject(collisionObject),
720	m_triangleMesh(triangleMesh)
721	{
722	}
723
724
725	virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
726	{
727	btCollisionWorld::LocalShapeInfo shapeInfo;
728	shapeInfo.m_shapePart = partId;
729	shapeInfo.m_triangleIndex = triangleIndex;
730	if (hitFraction <= m_resultCallback->m_closestHitFraction)
731	{
732
733	btCollisionWorld::LocalConvexResult convexResult
734	(m_collisionObject,
735	&shapeInfo,
736	hitNormalLocal,
737	hitPointLocal,
738	hitFraction);
739
740	bool normalInWorldSpace = false;
741
742	return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
743	}
744	return hitFraction;
745	}
746
747	};
748
749	BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
750	tccb.m_hitFraction = resultCallback.m_closestHitFraction;
751	tccb.m_allowedPenetration = allowedPenetration;
752	btVector3 boxMinLocal, boxMaxLocal;
753	castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
754
755	btVector3 rayAabbMinLocal = convexFromLocal;
756	rayAabbMinLocal.setMin(convexToLocal);
757	btVector3 rayAabbMaxLocal = convexFromLocal;
758	rayAabbMaxLocal.setMax(convexToLocal);
759	rayAabbMinLocal += boxMinLocal;
760	rayAabbMaxLocal += boxMaxLocal;
761	concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
762	}
763	}
764	} else {
765	///@todo : use AABB tree or other BVH acceleration structure!
766	if (collisionShape->isCompound())
767	{
768	BT_PROFILE("convexSweepCompound");
769	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
770	int i=0;
771	for (i=0;i<compoundShape->getNumChildShapes();i++)
772	{
773	btTransform childTrans = compoundShape->getChildTransform(i);
774	const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
775	btTransform childWorldTrans = colObjWorldTransform * childTrans;
776	// replace collision shape so that callback can determine the triangle
777	btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
778	collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
779	struct LocalInfoAdder : public ConvexResultCallback {
780	ConvexResultCallback* m_userCallback;
781	int m_i;
782
783	LocalInfoAdder (int i, ConvexResultCallback *user)
784	: m_userCallback(user), m_i(i)
785	{
786	m_closestHitFraction = m_userCallback->m_closestHitFraction;
787	}
788	virtual bool needsCollision(btBroadphaseProxy* p) const
789	{
790	return m_userCallback->needsCollision(p);
791	}
792	virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b)
793	{
794	btCollisionWorld::LocalShapeInfo shapeInfo;
795	shapeInfo.m_shapePart = -1;
796	shapeInfo.m_triangleIndex = m_i;
797	if (r.m_localShapeInfo == NULL)
798	r.m_localShapeInfo = &shapeInfo;
799	const btScalar result = m_userCallback->addSingleResult(r, b);
800	m_closestHitFraction = m_userCallback->m_closestHitFraction;
801	return result;
802
803	}
804	};
805
806	LocalInfoAdder my_cb(i, &resultCallback);
807
808
809	objectQuerySingle(castShape, convexFromTrans,convexToTrans,
810	collisionObject,
811	childCollisionShape,
812	childWorldTrans,
813	my_cb, allowedPenetration);
814	// restore
815	collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
816	}
817	}
818	}
819	}
820	}
821
822
823	struct btSingleRayCallback : public btBroadphaseRayCallback
824	{
825
826	btVector3 m_rayFromWorld;
827	btVector3 m_rayToWorld;
828	btTransform m_rayFromTrans;
829	btTransform m_rayToTrans;
830	btVector3 m_hitNormal;
831
832	const btCollisionWorld* m_world;
833	btCollisionWorld::RayResultCallback& m_resultCallback;
834
835	btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
836	:m_rayFromWorld(rayFromWorld),
837	m_rayToWorld(rayToWorld),
838	m_world(world),
839	m_resultCallback(resultCallback)
840	{
841	m_rayFromTrans.setIdentity();
842	m_rayFromTrans.setOrigin(m_rayFromWorld);
843	m_rayToTrans.setIdentity();
844	m_rayToTrans.setOrigin(m_rayToWorld);
845
846	btVector3 rayDir = (rayToWorld-rayFromWorld);
847
848	rayDir.normalize ();
849	///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
850	m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
851	m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
852	m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
853	m_signs[0] = m_rayDirectionInverse[0] < 0.0;
854	m_signs[1] = m_rayDirectionInverse[1] < 0.0;
855	m_signs[2] = m_rayDirectionInverse[2] < 0.0;
856
857	m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
858
859	}
860
861
862
863	virtual bool process(const btBroadphaseProxy* proxy)
864	{
865	///terminate further ray tests, once the closestHitFraction reached zero
866	if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
867	return false;
868
869	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
870
871	//only perform raycast if filterMask matches
872	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
873	{
874	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
875	//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
876	#if 0
877	#ifdef RECALCULATE_AABB
878	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
879	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
880	#else
881	//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
882	const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
883	const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
884	#endif
885	#endif
886	//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
887	//culling already done by broadphase
888	//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
889	{
890	m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
891	collisionObject,
892	collisionObject->getCollisionShape(),
893	collisionObject->getWorldTransform(),
894	m_resultCallback);
895	}
896	}
897	return true;
898	}
899	};
900
901	void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
902	{
903	//BT_PROFILE("rayTest");
904	/// use the broadphase to accelerate the search for objects, based on their aabb
905	/// and for each object with ray-aabb overlap, perform an exact ray test
906	btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
907
908	#ifndef USE_BRUTEFORCE_RAYBROADPHASE
909	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
910	#else
911	for (int i=0;i<this->getNumCollisionObjects();i++)
912	{
913	rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
914	}
915	#endif //USE_BRUTEFORCE_RAYBROADPHASE
916
917	}
918
919
920	struct btSingleSweepCallback : public btBroadphaseRayCallback
921	{
922
923	btTransform m_convexFromTrans;
924	btTransform m_convexToTrans;
925	btVector3 m_hitNormal;
926	const btCollisionWorld* m_world;
927	btCollisionWorld::ConvexResultCallback& m_resultCallback;
928	btScalar m_allowedCcdPenetration;
929	const btConvexShape* m_castShape;
930
931
932	btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
933	:m_convexFromTrans(convexFromTrans),
934	m_convexToTrans(convexToTrans),
935	m_world(world),
936	m_resultCallback(resultCallback),
937	m_allowedCcdPenetration(allowedPenetration),
938	m_castShape(castShape)
939	{
940	btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
941	btVector3 rayDir = unnormalizedRayDir.normalized();
942	///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
943	m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
944	m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
945	m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
946	m_signs[0] = m_rayDirectionInverse[0] < 0.0;
947	m_signs[1] = m_rayDirectionInverse[1] < 0.0;
948	m_signs[2] = m_rayDirectionInverse[2] < 0.0;
949
950	m_lambda_max = rayDir.dot(unnormalizedRayDir);
951
952	}
953
954	virtual bool process(const btBroadphaseProxy* proxy)
955	{
956	///terminate further convex sweep tests, once the closestHitFraction reached zero
957	if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
958	return false;
959
960	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
961
962	//only perform raycast if filterMask matches
963	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
964	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
965	m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
966	collisionObject,
967	collisionObject->getCollisionShape(),
968	collisionObject->getWorldTransform(),
969	m_resultCallback,
970	m_allowedCcdPenetration);
971	}
972
973	return true;
974	}
975	};
976
977
978
979	void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
980	{
981
982	BT_PROFILE("convexSweepTest");
983	/// use the broadphase to accelerate the search for objects, based on their aabb
984	/// and for each object with ray-aabb overlap, perform an exact ray test
985	/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
986
987
988
989	btTransform convexFromTrans,convexToTrans;
990	convexFromTrans = convexFromWorld;
991	convexToTrans = convexToWorld;
992	btVector3 castShapeAabbMin, castShapeAabbMax;
993	/* Compute AABB that encompasses angular movement */
994	{
995	btVector3 linVel, angVel;
996	btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
997	btVector3 zeroLinVel;
998	zeroLinVel.setValue(0,0,0);
999	btTransform R;
1000	R.setIdentity ();
1001	R.setRotation (convexFromTrans.getRotation());
1002	castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
1003	}
1004
1005	#ifndef USE_BRUTEFORCE_RAYBROADPHASE
1006
1007	btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
1008
1009	m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
1010
1011	#else
1012	/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
1013	// do a ray-shape query using convexCaster (CCD)
1014	int i;
1015	for (i=0;i<m_collisionObjects.size();i++)
1016	{
1017	btCollisionObject* collisionObject= m_collisionObjects[i];
1018	//only perform raycast if filterMask matches
1019	if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
1020	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
1021	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
1022	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
1023	AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
1024	btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
1025	btVector3 hitNormal;
1026	if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
1027	{
1028	objectQuerySingle(castShape, convexFromTrans,convexToTrans,
1029	collisionObject,
1030	collisionObject->getCollisionShape(),
1031	collisionObject->getWorldTransform(),
1032	resultCallback,
1033	allowedCcdPenetration);
1034	}
1035	}
1036	}
1037	#endif //USE_BRUTEFORCE_RAYBROADPHASE
1038	}
1039
1040
1041
1042	struct btBridgedManifoldResult : public btManifoldResult
1043	{
1044
1045	btCollisionWorld::ContactResultCallback& m_resultCallback;
1046
1047	btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback )
1048	:btManifoldResult(obj0,obj1),
1049	m_resultCallback(resultCallback)
1050	{
1051	}
1052
1053	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
1054	{
1055	bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
1056	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
1057	btVector3 localA;
1058	btVector3 localB;
1059	if (isSwapped)
1060	{
1061	localA = m_rootTransB.invXform(pointA );
1062	localB = m_rootTransA.invXform(pointInWorld);
1063	} else
1064	{
1065	localA = m_rootTransA.invXform(pointA );
1066	localB = m_rootTransB.invXform(pointInWorld);
1067	}
1068
1069	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
1070	newPt.m_positionWorldOnA = pointA;
1071	newPt.m_positionWorldOnB = pointInWorld;
1072
1073	//BP mod, store contact triangles.
1074	if (isSwapped)
1075	{
1076	newPt.m_partId0 = m_partId1;
1077	newPt.m_partId1 = m_partId0;
1078	newPt.m_index0 = m_index1;
1079	newPt.m_index1 = m_index0;
1080	} else
1081	{
1082	newPt.m_partId0 = m_partId0;
1083	newPt.m_partId1 = m_partId1;
1084	newPt.m_index0 = m_index0;
1085	newPt.m_index1 = m_index1;
1086	}
1087
1088	//experimental feature info, for per-triangle material etc.
1089	btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
1090	btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
1091	m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
1092
1093	}
1094
1095	};
1096
1097
1098
1099	struct btSingleContactCallback : public btBroadphaseAabbCallback
1100	{
1101
1102	btCollisionObject* m_collisionObject;
1103	btCollisionWorld* m_world;
1104	btCollisionWorld::ContactResultCallback& m_resultCallback;
1105
1106
1107	btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
1108	:m_collisionObject(collisionObject),
1109	m_world(world),
1110	m_resultCallback(resultCallback)
1111	{
1112	}
1113
1114	virtual bool process(const btBroadphaseProxy* proxy)
1115	{
1116	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
1117	if (collisionObject == m_collisionObject)
1118	return true;
1119
1120	//only perform raycast if filterMask matches
1121	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
1122	{
1123	btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject);
1124	if (algorithm)
1125	{
1126	btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback);
1127	//discrete collision detection query
1128	algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult);
1129
1130	algorithm->~btCollisionAlgorithm();
1131	m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
1132	}
1133	}
1134	return true;
1135	}
1136	};
1137
1138
1139	///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
1140	///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
1141	void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
1142	{
1143	btVector3 aabbMin,aabbMax;
1144	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
1145	btSingleContactCallback contactCB(colObj,this,resultCallback);
1146
1147	m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
1148	}
1149
1150
1151	///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
1152	///it reports one or more contact points (including the one with deepest penetration)
1153	void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
1154	{
1155	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
1156	if (algorithm)
1157	{
1158	btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
1159	//discrete collision detection query
1160	algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
1161
1162	algorithm->~btCollisionAlgorithm();
1163	getDispatcher()->freeCollisionAlgorithm(algorithm);
1164	}
1165
1166	}
1167
1168
1169
1170
1171	class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
1172	{
1173	btIDebugDraw* m_debugDrawer;
1174	btVector3 m_color;
1175	btTransform m_worldTrans;
1176
1177	public:
1178
1179	DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
1180	m_debugDrawer(debugDrawer),
1181	m_color(color),
1182	m_worldTrans(worldTrans)
1183	{
1184	}
1185
1186	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
1187	{
1188	processTriangle(triangle,partId,triangleIndex);
1189	}
1190
1191	virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
1192	{
1193	(void)partId;
1194	(void)triangleIndex;
1195
1196	btVector3 wv0,wv1,wv2;
1197	wv0 = m_worldTrans*triangle[0];
1198	wv1 = m_worldTrans*triangle[1];
1199	wv2 = m_worldTrans*triangle[2];
1200	btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
1201
1202	btVector3 normal = (wv1-wv0).cross(wv2-wv0);
1203	normal.normalize();
1204	btVector3 normalColor(1,1,0);
1205	m_debugDrawer->drawLine(center,center+normal,normalColor);
1206
1207
1208
1209
1210	m_debugDrawer->drawLine(wv0,wv1,m_color);
1211	m_debugDrawer->drawLine(wv1,wv2,m_color);
1212	m_debugDrawer->drawLine(wv2,wv0,m_color);
1213	}
1214	};
1215
1216
1217	void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
1218	{
1219	// Draw a small simplex at the center of the object
1220	getDebugDrawer()->drawTransform(worldTransform,1);
1221
1222	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1223	{
1224	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
1225	for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
1226	{
1227	btTransform childTrans = compoundShape->getChildTransform(i);
1228	const btCollisionShape* colShape = compoundShape->getChildShape(i);
1229	debugDrawObject(worldTransform*childTrans,colShape,color);
1230	}
1231
1232	} else
1233	{
1234	switch (shape->getShapeType())
1235	{
1236
1237	case BOX_SHAPE_PROXYTYPE:
1238	{
1239	const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
1240	btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
1241	getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
1242	break;
1243	}
1244
1245	case SPHERE_SHAPE_PROXYTYPE:
1246	{
1247	const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1248	btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
1249
1250	getDebugDrawer()->drawSphere(radius, worldTransform, color);
1251	break;
1252	}
1253	case MULTI_SPHERE_SHAPE_PROXYTYPE:
1254	{
1255	const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1256
1257	btTransform childTransform;
1258	childTransform.setIdentity();
1259
1260	for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
1261	{
1262	childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
1263	getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
1264	}
1265
1266	break;
1267	}
1268	case CAPSULE_SHAPE_PROXYTYPE:
1269	{
1270	const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1271
1272	btScalar radius = capsuleShape->getRadius();
1273	btScalar halfHeight = capsuleShape->getHalfHeight();
1274
1275	int upAxis = capsuleShape->getUpAxis();
1276	getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
1277	break;
1278	}
1279	case CONE_SHAPE_PROXYTYPE:
1280	{
1281	const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1282	btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
1283	btScalar height = coneShape->getHeight();//+coneShape->getMargin();
1284
1285	int upAxis= coneShape->getConeUpIndex();
1286	getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
1287	break;
1288
1289	}
1290	case CYLINDER_SHAPE_PROXYTYPE:
1291	{
1292	const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1293	int upAxis = cylinder->getUpAxis();
1294	btScalar radius = cylinder->getRadius();
1295	btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1296	getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
1297	break;
1298	}
1299
1300	case STATIC_PLANE_PROXYTYPE:
1301	{
1302	const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1303	btScalar planeConst = staticPlaneShape->getPlaneConstant();
1304	const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1305	getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
1306	break;
1307
1308	}
1309	default:
1310	{
1311
1312	if (shape->isConcave())
1313	{
1314	btConcaveShape* concaveMesh = (btConcaveShape*) shape;
1315
1316	///@todo pass camera, for some culling? no -> we are not a graphics lib
1317	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1318	btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1319
1320	DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1321	concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
1322
1323	}
1324
1325	if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1326	{
1327	btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
1328	//todo: pass camera for some culling
1329	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1330	btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1331	//DebugDrawcallback drawCallback;
1332	DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1333	convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
1334	}
1335
1336
1337	/// for polyhedral shapes
1338	if (shape->isPolyhedral())
1339	{
1340	btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
1341
1342	int i;
1343	if (polyshape->getConvexPolyhedron())
1344	{
1345	const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
1346	for (i=0;i<poly->m_faces.size();i++)
1347	{
1348	btVector3 centroid(0,0,0);
1349	int numVerts = poly->m_faces[i].m_indices.size();
1350	if (numVerts)
1351	{
1352	int lastV = poly->m_faces[i].m_indices[numVerts-1];
1353	for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
1354	{
1355	int curVert = poly->m_faces[i].m_indices[v];
1356	centroid+=poly->m_vertices[curVert];
1357	getDebugDrawer()->drawLine(worldTransformpoly->m_vertices[lastV],worldTransformpoly->m_vertices[curVert],color);
1358	lastV = curVert;
1359	}
1360	}
1361	centroid*= 1./btScalar(numVerts);
1362
1363	btVector3 normalColor(1,1,0);
1364	btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
1365	getDebugDrawer()->drawLine(worldTransformcentroid,worldTransform(centroid+faceNormal),normalColor);
1366
1367
1368	}
1369
1370
1371	} else
1372	{
1373	for (i=0;i<polyshape->getNumEdges();i++)
1374	{
1375	btVector3 a,b;
1376	polyshape->getEdge(i,a,b);
1377	btVector3 wa = worldTransform * a;
1378	btVector3 wb = worldTransform * b;
1379	getDebugDrawer()->drawLine(wa,wb,color);
1380	}
1381	}
1382
1383
1384	}
1385	}
1386	}
1387	}
1388	}
1389
1390
1391	void btCollisionWorld::debugDrawWorld()
1392	{
1393	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
1394	{
1395	int numManifolds = getDispatcher()->getNumManifolds();
1396	btVector3 color(0,0,0);
1397	for (int i=0;i<numManifolds;i++)
1398	{
1399	btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
1400	//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
1401	//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
1402
1403	int numContacts = contactManifold->getNumContacts();
1404	for (int j=0;j<numContacts;j++)
1405	{
1406	btManifoldPoint& cp = contactManifold->getContactPoint(j);
1407	getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1408	}
1409	}
1410	}
1411
1412	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe \| btIDebugDraw::DBG_DrawAabb))
1413	{
1414	int i;
1415
1416	for ( i=0;i<m_collisionObjects.size();i++)
1417	{
1418	btCollisionObject* colObj = m_collisionObjects[i];
1419	if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
1420	{
1421	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
1422	{
1423	btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
1424	switch(colObj->getActivationState())
1425	{
1426	case ACTIVE_TAG:
1427	color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
1428	case ISLAND_SLEEPING:
1429	color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
1430	case WANTS_DEACTIVATION:
1431	color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
1432	case DISABLE_DEACTIVATION:
1433	color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
1434	case DISABLE_SIMULATION:
1435	color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
1436	default:
1437	{
1438	color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
1439	}
1440	};
1441
1442	debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
1443	}
1444	if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
1445	{
1446	btVector3 minAabb,maxAabb;
1447	btVector3 colorvec(1,0,0);
1448	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
1449	btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
1450	minAabb -= contactThreshold;
1451	maxAabb += contactThreshold;
1452
1453	btVector3 minAabb2,maxAabb2;
1454
1455	if(colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
1456	{
1457	colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
1458	minAabb2 -= contactThreshold;
1459	maxAabb2 += contactThreshold;
1460	minAabb.setMin(minAabb2);
1461	maxAabb.setMax(maxAabb2);
1462	}
1463
1464	m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
1465	}
1466	}
1467
1468	}
1469	}
1470	}
1471
1472
1473	void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
1474	{
1475	int i;
1476	//serialize all collision objects
1477	for (i=0;i<m_collisionObjects.size();i++)
1478	{
1479	btCollisionObject* colObj = m_collisionObjects[i];
1480	if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
1481	{
1482	colObj->serializeSingleObject(serializer);
1483	}
1484	}
1485
1486	///keep track of shapes already serialized
1487	btHashMap<btHashPtr,btCollisionShape*> serializedShapes;
1488
1489	for (i=0;i<m_collisionObjects.size();i++)
1490	{
1491	btCollisionObject* colObj = m_collisionObjects[i];
1492	btCollisionShape* shape = colObj->getCollisionShape();
1493
1494	if (!serializedShapes.find(shape))
1495	{
1496	serializedShapes.insert(shape,shape);
1497	shape->serializeSingleShape(serializer);
1498	}
1499	}
1500
1501	}
1502
1503
1504	void btCollisionWorld::serialize(btSerializer* serializer)
1505	{
1506
1507	serializer->startSerialization();
1508
1509	serializeCollisionObjects(serializer);
1510
1511	serializer->finishSerialization();
1512	}
1513

Note: See TracBrowser for help on using the repository browser.

Download in other formats: