From 9ad17dda227a99053016803e915fec97e1476dba Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Fri, 10 Oct 2008 17:32:52 +0000 Subject: [PATCH] Add aabbMin/aabbMax to broadphase proxy, and improve rayTest performance. Todo: use broadphase acceleration structure (btDbvtBroadphase etc) for ray test --- .../SpuRaycastTask/SpuRaycastTask.cpp | 12 +++--- .../BroadphaseCollision/btAxisSweep3.h | 40 ++++++++++++++++++- .../btBroadphaseInterface.h | 3 +- .../BroadphaseCollision/btBroadphaseProxy.h | 10 ++++- .../BroadphaseCollision/btDbvtBroadphase.cpp | 31 +++++++++----- .../BroadphaseCollision/btDbvtBroadphase.h | 7 ++-- .../btMultiSapBroadphase.cpp | 8 ++++ .../btMultiSapBroadphase.h | 3 +- .../btSimpleBroadphase.cpp | 19 ++++++--- .../BroadphaseCollision/btSimpleBroadphase.h | 12 ++++-- .../CollisionDispatch/btCollisionObject.h | 4 +- .../CollisionDispatch/btCollisionWorld.cpp | 27 ++++++++----- .../CollisionDispatch/btCollisionWorld.h | 5 +++ 13 files changed, 137 insertions(+), 44 deletions(-) diff --git a/Extras/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp b/Extras/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp index 49b8827a2..29170cbcc 100644 --- a/Extras/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp +++ b/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)) diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index 46c55a636..2eb2f0e9f 100644 --- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/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 void btAxisSweep3Internal::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) { Handle* handle = static_cast(proxy); + handle->m_aabbMin = aabbMin; + handle->m_aabbMax = aabbMax; updateHandle(static_cast(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); } +template +void btAxisSweep3Internal::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + Handle* pHandle = static_cast(proxy); + aabbMin = pHandle->m_aabbMin; + aabbMax = pHandle->m_aabbMax; +} + + +template +void btAxisSweep3Internal::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + Handle* pHandle = static_cast(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; +} diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index 200ac3653..79b035033 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/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; diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index ac7f16c7e..0547502b1 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/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) - :m_clientObject(userPtr), + btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0) + :m_aabbMin(aabbMin), + m_aabbMax(aabbMax), + m_clientObject(userPtr), m_collisionFilterGroup(collisionFilterGroup), m_collisionFilterMask(collisionFilterMask) { diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 4a24c025f..e00780f79 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/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[1].m_root,proxy->aabb,collider); + btDbvt::collideTV(m_sets[0].m_root,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); diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 556d02e00..f01f190a0 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/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) : - btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask) +btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int 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; diff --git a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp index 285957f32..5aaa6c871 100644 --- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ b/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(proxy); + aabbMin = multiProxy->m_aabbMin; + aabbMax = multiProxy->m_aabbMax; +} + + //#include void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) diff --git a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h index b5c38ca42..ead05f1b1 100644 --- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ b/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); diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index a57952ffa..e775f45dd 100644 --- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/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_max = aabbMax; + sbp->m_aabbMin = aabbMin; + 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] && - proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] && - proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2]; + return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && + proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] && + proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2]; } diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index e2ebb8257..564383fe9 100644 --- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/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), - m_min(minpt),m_max(maxpt) + :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy) { (void)shapeType; } @@ -95,6 +92,12 @@ protected: return proxy0; } + inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const + { + const btSimpleBroadphaseProxy* proxy0 = static_cast(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() { diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 0961f3e75..d1d78684a 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/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; } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index d8674a320..603de71e8 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/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; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + //btVector3 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, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback); + if (btRayAabb(rayFromWorld,rayToWorld,collisionObject->getBroadphaseHandle()->m_aabbMin,collisionObject->getBroadphaseHandle()->m_aabbMax,hitLambda,hitNormal)) + { + rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); + } + } } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index bae2e860b..20362349d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/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;