Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 8043

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

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

Download in other formats: