diff --git a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp index f1f1bbecd..b8df1b4bd 100644 --- a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp +++ b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp @@ -1,211 +1,213 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuGatheringCollisionDispatcher.h" -#include "SpuCollisionTaskProcess.h" - - -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" -#include "SpuContactManifoldCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" - - - - -SpuGatheringCollisionDispatcher::SpuGatheringCollisionDispatcher(class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks) -:m_spuCollisionTaskProcess(0), -m_threadInterface(threadInterface), -m_maxNumOutstandingTasks(maxNumOutstandingTasks) -{ - -} - - -bool SpuGatheringCollisionDispatcher::supportsDispatchPairOnSpu(int proxyType0,int proxyType1) -{ - bool supported0 = ( - (proxyType0 == BOX_SHAPE_PROXYTYPE) || - (proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) || - (proxyType0 == SPHERE_SHAPE_PROXYTYPE) || - (proxyType0 == CAPSULE_SHAPE_PROXYTYPE) || - (proxyType0 == CYLINDER_SHAPE_PROXYTYPE) || -// (proxyType0 == CONE_SHAPE_PROXYTYPE) || - (proxyType0 == TRIANGLE_MESH_SHAPE_PROXYTYPE) || - (proxyType0 == CONVEX_HULL_SHAPE_PROXYTYPE) - ); - - bool supported1 = ( - (proxyType1 == BOX_SHAPE_PROXYTYPE) || - (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE) || - (proxyType1 == SPHERE_SHAPE_PROXYTYPE) || - (proxyType1 == CAPSULE_SHAPE_PROXYTYPE) || - (proxyType1 == CYLINDER_SHAPE_PROXYTYPE) || -// (proxyType1 == CONE_SHAPE_PROXYTYPE) || - (proxyType1 == TRIANGLE_MESH_SHAPE_PROXYTYPE) || - (proxyType1 == CONVEX_HULL_SHAPE_PROXYTYPE) - ); - - return supported0 && supported1; -} - - - -SpuGatheringCollisionDispatcher::~SpuGatheringCollisionDispatcher() -{ - if (m_spuCollisionTaskProcess) - delete m_spuCollisionTaskProcess; - -} - -#include "stdio.h" - - - -///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) -///this is useful for the collision dispatcher. -class btSpuCollisionPairCallback : public btOverlapCallback -{ - btDispatcherInfo& m_dispatchInfo; - SpuGatheringCollisionDispatcher* m_dispatcher; - -public: - - btSpuCollisionPairCallback(btDispatcherInfo& dispatchInfo,SpuGatheringCollisionDispatcher* dispatcher) - :m_dispatchInfo(dispatchInfo), - m_dispatcher(dispatcher) - { - } - - virtual bool processOverlap(btBroadphasePair& collisionPair) - { - - - //PPU version - //(*m_dispatcher->getNearCallback())(collisionPair,*m_dispatcher,m_dispatchInfo); - - //only support discrete collision detection for now, we could fallback on PPU/unoptimized version for TOI/CCD - btAssert(m_dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE); - - //by default, Bullet will use this near callback - { - ///userInfo is used to determine if the SPU has to handle this case or not (skip PPU tasks) - if (!collisionPair.m_userInfo) - { - collisionPair.m_userInfo = (void*) 1; - } - if (!collisionPair.m_algorithm) - { - btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - - btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher = m_dispatcher; - ci.m_manifold = 0; - - if (m_dispatcher->needsCollision(colObj0,colObj1)) - { - int proxyType0 = colObj0->getCollisionShape()->getShapeType(); - int proxyType1 = colObj1->getCollisionShape()->getShapeType(); - if (m_dispatcher->supportsDispatchPairOnSpu(proxyType0,proxyType1)) - { - collisionPair.m_algorithm = new SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1); - collisionPair.m_userInfo = (void*) 2; - } else - { - collisionPair.m_algorithm = m_dispatcher->findAlgorithm(colObj0,colObj1); - collisionPair.m_userInfo = (void*)3; - } - } else - { - //create an empty algorithm - collisionPair.m_algorithm = new btEmptyAlgorithm(ci); - } - } - } - return false; - } -}; - -void SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo) -{ - - if (dispatchInfo.m_enableSPU) - { - if (!m_spuCollisionTaskProcess) - m_spuCollisionTaskProcess = new SpuCollisionTaskProcess(m_threadInterface,m_maxNumOutstandingTasks); - - m_spuCollisionTaskProcess->initialize2(); - - ///modified version of btCollisionDispatcher::dispatchAllCollisionPairs: - { - btSpuCollisionPairCallback collisionCallback(dispatchInfo,this); - - pairCache->processAllOverlappingPairs(&collisionCallback); - } - - //send one big batch - int numTotalPairs = pairCache->getNumOverlappingPairs(); - btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr(); - int i; - for (i=0;iaddWorkToTask(pairPtr,i,endIndex); - i = endIndex; - } - - //handle PPU fallback pairs - for (i=0;im_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - btManifoldResult contactPointResult(colObj0,colObj1); - - if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) - { - //discrete collision detection query - collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult); - } else - { - //continuous collision detection query, time of impact (toi) - btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); - if (dispatchInfo.m_timeOfImpact > toi) - dispatchInfo.m_timeOfImpact = toi; - - } - } - } - } - - //make sure all SPU work is done - m_spuCollisionTaskProcess->flush2(); - - } else - { - ///PPU fallback - ///!Need to make sure to clear all 'algorithms' when switching between SPU and PPU - btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo); - } -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SpuGatheringCollisionDispatcher.h" +#include "SpuCollisionTaskProcess.h" + + +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" +#include "SpuContactManifoldCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" + + + + +SpuGatheringCollisionDispatcher::SpuGatheringCollisionDispatcher(class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks) +:m_spuCollisionTaskProcess(0), +m_threadInterface(threadInterface), +m_maxNumOutstandingTasks(maxNumOutstandingTasks) +{ + +} + + +bool SpuGatheringCollisionDispatcher::supportsDispatchPairOnSpu(int proxyType0,int proxyType1) +{ + bool supported0 = ( + (proxyType0 == BOX_SHAPE_PROXYTYPE) || + (proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) || + (proxyType0 == SPHERE_SHAPE_PROXYTYPE) || + (proxyType0 == CAPSULE_SHAPE_PROXYTYPE) || + (proxyType0 == CYLINDER_SHAPE_PROXYTYPE) || +// (proxyType0 == CONE_SHAPE_PROXYTYPE) || + (proxyType0 == TRIANGLE_MESH_SHAPE_PROXYTYPE) || + (proxyType0 == CONVEX_HULL_SHAPE_PROXYTYPE)|| + (proxyType0 == COMPOUND_SHAPE_PROXYTYPE) + ); + + bool supported1 = ( + (proxyType1 == BOX_SHAPE_PROXYTYPE) || + (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE) || + (proxyType1 == SPHERE_SHAPE_PROXYTYPE) || + (proxyType1 == CAPSULE_SHAPE_PROXYTYPE) || + (proxyType1 == CYLINDER_SHAPE_PROXYTYPE) || +// (proxyType1 == CONE_SHAPE_PROXYTYPE) || + (proxyType1 == TRIANGLE_MESH_SHAPE_PROXYTYPE) || + (proxyType1 == CONVEX_HULL_SHAPE_PROXYTYPE) || + (proxyType1 == COMPOUND_SHAPE_PROXYTYPE) + ); + + return supported0 && supported1; +} + + + +SpuGatheringCollisionDispatcher::~SpuGatheringCollisionDispatcher() +{ + if (m_spuCollisionTaskProcess) + delete m_spuCollisionTaskProcess; + +} + +#include "stdio.h" + + + +///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) +///this is useful for the collision dispatcher. +class btSpuCollisionPairCallback : public btOverlapCallback +{ + btDispatcherInfo& m_dispatchInfo; + SpuGatheringCollisionDispatcher* m_dispatcher; + +public: + + btSpuCollisionPairCallback(btDispatcherInfo& dispatchInfo,SpuGatheringCollisionDispatcher* dispatcher) + :m_dispatchInfo(dispatchInfo), + m_dispatcher(dispatcher) + { + } + + virtual bool processOverlap(btBroadphasePair& collisionPair) + { + + + //PPU version + //(*m_dispatcher->getNearCallback())(collisionPair,*m_dispatcher,m_dispatchInfo); + + //only support discrete collision detection for now, we could fallback on PPU/unoptimized version for TOI/CCD + btAssert(m_dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE); + + //by default, Bullet will use this near callback + { + ///userInfo is used to determine if the SPU has to handle this case or not (skip PPU tasks) + if (!collisionPair.m_userInfo) + { + collisionPair.m_userInfo = (void*) 1; + } + if (!collisionPair.m_algorithm) + { + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + + btCollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher = m_dispatcher; + ci.m_manifold = 0; + + if (m_dispatcher->needsCollision(colObj0,colObj1)) + { + int proxyType0 = colObj0->getCollisionShape()->getShapeType(); + int proxyType1 = colObj1->getCollisionShape()->getShapeType(); + if (m_dispatcher->supportsDispatchPairOnSpu(proxyType0,proxyType1)) + { + collisionPair.m_algorithm = new SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1); + collisionPair.m_userInfo = (void*) 2; + } else + { + collisionPair.m_algorithm = m_dispatcher->findAlgorithm(colObj0,colObj1); + collisionPair.m_userInfo = (void*)3; + } + } else + { + //create an empty algorithm + collisionPair.m_algorithm = new btEmptyAlgorithm(ci); + } + } + } + return false; + } +}; + +void SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo) +{ + + if (dispatchInfo.m_enableSPU) + { + if (!m_spuCollisionTaskProcess) + m_spuCollisionTaskProcess = new SpuCollisionTaskProcess(m_threadInterface,m_maxNumOutstandingTasks); + + m_spuCollisionTaskProcess->initialize2(); + + ///modified version of btCollisionDispatcher::dispatchAllCollisionPairs: + { + btSpuCollisionPairCallback collisionCallback(dispatchInfo,this); + + pairCache->processAllOverlappingPairs(&collisionCallback); + } + + //send one big batch + int numTotalPairs = pairCache->getNumOverlappingPairs(); + btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr(); + int i; + for (i=0;iaddWorkToTask(pairPtr,i,endIndex); + i = endIndex; + } + + //handle PPU fallback pairs + for (i=0;im_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + btManifoldResult contactPointResult(colObj0,colObj1); + + if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) + { + //discrete collision detection query + collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult); + } else + { + //continuous collision detection query, time of impact (toi) + btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + + } + } + } + } + + //make sure all SPU work is done + m_spuCollisionTaskProcess->flush2(); + + } else + { + ///PPU fallback + ///!Need to make sure to clear all 'algorithms' when switching between SPU and PPU + btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo); + } +} + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp index b06c7dccb..9bfac71e4 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp @@ -1,171 +1,193 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuContactResult.h" - - -//#define DEBUG_SPU_COLLISION_DETECTION 1 - - -#include "SpuContactResult.h" - - -SpuContactResult::SpuContactResult() -{ - m_manifoldAddress = 0; - m_spuManifold = NULL; - m_RequiresWriteBack = false; -} - - SpuContactResult::~SpuContactResult() -{ - g_manifoldDmaExport.swapBuffers(); -} - - void SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, uint64_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1) - { -// spu_printf("SpuContactResult::setContactInfo\n"); - m_rootWorldTransform0 = worldTrans0; - m_rootWorldTransform1 = worldTrans1; - m_manifoldAddress = manifoldAddress; - m_spuManifold = spuManifold; - } - - void SpuContactResult::setShapeIdentifiers(int partId0,int index0, int partId1,int index1) - { - - } - - - - ///return true if it requires a dma transfer back -bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld, - const btVector3& pointInWorld, - float depth, - btPersistentManifold* manifoldPtr, - btTransform& transA, - btTransform& transB - ) -{ - - float contactTreshold = manifoldPtr->getContactBreakingThreshold(); - - //spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr); - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: contactTreshold %f\n",contactTreshold); -#endif //DEBUG_SPU_COLLISION_DETECTION - if (depth > manifoldPtr->getContactBreakingThreshold()) - return false; - - //provide inverses or just calculate? - btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform; - btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform; - - btVector3 pointA = pointInWorld + normalOnBInWorld * depth; - btVector3 localA = transAInv(pointA ); - btVector3 localB = transBInv(pointInWorld); - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); - - int insertIndex = manifoldPtr->getCacheEntry(newPt); - if (insertIndex >= 0) - { -// manifoldPtr->replaceContactPoint(newPt,insertIndex); -// return true; - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: same contact detected, nothing done\n"); -#endif //DEBUG_SPU_COLLISION_DETECTION - // This is not needed, just use the old info! saves a DMA transfer as well - } else - { - - newPt.m_combinedFriction = 0.25f;//calculateCombinedFriction(m_body0,m_body1); - newPt.m_combinedRestitution = 0.0f;//calculateCombinedRestitution(m_body0,m_body1); - - /* - //potential TODO: SPU callbacks, either immediate (local on the SPU), or deferred - //User can override friction and/or restitution - if (gContactAddedCallback && - //and if either of the two bodies requires custom material - ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) || - (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback))) - { - //experimental feature info, for per-triangle material etc. - (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1); - } - */ - manifoldPtr->AddManifoldPoint(newPt); - return true; - - } - return false; - -} - - -void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold) -{ - memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold)); - - g_manifoldDmaExport.swapBuffers(); - g_manifoldDmaExport.backBufferDmaPut((uint64_t)mmManifold, sizeof(btPersistentManifold), DMA_TAG(9)); - // Should there be any kind of wait here? What if somebody tries to use this tag again? What if we call this function again really soon? - //no, the swapBuffers does the wait -} - -void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth) -{ -// spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth); - -#ifdef DEBUG_SPU_COLLISION_DETECTION - // int sman = sizeof(rage::phManifold); -// spu_printf("sizeof_manifold = %i\n",sman); -#endif //DEBUG_SPU_COLLISION_DETECTION - - btPersistentManifold* localManifold = m_spuManifold; - - btVector3 normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ()); - btVector3 pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ()); - - //process the contact point - const bool retVal = ManifoldResultAddContactPoint(normalB, - pointWrld, - depth, - localManifold, - m_rootWorldTransform0, - m_rootWorldTransform1 - ); - m_RequiresWriteBack = m_RequiresWriteBack || retVal; -} - -void SpuContactResult::flush() -{ - if (m_RequiresWriteBack) - { -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: Start rage::phManifold Write (Put) DMA\n"); -#endif //DEBUG_SPU_COLLISION_DETECTION - // spu_printf("writeDoubleBufferedManifold\n"); - writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress); -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: Finished (Put) DMA\n"); -#endif //DEBUG_SPU_COLLISION_DETECTION - } - m_spuManifold = NULL; - m_RequiresWriteBack = false; -} - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SpuContactResult.h" + + +//#define DEBUG_SPU_COLLISION_DETECTION 1 + + +#include "SpuContactResult.h" + + +SpuContactResult::SpuContactResult() +{ + m_manifoldAddress = 0; + m_spuManifold = NULL; + m_RequiresWriteBack = false; +} + + SpuContactResult::~SpuContactResult() +{ + g_manifoldDmaExport.swapBuffers(); +} + + void SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, uint64_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, bool isSwapped) + { + //spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress); + m_rootWorldTransform0 = worldTrans0; + m_rootWorldTransform1 = worldTrans1; + m_manifoldAddress = manifoldAddress; + m_spuManifold = spuManifold; + m_isSwapped = isSwapped; + } + + void SpuContactResult::setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + + } + + + + ///return true if it requires a dma transfer back +bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld, + const btVector3& pointInWorld, + float depth, + btPersistentManifold* manifoldPtr, + btTransform& transA, + btTransform& transB, + bool isSwapped) +{ + + float contactTreshold = manifoldPtr->getContactBreakingThreshold(); + + //spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU: contactTreshold %f\n",contactTreshold); +#endif //DEBUG_SPU_COLLISION_DETECTION + if (depth > manifoldPtr->getContactBreakingThreshold()) + return false; + + //provide inverses or just calculate? + btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform; + btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform; + + btVector3 pointA; + btVector3 localA; + btVector3 localB; + btVector3 normal; + + if (isSwapped) + { + normal = normalOnBInWorld * -1; + pointA = pointInWorld + normal * depth; + localA = transAInv(pointA ); + localB = transBInv(pointInWorld); + /*localA = transBInv(pointA ); + localB = transAInv(pointInWorld);*/ + } + else + { + normal = normalOnBInWorld; + pointA = pointInWorld + normal * depth; + localA = transAInv(pointA ); + localB = transBInv(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normal,depth); + + int insertIndex = manifoldPtr->getCacheEntry(newPt); + if (insertIndex >= 0) + { +// manifoldPtr->replaceContactPoint(newPt,insertIndex); +// return true; + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU: same contact detected, nothing done\n"); +#endif //DEBUG_SPU_COLLISION_DETECTION + // This is not needed, just use the old info! saves a DMA transfer as well + } else + { + + newPt.m_combinedFriction = 0.25f;//calculateCombinedFriction(m_body0,m_body1); + newPt.m_combinedRestitution = 0.0f;//calculateCombinedRestitution(m_body0,m_body1); + + /* + //potential TODO: SPU callbacks, either immediate (local on the SPU), or deferred + //User can override friction and/or restitution + if (gContactAddedCallback && + //and if either of the two bodies requires custom material + ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) || + (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback))) + { + //experimental feature info, for per-triangle material etc. + (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1); + } + */ + manifoldPtr->AddManifoldPoint(newPt); + return true; + + } + return false; + +} + + +void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold) +{ + memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold)); + + g_manifoldDmaExport.swapBuffers(); + g_manifoldDmaExport.backBufferDmaPut((uint64_t)mmManifold, sizeof(btPersistentManifold), DMA_TAG(9)); + // Should there be any kind of wait here? What if somebody tries to use this tag again? What if we call this function again really soon? + //no, the swapBuffers does the wait +} + +void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth) +{ + //spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + // int sman = sizeof(rage::phManifold); +// spu_printf("sizeof_manifold = %i\n",sman); +#endif //DEBUG_SPU_COLLISION_DETECTION + + btPersistentManifold* localManifold = m_spuManifold; + + btVector3 normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ()); + btVector3 pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ()); + + //process the contact point + const bool retVal = ManifoldResultAddContactPoint(normalB, + pointWrld, + depth, + localManifold, + m_rootWorldTransform0, + m_rootWorldTransform1, + m_isSwapped); + m_RequiresWriteBack = m_RequiresWriteBack || retVal; +} + +void SpuContactResult::flush() +{ + if (m_RequiresWriteBack) + { +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n"); + spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts()); + spu_printf("Manifold address: %llu\n", m_manifoldAddress); +#endif //DEBUG_SPU_COLLISION_DETECTION + // spu_printf("writeDoubleBufferedManifold\n"); + writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress); +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU: Finished (Put) DMA\n"); +#endif //DEBUG_SPU_COLLISION_DETECTION + } + m_spuManifold = NULL; + m_RequiresWriteBack = false; +} + + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h index 51f12b042..abcad7102 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h @@ -1,110 +1,111 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SPU_CONTACT_RESULT2_H -#define SPU_CONTACT_RESULT2_H - - -#ifndef WIN32 -#include -#endif - - - -#include "../SpuDoubleBuffer.h" - - -#include "LinearMath/btTransform.h" -#include "LinearMath/btPoint3.h" - - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - - -struct SpuCollisionPairInput -{ - uint64_t m_collisionShapes[2]; - void* m_spuCollisionShapes[2]; - - uint64_t m_persistentManifoldPtr; - btVector3 m_primitiveDimensions0; - btVector3 m_primitiveDimensions1; - int m_shapeType0; - int m_shapeType1; - float m_collisionMargin0; - float m_collisionMargin1; - - btTransform m_worldTransform0; - btTransform m_worldTransform1; - - bool m_isSwapped; - - - -}; - - -struct SpuClosestPointInput -{ - SpuClosestPointInput() - :m_maximumDistanceSquared(float(1e30)), - m_stackAlloc(0) - { - } - - btTransform m_transformA; - btTransform m_transformB; - float m_maximumDistanceSquared; - class btStackAlloc* m_stackAlloc; - struct SpuConvexPolyhedronVertexData* m_convexVertexData; -}; - -///SpuContactResult exports the contact points using double-buffered DMA transfers, only when needed -///So when an existing contact point is duplicated, no transfer/refresh is performed. -class SpuContactResult -{ - btTransform m_rootWorldTransform0; - btTransform m_rootWorldTransform1; - uint64_t m_manifoldAddress; - - btPersistentManifold* m_spuManifold; - bool m_RequiresWriteBack; - - DoubleBuffer g_manifoldDmaExport; - - public: - SpuContactResult(); - virtual ~SpuContactResult(); - - btPersistentManifold* GetSpuManifold() const - { - return m_spuManifold; - } - - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1); - - void setContactInfo(btPersistentManifold* spuManifold, uint64_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1); - - void writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold); - - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth); - - void flush(); -}; - - - -#endif //SPU_CONTACT_RESULT2_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SPU_CONTACT_RESULT2_H +#define SPU_CONTACT_RESULT2_H + + +#ifndef WIN32 +#include +#endif + + + +#include "../SpuDoubleBuffer.h" + + +#include "LinearMath/btTransform.h" +#include "LinearMath/btPoint3.h" + + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + + +struct SpuCollisionPairInput +{ + uint64_t m_collisionShapes[2]; + void* m_spuCollisionShapes[2]; + + uint64_t m_persistentManifoldPtr; + btVector3 m_primitiveDimensions0; + btVector3 m_primitiveDimensions1; + int m_shapeType0; + int m_shapeType1; + float m_collisionMargin0; + float m_collisionMargin1; + + btTransform m_worldTransform0; + btTransform m_worldTransform1; + + bool m_isSwapped; + + + +}; + + +struct SpuClosestPointInput +{ + SpuClosestPointInput() + :m_maximumDistanceSquared(float(1e30)), + m_stackAlloc(0) + { + } + + btTransform m_transformA; + btTransform m_transformB; + float m_maximumDistanceSquared; + class btStackAlloc* m_stackAlloc; + struct SpuConvexPolyhedronVertexData* m_convexVertexData; +}; + +///SpuContactResult exports the contact points using double-buffered DMA transfers, only when needed +///So when an existing contact point is duplicated, no transfer/refresh is performed. +class SpuContactResult +{ + btTransform m_rootWorldTransform0; + btTransform m_rootWorldTransform1; + uint64_t m_manifoldAddress; + + btPersistentManifold* m_spuManifold; + bool m_RequiresWriteBack; + bool m_isSwapped; + + DoubleBuffer g_manifoldDmaExport; + + public: + SpuContactResult(); + virtual ~SpuContactResult(); + + btPersistentManifold* GetSpuManifold() const + { + return m_spuManifold; + } + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1); + + void setContactInfo(btPersistentManifold* spuManifold, uint64_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, bool isSwapped = false); + + void writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold); + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth); + + void flush(); +}; + + + +#endif //SPU_CONTACT_RESULT2_H + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp index 6d63b0f86..e21ec1c83 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp @@ -19,6 +19,7 @@ #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btConvexHullShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "SpuMinkowskiPenetrationDepthSolver.h" #include "SpuGjkPairDetector.h" @@ -26,6 +27,9 @@ #include "SpuLocalSupport.h" //definition of SpuConvexPolyhedronVertexData +#ifdef USE_SN_TUNER +#include +#endif //USE_SN_TUNER #ifdef WIN32 @@ -80,14 +84,18 @@ struct CollisionTask_LocalStoreMemory ///only a single mesh part for now, we can add support for multiple parts, but quantized trees don't support this at the moment ATTRIBUTE_ALIGNED16(btIndexedMesh gIndexMesh); - #define MAX_SPU_SUBTREE_HEADERS 32 +#define MAX_SPU_SUBTREE_HEADERS 32 //1024 ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]); ATTRIBUTE_ALIGNED16(btQuantizedBvhNode gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]); SpuConvexPolyhedronVertexData convexVertexData; - + // Compound data +#define MAX_SPU_COMPOUND_SUBSHAPES 16 + ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES*2]); + ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES*2][MAX_SHAPE_SIZE]); + }; @@ -125,13 +133,13 @@ inline bool spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* a void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex) { - + int curIndex = startNodeIndex; int walkIterations = 0; int subTreeSize = endNodeIndex - startNodeIndex; int escapeIndex; - + bool aabbOverlap, isLeafNode; while (curIndex < endNodeIndex) @@ -142,14 +150,14 @@ void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned walkIterations++; aabbOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); isLeafNode = rootNode->isLeafNode(); - + if (isLeafNode && aabbOverlap) { //printf("overlap with node %d\n",rootNode->getTriangleIndex()); nodeCallback->processNode(0,rootNode->getTriangleIndex()); -// spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex()); + // spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex()); } - + if (aabbOverlap || isLeafNode) { rootNode++; @@ -161,19 +169,19 @@ void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned curIndex += escapeIndex; } } - + } void small_cache_read(void* buffer, uint64_t ea, size_t size) { #if USE_SOFTWARE_CACHE - // Check for alignment requirements. We need to make sure the entire request fits within one cache line, - // so the first and last bytes should fall on the same cache line - btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK)); + // Check for alignment requirements. We need to make sure the entire request fits within one cache line, + // so the first and last bytes should fall on the same cache line + btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK)); - void* ls = spe_cache_read(ea); - memcpy(buffer, ls, size); + void* ls = spe_cache_read(ea); + memcpy(buffer, ls, size); #else stallingUnalignedDmaSmallGet(buffer,ea,size); #endif @@ -196,7 +204,7 @@ public: spuNodeCallback(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts) : m_wuInput(wuInput), m_lsMemPtr(lsMemPtr), - m_spuContacts(spuContacts) + m_spuContacts(spuContacts) { } @@ -205,9 +213,9 @@ public: ///Create a triangle on the stack, call process collision, with GJK ///DMA the vertices, can benefit from software caching -// spu_printf("processNode with triangleIndex %d\n",triangleIndex); - - + // spu_printf("processNode with triangleIndex %d\n",triangleIndex); + + int* indexBasePtr = (int*)(m_lsMemPtr->gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->gIndexMesh.m_triangleIndexStride); @@ -216,52 +224,52 @@ public: small_cache_read(&m_lsMemPtr->spuIndices[1],(uint64_t)&indexBasePtr[1],sizeof(int)); small_cache_read(&m_lsMemPtr->spuIndices[2],(uint64_t)&indexBasePtr[2],sizeof(int)); -// spu_printf("SPU index0=%d ,",spuIndices[0]); -// spu_printf("SPU index1=%d ,",spuIndices[1]); -// spu_printf("SPU index2=%d ,",spuIndices[2]); -// spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr); + // spu_printf("SPU index0=%d ,",spuIndices[0]); + // spu_printf("SPU index1=%d ,",spuIndices[1]); + // spu_printf("SPU index2=%d ,",spuIndices[2]); + // spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr); const btVector3& meshScaling = m_lsMemPtr->gTriangleMeshInterface.getScaling(); for (int j=2;j>=0;j--) { int graphicsindex = m_lsMemPtr->spuIndices[j]; -// spu_printf("SPU index=%d ,",graphicsindex); + // spu_printf("SPU index=%d ,",graphicsindex); btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->gIndexMesh.m_vertexStride); -// spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); - + // spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); + ///handle un-aligned vertices... - + //another DMA for each vertex small_cache_read(&spuUnscaledVertex[0],(uint64_t)&graphicsbasePtr[0],sizeof(btScalar)); small_cache_read(&spuUnscaledVertex[1],(uint64_t)&graphicsbasePtr[1],sizeof(btScalar)); small_cache_read(&spuUnscaledVertex[2],(uint64_t)&graphicsbasePtr[2],sizeof(btScalar)); - - spuTriangleVertices[j] = btVector3( + + spuTriangleVertices[j] = btVector3( spuUnscaledVertex[0]*meshScaling.getX(), spuUnscaledVertex[1]*meshScaling.getY(), spuUnscaledVertex[2]*meshScaling.getZ()); -// spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z()); + // spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z()); } - + //btTriangleShape tmpTriangleShape(spuTriangleVertices[0],spuTriangleVertices[1],spuTriangleVertices[2]); - + SpuCollisionPairInput triangleConcaveInput(*m_wuInput); triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0]; triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE; m_spuContacts.setShapeIdentifiers(-1,-1,subPart,triangleIndex); -// m_spuContacts.flush(); + // m_spuContacts.flush(); ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts); ///this flush should be automatic - // m_spuContacts.flush(); + // m_spuContacts.flush(); } }; @@ -283,11 +291,11 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1]; //need the mesh interface, for access to triangle vertices { - int dmaSize = sizeof(btTriangleIndexVertexArray); - uint64_t dmaPpuAddress2 = reinterpret_cast(trimeshShape->getMeshInterface()); + int dmaSize = sizeof(btTriangleIndexVertexArray); + uint64_t dmaPpuAddress2 = reinterpret_cast(trimeshShape->getMeshInterface()); // spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2); - cellDmaGet(&lsMemPtr->gTriangleMeshInterface, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); + cellDmaGet(&lsMemPtr->gTriangleMeshInterface, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); } ///now DMA over the BVH @@ -298,7 +306,7 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT cellDmaGet(&lsMemPtr->gOptimizedBvh, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); cellDmaWaitTagStatusAll(DMA_MASK(1)); } - + btVector3 aabbMin(-1,-400,-1); btVector3 aabbMax(1,400,1); @@ -311,7 +319,7 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT switch (wuInput->m_shapeType0) { case CYLINDER_SHAPE_PROXYTYPE: - + case BOX_SHAPE_PROXYTYPE: { float margin=convexShape->getMarginNV(); @@ -320,8 +328,8 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT btMatrix3x3 abs_b = t.getBasis().absolute(); btPoint3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents), - abs_b[1].dot(halfExtents), - abs_b[2].dot(halfExtents)); + abs_b[1].dot(halfExtents), + abs_b[2].dot(halfExtents)); extent += btVector3(margin,margin,margin); aabbMin = center - extent; aabbMax = center + extent; @@ -330,7 +338,7 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT case CAPSULE_SHAPE_PROXYTYPE: { - float margin=convexShape->getMarginNV(); + float margin=convexShape->getMarginNV(); btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); //add the radius to y-axis to get full height btScalar radius = halfExtents[0]; @@ -339,8 +347,8 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT btMatrix3x3 abs_b = t.getBasis().absolute(); btPoint3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents), - abs_b[1].dot(halfExtents), - abs_b[2].dot(halfExtents)); + abs_b[1].dot(halfExtents), + abs_b[2].dot(halfExtents)); extent += btVector3(margin,margin,margin); aabbMin = center - extent; aabbMax = center + extent; @@ -370,14 +378,14 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT cellDmaWaitTagStatusAll(DMA_MASK(1)); btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; btTransform& t = wuInput->m_worldTransform0; - + btScalar margin = convexShape->getMarginNV(); localPtr->getNonvirtualAabb(t,aabbMin,aabbMax,margin); //spu_printf("SPU convex aabbMin=%f,%f,%f=\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ()); //spu_printf("SPU convex aabbMax=%f,%f,%f=\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ()); - + break; } @@ -388,11 +396,11 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); //convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); -// btScalar extraMargin = collisionMarginTriangle; -// btVector3 extra(extraMargin,extraMargin,extraMargin); -// aabbMax += extra; -// aabbMin -= extra; - + // btScalar extraMargin = collisionMarginTriangle; + // btVector3 extra(extraMargin,extraMargin,extraMargin); + // aabbMax += extra; + // aabbMin -= extra; + ///quantize query AABB @@ -411,7 +419,7 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT //spu_printf("SPU:indexArray.size() = %d\n",indexArray.size()); -// spu_printf("SPU: numSubTrees = %d\n",subTrees.size()); + // spu_printf("SPU: numSubTrees = %d\n",subTrees.size()); //not likely to happen if (subTrees.size() && indexArray.size() == 1) { @@ -435,12 +443,12 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT { int dmaSize = nextBatch* sizeof(btBvhSubtreeInfo); uint64_t dmaPpuAddress2 = reinterpret_cast(&subTrees[i]); -// spu_printf("&subtree[i]=%llx, dmaSize = %d\n",dmaPpuAddress2,dmaSize); + // spu_printf("&subtree[i]=%llx, dmaSize = %d\n",dmaPpuAddress2,dmaSize); cellDmaGet(&lsMemPtr->gSubtreeHeaders[0], dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); cellDmaWaitTagStatusAll(DMA_MASK(1)); } -// spu_printf("nextBatch = %d\n",nextBatch); + // spu_printf("nextBatch = %d\n",nextBatch); for (int j=0;jgSubtreeNodes[0], @@ -469,28 +477,28 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT } -// spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize); + // spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize); } - // unsigned short int m_quantizedAabbMin[3]; - // unsigned short int m_quantizedAabbMax[3]; - // int m_rootNodeIndex; - // int m_subtreeSize; + // unsigned short int m_quantizedAabbMin[3]; + // unsigned short int m_quantizedAabbMax[3]; + // int m_rootNodeIndex; + // int m_subtreeSize; i+=nextBatch; } //pre-fetch first tree, then loop and double buffer - + } - + } ///getShapeTypeSize could easily be optimized, but it is not likely a bottleneck int getShapeTypeSize(int shapeType) { - + switch (shapeType) { @@ -508,30 +516,37 @@ int getShapeTypeSize(int shapeType) } case SPHERE_SHAPE_PROXYTYPE: { - int shapeSize = sizeof(btSphereShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; + int shapeSize = sizeof(btSphereShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { - int shapeSize = sizeof(btBvhTriangleMeshShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; + int shapeSize = sizeof(btBvhTriangleMeshShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; } case CAPSULE_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btCapsuleShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } + { + int shapeSize = sizeof(btCapsuleShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } case CONVEX_HULL_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btConvexHullShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - + { + int shapeSize = sizeof(btConvexHullShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + + case COMPOUND_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btCompoundShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + default: btAssert(0); //unsupported shapetype, please add here @@ -554,7 +569,7 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa //CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0]; //CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1]; btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr; - + bool genericGjk = true; @@ -583,7 +598,7 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa cellDmaWaitTagStatusAll(DMA_MASK(1)); } btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; - + lsMemPtr->convexVertexData.gNumConvexPoints0 = localPtr->getNumPoints(); if (lsMemPtr->convexVertexData.gNumConvexPoints0>MAX_NUM_SPU_CONVEX_POINTS) { @@ -609,7 +624,7 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]); - // spu_printf("SPU: DMA btConvexHullShape\n"); + // spu_printf("SPU: DMA btConvexHullShape\n"); { int dmaSize = sizeof(btConvexHullShape); uint64_t dmaPpuAddress2 = wuInput->m_collisionShapes[1]; @@ -617,7 +632,7 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa cellDmaWaitTagStatusAll(DMA_MASK(1)); } btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape1; - + lsMemPtr->convexVertexData.gNumConvexPoints1 = localPtr->getNumPoints(); if (lsMemPtr->convexVertexData.gNumConvexPoints1>MAX_NUM_SPU_CONVEX_POINTS) { @@ -648,27 +663,365 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa int shapeType1 = wuInput->m_shapeType1; float marginA = wuInput->m_collisionMargin0; float marginB = wuInput->m_collisionMargin1; - + SpuClosestPointInput cpInput; cpInput.m_convexVertexData = &lsMemPtr->convexVertexData; cpInput.m_transformA = wuInput->m_worldTransform0; cpInput.m_transformB = wuInput->m_worldTransform1; - float sumMargin = (marginA+marginB); + float sumMargin = (marginA+marginB+lsMemPtr->gPersistentManifold.getContactBreakingThreshold()); cpInput.m_maximumDistanceSquared = sumMargin * sumMargin; uint64_t manifoldAddress = (uint64_t)manifold; btPersistentManifold* spuManifold=&lsMemPtr->gPersistentManifold; - spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1); - + //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped); + spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->gColObj0.getWorldTransform(),lsMemPtr->gColObj1.getWorldTransform(),wuInput->m_isSwapped); SpuGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&vsSolver,&penetrationSolver); gjk.getClosestPoints(cpInput,spuContacts);//,debugDraw); } - + } +template +void DoSwap(T& a, T& b) +{ + char tmp[sizeof(T)]; + memcpy(tmp, &a, sizeof(T)); + memcpy(&a, &b, sizeof(T)); + memcpy(&b, tmp, sizeof(T)); +} +void dmaAndSetupCollisionObjects(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem) +{ + { + int dmaSize = sizeof(btCollisionObject); + uint64_t dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (uint64_t)lsMem.gProxyPtr1->m_clientObject :*/ (uint64_t)lsMem.gProxyPtr0->m_clientObject; + cellDmaGet(&lsMem.gColObj0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + } + { + int dmaSize = sizeof(btCollisionObject); + uint64_t dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (uint64_t)lsMem.gProxyPtr0->m_clientObject :*/ (uint64_t)lsMem.gProxyPtr1->m_clientObject; + cellDmaGet(&lsMem.gColObj1, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + } + + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + collisionPairInput.m_worldTransform0 = lsMem.gColObj0.getWorldTransform(); + collisionPairInput.m_worldTransform1 = lsMem.gColObj1.getWorldTransform(); + + + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU worldTrans0.origin = (%f,%f,%f)\n", + collisionPairInput->m_worldTransform0.getOrigin().getX(), + collisionPairInput->m_worldTransform0.getOrigin().getY(), + collisionPairInput->m_worldTransform0.getOrigin().getZ()); + + spu_printf("SPU worldTrans1.origin = (%f,%f,%f)\n", + collisionPairInput->m_worldTransform1.getOrigin().getX(), + collisionPairInput->m_worldTransform1.getOrigin().getY(), + collisionPairInput->m_worldTransform1.getOrigin().getZ()); +#endif //DEBUG_SPU_COLLISION_DETECTION + +} + +void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem, + SpuContactResult &spuContacts, + uint64_t collisionShape0Ptr, void* collisionShape0Loc, + uint64_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true) +{ + if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) + && btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) + { + + //dmaAndSetupCollisionObjects(collisionPairInput, lsMem); + + if (dmaShapes) + { + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); + //uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gColObj0.getCollisionShape(); + uint64_t dmaPpuAddress2 = collisionShape0Ptr; + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); + uint64_t dmaPpuAddress2 = collisionShape1Ptr; + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + } + } + + btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc; + btConvexInternalShape* spuConvexShape1 = (btConvexInternalShape*)collisionShape1Loc; + + btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions(); + btVector3 dim1 = spuConvexShape1->getImplicitShapeDimensions(); + + collisionPairInput.m_primitiveDimensions0 = dim0; + collisionPairInput.m_primitiveDimensions1 = dim1; + collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr; + collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr; + collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0; + collisionPairInput.m_spuCollisionShapes[1] = spuConvexShape1; + ProcessSpuConvexConvexCollision(&collisionPairInput,&lsMem,spuContacts); + } + else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) && + btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1)) + { + //snPause(); + + // Both are compounds, do N^2 CD for now + // TODO: add some AABB-based pruning + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); + uint64_t dmaPpuAddress2 = collisionShape0Ptr; + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); + uint64_t dmaPpuAddress2 = collisionShape1Ptr; + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + } + + btCompoundShape* spuCompoundShape0 = (btCompoundShape*)collisionShape0Loc; + btCompoundShape* spuCompoundShape1 = (btCompoundShape*)collisionShape1Loc; + + int childShapeCount0 = spuCompoundShape0->getNumChildShapes(); + int childShapeCount1 = spuCompoundShape1->getNumChildShapes(); + + // dma the first list of child shapes + { + int dmaSize = childShapeCount0 * sizeof(btCompoundShapeChild); + uint64_t dmaPpuAddress2 = (uint64_t)spuCompoundShape0->getChildList(); + cellDmaGet(lsMem.gSubshapes, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + // dma the second list of child shapes + { + int dmaSize = childShapeCount1 * sizeof(btCompoundShapeChild); + uint64_t dmaPpuAddress2 = (uint64_t)spuCompoundShape1->getChildList(); + cellDmaGet(&lsMem.gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES], dmaPpuAddress2, dmaSize, DMA_TAG(2), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + } + + // DMA all the subshapes + for (int i = 0; i < childShapeCount0; ++i) + { + btCompoundShapeChild& childShape = lsMem.gSubshapes[i]; + + int dmaSize = getShapeTypeSize(childShape.m_childShapeType); + uint64_t dmaPpuAddress2 = (uint64_t)childShape.m_childShape; + cellDmaGet(lsMem.gSubshapeShape[i], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + for (int i = 0; i < childShapeCount1; ++i) + { + btCompoundShapeChild& childShape = lsMem.gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES+i]; + + int dmaSize = getShapeTypeSize(childShape.m_childShapeType); + uint64_t dmaPpuAddress2 = (uint64_t)childShape.m_childShape; + cellDmaGet(lsMem.gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES+i], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + // Start the N^2 + for (int i = 0; i < childShapeCount0; ++i) + { + btCompoundShapeChild& childShape0 = lsMem.gSubshapes[i]; + + for (int j = 0; j < childShapeCount1; ++j) + { + btCompoundShapeChild& childShape1 = lsMem.gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES+j]; + + SpuCollisionPairInput cinput (collisionPairInput); + cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape0.m_transform; + cinput.m_shapeType0 = childShape0.m_childShapeType; + cinput.m_collisionMargin0 = childShape0.m_childMargin; + + cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape1.m_transform; + cinput.m_shapeType1 = childShape1.m_childShapeType; + cinput.m_collisionMargin1 = childShape1.m_childMargin; + + handleCollisionPair(cinput, lsMem, spuContacts, + (uint64_t)childShape0.m_childShape, lsMem.gSubshapeShape[i], + (uint64_t)childShape1.m_childShape, lsMem.gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES+i], false); + } + } + } + else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) ) + { + //snPause(); + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); + uint64_t dmaPpuAddress2 = collisionShape0Ptr; + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); + uint64_t dmaPpuAddress2 = collisionShape1Ptr; + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + } + + // object 0 compound, object 1 non-compound + btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc; + + int childShapeCount = spuCompoundShape->getNumChildShapes(); + + // dma the list of child shapes + { + int dmaSize = childShapeCount * sizeof(btCompoundShapeChild); + uint64_t dmaPpuAddress2 = (uint64_t)spuCompoundShape->getChildList(); + cellDmaGet(lsMem.gSubshapes, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + for (int i = 0; i < childShapeCount; ++i) + { + btCompoundShapeChild& childShape = lsMem.gSubshapes[i]; + + // Dma the child shape + { + int dmaSize = getShapeTypeSize(childShape.m_childShapeType); + uint64_t dmaPpuAddress2 = (uint64_t)childShape.m_childShape; + cellDmaGet(lsMem.gSubshapeShape[i], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + SpuCollisionPairInput cinput (collisionPairInput); + cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape.m_transform; + cinput.m_shapeType0 = childShape.m_childShapeType; + cinput.m_collisionMargin0 = childShape.m_childMargin; + + handleCollisionPair(cinput, lsMem, spuContacts, + (uint64_t)childShape.m_childShape, lsMem.gSubshapeShape[i], + collisionShape1Ptr, collisionShape1Loc, false); + } + } + else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) ) + { + //snPause(); + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); + uint64_t dmaPpuAddress2 = collisionShape0Ptr; + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); + uint64_t dmaPpuAddress2 = collisionShape1Ptr; + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + } + + // object 0 non-compound, object 1 compound + btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc; + + int childShapeCount = spuCompoundShape->getNumChildShapes(); + + // dma the list of child shapes + { + int dmaSize = childShapeCount * sizeof(btCompoundShapeChild); + uint64_t dmaPpuAddress2 = (uint64_t)spuCompoundShape->getChildList(); + cellDmaGet(lsMem.gSubshapes, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + for (int i = 0; i < childShapeCount; ++i) + { + btCompoundShapeChild& childShape = lsMem.gSubshapes[i]; + + // Dma the child shape + { + int dmaSize = getShapeTypeSize(childShape.m_childShapeType); + uint64_t dmaPpuAddress2 = (uint64_t)childShape.m_childShape; + cellDmaGet(lsMem.gSubshapeShape[i], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + SpuCollisionPairInput cinput (collisionPairInput); + cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape.m_transform; + cinput.m_shapeType1 = childShape.m_childShapeType; + cinput.m_collisionMargin1 = childShape.m_childMargin; + + handleCollisionPair(cinput, lsMem, spuContacts, + collisionShape0Ptr, collisionShape0Loc, + (uint64_t)childShape.m_childShape, lsMem.gSubshapeShape[i], false); + } + + } + else + { + //a non-convex shape is involved + bool handleConvexConcave = false; + + //snPause(); + + if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) && + btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) + { + // Swap stuff + DoSwap(collisionShape0Ptr, collisionShape1Ptr); + DoSwap(collisionShape0Loc, collisionShape1Loc); + DoSwap(collisionPairInput.m_shapeType0, collisionPairInput.m_shapeType1); + DoSwap(collisionPairInput.m_worldTransform0, collisionPairInput.m_worldTransform1); + DoSwap(collisionPairInput.m_collisionMargin0, collisionPairInput.m_collisionMargin1); + + collisionPairInput.m_isSwapped = true; + } + + if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)&& + btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType1)) + { + handleConvexConcave = true; + } + if (handleConvexConcave) + { + + if (dmaShapes) + { + ///dma and initialize the convex object + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); + //uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gColObj0.getCollisionShape(); + uint64_t dmaPpuAddress2 = collisionShape0Ptr; + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + ///dma and initialize the concave object + { + int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); + uint64_t dmaPpuAddress2 = collisionShape1Ptr; + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + } + } + + btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc; + btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc; + + btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions(); + collisionPairInput.m_primitiveDimensions0 = dim0; + collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr; + collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr; + collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0; + collisionPairInput.m_spuCollisionShapes[1] = trimeshShape; + + ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts); + } + + } + + spuContacts.flush(); +} void processCollisionTask(void* userPtr, void* lsMemPtr) { @@ -677,17 +1030,17 @@ void processCollisionTask(void* userPtr, void* lsMemPtr) SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr; CollisionTask_LocalStoreMemory* colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr; CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr); - -// spu_printf("taskDescPtr=%llx\n",taskDescPtr); - + + // spu_printf("taskDescPtr=%llx\n",taskDescPtr); + SpuContactResult spuContacts; -//////////////////// - + //////////////////// + uint64_t dmaInPtr = taskDesc.inPtr; unsigned int numPages = taskDesc.numPages; unsigned int numOnLastPage = taskDesc.numOnLastPage; - + // prefetch first set of inputs and wait unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage; lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); @@ -724,14 +1077,14 @@ void processCollisionTask(void* userPtr, void* lsMemPtr) int numPairs = wuInputs[j].m_endIndex - wuInputs[j].m_startIndex; if (numPairs) { - + { int dmaSize = numPairs*sizeof(btBroadphasePair); uint64_t dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair); cellDmaGet(&lsMem.gBroadphasePairs, dmaPpuAddress , dmaSize, DMA_TAG(1), 0, 0); cellDmaWaitTagStatusAll(DMA_MASK(1)); } - + for (int p=0;pm_persistentManifoldPtr); #endif //DEBUG_SPU_COLLISION_DETECTION - { - int dmaSize = sizeof(btBroadphaseProxy); - //spu_printf("dmaSize btBroadphaseProxy1 = %d\n",dmaSize); - uint64_t dmaPpuAddress2 = (uint64_t)pair.m_pProxy0; - lsMem.gProxyPtr0 = (btBroadphaseProxy*) lsMem.bufferProxy0; - //spu_printf("dmaPpuAddress2 btBroadphaseProxy1 = %llx, gProxyPtr0 = %d\n",dmaPpuAddress2,gProxyPtr0); - stallingUnalignedDmaSmallGet(lsMem.gProxyPtr0, dmaPpuAddress2 , dmaSize); - - } - { - int dmaSize = sizeof(btBroadphaseProxy); - uint64_t dmaPpuAddress2 = (uint64_t)pair.m_pProxy1; - lsMem.gProxyPtr1 = (btBroadphaseProxy*) lsMem.bufferProxy1; - stallingUnalignedDmaSmallGet(lsMem.gProxyPtr1, dmaPpuAddress2 , dmaSize); - } - - //btCollisionObject* colObj0 = (btCollisionObject*)gProxy0.m_clientObject; - //btCollisionObject* colObj1 = (btCollisionObject*)gProxy1.m_clientObject; - { - int dmaSize = sizeof(btCollisionObject); - uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gProxyPtr0->m_clientObject; - cellDmaGet(&lsMem.gColObj0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - { - int dmaSize = sizeof(btCollisionObject); - uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gProxyPtr1->m_clientObject; - cellDmaGet(&lsMem.gColObj1, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(2)); - } + int dmaSize = sizeof(btBroadphaseProxy); + //spu_printf("dmaSize btBroadphaseProxy1 = %d\n",dmaSize); + uint64_t dmaPpuAddress2 = (uint64_t)pair.m_pProxy0; + lsMem.gProxyPtr0 = (btBroadphaseProxy*) lsMem.bufferProxy0; + //spu_printf("dmaPpuAddress2 btBroadphaseProxy1 = %llx, gProxyPtr0 = %d\n",dmaPpuAddress2,gProxyPtr0); + stallingUnalignedDmaSmallGet(lsMem.gProxyPtr0, dmaPpuAddress2 , dmaSize); + + } + { + int dmaSize = sizeof(btBroadphaseProxy); + uint64_t dmaPpuAddress2 = (uint64_t)pair.m_pProxy1; + lsMem.gProxyPtr1 = (btBroadphaseProxy*) lsMem.bufferProxy1; + stallingUnalignedDmaSmallGet(lsMem.gProxyPtr1, dmaPpuAddress2 , dmaSize); + } + + //btCollisionObject* colObj0 = (btCollisionObject*)gProxy0.m_clientObject; + //btCollisionObject* colObj1 = (btCollisionObject*)gProxy1.m_clientObject; + if (1) { - - + ///can wait on the combined DMA_MASK, or dma on the same tag collisionPairInput.m_shapeType0 = lsMem.gSpuContactManifoldAlgo.getShapeType0(); @@ -809,160 +1151,32 @@ void processCollisionTask(void* userPtr, void* lsMemPtr) collisionPairInput.m_collisionMargin0 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin0(); collisionPairInput.m_collisionMargin1 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin1(); - #ifdef DEBUG_SPU_COLLISION_DETECTION +#ifdef DEBUG_SPU_COLLISION_DETECTION spu_printf("SPU collisionPairInput->m_shapeType0 = %d\n",collisionPairInput->m_shapeType0); spu_printf("SPU collisionPairInput->m_shapeType1 = %d\n",collisionPairInput->m_shapeType1); - #endif //DEBUG_SPU_COLLISION_DETECTION +#endif //DEBUG_SPU_COLLISION_DETECTION + + { + int dmaSize = sizeof(btPersistentManifold); + + uint64_t dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr; + cellDmaGet(&lsMem.gPersistentManifold, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } if (1) { + //snPause(); - collisionPairInput.m_worldTransform0 = lsMem.gColObj0.getWorldTransform(); - collisionPairInput.m_worldTransform1 = lsMem.gColObj1.getWorldTransform(); - - - - #ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU worldTrans0.origin = (%f,%f,%f)\n", - collisionPairInput->m_worldTransform0.getOrigin().getX(), - collisionPairInput->m_worldTransform0.getOrigin().getY(), - collisionPairInput->m_worldTransform0.getOrigin().getZ()); - - spu_printf("SPU worldTrans1.origin = (%f,%f,%f)\n", - collisionPairInput->m_worldTransform1.getOrigin().getX(), - collisionPairInput->m_worldTransform1.getOrigin().getY(), - collisionPairInput->m_worldTransform1.getOrigin().getZ()); - #endif //DEBUG_SPU_COLLISION_DETECTION - - - { - int dmaSize = sizeof(btPersistentManifold); - - uint64_t dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr; - if (dmaPpuAddress2 & 0x0f) - { -#ifndef IGNORE_ALIGNMENT - spu_printf("SPU ALIGNMENT ERROR\n"); - - spuContacts.flush(); - return; -#endif - } - cellDmaGet(&lsMem.gPersistentManifold, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) - && btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) - { - - { - int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); - uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gColObj0.getCollisionShape(); - cellDmaGet(lsMem.gCollisionShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - { - int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); - uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gColObj1.getCollisionShape(); - if (dmaPpuAddress2 & 0x0f) - { -#ifndef IGNORE_ALIGNMENT - spu_printf("SPU ALIGNMENT ERROR2\n"); - - spuContacts.flush(); - return; -#endif //IGNORE_ALIGNMENT - } - cellDmaGet(lsMem.gCollisionShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(2)); - } - - btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)lsMem.gCollisionShape0; - btConvexInternalShape* spuConvexShape1 = (btConvexInternalShape*)lsMem.gCollisionShape1; - - btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions(); - btVector3 dim1 = spuConvexShape1->getImplicitShapeDimensions(); - - collisionPairInput.m_primitiveDimensions0 = dim0; - collisionPairInput.m_primitiveDimensions1 = dim1; - collisionPairInput.m_collisionShapes[0] = (uint64_t)lsMem.gColObj0.getCollisionShape(); - collisionPairInput.m_collisionShapes[1] = (uint64_t)lsMem.gColObj1.getCollisionShape(); - collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0; - collisionPairInput.m_spuCollisionShapes[1] = spuConvexShape1; - ProcessSpuConvexConvexCollision(&collisionPairInput,&lsMem,spuContacts); - } else - { - //a non-convex shape is involved - - bool isSwapped = false; - bool handleConvexConcave = false; - - if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) && - btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) - { - isSwapped = true; - spu_printf("SPU convex/concave swapped, unsupported!\n"); - handleConvexConcave = true; - } - if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)&& - btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType1)) - { - handleConvexConcave = true; - } - if (handleConvexConcave && !isSwapped) - { - // spu_printf("SPU: non-convex detected\n"); - - { - // uint64_t dmaPpuAddress2 = (uint64_t)gProxy1.m_clientObject; - // spu_printf("SPU: gColObj1 trimesh = %llx\n",dmaPpuAddress2); - } - - ///dma and initialize the convex object - { - int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); - uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gColObj0.getCollisionShape(); - cellDmaGet(lsMem.gCollisionShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - ///dma and initialize the convex object - { - int dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); - uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gColObj1.getCollisionShape(); - if (dmaPpuAddress2 & 0x0f) - { -#ifndef IGNORE_ALIGNMENT - spu_printf("SPU ALIGNMENT ERROR3\n"); - - spuContacts.flush(); - return; -#endif // - } - // spu_printf("SPU: trimesh = %llx\n",dmaPpuAddress2); - cellDmaGet(lsMem.gCollisionShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(2)); - } - btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)lsMem.gCollisionShape0; - btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)lsMem.gCollisionShape1; - - btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions(); - collisionPairInput.m_primitiveDimensions0 = dim0; - collisionPairInput.m_collisionShapes[0] = (uint64_t)lsMem.gColObj0.getCollisionShape(); - collisionPairInput.m_collisionShapes[1] = (uint64_t)lsMem.gColObj1.getCollisionShape(); - collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0; - collisionPairInput.m_spuCollisionShapes[1] = trimeshShape; - - ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts); - } - - } - - spuContacts.flush(); + // Get the collision objects + dmaAndSetupCollisionObjects(collisionPairInput, lsMem); + handleCollisionPair(collisionPairInput, lsMem, spuContacts, + (uint64_t)lsMem.gColObj0.getCollisionShape(), lsMem.gCollisionShape0, + (uint64_t)lsMem.gColObj1.getCollisionShape(), lsMem.gCollisionShape1); } } - + } } } diff --git a/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index a4712b3e9..2a2c7cc63 100644 --- a/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -35,8 +35,15 @@ btCompoundShape::~btCompoundShape() void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) { - m_childTransforms.push_back(localTransform); - m_childShapes.push_back(shape); + //m_childTransforms.push_back(localTransform); + //m_childShapes.push_back(shape); + btCompoundShapeChild child; + child.m_transform = localTransform; + child.m_childShape = shape; + child.m_childShapeType = shape->getShapeType(); + child.m_childMargin = shape->getMargin(); + + m_children.push_back(child); //extend the local aabbMin/aabbMax btVector3 localAabbMin,localAabbMax; diff --git a/src/BulletCollision/CollisionShapes/btCompoundShape.h b/src/BulletCollision/CollisionShapes/btCompoundShape.h index cd2df89e0..cad3e50f6 100644 --- a/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -26,12 +26,21 @@ subject to the following restrictions: class btOptimizedBvh; +ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild +{ + btTransform m_transform; + btCollisionShape* m_childShape; + int m_childShapeType; + btScalar m_childMargin; +}; + /// btCompoundShape allows to store multiple other btCollisionShapes /// This allows for concave collision objects. This is more general then the Static Concave btTriangleMeshShape. -class btCompoundShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape { - btAlignedObjectArray m_childTransforms; - btAlignedObjectArray m_childShapes; + //btAlignedObjectArray m_childTransforms; + //btAlignedObjectArray m_childShapes; + btAlignedObjectArray m_children; btVector3 m_localAabbMin; btVector3 m_localAabbMax; @@ -46,25 +55,31 @@ public: int getNumChildShapes() const { - return int (m_childShapes.size()); + return int (m_children.size()); } btCollisionShape* getChildShape(int index) { - return m_childShapes[index]; + return m_children[index].m_childShape; } const btCollisionShape* getChildShape(int index) const { - return m_childShapes[index]; + return m_children[index].m_childShape; } - btTransform& getChildTransform(int index) + btTransform getChildTransform(int index) { - return m_childTransforms[index]; + return m_children[index].m_transform; } - const btTransform& getChildTransform(int index) const + const btTransform getChildTransform(int index) const { - return m_childTransforms[index]; + return m_children[index].m_transform; + } + + + btCompoundShapeChild* getChildList() + { + return &m_children[0]; } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version