Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/libraries/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 5649

Last change on this file since 5649 was 2882, checked in by rgrieder, 16 years ago
Update from Bullet 2.73 to 2.74.
Property svn:eol-style set to `native`
File size: 29.7 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
30	#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
31	#include "LinearMath/btAabbUtil2.h"
32	#include "LinearMath/btQuickprof.h"
33	#include "LinearMath/btStackAlloc.h"
34
35	//#define USE_BRUTEFORCE_RAYBROADPHASE 1
36	//RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest
37	//#define RECALCULATE_AABB_RAYCAST 1
38
39	//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
40	#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
41	#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
42	#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
43
44
45	btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
46	:m_dispatcher1(dispatcher),
47	m_broadphasePairCache(pairCache),
48	m_debugDrawer(0)
49	{
50	m_stackAlloc = collisionConfiguration->getStackAllocator();
51	m_dispatchInfo.m_stackAllocator = m_stackAlloc;
52	}
53
54
55	btCollisionWorld::~btCollisionWorld()
56	{
57
58	//clean up remaining objects
59	int i;
60	for (i=0;i<m_collisionObjects.size();i++)
61	{
62	btCollisionObject* collisionObject= m_collisionObjects[i];
63
64	btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
65	if (bp)
66	{
67	//
68	// only clear the cached algorithms
69	//
70	getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
71	getBroadphase()->destroyProxy(bp,m_dispatcher1);
72	collisionObject->setBroadphaseHandle(0);
73	}
74	}
75
76
77	}
78
79
80
81
82
83
84
85
86
87
88	void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
89	{
90
91	//check that the object isn't already added
92	btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
93
94	m_collisionObjects.push_back(collisionObject);
95
96	//calculate new AABB
97	btTransform trans = collisionObject->getWorldTransform();
98
99	btVector3 minAabb;
100	btVector3 maxAabb;
101	collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
102
103	int type = collisionObject->getCollisionShape()->getShapeType();
104	collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
105	minAabb,
106	maxAabb,
107	type,
108	collisionObject,
109	collisionFilterGroup,
110	collisionFilterMask,
111	m_dispatcher1,0
112	)) ;
113
114
115
116
117
118	}
119
120
121
122	void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
123	{
124	btVector3 minAabb,maxAabb;
125	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
126	//need to increase the aabb for contact thresholds
127	btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
128	minAabb -= contactThreshold;
129	maxAabb += contactThreshold;
130
131	btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
132
133	//moving objects should be moderately sized, probably something wrong if not
134	if ( colObj->isStaticObject() \|\| ((maxAabb-minAabb).length2() < btScalar(1e12)))
135	{
136	bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
137	} else
138	{
139	//something went wrong, investigate
140	//this assert is unwanted in 3D modelers (danger of loosing work)
141	colObj->setActivationState(DISABLE_SIMULATION);
142
143	static bool reportMe = true;
144	if (reportMe && m_debugDrawer)
145	{
146	reportMe = false;
147	m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
148	m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
149	m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
150	m_debugDrawer->reportErrorWarning("Thanks.\n");
151	}
152	}
153	}
154
155	void btCollisionWorld::updateAabbs()
156	{
157	BT_PROFILE("updateAabbs");
158
159	btTransform predictedTrans;
160	for ( int i=0;i<m_collisionObjects.size();i++)
161	{
162	btCollisionObject* colObj = m_collisionObjects[i];
163
164	//only update aabb of active objects
165	if (colObj->isActive())
166	{
167	updateSingleAabb(colObj);
168	}
169	}
170	}
171
172
173
174	void btCollisionWorld::performDiscreteCollisionDetection()
175	{
176	BT_PROFILE("performDiscreteCollisionDetection");
177
178	btDispatcherInfo& dispatchInfo = getDispatchInfo();
179
180	updateAabbs();
181
182	{
183	BT_PROFILE("calculateOverlappingPairs");
184	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
185	}
186
187
188	btDispatcher* dispatcher = getDispatcher();
189	{
190	BT_PROFILE("dispatchAllCollisionPairs");
191	if (dispatcher)
192	dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
193	}
194
195	}
196
197
198
199	void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
200	{
201
202
203	//bool removeFromBroadphase = false;
204
205	{
206
207	btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
208	if (bp)
209	{
210	//
211	// only clear the cached algorithms
212	//
213	getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
214	getBroadphase()->destroyProxy(bp,m_dispatcher1);
215	collisionObject->setBroadphaseHandle(0);
216	}
217	}
218
219
220	//swapremove
221	m_collisionObjects.remove(collisionObject);
222
223	}
224
225
226
227	void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
228	btCollisionObject* collisionObject,
229	const btCollisionShape* collisionShape,
230	const btTransform& colObjWorldTransform,
231	RayResultCallback& resultCallback)
232	{
233	btSphereShape pointShape(btScalar(0.0));
234	pointShape.setMargin(0.f);
235	const btConvexShape* castShape = &pointShape;
236
237	if (collisionShape->isConvex())
238	{
239	// BT_PROFILE("rayTestConvex");
240	btConvexCast::CastResult castResult;
241	castResult.m_fraction = resultCallback.m_closestHitFraction;
242
243	btConvexShape* convexShape = (btConvexShape*) collisionShape;
244	btVoronoiSimplexSolver simplexSolver;
245	#define USE_SUBSIMPLEX_CONVEX_CAST 1
246	#ifdef USE_SUBSIMPLEX_CONVEX_CAST
247	btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
248	#else
249	//btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
250	//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
251	#endif //#USE_SUBSIMPLEX_CONVEX_CAST
252
253	if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
254	{
255	//add hit
256	if (castResult.m_normal.length2() > btScalar(0.0001))
257	{
258	if (castResult.m_fraction < resultCallback.m_closestHitFraction)
259	{
260	#ifdef USE_SUBSIMPLEX_CONVEX_CAST
261	//rotate normal into worldspace
262	castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
263	#endif //USE_SUBSIMPLEX_CONVEX_CAST
264
265	castResult.m_normal.normalize();
266	btCollisionWorld::LocalRayResult localRayResult
267	(
268	collisionObject,
269	0,
270	castResult.m_normal,
271	castResult.m_fraction
272	);
273
274	bool normalInWorldSpace = true;
275	resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
276
277	}
278	}
279	}
280	} else {
281	if (collisionShape->isConcave())
282	{
283	// BT_PROFILE("rayTestConcave");
284	if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
285	{
286	///optimized version for btBvhTriangleMeshShape
287	btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
288	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
289	btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
290	btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
291
292	//ConvexCast::CastResult
293	struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
294	{
295	btCollisionWorld::RayResultCallback* m_resultCallback;
296	btCollisionObject* m_collisionObject;
297	btTriangleMeshShape* m_triangleMesh;
298
299	BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
300	btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh):
301	//@BP Mod
302	btTriangleRaycastCallback(from,to, resultCallback->m_flags),
303	m_resultCallback(resultCallback),
304	m_collisionObject(collisionObject),
305	m_triangleMesh(triangleMesh)
306	{
307	}
308
309
310	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
311	{
312	btCollisionWorld::LocalShapeInfo shapeInfo;
313	shapeInfo.m_shapePart = partId;
314	shapeInfo.m_triangleIndex = triangleIndex;
315
316	btCollisionWorld::LocalRayResult rayResult
317	(m_collisionObject,
318	&shapeInfo,
319	hitNormalLocal,
320	hitFraction);
321
322	bool normalInWorldSpace = false;
323	return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
324	}
325
326	};
327
328	BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
329	rcb.m_hitFraction = resultCallback.m_closestHitFraction;
330	triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
331	} else
332	{
333	//generic (slower) case
334	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
335
336	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
337
338	btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
339	btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
340
341	//ConvexCast::CastResult
342
343	struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
344	{
345	btCollisionWorld::RayResultCallback* m_resultCallback;
346	btCollisionObject* m_collisionObject;
347	btConcaveShape* m_triangleMesh;
348
349	BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
350	btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
351	//@BP Mod
352	btTriangleRaycastCallback(from,to, resultCallback->m_flags),
353	m_resultCallback(resultCallback),
354	m_collisionObject(collisionObject),
355	m_triangleMesh(triangleMesh)
356	{
357	}
358
359
360	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
361	{
362	btCollisionWorld::LocalShapeInfo shapeInfo;
363	shapeInfo.m_shapePart = partId;
364	shapeInfo.m_triangleIndex = triangleIndex;
365
366	btCollisionWorld::LocalRayResult rayResult
367	(m_collisionObject,
368	&shapeInfo,
369	hitNormalLocal,
370	hitFraction);
371
372	bool normalInWorldSpace = false;
373	return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
374
375
376	}
377
378	};
379
380
381	BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
382	rcb.m_hitFraction = resultCallback.m_closestHitFraction;
383
384	btVector3 rayAabbMinLocal = rayFromLocal;
385	rayAabbMinLocal.setMin(rayToLocal);
386	btVector3 rayAabbMaxLocal = rayFromLocal;
387	rayAabbMaxLocal.setMax(rayToLocal);
388
389	concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
390	}
391	} else {
392	// BT_PROFILE("rayTestCompound");
393	///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
394	if (collisionShape->isCompound())
395	{
396	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
397	int i=0;
398	for (i=0;i<compoundShape->getNumChildShapes();i++)
399	{
400	btTransform childTrans = compoundShape->getChildTransform(i);
401	const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
402	btTransform childWorldTrans = colObjWorldTransform * childTrans;
403	// replace collision shape so that callback can determine the triangle
404	btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
405	collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
406	rayTestSingle(rayFromTrans,rayToTrans,
407	collisionObject,
408	childCollisionShape,
409	childWorldTrans,
410	resultCallback);
411	// restore
412	collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
413	}
414	}
415	}
416	}
417	}
418
419	void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
420	btCollisionObject* collisionObject,
421	const btCollisionShape* collisionShape,
422	const btTransform& colObjWorldTransform,
423	ConvexResultCallback& resultCallback, btScalar allowedPenetration)
424	{
425	if (collisionShape->isConvex())
426	{
427	//BT_PROFILE("convexSweepConvex");
428	btConvexCast::CastResult castResult;
429	castResult.m_allowedPenetration = allowedPenetration;
430	castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
431
432	btConvexShape* convexShape = (btConvexShape*) collisionShape;
433	btVoronoiSimplexSolver simplexSolver;
434	btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
435
436	btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
437	//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
438	//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
439
440	btConvexCast* castPtr = &convexCaster1;
441
442
443
444	if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
445	{
446	//add hit
447	if (castResult.m_normal.length2() > btScalar(0.0001))
448	{
449	if (castResult.m_fraction < resultCallback.m_closestHitFraction)
450	{
451	castResult.m_normal.normalize();
452	btCollisionWorld::LocalConvexResult localConvexResult
453	(
454	collisionObject,
455	0,
456	castResult.m_normal,
457	castResult.m_hitPoint,
458	castResult.m_fraction
459	);
460
461	bool normalInWorldSpace = true;
462	resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
463
464	}
465	}
466	}
467	} else {
468	if (collisionShape->isConcave())
469	{
470	if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
471	{
472	//BT_PROFILE("convexSweepbtBvhTriangleMesh");
473	btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
474	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
475	btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
476	btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
477	// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
478	btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
479
480	//ConvexCast::CastResult
481	struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
482	{
483	btCollisionWorld::ConvexResultCallback* m_resultCallback;
484	btCollisionObject* m_collisionObject;
485	btTriangleMeshShape* m_triangleMesh;
486
487	BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
488	btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
489	btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
490	m_resultCallback(resultCallback),
491	m_collisionObject(collisionObject),
492	m_triangleMesh(triangleMesh)
493	{
494	}
495
496
497	virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
498	{
499	btCollisionWorld::LocalShapeInfo shapeInfo;
500	shapeInfo.m_shapePart = partId;
501	shapeInfo.m_triangleIndex = triangleIndex;
502	if (hitFraction <= m_resultCallback->m_closestHitFraction)
503	{
504
505	btCollisionWorld::LocalConvexResult convexResult
506	(m_collisionObject,
507	&shapeInfo,
508	hitNormalLocal,
509	hitPointLocal,
510	hitFraction);
511
512	bool normalInWorldSpace = true;
513
514
515	return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
516	}
517	return hitFraction;
518	}
519
520	};
521
522	BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
523	tccb.m_hitFraction = resultCallback.m_closestHitFraction;
524	btVector3 boxMinLocal, boxMaxLocal;
525	castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
526	triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
527	} else
528	{
529	//BT_PROFILE("convexSweepConcave");
530	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
531	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
532	btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
533	btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
534	// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
535	btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
536
537	//ConvexCast::CastResult
538	struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
539	{
540	btCollisionWorld::ConvexResultCallback* m_resultCallback;
541	btCollisionObject* m_collisionObject;
542	btConcaveShape* m_triangleMesh;
543
544	BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
545	btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
546	btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
547	m_resultCallback(resultCallback),
548	m_collisionObject(collisionObject),
549	m_triangleMesh(triangleMesh)
550	{
551	}
552
553
554	virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
555	{
556	btCollisionWorld::LocalShapeInfo shapeInfo;
557	shapeInfo.m_shapePart = partId;
558	shapeInfo.m_triangleIndex = triangleIndex;
559	if (hitFraction <= m_resultCallback->m_closestHitFraction)
560	{
561
562	btCollisionWorld::LocalConvexResult convexResult
563	(m_collisionObject,
564	&shapeInfo,
565	hitNormalLocal,
566	hitPointLocal,
567	hitFraction);
568
569	bool normalInWorldSpace = false;
570
571	return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
572	}
573	return hitFraction;
574	}
575
576	};
577
578	BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
579	tccb.m_hitFraction = resultCallback.m_closestHitFraction;
580	btVector3 boxMinLocal, boxMaxLocal;
581	castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
582
583	btVector3 rayAabbMinLocal = convexFromLocal;
584	rayAabbMinLocal.setMin(convexToLocal);
585	btVector3 rayAabbMaxLocal = convexFromLocal;
586	rayAabbMaxLocal.setMax(convexToLocal);
587	rayAabbMinLocal += boxMinLocal;
588	rayAabbMaxLocal += boxMaxLocal;
589	concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
590	}
591	} else {
592	///@todo : use AABB tree or other BVH acceleration structure!
593	if (collisionShape->isCompound())
594	{
595	BT_PROFILE("convexSweepCompound");
596	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
597	int i=0;
598	for (i=0;i<compoundShape->getNumChildShapes();i++)
599	{
600	btTransform childTrans = compoundShape->getChildTransform(i);
601	const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
602	btTransform childWorldTrans = colObjWorldTransform * childTrans;
603	// replace collision shape so that callback can determine the triangle
604	btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
605	collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
606	objectQuerySingle(castShape, convexFromTrans,convexToTrans,
607	collisionObject,
608	childCollisionShape,
609	childWorldTrans,
610	resultCallback, allowedPenetration);
611	// restore
612	collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
613	}
614	}
615	}
616	}
617	}
618
619
620	struct btSingleRayCallback : public btBroadphaseRayCallback
621	{
622
623	btVector3 m_rayFromWorld;
624	btVector3 m_rayToWorld;
625	btTransform m_rayFromTrans;
626	btTransform m_rayToTrans;
627	btVector3 m_hitNormal;
628
629	const btCollisionWorld* m_world;
630	btCollisionWorld::RayResultCallback& m_resultCallback;
631
632	btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
633	:m_rayFromWorld(rayFromWorld),
634	m_rayToWorld(rayToWorld),
635	m_world(world),
636	m_resultCallback(resultCallback)
637	{
638	m_rayFromTrans.setIdentity();
639	m_rayFromTrans.setOrigin(m_rayFromWorld);
640	m_rayToTrans.setIdentity();
641	m_rayToTrans.setOrigin(m_rayToWorld);
642
643	btVector3 rayDir = (rayToWorld-rayFromWorld);
644
645	rayDir.normalize ();
646	///what about division by zero? --> just set rayDirection[i] to INF/1e30
647	m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
648	m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
649	m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
650	m_signs[0] = m_rayDirectionInverse[0] < 0.0;
651	m_signs[1] = m_rayDirectionInverse[1] < 0.0;
652	m_signs[2] = m_rayDirectionInverse[2] < 0.0;
653
654	m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
655
656	}
657
658
659
660	virtual bool process(const btBroadphaseProxy* proxy)
661	{
662	///terminate further ray tests, once the closestHitFraction reached zero
663	if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
664	return false;
665
666	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
667
668	//only perform raycast if filterMask matches
669	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
670	{
671	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
672	//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
673	#if 0
674	#ifdef RECALCULATE_AABB
675	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
676	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
677	#else
678	//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
679	const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
680	const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
681	#endif
682	#endif
683	//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
684	//culling already done by broadphase
685	//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
686	{
687	m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
688	collisionObject,
689	collisionObject->getCollisionShape(),
690	collisionObject->getWorldTransform(),
691	m_resultCallback);
692	}
693	}
694	return true;
695	}
696	};
697
698	void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
699	{
700	BT_PROFILE("rayTest");
701	/// use the broadphase to accelerate the search for objects, based on their aabb
702	/// and for each object with ray-aabb overlap, perform an exact ray test
703	btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
704
705	#ifndef USE_BRUTEFORCE_RAYBROADPHASE
706	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
707	#else
708	for (int i=0;i<this->getNumCollisionObjects();i++)
709	{
710	rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
711	}
712	#endif //USE_BRUTEFORCE_RAYBROADPHASE
713
714	}
715
716
717	struct btSingleSweepCallback : public btBroadphaseRayCallback
718	{
719
720	btTransform m_convexFromTrans;
721	btTransform m_convexToTrans;
722	btVector3 m_hitNormal;
723	const btCollisionWorld* m_world;
724	btCollisionWorld::ConvexResultCallback& m_resultCallback;
725	btScalar m_allowedCcdPenetration;
726	const btConvexShape* m_castShape;
727
728
729	btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
730	:m_convexFromTrans(convexFromTrans),
731	m_convexToTrans(convexToTrans),
732	m_world(world),
733	m_resultCallback(resultCallback),
734	m_allowedCcdPenetration(allowedPenetration),
735	m_castShape(castShape)
736	{
737	btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
738	btVector3 rayDir = unnormalizedRayDir.normalized();
739	///what about division by zero? --> just set rayDirection[i] to INF/1e30
740	m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
741	m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
742	m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
743	m_signs[0] = m_rayDirectionInverse[0] < 0.0;
744	m_signs[1] = m_rayDirectionInverse[1] < 0.0;
745	m_signs[2] = m_rayDirectionInverse[2] < 0.0;
746
747	m_lambda_max = rayDir.dot(unnormalizedRayDir);
748
749	}
750
751	virtual bool process(const btBroadphaseProxy* proxy)
752	{
753	///terminate further convex sweep tests, once the closestHitFraction reached zero
754	if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
755	return false;
756
757	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
758
759	//only perform raycast if filterMask matches
760	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
761	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
762	m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
763	collisionObject,
764	collisionObject->getCollisionShape(),
765	collisionObject->getWorldTransform(),
766	m_resultCallback,
767	m_allowedCcdPenetration);
768	}
769
770	return true;
771	}
772	};
773
774
775
776	void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
777	{
778
779	BT_PROFILE("convexSweepTest");
780	/// use the broadphase to accelerate the search for objects, based on their aabb
781	/// and for each object with ray-aabb overlap, perform an exact ray test
782	/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
783
784
785
786	btTransform convexFromTrans,convexToTrans;
787	convexFromTrans = convexFromWorld;
788	convexToTrans = convexToWorld;
789	btVector3 castShapeAabbMin, castShapeAabbMax;
790	/* Compute AABB that encompasses angular movement */
791	{
792	btVector3 linVel, angVel;
793	btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
794	btVector3 zeroLinVel;
795	zeroLinVel.setValue(0,0,0);
796	btTransform R;
797	R.setIdentity ();
798	R.setRotation (convexFromTrans.getRotation());
799	castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
800	}
801
802	#ifndef USE_BRUTEFORCE_RAYBROADPHASE
803
804	btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
805
806	m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
807
808	#else
809	/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
810	// do a ray-shape query using convexCaster (CCD)
811	int i;
812	for (i=0;i<m_collisionObjects.size();i++)
813	{
814	btCollisionObject* collisionObject= m_collisionObjects[i];
815	//only perform raycast if filterMask matches
816	if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
817	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
818	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
819	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
820	AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
821	btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
822	btVector3 hitNormal;
823	if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
824	{
825	objectQuerySingle(castShape, convexFromTrans,convexToTrans,
826	collisionObject,
827	collisionObject->getCollisionShape(),
828	collisionObject->getWorldTransform(),
829	resultCallback,
830	allowedCcdPenetration);
831	}
832	}
833	}
834	#endif //USE_BRUTEFORCE_RAYBROADPHASE
835	}

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

Download in other formats: