Add aabbMin/aabbMax to broadphase proxy, and improve rayTest performance.

Todo: use broadphase acceleration structure (btDbvtBroadphase etc) for ray test

Extras/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp

@@ -681,16 +681,17 @@ void	processRaycastTask(void* userPtr, void* lsMemory)
 
 	//spu_printf("in processRaycastTask %d\n", taskDesc.numSpuCollisionObjectWrappers);
 	/* for each object */
+	RaycastGatheredObjectData gatheredObjectData;
 	for (int objectId = 0; objectId < taskDesc.numSpuCollisionObjectWrappers; objectId++)
 	{
 		//spu_printf("%d / %d\n", objectId, taskDesc.numSpuCollisionObjectWrappers);
-		RaycastGatheredObjectData gatheredObjectData;
+		
 		/* load initial collision shape */
 		GatherCollisionObjectAndShapeData (&gatheredObjectData, localMemory, (ppu_address_t)&cows[objectId]);
 
 		if (btBroadphaseProxy::isConcave (gatheredObjectData.m_shapeType))
 		{
-			SpuRaycastTaskWorkUnitOut tWorkUnitsOut[taskDesc.numWorkUnits];
+			SpuRaycastTaskWorkUnitOut tWorkUnitsOut[SPU_RAYCAST_WORK_UNITS_PER_TASK];
 			for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
 			{
 				tWorkUnitsOut[rayId].hitFraction = 1.0;
@@ -721,12 +722,13 @@ void	processRaycastTask(void* userPtr, void* lsMemory)
 				cellDmaWaitTagStatusAll(DMA_MASK(1));
 			}
 		} else if (btBroadphaseProxy::isConvex (gatheredObjectData.m_shapeType)) {
+
+			btVector3 objectBoxMin, objectBoxMax;
+			computeAabb (objectBoxMin, objectBoxMax, (btConvexInternalShape*)gatheredObjectData.m_spuCollisionShape, gatheredObjectData.m_collisionShape, gatheredObjectData.m_shapeType, gatheredObjectData.m_worldTransform);
 			for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
 			{
 				const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
-				btVector3 objectBoxMin, objectBoxMax;
+			
-				computeAabb (objectBoxMin, objectBoxMax, (btConvexInternalShape*)gatheredObjectData.m_spuCollisionShape, gatheredObjectData.m_collisionShape, gatheredObjectData.m_shapeType, gatheredObjectData.m_worldTransform);
-
 				btScalar ignored_param = 1.0;
 				btVector3 ignored_normal;
 				if (btRayAabb(workUnit.rayFrom, workUnit.rayTo, objectBoxMin, objectBoxMax, ignored_param, ignored_normal))

src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -62,7 +62,6 @@ public:
 		BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];		// 6 * 2 = 12
 //		BP_FP_INT_TYPE m_uniqueId;
 		BP_FP_INT_TYPE m_pad;
-		
 		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
 	
 		SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
@@ -108,7 +107,7 @@ protected:
 	//Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
 	//void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
 
-	void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
+	
 
 	void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
 	void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
@@ -139,6 +138,11 @@ public:
 	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+	virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+	
+	void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
+	///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
+	void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
 	
 	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
 
@@ -234,10 +238,42 @@ template <typename BP_FP_INT_TYPE>
 void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
 {
 	Handle* handle = static_cast<Handle*>(proxy);
+	handle->m_aabbMin = aabbMin;
+	handle->m_aabbMax = aabbMax;
 	updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
 
 }
 
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	Handle* pHandle = static_cast<Handle*>(proxy);
+	aabbMin = pHandle->m_aabbMin;
+	aabbMax = pHandle->m_aabbMax;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	Handle* pHandle = static_cast<Handle*>(proxy);
+
+	unsigned short vecInMin[3];
+	unsigned short vecInMax[3];
+
+	vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ;
+	vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ;
+	vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ;
+	vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ;
+	vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ;
+	vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ;
+	
+	aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ()));
+	aabbMin += m_worldAabbMin;
+	
+	aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ()));
+	aabbMax += m_worldAabbMin;
+}
 
 
 

src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

@@ -36,7 +36,8 @@ public:
 	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0;
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
-	
+	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;
+
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher)=0;
 

src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BROADPHASE_PROXY_H
 
 #include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
+#include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedAllocator.h"
 
 
@@ -90,6 +91,9 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	        AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
 	};
 
+	btVector3	m_aabbMin;
+	btVector3	m_aabbMax;
+
 	//Usually the client btCollisionObject or Rigidbody class
 	void*	m_clientObject;
 
@@ -111,8 +115,10 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	{
 	}
 
-	btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
+	btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
-		:m_clientObject(userPtr),
+		:m_aabbMin(aabbMin),
+		m_aabbMax(aabbMax),
+		m_clientObject(userPtr),
 		m_collisionFilterGroup(collisionFilterGroup),
 		m_collisionFilterMask(collisionFilterMask)
 	{

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -167,20 +167,23 @@ btBroadphaseProxy*				btDbvtBroadphase::createProxy(	const btVector3& aabbMin,
 																btDispatcher* /*dispatcher*/,
 																void* /*multiSapProxy*/)
 {
-btDbvtProxy*		proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	userPtr,
+btDbvtProxy*		proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	aabbMin,aabbMax,userPtr,
 																					collisionFilterGroup,
 																					collisionFilterMask);
-proxy->aabb			=	btDbvtVolume::FromMM(aabbMin,aabbMax);
+
+btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
+
+//bproxy->aabb			=	btDbvtVolume::FromMM(aabbMin,aabbMax);
 proxy->stage		=	m_stageCurrent;
 proxy->m_uniqueId	=	++m_gid;
-proxy->leaf			=	m_sets[0].insert(proxy->aabb,proxy);
+proxy->leaf			=	m_sets[0].insert(aabb,proxy);
 listappend(proxy,m_stageRoots[m_stageCurrent]);
 if(!m_deferedcollide)
 	{
 	btDbvtTreeCollider	collider(this);
 	collider.proxy=proxy;
-	btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider);
+	btDbvt::collideTV(m_sets[0].m_root,aabb,collider);
-	btDbvt::collideTV(m_sets[1].m_root,proxy->aabb,collider);
+	btDbvt::collideTV(m_sets[1].m_root,aabb,collider);
 	}
 return(proxy);
 }
@@ -200,6 +203,13 @@ btAlignedFree(proxy);
 m_needcleanup=true;
 }
 
+void	btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	btDbvtProxy*						proxy=(btDbvtProxy*)absproxy;
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+
 //
 void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
 																const btVector3& aabbMin,
@@ -224,8 +234,9 @@ if(NotEqual(aabb,proxy->leaf->volume))
 		++m_updates_call;
 		if(Intersect(proxy->leaf->volume,aabb))
 			{/* Moving				*/ 
-			const btVector3	delta=aabbMin-proxy->aabb.Mins();
+
-			btVector3		velocity(aabb.Extents()*m_prediction);
+			const btVector3	delta=aabbMin-proxy->m_aabbMin;
+			btVector3		velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
 			if(delta[0]<0) velocity[0]=-velocity[0];
 			if(delta[1]<0) velocity[1]=-velocity[1];
 			if(delta[2]<0) velocity[2]=-velocity[2];
@@ -249,7 +260,8 @@ if(NotEqual(aabb,proxy->leaf->volume))
 			}	
 		}
 	listremove(proxy,m_stageRoots[proxy->stage]);
-	proxy->aabb		=	aabb;
+	proxy->m_aabbMin = aabbMin;
+	proxy->m_aabbMax = aabbMax;
 	proxy->stage	=	m_stageCurrent;
 	listappend(proxy,m_stageRoots[m_stageCurrent]);
 	if(docollide)
@@ -319,7 +331,8 @@ if(current)
 		btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
 		#endif
 		m_sets[0].remove(current->leaf);
-		current->leaf	=	m_sets[1].insert(current->aabb,current);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)	curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
+		current->leaf	=	m_sets[1].insert(curAabb,current);
 		current->stage	=	STAGECOUNT;	
 		current			=	next;
 		} while(current);

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -41,13 +41,13 @@ subject to the following restrictions:
 struct btDbvtProxy : btBroadphaseProxy
 {
 /* Fields		*/ 
-btDbvtAabbMm	aabb;
+//btDbvtAabbMm	aabb;
 btDbvtNode*		leaf;
 btDbvtProxy*	links[2];
 int				stage;
 /* ctor			*/ 
-btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
+btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
-	btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask)
+	btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
 	{
 	links[0]=links[1]=0;
 	}
@@ -105,6 +105,7 @@ void							optimize();
 btBroadphaseProxy*				createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
 void							destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 void							setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
 void							calculateOverlappingPairs(btDispatcher* dispatcher);
 btOverlappingPairCache*			getOverlappingPairCache();
 const btOverlappingPairCache*	getOverlappingPairCache() const;

src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

@@ -149,6 +149,14 @@ amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
 
 
 
+void	btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
+	aabbMin = multiProxy->m_aabbMin;
+	aabbMax = multiProxy->m_aabbMax;
+}
+
+
 //#include <stdio.h>
 
 void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)

src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

@@ -72,7 +72,7 @@ public:
 		short int	m_collisionFilterMask;
 */
 		btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
-			:btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
+			:btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask),
 			m_aabbMin(aabbMin),
 			m_aabbMax(aabbMax),
 			m_shapeType(shapeType)
@@ -108,6 +108,7 @@ public:
 	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
+	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
 
 	void	addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase);
 

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -88,7 +88,7 @@ btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,
 		btAssert(0);
 		return 0; //should never happen, but don't let the game crash ;-)
 	}
-	assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
+	btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
 
 	int newHandleIndex = allocHandle();
 	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
@@ -137,11 +137,18 @@ void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher*
 		
 }
 
+void	btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
+	aabbMin = sbp->m_aabbMin;
+	aabbMax = sbp->m_aabbMax;
+}
+
 void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
 {
 	btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
-	sbp->m_min = aabbMin;
+	sbp->m_aabbMin = aabbMin;
-	sbp->m_max = aabbMax;
+	sbp->m_aabbMax = aabbMax;
 }
 
 
@@ -154,9 +161,9 @@ void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbM
 
 bool	btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
 {
-	return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] && 
+	return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && 
-		   proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] &&
+		   proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
-		   proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2];
+		   proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
 
 }
 

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

@@ -22,8 +22,6 @@ subject to the following restrictions:
 
 struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 {
-	btVector3	m_min;
-	btVector3	m_max;
 	int			m_nextFree;
 	
 //	int			m_handleId;
@@ -32,8 +30,7 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 	btSimpleBroadphaseProxy() {};
 
 	btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
-	:btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy),
+	:btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
-	m_min(minpt),m_max(maxpt)		
 	{
 		(void)shapeType;
 	}
@@ -95,6 +92,12 @@ protected:
 		return proxy0;
 	}
 
+	inline const btSimpleBroadphaseProxy*	getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
+	{
+		const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy);
+		return proxy0;
+	}
+
 
 	void	validate();
 
@@ -117,6 +120,7 @@ public:
 
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
+	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
 		
 	btOverlappingPairCache*	getOverlappingPairCache()
 	{

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -237,12 +237,12 @@ public:
 	}
 
 
-	btBroadphaseProxy*	getBroadphaseHandle()
+	SIMD_FORCE_INLINE btBroadphaseProxy*	getBroadphaseHandle()
 	{
 		return m_broadphaseHandle;
 	}
 
-	const btBroadphaseProxy*	getBroadphaseHandle() const
+	SIMD_FORCE_INLINE const btBroadphaseProxy*	getBroadphaseHandle() const
 	{
 		return m_broadphaseHandle;
 	}

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -226,6 +226,7 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 
 	if (collisionShape->isConvex())
 	{
+		BT_PROFILE("rayTestConvex");
 		btConvexCast::CastResult castResult;
 		castResult.m_fraction = resultCallback.m_closestHitFraction;
 
@@ -269,6 +270,7 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 	} else {
 		if (collisionShape->isConcave())
 		{
+			BT_PROFILE("rayTestConcave");
 			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
 			{
 				///optimized version for btBvhTriangleMeshShape
@@ -374,6 +376,7 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 				triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
 			}
 		} else {
+			BT_PROFILE("rayTestCompound");
 			//todo: use AABB tree or other BVH acceleration structure!
 			if (collisionShape->isCompound())
 			{
@@ -598,6 +601,7 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
 
 void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 {
+	BT_PROFILE("rayTest");
 
 
 	btTransform	rayFromTrans,rayToTrans;
@@ -618,20 +622,25 @@ void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
 
 		btCollisionObject*	collisionObject= m_collisionObjects[i];
 		//only perform raycast if filterMask matches
-		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
+		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		{
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
-			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+			//collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
 
 			btScalar hitLambda = resultCallback.m_closestHitFraction;
 			btVector3 hitNormal;
-			if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
 			{
-				rayTestSingle(rayFromTrans,rayToTrans,
+				if (btRayAabb(rayFromWorld,rayToWorld,collisionObject->getBroadphaseHandle()->m_aabbMin,collisionObject->getBroadphaseHandle()->m_aabbMax,hitLambda,hitNormal))
-					collisionObject,
+				{
-						collisionObject->getCollisionShape(),
+					rayTestSingle(rayFromTrans,rayToTrans,
-						collisionObject->getWorldTransform(),
+						collisionObject,
-						resultCallback);
+							collisionObject->getCollisionShape(),
+							collisionObject->getWorldTransform(),
+							resultCallback);
+				}
+
 			}
 		}
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -107,6 +107,11 @@ public:
 		m_broadphasePairCache = pairCache;
 	}
 
+	const btBroadphaseInterface*	getBroadphase() const
+	{
+		return m_broadphasePairCache;
+	}
+
 	btBroadphaseInterface*	getBroadphase()
 	{
 		return m_broadphasePairCache;