Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 1972

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

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

Download in other formats: