Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 2274 was 2192, checked in by rgrieder, 16 years ago
Reverted all changes of attempt to update physics branch.
Property svn:eol-style set to `native`
File size: 24.1 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
	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
[1972]	599	void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
[1963]	600	{
	601
	602
[1972]	603	btTransform rayFromTrans,rayToTrans;
	604	rayFromTrans.setIdentity();
	605	rayFromTrans.setOrigin(rayFromWorld);
	606	rayToTrans.setIdentity();
[1963]	607
[1972]	608	rayToTrans.setOrigin(rayToWorld);
[1963]	609
[1972]	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++)
[1963]	614	{
	615	///terminate further ray tests, once the closestHitFraction reached zero
[1972]	616	if (resultCallback.m_closestHitFraction == btScalar(0.f))
	617	break;
[1963]	618
[1972]	619	btCollisionObject* collisionObject= m_collisionObjects[i];
[1963]	620	//only perform raycast if filterMask matches
[1972]	621	if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
[1963]	622	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
	623	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
	624	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
[1972]	625
	626	btScalar hitLambda = resultCallback.m_closestHitFraction;
[1963]	627	btVector3 hitNormal;
[1972]	628	if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
[1963]	629	{
[1972]	630	rayTestSingle(rayFromTrans,rayToTrans,
[1963]	631	collisionObject,
	632	collisionObject->getCollisionShape(),
	633	collisionObject->getWorldTransform(),
[1972]	634	resultCallback);
[1963]	635	}
	636	}
[1972]	637
[1963]	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: