Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 2430 was 2430, checked in by rgrieder, 16 years ago
Updated to Bullet 2.73 (first part).
Property svn:eol-style set to `native`
File size: 29.5 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);
	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
[2430]	119
	120
[1963]	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	{
[2430]	133	btVector3 minAabb,maxAabb;
[1963]	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	{
[2430]	234	// BT_PROFILE("rayTestConvex");
[1963]	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	{
[2430]	278	// BT_PROFILE("rayTestConcave");
[1963]	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	{
[2430]	327	//generic (slower) case
	328	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
[1963]	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;
[2430]	341	btConcaveShape* m_triangleMesh;
[1963]	342
	343	BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
[2430]	344	btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
[1963]	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
[2430]	374	BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
[1963]	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
[2430]	382	concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
[1963]	383	}
	384	} else {
[2430]	385	// BT_PROFILE("rayTestCompound");
	386	///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
[1963]	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	{
[2430]	420	//BT_PROFILE("convexSweepConvex");
[1963]	421	btConvexCast::CastResult castResult;
	422	castResult.m_allowedPenetration = allowedPenetration;
[2430]	423	castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
[1963]	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	{
[2430]	465	//BT_PROFILE("convexSweepbtBvhTriangleMesh");
[1963]	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	{
[2430]	522	//BT_PROFILE("convexSweepConcave");
	523	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
[1963]	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;
[2430]	535	btConcaveShape* m_triangleMesh;
[1963]	536
	537	BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
[2430]	538	btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
[1963]	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
[2430]	571	BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
[1963]	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;
[2430]	582	concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
[1963]	583	}
	584	} else {
[2430]	585	///@todo : use AABB tree or other BVH acceleration structure!
[1963]	586	if (collisionShape->isCompound())
	587	{
[2430]	588	BT_PROFILE("convexSweepCompound");
[1963]	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
[2430]	612
	613	struct btSingleRayCallback : public btBroadphaseRayCallback
[1963]	614	{
	615
[2430]	616	btVector3 m_rayFromWorld;
	617	btVector3 m_rayToWorld;
	618	btTransform m_rayFromTrans;
	619	btTransform m_rayToTrans;
	620	btVector3 m_hitNormal;
[1963]	621
[2430]	622	const btCollisionWorld* m_world;
	623	btCollisionWorld::RayResultCallback& m_resultCallback;
[1963]	624
[2430]	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);
[1963]	635
[2430]	636	btVector3 rayDir = (rayToWorld-rayFromWorld);
[1972]	637
[2430]	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)
[1963]	654	{
	655	///terminate further ray tests, once the closestHitFraction reached zero
[2430]	656	if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
	657	return false;
[1963]	658
[2430]	659	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
	660
[1963]	661	//only perform raycast if filterMask matches
[2430]	662	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
	663	{
[1963]	664	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
[2430]	665	//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
	666
	667	#ifdef RECALCULATE_AABB
[1963]	668	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
	669	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
[2430]	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))
[1963]	678	{
[2430]	679	m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
[1963]	680	collisionObject,
	681	collisionObject->getCollisionShape(),
	682	collisionObject->getWorldTransform(),
[2430]	683	m_resultCallback);
[1963]	684	}
	685	}
[2430]	686	return true;
[1963]	687	}
[2430]	688	};
[1963]	689
[2430]	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
[1963]	706	}
	707
[2430]	708
	709	struct btSingleSweepCallback : public btBroadphaseRayCallback
[1963]	710	{
[2430]	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
[1963]	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);
[2430]	786	btVector3 zeroLinVel;
	787	zeroLinVel.setValue(0,0,0);
[1963]	788	btTransform R;
	789	R.setIdentity ();
	790	R.setRotation (convexFromTrans.getRotation());
[2430]	791	castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
[1963]	792	}
	793
[2430]	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
[1963]	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,
[2430]	822	allowedCcdPenetration);
[1963]	823	}
	824	}
	825	}
[2430]	826	#endif //USE_BRUTEFORCE_RAYBROADPHASE
[1963]	827	}

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

Download in other formats: