Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics_merge/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 2518

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

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

Download in other formats: