Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 8690 was 8393, checked in by rgrieder, 15 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

Property svn:eol-style set to native

File size: 51.6 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"
[8351]	29	#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
[1963]	30	#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
[8393]	31	#include "BulletCollision/BroadphaseCollision/btDbvt.h"
[1963]	32	#include "LinearMath/btAabbUtil2.h"
	33	#include "LinearMath/btQuickprof.h"
	34	#include "LinearMath/btStackAlloc.h"
[8351]	35	#include "LinearMath/btSerializer.h"
[8393]	36	#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
[1963]	37
[8393]	38	//#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
	39
	40
[2430]	41	//#define USE_BRUTEFORCE_RAYBROADPHASE 1
	42	//RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest
	43	//#define RECALCULATE_AABB_RAYCAST 1
[1963]	44
	45	//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
	46	#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
	47	#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
	48	#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
	49
	50
[8351]	51	///for debug drawing
	52
	53	//for debug rendering
	54	#include "BulletCollision/CollisionShapes/btBoxShape.h"
	55	#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
	56	#include "BulletCollision/CollisionShapes/btCompoundShape.h"
	57	#include "BulletCollision/CollisionShapes/btConeShape.h"
	58	#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
	59	#include "BulletCollision/CollisionShapes/btCylinderShape.h"
	60	#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
	61	#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
	62	#include "BulletCollision/CollisionShapes/btSphereShape.h"
	63	#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
	64	#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
	65	#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
	66
	67
	68
[1963]	69	btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
	70	:m_dispatcher1(dispatcher),
	71	m_broadphasePairCache(pairCache),
[8351]	72	m_debugDrawer(0),
	73	m_forceUpdateAllAabbs(true)
[1963]	74	{
	75	m_stackAlloc = collisionConfiguration->getStackAllocator();
	76	m_dispatchInfo.m_stackAllocator = m_stackAlloc;
	77	}
	78
	79
	80	btCollisionWorld::~btCollisionWorld()
	81	{
	82
	83	//clean up remaining objects
	84	int i;
	85	for (i=0;i<m_collisionObjects.size();i++)
	86	{
	87	btCollisionObject* collisionObject= m_collisionObjects[i];
	88
	89	btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
	90	if (bp)
	91	{
	92	//
	93	// only clear the cached algorithms
	94	//
	95	getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
	96	getBroadphase()->destroyProxy(bp,m_dispatcher1);
[2882]	97	collisionObject->setBroadphaseHandle(0);
[1963]	98	}
	99	}
	100
	101
	102	}
	103
	104
	105
	106
	107
	108
	109
	110
	111
	112
	113	void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
	114	{
	115
[8351]	116	btAssert(collisionObject);
	117
[1963]	118	//check that the object isn't already added
[8351]	119	btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
[1963]	120
[8351]	121	m_collisionObjects.push_back(collisionObject);
[1963]	122
[8351]	123	//calculate new AABB
	124	btTransform trans = collisionObject->getWorldTransform();
[1963]	125
[8351]	126	btVector3 minAabb;
	127	btVector3 maxAabb;
	128	collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
[1963]	129
[8351]	130	int type = collisionObject->getCollisionShape()->getShapeType();
	131	collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
	132	minAabb,
	133	maxAabb,
	134	type,
	135	collisionObject,
	136	collisionFilterGroup,
	137	collisionFilterMask,
	138	m_dispatcher1,0
	139	)) ;
[1963]	140
	141
	142
	143
	144
	145	}
	146
[2430]	147
	148
[2882]	149	void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
	150	{
	151	btVector3 minAabb,maxAabb;
	152	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
	153	//need to increase the aabb for contact thresholds
	154	btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
	155	minAabb -= contactThreshold;
	156	maxAabb += contactThreshold;
	157
[8393]	158	if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
	159	{
	160	btVector3 minAabb2,maxAabb2;
	161	colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
	162	minAabb2 -= contactThreshold;
	163	maxAabb2 += contactThreshold;
	164	minAabb.setMin(minAabb2);
	165	maxAabb.setMax(maxAabb2);
	166	}
	167
[2882]	168	btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
	169
	170	//moving objects should be moderately sized, probably something wrong if not
	171	if ( colObj->isStaticObject() \|\| ((maxAabb-minAabb).length2() < btScalar(1e12)))
	172	{
	173	bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
	174	} else
	175	{
	176	//something went wrong, investigate
	177	//this assert is unwanted in 3D modelers (danger of loosing work)
	178	colObj->setActivationState(DISABLE_SIMULATION);
	179
	180	static bool reportMe = true;
	181	if (reportMe && m_debugDrawer)
	182	{
	183	reportMe = false;
	184	m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
	185	m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
	186	m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
	187	m_debugDrawer->reportErrorWarning("Thanks.\n");
	188	}
	189	}
	190	}
	191
[1963]	192	void btCollisionWorld::updateAabbs()
	193	{
	194	BT_PROFILE("updateAabbs");
	195
	196	btTransform predictedTrans;
	197	for ( int i=0;i<m_collisionObjects.size();i++)
	198	{
	199	btCollisionObject* colObj = m_collisionObjects[i];
	200
	201	//only update aabb of active objects
[8351]	202	if (m_forceUpdateAllAabbs \|\| colObj->isActive())
[1963]	203	{
[2882]	204	updateSingleAabb(colObj);
[1963]	205	}
	206	}
	207	}
	208
	209
	210
	211	void btCollisionWorld::performDiscreteCollisionDetection()
	212	{
	213	BT_PROFILE("performDiscreteCollisionDetection");
	214
	215	btDispatcherInfo& dispatchInfo = getDispatchInfo();
	216
	217	updateAabbs();
	218
	219	{
	220	BT_PROFILE("calculateOverlappingPairs");
	221	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
	222	}
	223
	224
	225	btDispatcher* dispatcher = getDispatcher();
	226	{
	227	BT_PROFILE("dispatchAllCollisionPairs");
	228	if (dispatcher)
	229	dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
	230	}
	231
	232	}
	233
	234
	235
	236	void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
	237	{
	238
	239
	240	//bool removeFromBroadphase = false;
	241
	242	{
	243
	244	btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
	245	if (bp)
	246	{
	247	//
	248	// only clear the cached algorithms
	249	//
	250	getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
	251	getBroadphase()->destroyProxy(bp,m_dispatcher1);
	252	collisionObject->setBroadphaseHandle(0);
	253	}
	254	}
	255
	256
	257	//swapremove
	258	m_collisionObjects.remove(collisionObject);
	259
	260	}
	261
	262
	263
	264	void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
[8351]	265	btCollisionObject* collisionObject,
	266	const btCollisionShape* collisionShape,
	267	const btTransform& colObjWorldTransform,
	268	RayResultCallback& resultCallback)
[1963]	269	{
	270	btSphereShape pointShape(btScalar(0.0));
	271	pointShape.setMargin(0.f);
	272	const btConvexShape* castShape = &pointShape;
	273
	274	if (collisionShape->isConvex())
	275	{
[8351]	276	// BT_PROFILE("rayTestConvex");
[1963]	277	btConvexCast::CastResult castResult;
	278	castResult.m_fraction = resultCallback.m_closestHitFraction;
	279
	280	btConvexShape* convexShape = (btConvexShape*) collisionShape;
	281	btVoronoiSimplexSolver simplexSolver;
	282	#define USE_SUBSIMPLEX_CONVEX_CAST 1
	283	#ifdef USE_SUBSIMPLEX_CONVEX_CAST
	284	btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
	285	#else
	286	//btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
	287	//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
	288	#endif //#USE_SUBSIMPLEX_CONVEX_CAST
	289
	290	if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
	291	{
	292	//add hit
	293	if (castResult.m_normal.length2() > btScalar(0.0001))
	294	{
	295	if (castResult.m_fraction < resultCallback.m_closestHitFraction)
	296	{
	297	#ifdef USE_SUBSIMPLEX_CONVEX_CAST
	298	//rotate normal into worldspace
	299	castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
	300	#endif //USE_SUBSIMPLEX_CONVEX_CAST
	301
	302	castResult.m_normal.normalize();
	303	btCollisionWorld::LocalRayResult localRayResult
	304	(
[8351]	305	collisionObject,
	306	0,
	307	castResult.m_normal,
	308	castResult.m_fraction
[1963]	309	);
	310
	311	bool normalInWorldSpace = true;
	312	resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
	313
	314	}
	315	}
	316	}
	317	} else {
	318	if (collisionShape->isConcave())
	319	{
[8351]	320	// BT_PROFILE("rayTestConcave");
[1963]	321	if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
	322	{
	323	///optimized version for btBvhTriangleMeshShape
	324	btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
	325	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
	326	btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
	327	btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
	328
	329	//ConvexCast::CastResult
	330	struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
	331	{
	332	btCollisionWorld::RayResultCallback* m_resultCallback;
	333	btCollisionObject* m_collisionObject;
	334	btTriangleMeshShape* m_triangleMesh;
	335
[8351]	336	btTransform m_colObjWorldTransform;
	337
[1963]	338	BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
[8351]	339	btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform):
	340	//@BP Mod
	341	btTriangleRaycastCallback(from,to, resultCallback->m_flags),
	342	m_resultCallback(resultCallback),
	343	m_collisionObject(collisionObject),
	344	m_triangleMesh(triangleMesh),
	345	m_colObjWorldTransform(colObjWorldTransform)
	346	{
	347	}
[1963]	348
	349
	350	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
	351	{
	352	btCollisionWorld::LocalShapeInfo shapeInfo;
	353	shapeInfo.m_shapePart = partId;
	354	shapeInfo.m_triangleIndex = triangleIndex;
	355
[8351]	356	btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
	357
[1963]	358	btCollisionWorld::LocalRayResult rayResult
[8351]	359	(m_collisionObject,
[1963]	360	&shapeInfo,
[8351]	361	hitNormalWorld,
[1963]	362	hitFraction);
	363
[8351]	364	bool normalInWorldSpace = true;
[1963]	365	return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
	366	}
	367
	368	};
	369
[8351]	370	BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
[1963]	371	rcb.m_hitFraction = resultCallback.m_closestHitFraction;
	372	triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
	373	} else
	374	{
[2430]	375	//generic (slower) case
	376	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
[1963]	377
	378	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
	379
	380	btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
	381	btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
	382
	383	//ConvexCast::CastResult
	384
	385	struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
	386	{
	387	btCollisionWorld::RayResultCallback* m_resultCallback;
	388	btCollisionObject* m_collisionObject;
[2430]	389	btConcaveShape* m_triangleMesh;
[1963]	390
[8351]	391	btTransform m_colObjWorldTransform;
	392
[1963]	393	BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
[8351]	394	btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
	395	//@BP Mod
	396	btTriangleRaycastCallback(from,to, resultCallback->m_flags),
	397	m_resultCallback(resultCallback),
	398	m_collisionObject(collisionObject),
	399	m_triangleMesh(triangleMesh),
	400	m_colObjWorldTransform(colObjWorldTransform)
	401	{
	402	}
[1963]	403
	404
	405	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
	406	{
	407	btCollisionWorld::LocalShapeInfo shapeInfo;
	408	shapeInfo.m_shapePart = partId;
	409	shapeInfo.m_triangleIndex = triangleIndex;
	410
[8351]	411	btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
	412
[1963]	413	btCollisionWorld::LocalRayResult rayResult
[8351]	414	(m_collisionObject,
[1963]	415	&shapeInfo,
[8351]	416	hitNormalWorld,
[1963]	417	hitFraction);
	418
[8351]	419	bool normalInWorldSpace = true;
[1963]	420	return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
	421	}
	422
	423	};
	424
	425
[8351]	426	BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
[1963]	427	rcb.m_hitFraction = resultCallback.m_closestHitFraction;
	428
	429	btVector3 rayAabbMinLocal = rayFromLocal;
	430	rayAabbMinLocal.setMin(rayToLocal);
	431	btVector3 rayAabbMaxLocal = rayFromLocal;
	432	rayAabbMaxLocal.setMax(rayToLocal);
	433
[2430]	434	concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
[1963]	435	}
	436	} else {
[8351]	437	// BT_PROFILE("rayTestCompound");
[1963]	438	if (collisionShape->isCompound())
	439	{
[8393]	440	struct LocalInfoAdder2 : public RayResultCallback
[1963]	441	{
[8393]	442	RayResultCallback* m_userCallback;
	443	int m_i;
	444
	445	LocalInfoAdder2 (int i, RayResultCallback *user)
	446	: m_userCallback(user), m_i(i)
	447	{
	448	m_closestHitFraction = m_userCallback->m_closestHitFraction;
	449	}
	450	virtual bool needsCollision(btBroadphaseProxy* p) const
	451	{
	452	return m_userCallback->needsCollision(p);
	453	}
[8351]	454
[8393]	455	virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
	456	{
	457	btCollisionWorld::LocalShapeInfo shapeInfo;
	458	shapeInfo.m_shapePart = -1;
	459	shapeInfo.m_triangleIndex = m_i;
	460	if (r.m_localShapeInfo == NULL)
	461	r.m_localShapeInfo = &shapeInfo;
[8351]	462
[8393]	463	const btScalar result = m_userCallback->addSingleResult(r, b);
	464	m_closestHitFraction = m_userCallback->m_closestHitFraction;
	465	return result;
	466	}
	467	};
	468
	469	struct RayTester : btDbvt::ICollide
	470	{
	471	btCollisionObject* m_collisionObject;
	472	const btCompoundShape* m_compoundShape;
	473	const btTransform& m_colObjWorldTransform;
	474	const btTransform& m_rayFromTrans;
	475	const btTransform& m_rayToTrans;
	476	RayResultCallback& m_resultCallback;
	477
	478	RayTester(btCollisionObject* collisionObject,
	479	const btCompoundShape* compoundShape,
	480	const btTransform& colObjWorldTransform,
	481	const btTransform& rayFromTrans,
	482	const btTransform& rayToTrans,
	483	RayResultCallback& resultCallback):
	484	m_collisionObject(collisionObject),
	485	m_compoundShape(compoundShape),
	486	m_colObjWorldTransform(colObjWorldTransform),
	487	m_rayFromTrans(rayFromTrans),
	488	m_rayToTrans(rayToTrans),
	489	m_resultCallback(resultCallback)
	490	{
	491
	492	}
	493
	494	void Process(int i)
	495	{
	496	const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
	497	const btTransform& childTrans = m_compoundShape->getChildTransform(i);
	498	btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
	499
	500	// replace collision shape so that callback can determine the triangle
	501	btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape();
	502	m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
[8351]	503
[8393]	504	LocalInfoAdder2 my_cb(i, &m_resultCallback);
	505
	506	rayTestSingle(
	507	m_rayFromTrans,
	508	m_rayToTrans,
	509	m_collisionObject,
	510	childCollisionShape,
	511	childWorldTrans,
	512	my_cb);
	513
	514	// restore
	515	m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
	516	}
	517
	518	void Process(const btDbvtNode* leaf)
	519	{
	520	Process(leaf->dataAsInt);
	521	}
	522	};
	523
	524	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
	525	const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
	526
	527
	528	RayTester rayCB(
	529	collisionObject,
	530	compoundShape,
	531	colObjWorldTransform,
	532	rayFromTrans,
	533	rayToTrans,
	534	resultCallback);
	535	#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
	536	if (dbvt)
	537	{
	538	btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
	539	btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
	540	btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
[1963]	541	}
[8393]	542	else
	543	#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
	544	{
	545	for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
	546	{
	547	rayCB.Process(i);
	548	}
	549	}
[1963]	550	}
	551	}
	552	}
	553	}
	554
	555	void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
[8351]	556	btCollisionObject* collisionObject,
	557	const btCollisionShape* collisionShape,
	558	const btTransform& colObjWorldTransform,
	559	ConvexResultCallback& resultCallback, btScalar allowedPenetration)
[1963]	560	{
	561	if (collisionShape->isConvex())
	562	{
[2430]	563	//BT_PROFILE("convexSweepConvex");
[1963]	564	btConvexCast::CastResult castResult;
	565	castResult.m_allowedPenetration = allowedPenetration;
[2430]	566	castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
[1963]	567
	568	btConvexShape* convexShape = (btConvexShape*) collisionShape;
	569	btVoronoiSimplexSolver simplexSolver;
	570	btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
[8351]	571
[1963]	572	btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
	573	//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
	574	//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
	575
	576	btConvexCast* castPtr = &convexCaster1;
[8351]	577
	578
	579
[1963]	580	if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
	581	{
	582	//add hit
	583	if (castResult.m_normal.length2() > btScalar(0.0001))
	584	{
	585	if (castResult.m_fraction < resultCallback.m_closestHitFraction)
	586	{
	587	castResult.m_normal.normalize();
	588	btCollisionWorld::LocalConvexResult localConvexResult
[8351]	589	(
	590	collisionObject,
	591	0,
	592	castResult.m_normal,
	593	castResult.m_hitPoint,
	594	castResult.m_fraction
	595	);
[1963]	596
	597	bool normalInWorldSpace = true;
	598	resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
	599
	600	}
	601	}
	602	}
	603	} else {
	604	if (collisionShape->isConcave())
	605	{
	606	if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
	607	{
[2430]	608	//BT_PROFILE("convexSweepbtBvhTriangleMesh");
[1963]	609	btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
	610	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
	611	btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
	612	btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
	613	// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
	614	btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
	615
	616	//ConvexCast::CastResult
	617	struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
	618	{
	619	btCollisionWorld::ConvexResultCallback* m_resultCallback;
	620	btCollisionObject* m_collisionObject;
	621	btTriangleMeshShape* m_triangleMesh;
	622
	623	BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
	624	btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
[8351]	625	btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
	626	m_resultCallback(resultCallback),
	627	m_collisionObject(collisionObject),
	628	m_triangleMesh(triangleMesh)
	629	{
	630	}
[1963]	631
	632
	633	virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
	634	{
	635	btCollisionWorld::LocalShapeInfo shapeInfo;
	636	shapeInfo.m_shapePart = partId;
	637	shapeInfo.m_triangleIndex = triangleIndex;
	638	if (hitFraction <= m_resultCallback->m_closestHitFraction)
	639	{
	640
	641	btCollisionWorld::LocalConvexResult convexResult
[8351]	642	(m_collisionObject,
[1963]	643	&shapeInfo,
	644	hitNormalLocal,
	645	hitPointLocal,
	646	hitFraction);
	647
	648	bool normalInWorldSpace = true;
	649
	650
	651	return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
	652	}
	653	return hitFraction;
	654	}
	655
	656	};
	657
	658	BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
	659	tccb.m_hitFraction = resultCallback.m_closestHitFraction;
[8393]	660	tccb.m_allowedPenetration = allowedPenetration;
[1963]	661	btVector3 boxMinLocal, boxMaxLocal;
	662	castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
	663	triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
	664	} else
	665	{
[8393]	666	if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
[1963]	667	{
[8393]	668	btConvexCast::CastResult castResult;
	669	castResult.m_allowedPenetration = allowedPenetration;
	670	castResult.m_fraction = resultCallback.m_closestHitFraction;
	671	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape;
	672	btContinuousConvexCollision convexCaster1(castShape,planeShape);
	673	btConvexCast* castPtr = &convexCaster1;
[1963]	674
[8393]	675	if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
[8351]	676	{
[8393]	677	//add hit
	678	if (castResult.m_normal.length2() > btScalar(0.0001))
	679	{
	680	if (castResult.m_fraction < resultCallback.m_closestHitFraction)
	681	{
	682	castResult.m_normal.normalize();
	683	btCollisionWorld::LocalConvexResult localConvexResult
	684	(
	685	collisionObject,
	686	0,
	687	castResult.m_normal,
	688	castResult.m_hitPoint,
	689	castResult.m_fraction
	690	);
	691
	692	bool normalInWorldSpace = true;
	693	resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
	694	}
	695	}
[8351]	696	}
[1963]	697
[8393]	698	} else
	699	{
	700	//BT_PROFILE("convexSweepConcave");
	701	btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
	702	btTransform worldTocollisionObject = colObjWorldTransform.inverse();
	703	btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
	704	btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
	705	// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
	706	btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
[1963]	707
[8393]	708	//ConvexCast::CastResult
	709	struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
[1963]	710	{
[8393]	711	btCollisionWorld::ConvexResultCallback* m_resultCallback;
	712	btCollisionObject* m_collisionObject;
	713	btConcaveShape* m_triangleMesh;
	714
	715	BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
	716	btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
	717	btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
	718	m_resultCallback(resultCallback),
	719	m_collisionObject(collisionObject),
	720	m_triangleMesh(triangleMesh)
[1963]	721	{
[8393]	722	}
[1963]	723
	724
[8393]	725	virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
	726	{
	727	btCollisionWorld::LocalShapeInfo shapeInfo;
	728	shapeInfo.m_shapePart = partId;
	729	shapeInfo.m_triangleIndex = triangleIndex;
	730	if (hitFraction <= m_resultCallback->m_closestHitFraction)
	731	{
[1963]	732
[8393]	733	btCollisionWorld::LocalConvexResult convexResult
	734	(m_collisionObject,
	735	&shapeInfo,
	736	hitNormalLocal,
	737	hitPointLocal,
	738	hitFraction);
	739
	740	bool normalInWorldSpace = false;
	741
	742	return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
	743	}
	744	return hitFraction;
[1963]	745	}
	746
[8393]	747	};
[1963]	748
[8393]	749	BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
	750	tccb.m_hitFraction = resultCallback.m_closestHitFraction;
	751	tccb.m_allowedPenetration = allowedPenetration;
	752	btVector3 boxMinLocal, boxMaxLocal;
	753	castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
[1963]	754
[8393]	755	btVector3 rayAabbMinLocal = convexFromLocal;
	756	rayAabbMinLocal.setMin(convexToLocal);
	757	btVector3 rayAabbMaxLocal = convexFromLocal;
	758	rayAabbMaxLocal.setMax(convexToLocal);
	759	rayAabbMinLocal += boxMinLocal;
	760	rayAabbMaxLocal += boxMaxLocal;
	761	concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
	762	}
[1963]	763	}
	764	} else {
[2430]	765	///@todo : use AABB tree or other BVH acceleration structure!
[1963]	766	if (collisionShape->isCompound())
	767	{
[2430]	768	BT_PROFILE("convexSweepCompound");
[1963]	769	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
	770	int i=0;
	771	for (i=0;i<compoundShape->getNumChildShapes();i++)
	772	{
	773	btTransform childTrans = compoundShape->getChildTransform(i);
	774	const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
	775	btTransform childWorldTrans = colObjWorldTransform * childTrans;
	776	// replace collision shape so that callback can determine the triangle
	777	btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
	778	collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
[8351]	779	struct LocalInfoAdder : public ConvexResultCallback {
	780	ConvexResultCallback* m_userCallback;
	781	int m_i;
	782
	783	LocalInfoAdder (int i, ConvexResultCallback *user)
	784	: m_userCallback(user), m_i(i)
	785	{
	786	m_closestHitFraction = m_userCallback->m_closestHitFraction;
	787	}
[8393]	788	virtual bool needsCollision(btBroadphaseProxy* p) const
	789	{
	790	return m_userCallback->needsCollision(p);
	791	}
[8351]	792	virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b)
	793	{
	794	btCollisionWorld::LocalShapeInfo shapeInfo;
	795	shapeInfo.m_shapePart = -1;
	796	shapeInfo.m_triangleIndex = m_i;
	797	if (r.m_localShapeInfo == NULL)
	798	r.m_localShapeInfo = &shapeInfo;
	799	const btScalar result = m_userCallback->addSingleResult(r, b);
	800	m_closestHitFraction = m_userCallback->m_closestHitFraction;
	801	return result;
	802
	803	}
	804	};
	805
	806	LocalInfoAdder my_cb(i, &resultCallback);
	807
	808
[1963]	809	objectQuerySingle(castShape, convexFromTrans,convexToTrans,
	810	collisionObject,
	811	childCollisionShape,
	812	childWorldTrans,
[8351]	813	my_cb, allowedPenetration);
[1963]	814	// restore
	815	collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
	816	}
	817	}
	818	}
	819	}
	820	}
	821
[2430]	822
	823	struct btSingleRayCallback : public btBroadphaseRayCallback
[1963]	824	{
	825
[2430]	826	btVector3 m_rayFromWorld;
	827	btVector3 m_rayToWorld;
	828	btTransform m_rayFromTrans;
	829	btTransform m_rayToTrans;
	830	btVector3 m_hitNormal;
[1963]	831
[2430]	832	const btCollisionWorld* m_world;
	833	btCollisionWorld::RayResultCallback& m_resultCallback;
[1963]	834
[2430]	835	btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
[8351]	836	:m_rayFromWorld(rayFromWorld),
	837	m_rayToWorld(rayToWorld),
	838	m_world(world),
	839	m_resultCallback(resultCallback)
[2430]	840	{
	841	m_rayFromTrans.setIdentity();
	842	m_rayFromTrans.setOrigin(m_rayFromWorld);
	843	m_rayToTrans.setIdentity();
	844	m_rayToTrans.setOrigin(m_rayToWorld);
[1963]	845
[2430]	846	btVector3 rayDir = (rayToWorld-rayFromWorld);
[1972]	847
[2430]	848	rayDir.normalize ();
[8351]	849	///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
	850	m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
	851	m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
	852	m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
[2430]	853	m_signs[0] = m_rayDirectionInverse[0] < 0.0;
	854	m_signs[1] = m_rayDirectionInverse[1] < 0.0;
	855	m_signs[2] = m_rayDirectionInverse[2] < 0.0;
	856
	857	m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
	858
	859	}
	860
	861
[8351]	862
[2430]	863	virtual bool process(const btBroadphaseProxy* proxy)
[1963]	864	{
	865	///terminate further ray tests, once the closestHitFraction reached zero
[2430]	866	if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
	867	return false;
[1963]	868
[2430]	869	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
	870
[1963]	871	//only perform raycast if filterMask matches
[2430]	872	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
	873	{
[1963]	874	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
[2430]	875	//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
[2882]	876	#if 0
[2430]	877	#ifdef RECALCULATE_AABB
[1963]	878	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
	879	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
[2430]	880	#else
	881	//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
	882	const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
	883	const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
	884	#endif
[2882]	885	#endif
[2430]	886	//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
	887	//culling already done by broadphase
	888	//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
[1963]	889	{
[2430]	890	m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
[1963]	891	collisionObject,
[8351]	892	collisionObject->getCollisionShape(),
	893	collisionObject->getWorldTransform(),
	894	m_resultCallback);
[1963]	895	}
	896	}
[2430]	897	return true;
[1963]	898	}
[2430]	899	};
[1963]	900
[2430]	901	void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
	902	{
[8351]	903	//BT_PROFILE("rayTest");
[2430]	904	/// use the broadphase to accelerate the search for objects, based on their aabb
	905	/// and for each object with ray-aabb overlap, perform an exact ray test
	906	btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
	907
	908	#ifndef USE_BRUTEFORCE_RAYBROADPHASE
	909	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
	910	#else
	911	for (int i=0;i<this->getNumCollisionObjects();i++)
	912	{
	913	rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
	914	}
	915	#endif //USE_BRUTEFORCE_RAYBROADPHASE
	916
[1963]	917	}
	918
[2430]	919
	920	struct btSingleSweepCallback : public btBroadphaseRayCallback
[1963]	921	{
[2430]	922
	923	btTransform m_convexFromTrans;
	924	btTransform m_convexToTrans;
	925	btVector3 m_hitNormal;
	926	const btCollisionWorld* m_world;
	927	btCollisionWorld::ConvexResultCallback& m_resultCallback;
	928	btScalar m_allowedCcdPenetration;
	929	const btConvexShape* m_castShape;
	930
	931
	932	btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
	933	:m_convexFromTrans(convexFromTrans),
	934	m_convexToTrans(convexToTrans),
	935	m_world(world),
	936	m_resultCallback(resultCallback),
	937	m_allowedCcdPenetration(allowedPenetration),
	938	m_castShape(castShape)
	939	{
	940	btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
	941	btVector3 rayDir = unnormalizedRayDir.normalized();
[8351]	942	///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
	943	m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
	944	m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
	945	m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
[2430]	946	m_signs[0] = m_rayDirectionInverse[0] < 0.0;
	947	m_signs[1] = m_rayDirectionInverse[1] < 0.0;
	948	m_signs[2] = m_rayDirectionInverse[2] < 0.0;
	949
	950	m_lambda_max = rayDir.dot(unnormalizedRayDir);
	951
	952	}
	953
	954	virtual bool process(const btBroadphaseProxy* proxy)
	955	{
	956	///terminate further convex sweep tests, once the closestHitFraction reached zero
	957	if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
	958	return false;
	959
	960	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
	961
	962	//only perform raycast if filterMask matches
	963	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
	964	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
	965	m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
[8351]	966	collisionObject,
	967	collisionObject->getCollisionShape(),
	968	collisionObject->getWorldTransform(),
	969	m_resultCallback,
	970	m_allowedCcdPenetration);
[2430]	971	}
[8351]	972
[2430]	973	return true;
	974	}
	975	};
	976
	977
	978
	979	void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
	980	{
	981
	982	BT_PROFILE("convexSweepTest");
	983	/// use the broadphase to accelerate the search for objects, based on their aabb
	984	/// and for each object with ray-aabb overlap, perform an exact ray test
	985	/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
	986
	987
[8351]	988
[1963]	989	btTransform convexFromTrans,convexToTrans;
	990	convexFromTrans = convexFromWorld;
	991	convexToTrans = convexToWorld;
	992	btVector3 castShapeAabbMin, castShapeAabbMax;
	993	/* Compute AABB that encompasses angular movement */
	994	{
	995	btVector3 linVel, angVel;
	996	btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
[2430]	997	btVector3 zeroLinVel;
	998	zeroLinVel.setValue(0,0,0);
[1963]	999	btTransform R;
	1000	R.setIdentity ();
	1001	R.setRotation (convexFromTrans.getRotation());
[2430]	1002	castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
[1963]	1003	}
	1004
[2430]	1005	#ifndef USE_BRUTEFORCE_RAYBROADPHASE
	1006
	1007	btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
	1008
	1009	m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
	1010
	1011	#else
[1963]	1012	/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
	1013	// do a ray-shape query using convexCaster (CCD)
	1014	int i;
	1015	for (i=0;i<m_collisionObjects.size();i++)
	1016	{
	1017	btCollisionObject* collisionObject= m_collisionObjects[i];
	1018	//only perform raycast if filterMask matches
	1019	if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
	1020	//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
	1021	btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
	1022	collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
	1023	AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
	1024	btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
	1025	btVector3 hitNormal;
	1026	if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
	1027	{
	1028	objectQuerySingle(castShape, convexFromTrans,convexToTrans,
	1029	collisionObject,
[8351]	1030	collisionObject->getCollisionShape(),
	1031	collisionObject->getWorldTransform(),
	1032	resultCallback,
	1033	allowedCcdPenetration);
[1963]	1034	}
	1035	}
	1036	}
[2430]	1037	#endif //USE_BRUTEFORCE_RAYBROADPHASE
[1963]	1038	}
[8351]	1039
	1040
	1041
	1042	struct btBridgedManifoldResult : public btManifoldResult
	1043	{
	1044
	1045	btCollisionWorld::ContactResultCallback& m_resultCallback;
	1046
	1047	btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback )
	1048	:btManifoldResult(obj0,obj1),
	1049	m_resultCallback(resultCallback)
	1050	{
	1051	}
	1052
	1053	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
	1054	{
	1055	bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
	1056	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
	1057	btVector3 localA;
	1058	btVector3 localB;
	1059	if (isSwapped)
	1060	{
	1061	localA = m_rootTransB.invXform(pointA );
	1062	localB = m_rootTransA.invXform(pointInWorld);
	1063	} else
	1064	{
	1065	localA = m_rootTransA.invXform(pointA );
	1066	localB = m_rootTransB.invXform(pointInWorld);
	1067	}
	1068
	1069	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
	1070	newPt.m_positionWorldOnA = pointA;
	1071	newPt.m_positionWorldOnB = pointInWorld;
	1072
	1073	//BP mod, store contact triangles.
	1074	if (isSwapped)
	1075	{
	1076	newPt.m_partId0 = m_partId1;
	1077	newPt.m_partId1 = m_partId0;
	1078	newPt.m_index0 = m_index1;
	1079	newPt.m_index1 = m_index0;
	1080	} else
	1081	{
	1082	newPt.m_partId0 = m_partId0;
	1083	newPt.m_partId1 = m_partId1;
	1084	newPt.m_index0 = m_index0;
	1085	newPt.m_index1 = m_index1;
	1086	}
	1087
	1088	//experimental feature info, for per-triangle material etc.
	1089	btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
	1090	btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
	1091	m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
	1092
	1093	}
	1094
	1095	};
	1096
	1097
	1098
	1099	struct btSingleContactCallback : public btBroadphaseAabbCallback
	1100	{
	1101
	1102	btCollisionObject* m_collisionObject;
	1103	btCollisionWorld* m_world;
	1104	btCollisionWorld::ContactResultCallback& m_resultCallback;
	1105
	1106
	1107	btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
	1108	:m_collisionObject(collisionObject),
	1109	m_world(world),
	1110	m_resultCallback(resultCallback)
	1111	{
	1112	}
	1113
	1114	virtual bool process(const btBroadphaseProxy* proxy)
	1115	{
	1116	btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
	1117	if (collisionObject == m_collisionObject)
	1118	return true;
	1119
	1120	//only perform raycast if filterMask matches
	1121	if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
	1122	{
	1123	btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject);
	1124	if (algorithm)
	1125	{
	1126	btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback);
	1127	//discrete collision detection query
	1128	algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult);
	1129
	1130	algorithm->~btCollisionAlgorithm();
	1131	m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
	1132	}
	1133	}
	1134	return true;
	1135	}
	1136	};
	1137
	1138
	1139	///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
	1140	///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
	1141	void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
	1142	{
	1143	btVector3 aabbMin,aabbMax;
	1144	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
	1145	btSingleContactCallback contactCB(colObj,this,resultCallback);
	1146
	1147	m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
	1148	}
	1149
	1150
	1151	///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
	1152	///it reports one or more contact points (including the one with deepest penetration)
	1153	void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
	1154	{
	1155	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
	1156	if (algorithm)
	1157	{
	1158	btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
	1159	//discrete collision detection query
	1160	algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
	1161
	1162	algorithm->~btCollisionAlgorithm();
	1163	getDispatcher()->freeCollisionAlgorithm(algorithm);
	1164	}
	1165
	1166	}
	1167
	1168
	1169
	1170
	1171	class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
	1172	{
	1173	btIDebugDraw* m_debugDrawer;
	1174	btVector3 m_color;
	1175	btTransform m_worldTrans;
	1176
	1177	public:
	1178
	1179	DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
	1180	m_debugDrawer(debugDrawer),
	1181	m_color(color),
	1182	m_worldTrans(worldTrans)
	1183	{
	1184	}
	1185
	1186	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
	1187	{
	1188	processTriangle(triangle,partId,triangleIndex);
	1189	}
	1190
	1191	virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
	1192	{
	1193	(void)partId;
	1194	(void)triangleIndex;
	1195
	1196	btVector3 wv0,wv1,wv2;
	1197	wv0 = m_worldTrans*triangle[0];
	1198	wv1 = m_worldTrans*triangle[1];
	1199	wv2 = m_worldTrans*triangle[2];
	1200	btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
	1201
	1202	btVector3 normal = (wv1-wv0).cross(wv2-wv0);
	1203	normal.normalize();
	1204	btVector3 normalColor(1,1,0);
	1205	m_debugDrawer->drawLine(center,center+normal,normalColor);
	1206
	1207
	1208
	1209
	1210	m_debugDrawer->drawLine(wv0,wv1,m_color);
	1211	m_debugDrawer->drawLine(wv1,wv2,m_color);
	1212	m_debugDrawer->drawLine(wv2,wv0,m_color);
	1213	}
	1214	};
	1215
	1216
	1217	void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
	1218	{
	1219	// Draw a small simplex at the center of the object
	1220	getDebugDrawer()->drawTransform(worldTransform,1);
	1221
	1222	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
	1223	{
	1224	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
	1225	for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
	1226	{
	1227	btTransform childTrans = compoundShape->getChildTransform(i);
	1228	const btCollisionShape* colShape = compoundShape->getChildShape(i);
	1229	debugDrawObject(worldTransform*childTrans,colShape,color);
	1230	}
	1231
	1232	} else
	1233	{
	1234	switch (shape->getShapeType())
	1235	{
	1236
	1237	case BOX_SHAPE_PROXYTYPE:
	1238	{
	1239	const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
	1240	btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
	1241	getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
	1242	break;
	1243	}
	1244
	1245	case SPHERE_SHAPE_PROXYTYPE:
	1246	{
	1247	const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
	1248	btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
	1249
	1250	getDebugDrawer()->drawSphere(radius, worldTransform, color);
	1251	break;
	1252	}
	1253	case MULTI_SPHERE_SHAPE_PROXYTYPE:
	1254	{
	1255	const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
	1256
	1257	btTransform childTransform;
	1258	childTransform.setIdentity();
	1259
	1260	for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
	1261	{
	1262	childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
	1263	getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
	1264	}
	1265
	1266	break;
	1267	}
	1268	case CAPSULE_SHAPE_PROXYTYPE:
	1269	{
	1270	const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
	1271
	1272	btScalar radius = capsuleShape->getRadius();
	1273	btScalar halfHeight = capsuleShape->getHalfHeight();
	1274
	1275	int upAxis = capsuleShape->getUpAxis();
[8393]	1276	getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
[8351]	1277	break;
	1278	}
	1279	case CONE_SHAPE_PROXYTYPE:
	1280	{
	1281	const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
	1282	btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
	1283	btScalar height = coneShape->getHeight();//+coneShape->getMargin();
	1284
	1285	int upAxis= coneShape->getConeUpIndex();
[8393]	1286	getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
	1287	break;
[8351]	1288
	1289	}
	1290	case CYLINDER_SHAPE_PROXYTYPE:
	1291	{
	1292	const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
	1293	int upAxis = cylinder->getUpAxis();
	1294	btScalar radius = cylinder->getRadius();
	1295	btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
[8393]	1296	getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
[8351]	1297	break;
	1298	}
	1299
	1300	case STATIC_PLANE_PROXYTYPE:
	1301	{
	1302	const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
	1303	btScalar planeConst = staticPlaneShape->getPlaneConstant();
	1304	const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
[8393]	1305	getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
[8351]	1306	break;
	1307
	1308	}
	1309	default:
	1310	{
	1311
	1312	if (shape->isConcave())
	1313	{
	1314	btConcaveShape* concaveMesh = (btConcaveShape*) shape;
	1315
	1316	///@todo pass camera, for some culling? no -> we are not a graphics lib
	1317	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
	1318	btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
	1319
	1320	DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
	1321	concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
	1322
	1323	}
	1324
	1325	if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
	1326	{
	1327	btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
	1328	//todo: pass camera for some culling
	1329	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
	1330	btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
	1331	//DebugDrawcallback drawCallback;
	1332	DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
	1333	convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
	1334	}
	1335
	1336
	1337	/// for polyhedral shapes
	1338	if (shape->isPolyhedral())
	1339	{
	1340	btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
	1341
	1342	int i;
[8393]	1343	if (polyshape->getConvexPolyhedron())
[8351]	1344	{
[8393]	1345	const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
	1346	for (i=0;i<poly->m_faces.size();i++)
	1347	{
	1348	btVector3 centroid(0,0,0);
	1349	int numVerts = poly->m_faces[i].m_indices.size();
	1350	if (numVerts)
	1351	{
	1352	int lastV = poly->m_faces[i].m_indices[numVerts-1];
	1353	for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
	1354	{
	1355	int curVert = poly->m_faces[i].m_indices[v];
	1356	centroid+=poly->m_vertices[curVert];
	1357	getDebugDrawer()->drawLine(worldTransformpoly->m_vertices[lastV],worldTransformpoly->m_vertices[curVert],color);
	1358	lastV = curVert;
	1359	}
	1360	}
	1361	centroid*= 1./btScalar(numVerts);
[8351]	1362
[8393]	1363	btVector3 normalColor(1,1,0);
	1364	btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
	1365	getDebugDrawer()->drawLine(worldTransformcentroid,worldTransform(centroid+faceNormal),normalColor);
	1366
	1367
	1368	}
	1369
	1370
	1371	} else
	1372	{
	1373	for (i=0;i<polyshape->getNumEdges();i++)
	1374	{
	1375	btVector3 a,b;
	1376	polyshape->getEdge(i,a,b);
	1377	btVector3 wa = worldTransform * a;
	1378	btVector3 wb = worldTransform * b;
	1379	getDebugDrawer()->drawLine(wa,wb,color);
	1380	}
[8351]	1381	}
	1382
	1383
	1384	}
	1385	}
	1386	}
	1387	}
	1388	}
	1389
	1390
	1391	void btCollisionWorld::debugDrawWorld()
	1392	{
	1393	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
	1394	{
	1395	int numManifolds = getDispatcher()->getNumManifolds();
	1396	btVector3 color(0,0,0);
	1397	for (int i=0;i<numManifolds;i++)
	1398	{
	1399	btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
	1400	//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
	1401	//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
	1402
	1403	int numContacts = contactManifold->getNumContacts();
	1404	for (int j=0;j<numContacts;j++)
	1405	{
	1406	btManifoldPoint& cp = contactManifold->getContactPoint(j);
	1407	getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
	1408	}
	1409	}
	1410	}
	1411
	1412	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe \| btIDebugDraw::DBG_DrawAabb))
	1413	{
	1414	int i;
	1415
	1416	for ( i=0;i<m_collisionObjects.size();i++)
	1417	{
	1418	btCollisionObject* colObj = m_collisionObjects[i];
	1419	if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
	1420	{
	1421	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
	1422	{
	1423	btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
	1424	switch(colObj->getActivationState())
	1425	{
	1426	case ACTIVE_TAG:
	1427	color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
	1428	case ISLAND_SLEEPING:
	1429	color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
	1430	case WANTS_DEACTIVATION:
	1431	color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
	1432	case DISABLE_DEACTIVATION:
	1433	color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
	1434	case DISABLE_SIMULATION:
	1435	color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
	1436	default:
	1437	{
	1438	color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
	1439	}
	1440	};
	1441
	1442	debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
	1443	}
	1444	if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
	1445	{
	1446	btVector3 minAabb,maxAabb;
	1447	btVector3 colorvec(1,0,0);
	1448	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
[8393]	1449	btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
	1450	minAabb -= contactThreshold;
	1451	maxAabb += contactThreshold;
	1452
	1453	btVector3 minAabb2,maxAabb2;
	1454
	1455	if(colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
	1456	{
	1457	colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
	1458	minAabb2 -= contactThreshold;
	1459	maxAabb2 += contactThreshold;
	1460	minAabb.setMin(minAabb2);
	1461	maxAabb.setMax(maxAabb2);
	1462	}
	1463
[8351]	1464	m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
	1465	}
	1466	}
	1467
	1468	}
	1469	}
	1470	}
	1471
	1472
	1473	void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
	1474	{
	1475	int i;
	1476	//serialize all collision objects
	1477	for (i=0;i<m_collisionObjects.size();i++)
	1478	{
	1479	btCollisionObject* colObj = m_collisionObjects[i];
	1480	if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
	1481	{
	1482	colObj->serializeSingleObject(serializer);
	1483	}
	1484	}
	1485
	1486	///keep track of shapes already serialized
	1487	btHashMap<btHashPtr,btCollisionShape*> serializedShapes;
	1488
	1489	for (i=0;i<m_collisionObjects.size();i++)
	1490	{
	1491	btCollisionObject* colObj = m_collisionObjects[i];
	1492	btCollisionShape* shape = colObj->getCollisionShape();
	1493
	1494	if (!serializedShapes.find(shape))
	1495	{
	1496	serializedShapes.insert(shape,shape);
	1497	shape->serializeSingleShape(serializer);
	1498	}
	1499	}
	1500
	1501	}
	1502
	1503
	1504	void btCollisionWorld::serialize(btSerializer* serializer)
	1505	{
	1506
	1507	serializer->startSerialization();
	1508
	1509	serializeCollisionObjects(serializer);
	1510
	1511	serializer->finishSerialization();
	1512	}
	1513

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

Download in other formats: