Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @ 5618

Last change on this file since 5618 was 2882, checked in by rgrieder, 17 years ago
Update from Bullet 2.73 to 2.74.
Property svn:eol-style set to `native`
File size: 29.7 KB

Rev	Line
[1963]	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
[2430]	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
[1963]	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);
[2882]	72	collisionObject->setBroadphaseHandle(0);
[1963]	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
[2430]	120
	121
[2882]	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
[1963]	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	{
[2882]	167	updateSingleAabb(colObj);
[1963]	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	{
[2430]	239	// BT_PROFILE("rayTestConvex");
[1963]	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	{
[2430]	283	// BT_PROFILE("rayTestConcave");
[1963]	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):
[2882]	301	//@BP Mod
	302	btTriangleRaycastCallback(from,to, resultCallback->m_flags),
[1963]	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	{
[2430]	333	//generic (slower) case
	334	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
[1963]	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;
[2430]	347	btConcaveShape* m_triangleMesh;
[1963]	348
	349	BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
[2430]	350	btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
[2882]	351	//@BP Mod
	352	btTriangleRaycastCallback(from,to, resultCallback->m_flags),
[1963]	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
[2430]	381	BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
[1963]	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
[2430]	389	concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
[1963]	390	}
	391	} else {
[2430]	392	// BT_PROFILE("rayTestCompound");
	393	///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
[1963]	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	{
[2430]	427	//BT_PROFILE("convexSweepConvex");
[1963]	428	btConvexCast::CastResult castResult;
	429	castResult.m_allowedPenetration = allowedPenetration;
[2430]	430	castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
[1963]	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	{
[2430]	472	//BT_PROFILE("convexSweepbtBvhTriangleMesh");
[1963]	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	{
[2430]	529	//BT_PROFILE("convexSweepConcave");
	530	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
[1963]	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;
[2430]	542	btConcaveShape* m_triangleMesh;
[1963]	543
	544	BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
[2430]	545	btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
[1963]	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
[2430]	578	BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
[1963]	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;
[2430]	589	concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
[1963]	590	}
	591	} else {
[2430]	592	///@todo : use AABB tree or other BVH acceleration structure!
[1963]	593	if (collisionShape->isCompound())
	594	{
[2430]	595	BT_PROFILE("convexSweepCompound");
[1963]	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
[2430]	619
	620	struct btSingleRayCallback : public btBroadphaseRayCallback
[1963]	621	{
	622
[2430]	623	btVector3 m_rayFromWorld;
	624	btVector3 m_rayToWorld;
	625	btTransform m_rayFromTrans;
	626	btTransform m_rayToTrans;
	627	btVector3 m_hitNormal;
[1963]	628
[2430]	629	const btCollisionWorld* m_world;
	630	btCollisionWorld::RayResultCallback& m_resultCallback;
[1963]	631
[2430]	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);
[1963]	642
[2430]	643	btVector3 rayDir = (rayToWorld-rayFromWorld);
[1972]	644
[2430]	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)
[1963]	661	{
	662	///terminate further ray tests, once the closestHitFraction reached zero
[2430]	663	if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
	664	return false;
[1963]	665
[2430]	666	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
	667
[1963]	668	//only perform raycast if filterMask matches
[2430]	669	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
	670	{
[1963]	671	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
[2430]	672	//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
[2882]	673	#if 0
[2430]	674	#ifdef RECALCULATE_AABB
[1963]	675	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
	676	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
[2430]	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
[2882]	682	#endif
[2430]	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))
[1963]	686	{
[2430]	687	m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
[1963]	688	collisionObject,
	689	collisionObject->getCollisionShape(),
	690	collisionObject->getWorldTransform(),
[2430]	691	m_resultCallback);
[1963]	692	}
	693	}
[2430]	694	return true;
[1963]	695	}
[2430]	696	};
[1963]	697
[2430]	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
[1963]	714	}
	715
[2430]	716
	717	struct btSingleSweepCallback : public btBroadphaseRayCallback
[1963]	718	{
[2430]	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
[1963]	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);
[2430]	794	btVector3 zeroLinVel;
	795	zeroLinVel.setValue(0,0,0);
[1963]	796	btTransform R;
	797	R.setIdentity ();
	798	R.setRotation (convexFromTrans.getRotation());
[2430]	799	castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
[1963]	800	}
	801
[2430]	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
[1963]	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,
[2430]	830	allowedCcdPenetration);
[1963]	831	}
	832	}
	833	}
[2430]	834	#endif //USE_BRUTEFORCE_RAYBROADPHASE
[1963]	835	}

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

Download in other formats: