diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp index 9bfac71e4..abec29eab 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp @@ -19,9 +19,6 @@ subject to the following restrictions: //#define DEBUG_SPU_COLLISION_DETECTION 1 -#include "SpuContactResult.h" - - SpuContactResult::SpuContactResult() { m_manifoldAddress = 0; diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h index 60e617f39..6152851f2 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h @@ -1,51 +1,51 @@ - -/* -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 CONVEX_PENETRATION_DEPTH_H -#define CONVEX_PENETRATION_DEPTH_H - - - -class btStackAlloc; -class btIDebugDraw; -class SpuVoronoiSimplexSolver; - -#include -#include - - -///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. -class SpuConvexPenetrationDepthSolver -{ -public: - - virtual ~SpuConvexPenetrationDepthSolver() {}; - virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, - void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, - btTransform& transA,const btTransform& transB, - btVector3& v, btPoint3& pa, btPoint3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, - struct SpuConvexPolyhedronVertexData* convexVertexData - ) const = 0; - - -}; - - - -#endif //CONVEX_PENETRATION_DEPTH_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 CONVEX_PENETRATION_DEPTH_H +#define CONVEX_PENETRATION_DEPTH_H + + + +class btStackAlloc; +class btIDebugDraw; +class SpuVoronoiSimplexSolver; + +#include +#include + + +///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. +class SpuConvexPenetrationDepthSolver +{ +public: + + virtual ~SpuConvexPenetrationDepthSolver() {}; + virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, + void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, + btTransform& transA,const btTransform& transB, + btVector3& v, btPoint3& pa, btPoint3& pb, + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, + struct SpuConvexPolyhedronVertexData* convexVertexData + ) const = 0; + + +}; + + + +#endif //CONVEX_PENETRATION_DEPTH_H + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp index cff34e2a2..dd3344fb2 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp @@ -1,1272 +1,1615 @@ - -#include "SpuGatheringCollisionTask.h" - -#include "../SpuDoubleBuffer.h" - -#include "../SpuCollisionTaskProcess.h" -#include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "../SpuContactManifoldCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "SpuContactResult.h" -#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" - -#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" -#include "SpuVoronoiSimplexSolver.h" - -#include "SpuLocalSupport.h" //definition of SpuConvexPolyhedronVertexData - -#ifdef __CELLOS_LV2__ -///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases -#define USE_SOFTWARE_CACHE 1 -#endif //__CELLOS_LV2__ - -//////////////////////////////////////////////// -/// software caching -#if USE_SOFTWARE_CACHE -#include -#include -#include -#include -#define SPE_CACHE_NWAY 4 -//#define SPE_CACHE_NSETS 32, 16 -#define SPE_CACHE_NSETS 8 -//#define SPE_CACHELINE_SIZE 512 -#define SPE_CACHELINE_SIZE 128 -#define SPE_CACHE_SET_TAGID(set) 15 -///make sure that spe_cache.h is below those defines! -#include "spe_cache.h" - - -int g_CacheMisses=0; -int g_CacheHits=0; - -#if 0 // Added to allow cache misses and hits to be tracked, change this to 1 to restore unmodified version -#define spe_cache_read(ea) _spe_cache_lookup_xfer_wait_(ea, 0, 1) -#else -#define spe_cache_read(ea) \ -({ \ - int set, idx, line, byte; \ - _spe_cache_nway_lookup_(ea, set, idx); \ - \ - if (unlikely(idx < 0)) { \ - ++g_CacheMisses; \ - idx = _spe_cache_miss_(ea, set, -1); \ - spu_writech(22, SPE_CACHE_SET_TAGMASK(set)); \ - spu_mfcstat(MFC_TAG_UPDATE_ALL); \ - } \ - else \ - { \ - ++g_CacheHits; \ - } \ - line = _spe_cacheline_num_(set, idx); \ - byte = _spe_cacheline_byte_offset_(ea); \ - (void *) &spe_cache_mem[line + byte]; \ -}) - -#endif - -#endif // USE_SOFTWARE_CACHE - - -#ifdef USE_SN_TUNER -#include -#endif //USE_SN_TUNER - -#ifdef WIN32 - -#define IGNORE_ALIGNMENT 1 - -#define spu_printf printf -#include -#endif - -#define MAX_SHAPE_SIZE 256 - -//int gNumConvexPoints0=0; - - - -///Make sure no destructors are called on this memory -struct CollisionTask_LocalStoreMemory -{ - - ATTRIBUTE_ALIGNED16(char bufferProxy0[16]); - ATTRIBUTE_ALIGNED16(char bufferProxy1[16]); - - ATTRIBUTE_ALIGNED16(btBroadphaseProxy* gProxyPtr0); - ATTRIBUTE_ALIGNED16(btBroadphaseProxy* gProxyPtr1); - - //ATTRIBUTE_ALIGNED16(btCollisionObject gColObj0); - //ATTRIBUTE_ALIGNED16(btCollisionObject gColObj1); - ATTRIBUTE_ALIGNED16(char gColObj0 [sizeof(btCollisionObject)+16]); - ATTRIBUTE_ALIGNED16(char gColObj1 [sizeof(btCollisionObject)+16]); - - btCollisionObject* getColObj0() - { - return (btCollisionObject*) gColObj0; - } - btCollisionObject* getColObj1() - { - return (btCollisionObject*) gColObj1; - } - - DoubleBuffer g_workUnitTaskBuffers; - ATTRIBUTE_ALIGNED16(btBroadphasePair gBroadphasePairs[SPU_BATCHSIZE_BROADPHASE_PAIRS]); - - - //SpuContactManifoldCollisionAlgorithm gSpuContactManifoldAlgo; - //ATTRIBUTE_ALIGNED16(char gSpuContactManifoldAlgo[sizeof(SpuContactManifoldCollisionAlgorithm)+128]); - - SpuContactManifoldCollisionAlgorithm gSpuContactManifoldAlgo; - - SpuContactManifoldCollisionAlgorithm* getlocalCollisionAlgorithm() - { - return (SpuContactManifoldCollisionAlgorithm*)&gSpuContactManifoldAlgo; - - } - btPersistentManifold gPersistentManifold; - - ATTRIBUTE_ALIGNED16(char gCollisionShape0[MAX_SHAPE_SIZE]); - ATTRIBUTE_ALIGNED16(char gCollisionShape1[MAX_SHAPE_SIZE]); - - ATTRIBUTE_ALIGNED16(int spuIndices[16]); - - //ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh); - ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]); - btOptimizedBvh* getOptimizedBvh() - { - return (btOptimizedBvh*) gOptimizedBvh; - } - - ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterface); - ///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 - //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]); - -}; - - - -#ifdef WIN32 -void* createCollisionLocalStoreMemory() -{ - return new CollisionTask_LocalStoreMemory; -}; - - -#elif defined(__CELLOS_LV2__) - -ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory gLocalStoreMemory); - -void* createCollisionLocalStoreMemory() -{ - return &gLocalStoreMemory; -} -#endif - - -void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts); - -#define USE_BRANCHFREE_TEST 1 -#ifdef USE_BRANCHFREE_TEST -unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) -{ - return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) - & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) - & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), - 1, 0); -} -#else -unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) -{ - unsigned int overlap = 1; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap; - return overlap; -} -#endif - - - -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; - - unsigned int aabbOverlap, isLeafNode; - - while (curIndex < endNodeIndex) - { - //catch bugs in tree data - assert (walkIterations < subTreeSize); - - 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()); - } - - if (aabbOverlap || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->getEscapeIndex(); - rootNode += escapeIndex; - 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)); - - void* ls = spe_cache_read(ea); - memcpy(buffer, ls, size); -#else - stallingUnalignedDmaSmallGet(buffer,ea,size); -#endif -} - - - -class spuNodeCallback : public btNodeOverlapCallback -{ - SpuCollisionPairInput* m_wuInput; - SpuContactResult& m_spuContacts; - CollisionTask_LocalStoreMemory* m_lsMemPtr; - - ATTRIBUTE_ALIGNED16(btVector3 spuTriangleVertices[3]); - ATTRIBUTE_ALIGNED16(btScalar spuUnscaledVertex[4]); - ATTRIBUTE_ALIGNED16(int spuIndices[16]); - - -public: - spuNodeCallback(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts) - : m_wuInput(wuInput), - m_lsMemPtr(lsMemPtr), - m_spuContacts(spuContacts) - { - } - - virtual void processNode(int subPart, int triangleIndex) - { - ///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); - - - - int* indexBasePtr = (int*)(m_lsMemPtr->gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->gIndexMesh.m_triangleIndexStride); - - ///DMA the indices - small_cache_read(&m_lsMemPtr->spuIndices[0],(uint64_t)&indexBasePtr[0],sizeof(int)); - 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); - - 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); - btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->gIndexMesh.m_vertexStride); - // 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( - 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()); - } - - - - //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(); - - ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts); - ///this flush should be automatic - // m_spuContacts.flush(); - } - -}; - - - - -//////////////////////// -/// Convex versus Concave triangle mesh collision detection (handles concave triangle mesh versus sphere, box, cylinder, triangle, cone, convex polyhedron etc) -/////////////////// -void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) -{ - //order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite - - - - - - 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()); - // spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2); - cellDmaGet(&lsMemPtr->gTriangleMeshInterface, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - ///now DMA over the BVH - { - int dmaSize = sizeof(btOptimizedBvh); - uint64_t dmaPpuAddress2 = reinterpret_cast(trimeshShape->getOptimizedBvh()); - //spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2); - cellDmaGet(&lsMemPtr->gOptimizedBvh, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - btVector3 aabbMin(-1,-400,-1); - btVector3 aabbMax(1,400,1); - - - //recalc aabbs - btTransform convexInTriangleSpace; - convexInTriangleSpace = wuInput->m_worldTransform1.inverse() * wuInput->m_worldTransform0; - btConvexInternalShape* convexShape = (btConvexInternalShape*)wuInput->m_spuCollisionShapes[0]; - //calculate the aabb, given the types... - switch (wuInput->m_shapeType0) - { - case CYLINDER_SHAPE_PROXYTYPE: - - case BOX_SHAPE_PROXYTYPE: - { - float margin=convexShape->getMarginNV(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - btTransform& t = convexInTriangleSpace; - 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)); - extent += btVector3(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - - case CAPSULE_SHAPE_PROXYTYPE: - { - float margin=convexShape->getMarginNV(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - //add the radius to y-axis to get full height - btScalar radius = halfExtents[0]; - halfExtents[1] += radius; - btTransform& t = convexInTriangleSpace; - 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)); - extent += btVector3(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - - - case SPHERE_SHAPE_PROXYTYPE: - { - float radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); - float margin = radius + convexShape->getMarginNV(); - btTransform& t = convexInTriangleSpace; - const btVector3& center = t.getOrigin(); - btVector3 extent(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - int dmaSize = sizeof(btConvexHullShape); - uint64_t dmaPpuAddress2 = wuInput->m_collisionShapes[0]; - - ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); - - cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; - btTransform& t = convexInTriangleSpace; - - 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; - } - - default: - spu_printf("SPU: unsupported shapetype %d in AABB calculation\n"); - }; - - //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; - - - - ///quantize query AABB - unsigned short int quantizedQueryAabbMin[3]; - unsigned short int quantizedQueryAabbMax[3]; - lsMemPtr->getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin); - lsMemPtr->getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax); - - QuantizedNodeArray& nodeArray = lsMemPtr->getOptimizedBvh()->getQuantizedNodeArray(); - //spu_printf("SPU: numNodes = %d\n",nodeArray.size()); - - BvhSubtreeInfoArray& subTrees = lsMemPtr->getOptimizedBvh()->getSubtreeInfoArray(); - - spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts); - IndexedMeshArray& indexArray = lsMemPtr->gTriangleMeshInterface.getIndexedMeshArray(); - //spu_printf("SPU:indexArray.size() = %d\n",indexArray.size()); - - - // spu_printf("SPU: numSubTrees = %d\n",subTrees.size()); - //not likely to happen - if (subTrees.size() && indexArray.size() == 1) - { - ///DMA in the index info - { - int dmaSize = sizeof(btIndexedMesh); - uint64_t dmaPpuAddress2 = reinterpret_cast(&indexArray[0]); - cellDmaGet(&lsMemPtr->gIndexMesh, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - //spu_printf("SPU gIndexMesh dma finished\n"); - - //display the headers - int numBatch = subTrees.size(); - for (int i=0;i(&subTrees[i]); - // 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); - - for (int j=0;jgSubtreeHeaders[j]; - - unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); - if (overlap) - { - btAssert(subtree.m_subtreeSize); - - //dma the actual nodes of this subtree - { - int dmaSize = subtree.m_subtreeSize* sizeof(btQuantizedBvhNode); - uint64_t dmaPpuAddress2 = reinterpret_cast(&nodeArray[subtree.m_rootNodeIndex]); - cellDmaGet(&lsMemPtr->gSubtreeNodes[0], dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - - - spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, - &lsMemPtr->gSubtreeNodes[0], - 0, - subtree.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; - 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) - { - case CYLINDER_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btCylinderShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - case BOX_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btBoxShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - case SPHERE_SHAPE_PROXYTYPE: - { - 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; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - 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; - } - - case COMPOUND_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btCompoundShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - - default: - btAssert(0); - //unsupported shapetype, please add here - break; - } -} - - - - -//////////////////////// -/// Convex versus Convex collision detection (handles collision between sphere, box, cylinder, triangle, cone, convex polyhedron etc) -/////////////////// -void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) -{ - -#ifdef DEBUG_SPU_COLLISION_DETECTION - //spu_printf("SPU: ProcessSpuConvexConvexCollision\n"); -#endif //DEBUG_SPU_COLLISION_DETECTION - //CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0]; - //CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1]; - btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr; - - - - bool genericGjk = true; - - - - if (genericGjk) - { - //try generic GJK - - SpuVoronoiSimplexSolver vsSolver; - SpuMinkowskiPenetrationDepthSolver penetrationSolver; - - - - ///DMA in the vertices for convex shapes - - if (wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE) - { - // spu_printf("SPU: DMA btConvexHullShape\n"); - ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); - { - int dmaSize = sizeof(btConvexHullShape); - uint64_t dmaPpuAddress2 = wuInput->m_collisionShapes[0]; - cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; - - lsMemPtr->convexVertexData.gNumConvexPoints0 = localPtr->getNumPoints(); - if (lsMemPtr->convexVertexData.gNumConvexPoints0>MAX_NUM_SPU_CONVEX_POINTS) - { - btAssert(0); - spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,lsMemPtr->convexVertexData.gNumConvexPoints0); - return; - } - - { - int dmaSize = lsMemPtr->convexVertexData.gNumConvexPoints0*sizeof(btPoint3); - uint64_t dmaPpuAddress2 = (uint64_t) localPtr->getPoints(); - cellDmaGet(&lsMemPtr->convexVertexData.g_convexPointBuffer0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - - lsMemPtr->convexVertexData.gSpuConvexShapePtr0 = wuInput->m_spuCollisionShapes[0]; - lsMemPtr->convexVertexData.gConvexPoints0 = &lsMemPtr->convexVertexData.g_convexPointBuffer0[0]; - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - } - - if (wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE) - { - - ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]); - - // spu_printf("SPU: DMA btConvexHullShape\n"); - { - int dmaSize = sizeof(btConvexHullShape); - uint64_t dmaPpuAddress2 = wuInput->m_collisionShapes[1]; - cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape1; - - lsMemPtr->convexVertexData.gNumConvexPoints1 = localPtr->getNumPoints(); - if (lsMemPtr->convexVertexData.gNumConvexPoints1>MAX_NUM_SPU_CONVEX_POINTS) - { - btAssert(0); - spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,lsMemPtr->convexVertexData.gNumConvexPoints1); - return; - } - - { - int dmaSize = lsMemPtr->convexVertexData.gNumConvexPoints1*sizeof(btPoint3); - uint64_t dmaPpuAddress2 = (uint64_t) localPtr->getPoints(); - cellDmaGet(&lsMemPtr->convexVertexData.g_convexPointBuffer1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - lsMemPtr->convexVertexData.gSpuConvexShapePtr1 = wuInput->m_spuCollisionShapes[1]; - lsMemPtr->convexVertexData.gConvexPoints1 = &lsMemPtr->convexVertexData.g_convexPointBuffer1[0]; - } - - } - - - - - - - void* shape0Ptr = wuInput->m_spuCollisionShapes[0]; - void* shape1Ptr = wuInput->m_spuCollisionShapes[1]; - int shapeType0 = wuInput->m_shapeType0; - 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+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,wuInput->m_isSwapped); - spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),lsMemPtr->getColObj1()->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.getColObj0()->getWorldTransform(); - collisionPairInput.m_worldTransform1 = lsMem.getColObj1()->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) -{ - - SpuGatherAndProcessPairsTaskDesc* taskDescPtr = (SpuGatherAndProcessPairsTaskDesc*)userPtr; - SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr; - CollisionTask_LocalStoreMemory* colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr; - CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr); - - // 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 - lsMem.g_workUnitTaskBuffers.init(); - - unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage; - lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); - dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE; - - for (unsigned int i = 0; i < numPages; i++) - { - // wait for back buffer dma and swap buffers - unsigned char *inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers(); - - // number on current page is number prefetched last iteration - unsigned int numOnPage = nextNumOnPage; - - unsigned int j; - - - // prefetch next set of inputs - if (i < numPages-1) - { - nextNumOnPage = (i == numPages-2)? numOnLastPage : MIDPHASE_NUM_WORKUNITS_PER_PAGE; - lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); - dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE; - } - - SpuGatherAndProcessWorkUnitInput* wuInputs = reinterpret_cast(inputPtr); - - for (j = 0; j < numOnPage; j++) - { -#ifdef DEBUG_SPU_COLLISION_DETECTION - printMidphaseInput(&wuInputs[j]); -#endif //DEBUG_SPU_COLLISION_DETECTION - - - 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_userInfo = %d\n",pair.m_userInfo); - spu_printf("pair->m_algorithm = %d\n",pair.m_algorithm); - spu_printf("pair->m_pProxy0 = %d\n",pair.m_pProxy0); - spu_printf("pair->m_pProxy1 = %d\n",pair.m_pProxy1); -#endif //DEBUG_SPU_COLLISION_DETECTION - - int userInfo = int(pair.m_userInfo); - - if (userInfo == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1) - { - - - { - int dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm); - uint64_t dmaPpuAddress2 = (uint64_t)pair.m_algorithm; - cellDmaGet(&lsMem.gSpuContactManifoldAlgo, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - - - SpuCollisionPairInput collisionPairInput; - collisionPairInput.m_persistentManifoldPtr = (uint64_t) lsMem.gSpuContactManifoldAlgo.getContactManifoldPtr(); - collisionPairInput.m_isSwapped = false; - //snPause(); - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: manifoldPtr: %llx",collisionPairInput->m_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; - - - if (1) - { - - ///can wait on the combined DMA_MASK, or dma on the same tag - - collisionPairInput.m_shapeType0 = lsMem.gSpuContactManifoldAlgo.getShapeType0(); - collisionPairInput.m_shapeType1 = lsMem.gSpuContactManifoldAlgo.getShapeType1(); - collisionPairInput.m_collisionMargin0 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin0(); - collisionPairInput.m_collisionMargin1 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin1(); - -#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 - - { - 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(); - - // Get the collision objects - dmaAndSetupCollisionObjects(collisionPairInput, lsMem); - - handleCollisionPair(collisionPairInput, lsMem, spuContacts, - (uint64_t)lsMem.getColObj0()->getCollisionShape(), lsMem.gCollisionShape0, - (uint64_t)lsMem.getColObj1()->getCollisionShape(), lsMem.gCollisionShape1); - } - } - - } - } - } - } //end for (j = 0; j < numOnPage; j++) - - }// for - - - return; -} + +#include "SpuGatheringCollisionTask.h" + +#include "../SpuDoubleBuffer.h" + +#include "../SpuCollisionTaskProcess.h" +#include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS + +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "../SpuContactManifoldCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "SpuContactResult.h" +#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" + +#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" +#include "SpuVoronoiSimplexSolver.h" + +#include "SpuLocalSupport.h" //definition of SpuConvexPolyhedronVertexData + +#ifdef __CELLOS_LV2__ +///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases +#define USE_SOFTWARE_CACHE 1 +#endif //__CELLOS_LV2__ + +//////////////////////////////////////////////// +/// software caching +#if USE_SOFTWARE_CACHE +#include +#include +#include +#include +#define SPE_CACHE_NWAY 4 +//#define SPE_CACHE_NSETS 32, 16 +#define SPE_CACHE_NSETS 8 +//#define SPE_CACHELINE_SIZE 512 +#define SPE_CACHELINE_SIZE 128 +#define SPE_CACHE_SET_TAGID(set) 15 +///make sure that spe_cache.h is below those defines! +#include "spe_cache.h" + + +int g_CacheMisses=0; +int g_CacheHits=0; + +#if 0 // Added to allow cache misses and hits to be tracked, change this to 1 to restore unmodified version +#define spe_cache_read(ea) _spe_cache_lookup_xfer_wait_(ea, 0, 1) +#else +#define spe_cache_read(ea) \ +({ \ + int set, idx, line, byte; \ + _spe_cache_nway_lookup_(ea, set, idx); \ + \ + if (btUnlikely(idx < 0)) { \ + ++g_CacheMisses; \ + idx = _spe_cache_miss_(ea, set, -1); \ + spu_writech(22, SPE_CACHE_SET_TAGMASK(set)); \ + spu_mfcstat(MFC_TAG_UPDATE_ALL); \ + } \ + else \ + { \ + ++g_CacheHits; \ + } \ + line = _spe_cacheline_num_(set, idx); \ + byte = _spe_cacheline_byte_offset_(ea); \ + (void *) &spe_cache_mem[line + byte]; \ +}) + +#endif + +#endif // USE_SOFTWARE_CACHE + + +#ifdef USE_SN_TUNER +#include +#endif //USE_SN_TUNER + +#ifdef WIN32 +#define IGNORE_ALIGNMENT 1 +#define spu_printf printf +#include +#endif + +#define MAX_SHAPE_SIZE 256 + +//int gNumConvexPoints0=0; + + + +///Make sure no destructors are called on this memory +struct CollisionTask_LocalStoreMemory +{ + + ATTRIBUTE_ALIGNED16(char bufferProxy0[16]); + ATTRIBUTE_ALIGNED16(char bufferProxy1[16]); + + ATTRIBUTE_ALIGNED16(btBroadphaseProxy* gProxyPtr0); + ATTRIBUTE_ALIGNED16(btBroadphaseProxy* gProxyPtr1); + + //ATTRIBUTE_ALIGNED16(btCollisionObject gColObj0); + //ATTRIBUTE_ALIGNED16(btCollisionObject gColObj1); + ATTRIBUTE_ALIGNED16(char gColObj0 [sizeof(btCollisionObject)+16]); + ATTRIBUTE_ALIGNED16(char gColObj1 [sizeof(btCollisionObject)+16]); + + btCollisionObject* getColObj0() + { + return (btCollisionObject*) gColObj0; + } + btCollisionObject* getColObj1() + { + return (btCollisionObject*) gColObj1; + } + + DoubleBuffer g_workUnitTaskBuffers; + ATTRIBUTE_ALIGNED16(btBroadphasePair gBroadphasePairs[SPU_BATCHSIZE_BROADPHASE_PAIRS]); + + + //SpuContactManifoldCollisionAlgorithm gSpuContactManifoldAlgo; + //ATTRIBUTE_ALIGNED16(char gSpuContactManifoldAlgo[sizeof(SpuContactManifoldCollisionAlgorithm)+128]); + + SpuContactManifoldCollisionAlgorithm gSpuContactManifoldAlgo; + + SpuContactManifoldCollisionAlgorithm* getlocalCollisionAlgorithm() + { + return (SpuContactManifoldCollisionAlgorithm*)&gSpuContactManifoldAlgo; + + } + btPersistentManifold gPersistentManifold; + + ATTRIBUTE_ALIGNED16(char gCollisionShape0[MAX_SHAPE_SIZE]); + ATTRIBUTE_ALIGNED16(char gCollisionShape1[MAX_SHAPE_SIZE]); + + ATTRIBUTE_ALIGNED16(int spuIndices[16]); + + //ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh); + ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]); + btOptimizedBvh* getOptimizedBvh() + { + return (btOptimizedBvh*) gOptimizedBvh; + } + + ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterface); + ///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 + //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]); + +}; + + + +#ifdef WIN32 +void* createCollisionLocalStoreMemory() +{ + return new CollisionTask_LocalStoreMemory; +}; + + +#elif defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) + +ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory gLocalStoreMemory); + +void* createCollisionLocalStoreMemory() +{ + return &gLocalStoreMemory; +} +#endif + + +void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts); + +#define USE_BRANCHFREE_TEST 1 +#ifdef USE_BRANCHFREE_TEST +unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) +{ + return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) + & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) + & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0); +} +#else + +unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) +{ + unsigned int overlap = 1; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap; + return overlap; +} +#endif + + + +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; + + unsigned int aabbOverlap, isLeafNode; + + while (curIndex < endNodeIndex) + { + //catch bugs in tree data + assert (walkIterations < subTreeSize); + + 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()); + } + + if (aabbOverlap || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + +} + + +#ifdef USE_ADDR64 +SIMD_FORCE_INLINE void small_cache_read(void* buffer, uint64_t ea, size_t size) +#else +SIMD_FORCE_INLINE void small_cache_read(void* buffer, uint32_t ea, size_t size) +#endif +{ +#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)); + + void* ls = spe_cache_read(ea); + memcpy(buffer, ls, size); +#else + stallingUnalignedDmaSmallGet(buffer,ea,size); +#endif +} + + +#ifdef USE_LIBSPE2 +#ifdef USE_ADDR64 +SIMD_FORCE_INLINE void small_cache_read_triple( void* ls0, uint64_t ea0, + void* ls1, uint64_t ea1, + void* ls2, uint64_t ea2, + size_t size) +#else +SIMD_FORCE_INLINE void small_cache_read_triple( void* ls0, uint32_t ea0, + void* ls1, uint32_t ea1, + void* ls2, uint32_t ea2, + size_t size) +#endif +{ + btAssert(size<16); + ATTRIBUTE_ALIGNED16(char tmpBuffer0[32]); + ATTRIBUTE_ALIGNED16(char tmpBuffer1[32]); + ATTRIBUTE_ALIGNED16(char tmpBuffer2[32]); + + uint32_t i; + + + ///make sure last 4 bits are the same, for cellDmaSmallGet + char* localStore0 = (char*)ls0; + uint32_t last4BitsOffset = ea0 & 0x0f; + char* tmpTarget0 = tmpBuffer0 + last4BitsOffset; + cellDmaSmallGet(tmpTarget0,ea0,size,DMA_TAG(1),0,0); + + + char* localStore1 = (char*)ls1; + last4BitsOffset = ea1 & 0x0f; + char* tmpTarget1 = tmpBuffer1 + last4BitsOffset; + cellDmaSmallGet(tmpTarget1,ea1,size,DMA_TAG(1),0,0); + + char* localStore2 = (char*)ls2; + last4BitsOffset = ea2 & 0x0f; + char* tmpTarget2 = tmpBuffer2 + last4BitsOffset; + cellDmaSmallGet(tmpTarget2,ea2,size,DMA_TAG(1),0,0); + + + cellDmaWaitTagStatusAll( DMA_MASK(1) ); + + //this is slowish, perhaps memcpy on SPU is smarter? + for (i=0; btLikely( igIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->gIndexMesh.m_triangleIndexStride); + + ///DMA the indices +#ifdef USE_LIBSPE2 +#ifdef USE_ADDR64 + small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(uint64_t)&indexBasePtr[0], + &m_lsMemPtr->spuIndices[1],(uint64_t)&indexBasePtr[1], + &m_lsMemPtr->spuIndices[2],(uint64_t)&indexBasePtr[2], + sizeof(int)); +#else + small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(uint32_t)&indexBasePtr[0], + &m_lsMemPtr->spuIndices[1],(uint32_t)&indexBasePtr[1], + &m_lsMemPtr->spuIndices[2],(uint32_t)&indexBasePtr[2], + sizeof(int)); +#endif +#else +#ifdef USE_ADDR64 + small_cache_read(&m_lsMemPtr->spuIndices[0],(uint64_t)&indexBasePtr[0],sizeof(int)); + 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)); +#else + small_cache_read(&m_lsMemPtr->spuIndices[0],(uint32_t)&indexBasePtr[0],sizeof(int)); + small_cache_read(&m_lsMemPtr->spuIndices[1],(uint32_t)&indexBasePtr[1],sizeof(int)); + small_cache_read(&m_lsMemPtr->spuIndices[2],(uint32_t)&indexBasePtr[2],sizeof(int)); +#endif +#endif + + // 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;btLikely( j>=0 );j--) + { + int graphicsindex = m_lsMemPtr->spuIndices[j]; + + // 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); + + + ///handle un-aligned vertices... + + //another DMA for each vertex +#ifdef USE_LIBSPE2 +#ifdef USE_ADDR64 + small_cache_read_triple( &spuUnscaledVertex[0],(uint64_t)&graphicsbasePtr[0], + &spuUnscaledVertex[1],(uint64_t)&graphicsbasePtr[1], + &spuUnscaledVertex[2],(uint64_t)&graphicsbasePtr[2], + sizeof(btScalar)); +#else + small_cache_read_triple( &spuUnscaledVertex[0],(uint32_t)&graphicsbasePtr[0], + &spuUnscaledVertex[1],(uint32_t)&graphicsbasePtr[1], + &spuUnscaledVertex[2],(uint32_t)&graphicsbasePtr[2], + sizeof(btScalar)); +#endif +#else +#ifdef USE_ADDR64 + 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)); +#else + small_cache_read(&spuUnscaledVertex[0],(uint32_t)&graphicsbasePtr[0],sizeof(btScalar)); + small_cache_read(&spuUnscaledVertex[1],(uint32_t)&graphicsbasePtr[1],sizeof(btScalar)); + small_cache_read(&spuUnscaledVertex[2],(uint32_t)&graphicsbasePtr[2],sizeof(btScalar)); +#endif +#endif + + 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()); + } + + + + //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(); + + ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts); + ///this flush should be automatic + // m_spuContacts.flush(); + } + +}; + + + + +//////////////////////// +/// Convex versus Concave triangle mesh collision detection (handles concave triangle mesh versus sphere, box, cylinder, triangle, cone, convex polyhedron etc) +/////////////////// +void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) +{ + //order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite + + + register int dmaSize; +#ifdef USE_ADDR64 + register uint64_t dmaPpuAddress2; +#else + register uint32_t dmaPpuAddress2; +#endif + + + btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1]; + //need the mesh interface, for access to triangle vertices + + dmaSize = sizeof(btTriangleIndexVertexArray); +#ifdef USE_ADDR64 + dmaPpuAddress2 = reinterpret_cast(trimeshShape->getMeshInterface()); +#else + dmaPpuAddress2 = reinterpret_cast(trimeshShape->getMeshInterface()); +#endif + // spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2); + cellDmaGet(&lsMemPtr->gTriangleMeshInterface, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + ///now DMA over the BVH + + dmaSize = sizeof(btOptimizedBvh); +#ifdef USE_ADDR64 + dmaPpuAddress2 = reinterpret_cast(trimeshShape->getOptimizedBvh()); +#else + dmaPpuAddress2 = reinterpret_cast(trimeshShape->getOptimizedBvh()); +#endif + //spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2); + cellDmaGet(&lsMemPtr->gOptimizedBvh, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(2)); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + + btVector3 aabbMin(-1,-400,-1); + btVector3 aabbMax(1,400,1); + + + //recalc aabbs + btTransform convexInTriangleSpace; + convexInTriangleSpace = wuInput->m_worldTransform1.inverse() * wuInput->m_worldTransform0; + btConvexInternalShape* convexShape = (btConvexInternalShape*)wuInput->m_spuCollisionShapes[0]; + //calculate the aabb, given the types... + switch (wuInput->m_shapeType0) + { + case CYLINDER_SHAPE_PROXYTYPE: + + case BOX_SHAPE_PROXYTYPE: + { + float margin=convexShape->getMarginNV(); + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + btTransform& t = convexInTriangleSpace; + 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)); + extent += btVector3(margin,margin,margin); + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + + case CAPSULE_SHAPE_PROXYTYPE: + { + float margin=convexShape->getMarginNV(); + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + //add the radius to y-axis to get full height + btScalar radius = halfExtents[0]; + halfExtents[1] += radius; + btTransform& t = convexInTriangleSpace; + 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)); + extent += btVector3(margin,margin,margin); + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + + + case SPHERE_SHAPE_PROXYTYPE: + { + float radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); + float margin = radius + convexShape->getMarginNV(); + btTransform& t = convexInTriangleSpace; + const btVector3& center = t.getOrigin(); + btVector3 extent(margin,margin,margin); + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + dmaSize = sizeof(btConvexHullShape); +#ifdef USE_ADDR64 + dmaPpuAddress2 = wuInput->m_collisionShapes[0]; +#else + dmaPpuAddress2 = wuInput->m_collisionShapes[0]; +#endif + ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); + + cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; + btTransform& t = convexInTriangleSpace; + + 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; + } + + default: + spu_printf("SPU: unsupported shapetype %d in AABB calculation\n"); + }; + + //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; + + + + ///quantize query AABB + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + lsMemPtr->getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin); + lsMemPtr->getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax); + + QuantizedNodeArray& nodeArray = lsMemPtr->getOptimizedBvh()->getQuantizedNodeArray(); + //spu_printf("SPU: numNodes = %d\n",nodeArray.size()); + + BvhSubtreeInfoArray& subTrees = lsMemPtr->getOptimizedBvh()->getSubtreeInfoArray(); + + spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts); + IndexedMeshArray& indexArray = lsMemPtr->gTriangleMeshInterface.getIndexedMeshArray(); + //spu_printf("SPU:indexArray.size() = %d\n",indexArray.size()); + + + // spu_printf("SPU: numSubTrees = %d\n",subTrees.size()); + //not likely to happen + if (subTrees.size() && indexArray.size() == 1) + { + ///DMA in the index info + + dmaSize = sizeof(btIndexedMesh); +#ifdef USE_ADDR64 + dmaPpuAddress2 = reinterpret_cast(&indexArray[0]); +#else + dmaPpuAddress2 = reinterpret_cast(&indexArray[0]); +#endif + cellDmaGet(&lsMemPtr->gIndexMesh, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + //spu_printf("SPU gIndexMesh dma finished\n"); + + //display the headers + int numBatch = subTrees.size(); + for (int i=0;i(&subTrees[i]); +#else + dmaPpuAddress2 = reinterpret_cast(&subTrees[i]); +#endif + // 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); + + for (int j=0;jgSubtreeHeaders[j]; + + unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap) + { + btAssert(subtree.m_subtreeSize); + + //dma the actual nodes of this subtree + + dmaSize = subtree.m_subtreeSize* sizeof(btQuantizedBvhNode); +#ifdef USE_ADDR64 + dmaPpuAddress2 = reinterpret_cast(&nodeArray[subtree.m_rootNodeIndex]); +#else + dmaPpuAddress2 = reinterpret_cast(&nodeArray[subtree.m_rootNodeIndex]); +#endif + cellDmaGet(&lsMemPtr->gSubtreeNodes[0], dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + + + + + spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, + &lsMemPtr->gSubtreeNodes[0], + 0, + subtree.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; + i+=nextBatch; + } + + //pre-fetch first tree, then loop and double buffer + + + + } + +} + +///getShapeTypeSize could easily be optimized, but it is not likely a bottleneck +SIMD_FORCE_INLINE int getShapeTypeSize(int shapeType) +{ + + + switch (shapeType) + { + case CYLINDER_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btCylinderShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + case BOX_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btBoxShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + case SPHERE_SHAPE_PROXYTYPE: + { + 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; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + 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; + } + + case COMPOUND_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btCompoundShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + + default: + btAssert(0); + //unsupported shapetype, please add here + break; + } +} + + + + +//////////////////////// +/// Convex versus Convex collision detection (handles collision between sphere, box, cylinder, triangle, cone, convex polyhedron etc) +/////////////////// +void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) +{ + + + register int dmaSize; +#ifdef USE_ADDR64 + register uint64_t dmaPpuAddress2; +#else + register uint32_t dmaPpuAddress2; +#endif + +#ifdef DEBUG_SPU_COLLISION_DETECTION + //spu_printf("SPU: ProcessSpuConvexConvexCollision\n"); +#endif //DEBUG_SPU_COLLISION_DETECTION + //CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0]; + //CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1]; + btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr; + + + + bool genericGjk = true; + + + + if (genericGjk) + { + //try generic GJK + + SpuVoronoiSimplexSolver vsSolver; + SpuMinkowskiPenetrationDepthSolver penetrationSolver; + + + + ///DMA in the vertices for convex shapes + ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); + ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]); + + if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + // spu_printf("SPU: DMA btConvexHullShape\n"); + + dmaSize = sizeof(btConvexHullShape); +#ifdef USE_ADDR64 + dmaPpuAddress2 = wuInput->m_collisionShapes[0]; +#else + dmaPpuAddress2 = wuInput->m_collisionShapes[0]; +#endif + cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + + + if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + + + // spu_printf("SPU: DMA btConvexHullShape\n"); + dmaSize = sizeof(btConvexHullShape); +#ifdef USE_ADDR64 + dmaPpuAddress2 = wuInput->m_collisionShapes[1]; +#else + dmaPpuAddress2 = wuInput->m_collisionShapes[1]; +#endif + cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + + + if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + + cellDmaWaitTagStatusAll(DMA_MASK(1)); + btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; + + lsMemPtr->convexVertexData.gNumConvexPoints0 = localPtr->getNumPoints(); + if (lsMemPtr->convexVertexData.gNumConvexPoints0>MAX_NUM_SPU_CONVEX_POINTS) + { + btAssert(0); + spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,lsMemPtr->convexVertexData.gNumConvexPoints0); + return; + } + + dmaSize = lsMemPtr->convexVertexData.gNumConvexPoints0*sizeof(btPoint3); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t) localPtr->getPoints(); +#else + dmaPpuAddress2 = (uint32_t) localPtr->getPoints(); +#endif + cellDmaGet(&lsMemPtr->convexVertexData.g_convexPointBuffer0, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + + lsMemPtr->convexVertexData.gSpuConvexShapePtr0 = wuInput->m_spuCollisionShapes[0]; + + + } + + + if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + + cellDmaWaitTagStatusAll(DMA_MASK(1)); + btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape1; + + lsMemPtr->convexVertexData.gNumConvexPoints1 = localPtr->getNumPoints(); + if (lsMemPtr->convexVertexData.gNumConvexPoints1>MAX_NUM_SPU_CONVEX_POINTS) + { + btAssert(0); + spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,lsMemPtr->convexVertexData.gNumConvexPoints1); + return; + } + + + dmaSize = lsMemPtr->convexVertexData.gNumConvexPoints1*sizeof(btPoint3); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t) localPtr->getPoints(); +#else + dmaPpuAddress2 = (uint32_t) localPtr->getPoints(); +#endif + cellDmaGet(&lsMemPtr->convexVertexData.g_convexPointBuffer1, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + + lsMemPtr->convexVertexData.gSpuConvexShapePtr1 = wuInput->m_spuCollisionShapes[1]; + + + } + + if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + cellDmaWaitTagStatusAll(DMA_MASK(2)); + + lsMemPtr->convexVertexData.gConvexPoints0 = &lsMemPtr->convexVertexData.g_convexPointBuffer0[0]; + } + + if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + cellDmaWaitTagStatusAll(DMA_MASK(2)); + + lsMemPtr->convexVertexData.gConvexPoints1 = &lsMemPtr->convexVertexData.g_convexPointBuffer1[0]; + } + + + void* shape0Ptr = wuInput->m_spuCollisionShapes[0]; + void* shape1Ptr = wuInput->m_spuCollisionShapes[1]; + int shapeType0 = wuInput->m_shapeType0; + 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+lsMemPtr->gPersistentManifold.getContactBreakingThreshold()); + cpInput.m_maximumDistanceSquared = sumMargin * sumMargin; + +#ifdef USE_ADDR64 + uint64_t manifoldAddress = (uint64_t)manifold; +#else + uint32_t manifoldAddress = (uint32_t)manifold; +#endif + btPersistentManifold* spuManifold=&lsMemPtr->gPersistentManifold; + //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped); + spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),lsMemPtr->getColObj1()->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)); +} + +SIMD_FORCE_INLINE void dmaAndSetupCollisionObjects(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem) +{ + register int dmaSize; +#ifdef USE_ADDR64 + register uint64_t dmaPpuAddress2; +#else + register uint32_t dmaPpuAddress2; +#endif + + + dmaSize = sizeof(btCollisionObject); +#ifdef USE_ADDR64 + dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (uint64_t)lsMem.gProxyPtr1->m_clientObject :*/ (uint64_t)lsMem.gProxyPtr0->m_clientObject; +#else + dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (uint32_t)lsMem.gProxyPtr1->m_clientObject :*/ (uint32_t)lsMem.gProxyPtr0->m_clientObject; +#endif + cellDmaGet(&lsMem.gColObj0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + + + dmaSize = sizeof(btCollisionObject); +#ifdef USE_ADDR64 + dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (uint64_t)lsMem.gProxyPtr0->m_clientObject :*/ (uint64_t)lsMem.gProxyPtr1->m_clientObject; +#else + dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (uint32_t)lsMem.gProxyPtr0->m_clientObject :*/ (uint32_t)lsMem.gProxyPtr1->m_clientObject; +#endif + cellDmaGet(&lsMem.gColObj1, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + + + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + collisionPairInput.m_worldTransform0 = lsMem.getColObj0()->getWorldTransform(); + collisionPairInput.m_worldTransform1 = lsMem.getColObj1()->getWorldTransform(); + + + +#ifdef DEBUG_SPU_COLLISION_DETECTION +#endif //DEBUG_SPU_COLLISION_DETECTION + +} + + + +#ifdef USE_ADDR64 +void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem, + SpuContactResult &spuContacts, + uint64_t collisionShape0Ptr, void* collisionShape0Loc, + uint64_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true) +#else +void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem, + SpuContactResult &spuContacts, + uint32_t collisionShape0Ptr, void* collisionShape0Loc, + uint32_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true) +#endif +{ + register int dmaSize; +#ifdef USE_ADDR64 + register uint64_t dmaPpuAddress2; +#else + register uint32_t dmaPpuAddress2; +#endif + + if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) + && btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) + { + + //dmaAndSetupCollisionObjects(collisionPairInput, lsMem); + + if (dmaShapes) + { + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); + //uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gColObj0.getCollisionShape(); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape0Ptr; +#else + dmaPpuAddress2 = collisionShape0Ptr; +#endif + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape1Ptr; +#else + dmaPpuAddress2 = collisionShape1Ptr; +#endif + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(2)); + + cellDmaWaitTagStatusAll(DMA_MASK(1) | 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 + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape0Ptr; +#else + dmaPpuAddress2 = collisionShape0Ptr; +#endif + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape1Ptr; +#else + dmaPpuAddress2 = collisionShape1Ptr; +#endif + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(2)); + + cellDmaWaitTagStatusAll(DMA_MASK(1) | 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 + + dmaSize = childShapeCount0 * sizeof(btCompoundShapeChild); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)spuCompoundShape0->getChildList(); +#else + dmaPpuAddress2 = (uint32_t)spuCompoundShape0->getChildList(); +#endif + cellDmaGet(lsMem.gSubshapes, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + // dma the second list of child shapes + + dmaSize = childShapeCount1 * sizeof(btCompoundShapeChild); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)spuCompoundShape1->getChildList(); +#else + dmaPpuAddress2 = (uint32_t)spuCompoundShape1->getChildList(); +#endif + cellDmaGet(&lsMem.gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES], dmaPpuAddress2, dmaSize, DMA_TAG(2), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(2)); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + + // DMA all the subshapes + for (int i = 0; i < childShapeCount0; ++i) + { + btCompoundShapeChild& childShape = lsMem.gSubshapes[i]; + + dmaSize = getShapeTypeSize(childShape.m_childShapeType); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)childShape.m_childShape; +#else + dmaPpuAddress2 = (uint32_t)childShape.m_childShape; +#endif + cellDmaGet(lsMem.gSubshapeShape[i], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + for (int i = 0; i < childShapeCount1; ++i) + { + btCompoundShapeChild& childShape = lsMem.gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES+i]; + + dmaSize = getShapeTypeSize(childShape.m_childShapeType); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)childShape.m_childShape; +#else + dmaPpuAddress2 = (uint32_t)childShape.m_childShape; +#endif + cellDmaGet(lsMem.gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES+i], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + 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; + +#ifdef USE_ADDR64 + 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 + handleCollisionPair(cinput, lsMem, spuContacts, + (uint32_t)childShape0.m_childShape, lsMem.gSubshapeShape[i], + (uint32_t)childShape1.m_childShape, lsMem.gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES+i], false); +#endif + } + } + } + else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) ) + { + //snPause(); + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape0Ptr; +#else + dmaPpuAddress2 = collisionShape0Ptr; +#endif + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape1Ptr; +#else + dmaPpuAddress2 = collisionShape1Ptr; +#endif + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); +// cellDmaWaitTagStatusAll(DMA_MASK(2)); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + + // object 0 compound, object 1 non-compound + btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc; + + int childShapeCount = spuCompoundShape->getNumChildShapes(); + + // dma the list of child shapes + + dmaSize = childShapeCount * sizeof(btCompoundShapeChild); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)spuCompoundShape->getChildList(); +#else + dmaPpuAddress2 = (uint32_t)spuCompoundShape->getChildList(); +#endif + 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 + + dmaSize = getShapeTypeSize(childShape.m_childShapeType); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)childShape.m_childShape; +#else + dmaPpuAddress2 = (uint32_t)childShape.m_childShape; +#endif + 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; + +#ifdef USE_ADDR64 + handleCollisionPair(cinput, lsMem, spuContacts, + (uint64_t)childShape.m_childShape, lsMem.gSubshapeShape[i], + collisionShape1Ptr, collisionShape1Loc, false); +#else + handleCollisionPair(cinput, lsMem, spuContacts, + (uint32_t)childShape.m_childShape, lsMem.gSubshapeShape[i], + collisionShape1Ptr, collisionShape1Loc, false); +#endif + } + } + else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) ) + { + //snPause(); + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape0Ptr; +#else + dmaPpuAddress2 = collisionShape0Ptr; +#endif + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape1Ptr; +#else + dmaPpuAddress2 = collisionShape1Ptr; +#endif + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(2)); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + + // object 0 non-compound, object 1 compound + btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc; + + int childShapeCount = spuCompoundShape->getNumChildShapes(); + + // dma the list of child shapes + + dmaSize = childShapeCount * sizeof(btCompoundShapeChild); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)spuCompoundShape->getChildList(); +#else + dmaPpuAddress2 = (uint32_t)spuCompoundShape->getChildList(); +#endif + 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 + + dmaSize = getShapeTypeSize(childShape.m_childShapeType); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)childShape.m_childShape; +#else + dmaPpuAddress2 = (uint32_t)childShape.m_childShape; +#endif + 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; + +#ifdef USE_ADDR64 + handleCollisionPair(cinput, lsMem, spuContacts, + collisionShape0Ptr, collisionShape0Loc, + (uint64_t)childShape.m_childShape, lsMem.gSubshapeShape[i], false); +#else + handleCollisionPair(cinput, lsMem, spuContacts, + collisionShape0Ptr, collisionShape0Loc, + (uint32_t)childShape.m_childShape, lsMem.gSubshapeShape[i], false); +#endif + } + + } + 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 + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType0); + //uint64_t dmaPpuAddress2 = (uint64_t)lsMem.gColObj0.getCollisionShape(); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape0Ptr; +#else + dmaPpuAddress2 = collisionShape0Ptr; +#endif + cellDmaGet(collisionShape0Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + ///dma and initialize the concave object + + dmaSize = getShapeTypeSize(collisionPairInput.m_shapeType1); +#ifdef USE_ADDR64 + dmaPpuAddress2 = collisionShape1Ptr; +#else + dmaPpuAddress2 = collisionShape1Ptr; +#endif + cellDmaGet(collisionShape1Loc, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(2)); + cellDmaWaitTagStatusAll(DMA_MASK(1) | 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) +{ + + SpuGatherAndProcessPairsTaskDesc* taskDescPtr = (SpuGatherAndProcessPairsTaskDesc*)userPtr; + SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr; + CollisionTask_LocalStoreMemory* colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr; + CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr); + + // 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 + lsMem.g_workUnitTaskBuffers.init(); + + unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage; + lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); + dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE; + + + register unsigned char *inputPtr; + register unsigned int numOnPage; + register unsigned int j; + SpuGatherAndProcessWorkUnitInput* wuInputs; + register int dmaSize; +#ifdef USE_ADDR64 + register uint64_t dmaPpuAddress; + register uint64_t dmaPpuAddress2; +#else + register uint32_t dmaPpuAddress; + register uint32_t dmaPpuAddress2; +#endif + int userInfo; + int numPairs; + register int p; + SpuCollisionPairInput collisionPairInput; + + for (unsigned int i = 0; btLikely(i < numPages); i++) + { + + // wait for back buffer dma and swap buffers + inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers(); + + // number on current page is number prefetched last iteration + numOnPage = nextNumOnPage; + + + // prefetch next set of inputs +#if MIDPHASE_NUM_WORKUNIT_PAGES > 2 + if ( btLikely( i < numPages-1 ) ) +#else + if ( btUnlikely( i < numPages-1 ) ) +#endif + { + nextNumOnPage = (i == numPages-2)? numOnLastPage : MIDPHASE_NUM_WORKUNITS_PER_PAGE; + lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); + dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE; + } + + wuInputs = reinterpret_cast(inputPtr); + + + for (j = 0; btLikely( j < numOnPage ); j++) + { +#ifdef DEBUG_SPU_COLLISION_DETECTION + // printMidphaseInput(&wuInputs[j]); +#endif //DEBUG_SPU_COLLISION_DETECTION + + + numPairs = wuInputs[j].m_endIndex - wuInputs[j].m_startIndex; + + if ( btLikely( numPairs ) ) + { + dmaSize = numPairs*sizeof(btBroadphasePair); +#ifdef USE_ADDR64 + dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair); +#else + dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair); +#endif + cellDmaGet(&lsMem.gBroadphasePairs, dmaPpuAddress , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + for (p=0;pm_userInfo = %d\n",pair.m_userInfo); + spu_printf("pair->m_algorithm = %d\n",pair.m_algorithm); + spu_printf("pair->m_pProxy0 = %d\n",pair.m_pProxy0); + spu_printf("pair->m_pProxy1 = %d\n",pair.m_pProxy1); +#endif //DEBUG_SPU_COLLISION_DETECTION + + userInfo = int(pair.m_userInfo); + + if (userInfo == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1) + { + + + + dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)pair.m_algorithm; +#else + dmaPpuAddress2 = (uint32_t)pair.m_algorithm; +#endif + cellDmaGet(&lsMem.gSpuContactManifoldAlgo, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + + + //snPause(); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + //spu_printf("SPU: manifoldPtr: %llx",collisionPairInput->m_persistentManifoldPtr); +#endif //DEBUG_SPU_COLLISION_DETECTION + + + dmaSize = sizeof(btBroadphaseProxy); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)pair.m_pProxy0; +#else + dmaPpuAddress2 = (uint32_t)pair.m_pProxy0; +#endif + lsMem.gProxyPtr0 = (btBroadphaseProxy*) lsMem.bufferProxy0; + stallingUnalignedDmaSmallGet(lsMem.gProxyPtr0, dmaPpuAddress2 , dmaSize); + + collisionPairInput.m_persistentManifoldPtr = (uint64_t) lsMem.gSpuContactManifoldAlgo.getContactManifoldPtr(); + collisionPairInput.m_isSwapped = false; + + + dmaSize = sizeof(btBroadphaseProxy); +#ifdef USE_ADDR64 + dmaPpuAddress2 = (uint64_t)pair.m_pProxy1; +#else + dmaPpuAddress2 = (uint32_t)pair.m_pProxy1; +#endif + 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 + + +#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 + + + dmaSize = sizeof(btPersistentManifold); + + dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr; + cellDmaGet(&lsMem.gPersistentManifold, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + + collisionPairInput.m_shapeType0 = lsMem.gSpuContactManifoldAlgo.getShapeType0(); + collisionPairInput.m_shapeType1 = lsMem.gSpuContactManifoldAlgo.getShapeType1(); + collisionPairInput.m_collisionMargin0 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin0(); + collisionPairInput.m_collisionMargin1 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin1(); + + + + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + if (1) + { + //snPause(); + + // Get the collision objects + dmaAndSetupCollisionObjects(collisionPairInput, lsMem); +#ifdef USE_ADDR64 + handleCollisionPair(collisionPairInput, lsMem, spuContacts, + (uint64_t)lsMem.getColObj0()->getCollisionShape(), lsMem.gCollisionShape0, + (uint64_t)lsMem.getColObj1()->getCollisionShape(), lsMem.gCollisionShape1); +#else + handleCollisionPair(collisionPairInput, lsMem, spuContacts, + (uint32_t)lsMem.getColObj0()->getCollisionShape(), lsMem.gCollisionShape0, + (uint32_t)lsMem.getColObj1()->getCollisionShape(), lsMem.gCollisionShape1); +#endif + } + } + + } + } + } + } //end for (j = 0; j < numOnPage; j++) + + }// for + + + return; +} diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h index 45c99baea..c84fc0856 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h @@ -1,25 +1,128 @@ -/* -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_GATHERING_COLLISION_TASK_H -#define SPU_GATHERING_COLLISION_TASK_H - -struct SpuGatherAndProcessPairsTaskDesc; - -void processCollisionTask(void* userPtr, void* lsMemory); - -void* createCollisionLocalStoreMemory(); - -#endif //SPU_GATHERING_COLLISION_TASK_H \ No newline at end of file +/* +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_GATHERING_COLLISION_TASK_H +#define SPU_GATHERING_COLLISION_TASK_H + +#include "../PlatformDefinitions.h" +//#define DEBUG_SPU_COLLISION_DETECTION 1 + + +///Task Description for SPU collision detection +struct SpuGatherAndProcessPairsTaskDesc +{ + uint64_t inPtr;//m_pairArrayPtr; + //mutex variable + uint32_t m_someMutexVariableInMainMemory; + + uint64_t m_dispatcher; + + uint32_t numOnLastPage; + + uint16_t numPages; + uint16_t taskId; + + struct CollisionTask_LocalStoreMemory* m_lsMemory; +} + +#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) +__attribute__ ((aligned (128))) +#endif +; + + +void processCollisionTask(void* userPtr, void* lsMemory); + +void* createCollisionLocalStoreMemory(); + + +#if defined(USE_LIBSPE2) && defined(__SPU__) +#include "../SpuLibspe2Support.h" +#include +#include +#include + + + +int main(unsigned long long speid, addr64 argp, addr64 envp) +{ + printf("SPU: hello \n"); + + ATTRIBUTE_ALIGNED128(btSpuStatus status); + ATTRIBUTE_ALIGNED16( SpuGatherAndProcessPairsTaskDesc taskDesc ) ; + unsigned int received_message = Spu_Mailbox_Event_Nothing; + bool shutdown = false; + + cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + status.m_status = Spu_Status_Free; + status.m_lsMemory.p = createCollisionLocalStoreMemory(); + + cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + + while ( btLikely( !shutdown ) ) + { + + received_message = spu_read_in_mbox(); + + if( btLikely( received_message == Spu_Mailbox_Event_Task )) + { + printf("SPU: received Spu_Mailbox_Event_Task\n"); + + // refresh the status + cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + btAssert(status.m_status==Spu_Status_Occupied); + + cellDmaGet(&taskDesc, status.m_taskDesc.p, sizeof(SpuGatherAndProcessPairsTaskDesc), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + printf("SPU:processCollisionTask\n"); + processCollisionTask((void*)&taskDesc, taskDesc.m_lsMemory); + printf("SPU:finished processCollisionTask\n"); + } + else + { + printf("SPU: received ShutDown\n"); + if( btLikely( received_message == Spu_Mailbox_Event_Shutdown ) ) + { + shutdown = true; + } + else + { + //printf("SPU - Sth. recieved\n"); + } + } + + // set to status free and wait for next task + status.m_status = Spu_Status_Free; + cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + + } + printf("SPU: shutdown\n"); + return 0; +} +#endif // USE_LIBSPE2 + + +#endif //SPU_GATHERING_COLLISION_TASK_H + + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp index 02aa69356..b598e4f23 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp @@ -1,311 +1,311 @@ -/* -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 "SpuGjkPairDetector.h" -#include "SpuConvexPenetrationDepthSolver.h" -#include "SpuLocalSupport.h" - - - -#if defined(DEBUG) || defined (_DEBUG) -#include //for debug printf -#ifdef __SPU__ -#include -#define printf spu_printf -#endif //__SPU__ -#endif - -//must be above the machine epsilon -#define REL_ERROR2 btScalar(1.0e-6) - -//temp globals, to improve GJK/EPA/penetration calculations -int gSpuNumDeepPenetrationChecks = 0; -int gSpuNumGjkChecks = 0; - - - -SpuGjkPairDetector::SpuGjkPairDetector(void* objectA,void* objectB,int shapeTypeA, int shapeTypeB, float marginA,float marginB,SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(float(0.),float(0.),float(1.)), -m_penetrationDepthSolver(penetrationDepthSolver), -m_simplexSolver(simplexSolver), -m_minkowskiA(objectA), -m_minkowskiB(objectB), -m_shapeTypeA(shapeTypeA), -m_shapeTypeB(shapeTypeB), -m_marginA(marginA), -m_marginB(marginB), -m_ignoreMargin(false), -m_lastUsedMethod(-1), -m_catchDegeneracies(1) -{ -} - -void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output) -{ - btScalar distance=btScalar(0.); - btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 pointOnA,pointOnB; - btTransform localTransA = input.m_transformA; - btTransform localTransB = input.m_transformB; - btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); - localTransA.getOrigin() -= positionOffset; - localTransB.getOrigin() -= positionOffset; - - btScalar marginA = m_marginA; - btScalar marginB = m_marginB; - - gSpuNumGjkChecks++; - - //for CCD we don't use margins - if (m_ignoreMargin) - { - marginA = btScalar(0.); - marginB = btScalar(0.); - } - - m_curIter = 0; - int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? - m_cachedSeparatingAxis.setValue(0,1,0); - - bool isValid = false; - bool checkSimplex = false; - bool checkPenetration = true; - m_degenerateSimplex = 0; - - m_lastUsedMethod = -1; - - { - btScalar squaredDistance = SIMD_INFINITY; - btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - - m_simplexSolver->reset(); - - for ( ; ; ) - //while (true) - { - - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); - -// btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); -// btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); - - btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData);//, &featureIndexA); - btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData);//, &featureIndexB); - - - btPoint3 pWorld = localTransA(pInA); - btPoint3 qWorld = localTransB(qInB); - - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); - - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) - { - checkPenetration = false; - break; - } - - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (m_simplexSolver->inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * REL_ERROR2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) - { - m_degenerateSimplex = 2; - } - checkSimplex = true; - break; - } - //add current vertex to simplex - m_simplexSolver->addVertex(w, pWorld, qWorld); - - //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = m_cachedSeparatingAxis.length2(); - - //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); - - //are we getting any closer ? - if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) - { - m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - checkSimplex = true; - break; - } - - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { - #if defined(DEBUG) || defined (_DEBUG) - - printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance, - m_shapeTypeA, - m_shapeTypeB); - - #endif - break; - - } - - - bool check = (!m_simplexSolver->fullSimplex()); - //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); - - if (!check) - { - //do we need this backup_closest here ? - m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - break; - } - } - - if (checkSimplex) - { - m_simplexSolver->compute_points(pointOnA, pointOnB); - normalInB = pointOnA-pointOnB; - btScalar lenSqr = m_cachedSeparatingAxis.length2(); - //valid normal - if (lenSqr < 0.0001) - { - m_degenerateSimplex = 5; - } - if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - normalInB *= rlen; //normalize - btScalar s = btSqrt(squaredDistance); - - btAssert(s > btScalar(0.0)); - pointOnA -= m_cachedSeparatingAxis * (marginA / s); - pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((btScalar(1.)/rlen) - margin); - isValid = true; - - m_lastUsedMethod = 1; - } else - { - m_lastUsedMethod = 2; - } - } - - bool catchDegeneratePenetrationCase = - (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); - - //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) - { - //penetration case - - //if there is no way to handle penetrations, bail out - if (m_penetrationDepthSolver) - { - // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - gSpuNumDeepPenetrationChecks++; - - bool isValid2 = m_penetrationDepthSolver->calcPenDepth( - *m_simplexSolver, - m_minkowskiA,m_minkowskiB, - m_shapeTypeA, m_shapeTypeB, - marginA, marginB, - localTransA,localTransB, - m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - 0,input.m_stackAlloc,input.m_convexVertexData - ); - - if (isValid2) - { - btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); - //only replace valid penetrations when the result is deeper (check) - if (!isValid || (distance2 < distance)) - { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - isValid = true; - m_lastUsedMethod = 3; - } else - { - - } - } else - { - //isValid = false; - m_lastUsedMethod = 4; - } - } else - { - m_lastUsedMethod = 5; - } - - } - } - } - - if (isValid) - { -#ifdef __SPU__ - //spu_printf("distance\n"); -#endif //__CELLOS_LV2__ - - - output.addContactPoint( - normalInB, - pointOnB+positionOffset, - distance); - //printf("gjk add:%f",distance); - } - - -} - - - - - +/* +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 "SpuGjkPairDetector.h" +#include "SpuConvexPenetrationDepthSolver.h" +#include "SpuLocalSupport.h" + + + +#if defined(DEBUG) || defined (_DEBUG) +#include //for debug printf +#ifdef __SPU__ +#include +#define printf spu_printf +#endif //__SPU__ +#endif + +//must be above the machine epsilon +#define REL_ERROR2 btScalar(1.0e-6) + +//temp globals, to improve GJK/EPA/penetration calculations +int gSpuNumDeepPenetrationChecks = 0; +int gSpuNumGjkChecks = 0; + + + +SpuGjkPairDetector::SpuGjkPairDetector(void* objectA,void* objectB,int shapeTypeA, int shapeTypeB, float marginA,float marginB,SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver) +:m_cachedSeparatingAxis(float(0.),float(0.),float(1.)), +m_penetrationDepthSolver(penetrationDepthSolver), +m_simplexSolver(simplexSolver), +m_minkowskiA(objectA), +m_minkowskiB(objectB), +m_shapeTypeA(shapeTypeA), +m_shapeTypeB(shapeTypeB), +m_marginA(marginA), +m_marginB(marginB), +m_ignoreMargin(false), +m_lastUsedMethod(-1), +m_catchDegeneracies(1) +{ +} + +void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output) +{ + btScalar distance=btScalar(0.); + btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 pointOnA,pointOnB; + btTransform localTransA = input.m_transformA; + btTransform localTransB = input.m_transformB; + btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); + localTransA.getOrigin() -= positionOffset; + localTransB.getOrigin() -= positionOffset; + + btScalar marginA = m_marginA; + btScalar marginB = m_marginB; + + gSpuNumGjkChecks++; + + //for CCD we don't use margins + if (m_ignoreMargin) + { + marginA = btScalar(0.); + marginB = btScalar(0.); + } + + m_curIter = 0; + int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? + m_cachedSeparatingAxis.setValue(0,1,0); + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + m_degenerateSimplex = 0; + + m_lastUsedMethod = -1; + + { + btScalar squaredDistance = SIMD_INFINITY; + btScalar delta = btScalar(0.); + + btScalar margin = marginA + marginB; + + + + m_simplexSolver->reset(); + + for ( ; ; ) + //while (true) + { + + btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); + btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); + +// btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); +// btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); + + btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData);//, &featureIndexA); + btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData);//, &featureIndexB); + + + btPoint3 pWorld = localTransA(pInA); + btPoint3 qWorld = localTransB(qInB); + + btVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) + { + checkPenetration = false; + break; + } + + //exit 0: the new point is already in the simplex, or we didn't come any closer + if (m_simplexSolver->inSimplex(w)) + { + m_degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * REL_ERROR2; + + if (f0 <= f1) + { + if (f0 <= btScalar(0.)) + { + m_degenerateSimplex = 2; + } + checkSimplex = true; + break; + } + //add current vertex to simplex + m_simplexSolver->addVertex(w, pWorld, qWorld); + + //calculate the closest point to the origin (update vector v) + if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) + { + m_degenerateSimplex = 3; + checkSimplex = true; + break; + } + + btScalar previousSquaredDistance = squaredDistance; + squaredDistance = m_cachedSeparatingAxis.length2(); + + //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); + + //are we getting any closer ? + if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) + { + m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + checkSimplex = true; + break; + } + + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (m_curIter++ > gGjkMaxIter) + { + #if defined(DEBUG) || defined (_DEBUG) + + printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance, + m_shapeTypeA, + m_shapeTypeB); + + #endif + break; + + } + + + bool check = (!m_simplexSolver->fullSimplex()); + //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); + + if (!check) + { + //do we need this backup_closest here ? + m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + break; + } + } + + if (checkSimplex) + { + m_simplexSolver->compute_points(pointOnA, pointOnB); + normalInB = pointOnA-pointOnB; + btScalar lenSqr = m_cachedSeparatingAxis.length2(); + //valid normal + if (lenSqr < 0.0001) + { + m_degenerateSimplex = 5; + } + if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + normalInB *= rlen; //normalize + btScalar s = btSqrt(squaredDistance); + + btAssert(s > btScalar(0.0)); + pointOnA -= m_cachedSeparatingAxis * (marginA / s); + pointOnB += m_cachedSeparatingAxis * (marginB / s); + distance = ((btScalar(1.)/rlen) - margin); + isValid = true; + + m_lastUsedMethod = 1; + } else + { + m_lastUsedMethod = 2; + } + } + + bool catchDegeneratePenetrationCase = + (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); + + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) + { + //penetration case + + //if there is no way to handle penetrations, bail out + if (m_penetrationDepthSolver) + { + // Penetration depth case. + btVector3 tmpPointOnA,tmpPointOnB; + + gSpuNumDeepPenetrationChecks++; + + bool isValid2 = m_penetrationDepthSolver->calcPenDepth( + *m_simplexSolver, + m_minkowskiA,m_minkowskiB, + m_shapeTypeA, m_shapeTypeB, + marginA, marginB, + localTransA,localTransB, + m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, + 0,input.m_stackAlloc,input.m_convexVertexData + ); + + if (isValid2) + { + btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + m_lastUsedMethod = 3; + } else + { + + } + } else + { + //isValid = false; + m_lastUsedMethod = 4; + } + } else + { + m_lastUsedMethod = 5; + } + + } + } + } + + if (isValid) + { +#ifdef __SPU__ + //spu_printf("distance\n"); +#endif //__SPU__ + + + output.addContactPoint( + normalInB, + pointOnB+positionOffset, + distance); + //printf("gjk add:%f",distance); + } + + +} + + + + + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h index ed3cd5814..449c13946 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h @@ -1,93 +1,93 @@ - -/* -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_GJK_PAIR_DETECTOR_H -#define SPU_GJK_PAIR_DETECTOR_H - - - -#include "SpuContactResult.h" - - -#include "SpuVoronoiSimplexSolver.h" -class SpuConvexPenetrationDepthSolver; - -/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface -class SpuGjkPairDetector -{ - - - btVector3 m_cachedSeparatingAxis; - const SpuConvexPenetrationDepthSolver* m_penetrationDepthSolver; - SpuVoronoiSimplexSolver* m_simplexSolver; - void* m_minkowskiA; - void* m_minkowskiB; - int m_shapeTypeA; - int m_shapeTypeB; - float m_marginA; - float m_marginB; - bool m_ignoreMargin; - - -public: - - //some debugging to fix degeneracy problems - int m_lastUsedMethod; - int m_curIter; - int m_degenerateSimplex; - int m_catchDegeneracies; - - - SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver); - virtual ~SpuGjkPairDetector() {}; - - virtual void getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output); - - void setMinkowskiA(void* minkA) - { - m_minkowskiA = minkA; - } - - void setMinkowskiB(void* minkB) - { - m_minkowskiB = minkB; - } - - void setCachedSeperatingAxis(const btVector3& seperatingAxis) - { - m_cachedSeparatingAxis = seperatingAxis; - } - - void setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver* penetrationDepthSolver) - { - m_penetrationDepthSolver = penetrationDepthSolver; - } - - ///don't use setIgnoreMargin, it's for Bullet's internal use - void setIgnoreMargin(bool ignoreMargin) - { - m_ignoreMargin = ignoreMargin; - } - - -}; - - - -#endif //SPU_GJK_PAIR_DETECTOR_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_GJK_PAIR_DETECTOR_H +#define SPU_GJK_PAIR_DETECTOR_H + + + +#include "SpuContactResult.h" + + +#include "SpuVoronoiSimplexSolver.h" +class SpuConvexPenetrationDepthSolver; + +/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface +class SpuGjkPairDetector +{ + + + btVector3 m_cachedSeparatingAxis; + const SpuConvexPenetrationDepthSolver* m_penetrationDepthSolver; + SpuVoronoiSimplexSolver* m_simplexSolver; + void* m_minkowskiA; + void* m_minkowskiB; + int m_shapeTypeA; + int m_shapeTypeB; + float m_marginA; + float m_marginB; + bool m_ignoreMargin; + + +public: + + //some debugging to fix degeneracy problems + int m_lastUsedMethod; + int m_curIter; + int m_degenerateSimplex; + int m_catchDegeneracies; + + + SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver); + virtual ~SpuGjkPairDetector() {}; + + virtual void getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output); + + void setMinkowskiA(void* minkA) + { + m_minkowskiA = minkA; + } + + void setMinkowskiB(void* minkB) + { + m_minkowskiB = minkB; + } + + void setCachedSeperatingAxis(const btVector3& seperatingAxis) + { + m_cachedSeparatingAxis = seperatingAxis; + } + + void setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver* penetrationDepthSolver) + { + m_penetrationDepthSolver = penetrationDepthSolver; + } + + ///don't use setIgnoreMargin, it's for Bullet's internal use + void setIgnoreMargin(bool ignoreMargin) + { + m_ignoreMargin = ignoreMargin; + } + + +}; + + + +#endif //SPU_GJK_PAIR_DETECTOR_H diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h index 02017e87a..c8f653109 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h @@ -1,249 +1,249 @@ -/* -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 "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" - -#define MAX_NUM_SPU_CONVEX_POINTS 128 - -struct SpuConvexPolyhedronVertexData -{ - void* gSpuConvexShapePtr0; - void* gSpuConvexShapePtr1; - btPoint3* gConvexPoints0; - btPoint3* gConvexPoints1; - int gNumConvexPoints0; - int gNumConvexPoints1; - ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer0[MAX_NUM_SPU_CONVEX_POINTS]); - ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer1[MAX_NUM_SPU_CONVEX_POINTS]); - -}; - - -inline btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex) -{ - switch (shapeType) - { - case SPHERE_SHAPE_PROXYTYPE: - { - return btPoint3(0,0,0); - } - case BOX_SHAPE_PROXYTYPE: - { -// spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n"); - btConvexInternalShape* convexShape = (btConvexInternalShape*)shape; - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - float margin = convexShape->getMarginNV(); - halfExtents -= btVector3(margin,margin,margin); - return btPoint3( - localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(), - localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(), - localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()); - } - - case TRIANGLE_SHAPE_PROXYTYPE: - { - - btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3* vertices = (btVector3*)shape; - btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); - btVector3 sup = vertices[dots.maxAxis()]; - return btPoint3(sup.getX(),sup.getY(),sup.getZ()); - break; - } - - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)shape; - - //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) - - btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); - btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); - - int cylinderUpAxis = cylShape->getUpAxis(); - int XX(1),YY(0),ZZ(2); - - switch (cylinderUpAxis) - { - case 0: - { - XX = 1; - YY = 0; - ZZ = 2; - break; - } - case 1: - { - XX = 0; - YY = 1; - ZZ = 2; - break; - } - case 2: - { - XX = 0; - YY = 2; - ZZ = 1; - break; - } - default: - btAssert(0); - //printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n"); - }; - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; - - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ()); - } - else - { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ()); - } - } - - case CAPSULE_SHAPE_PROXYTYPE: - { - //spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n"); - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - - btConvexInternalShape* cnvxShape = (btConvexInternalShape*)shape; - btVector3 halfExtents = cnvxShape->getImplicitShapeDimensions(); - btScalar halfHeight = halfExtents.getY(); - btScalar radius = halfExtents.getX(); - btVector3 supVec(0,0,0); - - btScalar maxDot(btScalar(-1e30)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - btVector3 vtx; - btScalar newDot; - { - btVector3 pos(0,halfHeight,0); - vtx = pos +vec*(radius); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - { - btVector3 pos(0,-halfHeight,0); - vtx = pos +vec*(radius); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ()); - break; - }; - - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - //spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n"); - - - - btPoint3* points = 0; - int numPoints = 0; - if (shape==convexVertexData->gSpuConvexShapePtr0) - { - points = convexVertexData->gConvexPoints0; - numPoints = convexVertexData->gNumConvexPoints0; - } - if (shape == convexVertexData->gSpuConvexShapePtr1) - { - points = convexVertexData->gConvexPoints1; - numPoints = convexVertexData->gNumConvexPoints1; - } - - // spu_printf("numPoints = %d\n",numPoints); - - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - btScalar newDot,maxDot = btScalar(-1e30); - - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - - for (int i=0;i maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ()); - - break; - }; - - default: - - //spu_printf("SPU:(type %i) missing support function\n",shapeType); - - -#if __ASSERT - spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType); -#endif // __ASSERT - return btPoint3(0.f, 0.f, 0.f); - } -} - - +/* +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 "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" + +#define MAX_NUM_SPU_CONVEX_POINTS 128 + +struct SpuConvexPolyhedronVertexData +{ + void* gSpuConvexShapePtr0; + void* gSpuConvexShapePtr1; + btPoint3* gConvexPoints0; + btPoint3* gConvexPoints1; + int gNumConvexPoints0; + int gNumConvexPoints1; + ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer0[MAX_NUM_SPU_CONVEX_POINTS]); + ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer1[MAX_NUM_SPU_CONVEX_POINTS]); + +}; + + +inline btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3 localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex) +{ + switch (shapeType) + { + case SPHERE_SHAPE_PROXYTYPE: + { + return btPoint3(0,0,0); + } + case BOX_SHAPE_PROXYTYPE: + { +// spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n"); + btConvexInternalShape* convexShape = (btConvexInternalShape*)shape; + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + float margin = convexShape->getMarginNV(); + halfExtents -= btVector3(margin,margin,margin); + return btPoint3( + localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(), + localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(), + localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()); + } + + case TRIANGLE_SHAPE_PROXYTYPE: + { + + btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); + btVector3* vertices = (btVector3*)shape; + btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); + btVector3 sup = vertices[dots.maxAxis()]; + return btPoint3(sup.getX(),sup.getY(),sup.getZ()); + break; + } + + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)shape; + + //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) + + btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); + btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); + + int cylinderUpAxis = cylShape->getUpAxis(); + int XX(1),YY(0),ZZ(2); + + switch (cylinderUpAxis) + { + case 0: + { + XX = 1; + YY = 0; + ZZ = 2; + break; + } + case 1: + { + XX = 0; + YY = 1; + ZZ = 2; + break; + } + case 2: + { + XX = 0; + YY = 2; + ZZ = 1; + break; + } + default: + btAssert(0); + //printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n"); + }; + + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; + + btVector3 tmp; + btScalar d ; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ()); + } + else + { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ()); + } + } + + case CAPSULE_SHAPE_PROXYTYPE: + { + //spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n"); + btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + + btConvexInternalShape* cnvxShape = (btConvexInternalShape*)shape; + btVector3 halfExtents = cnvxShape->getImplicitShapeDimensions(); + btScalar halfHeight = halfExtents.getY(); + btScalar radius = halfExtents.getX(); + btVector3 supVec(0,0,0); + + btScalar maxDot(btScalar(-1e30)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + btVector3 vtx; + btScalar newDot; + { + btVector3 pos(0,halfHeight,0); + vtx = pos +vec*(radius); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + { + btVector3 pos(0,-halfHeight,0); + vtx = pos +vec*(radius); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ()); + break; + }; + + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + //spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n"); + + + + btPoint3* points = 0; + int numPoints = 0; + if (shape==convexVertexData->gSpuConvexShapePtr0) + { + points = convexVertexData->gConvexPoints0; + numPoints = convexVertexData->gNumConvexPoints0; + } + if (shape == convexVertexData->gSpuConvexShapePtr1) + { + points = convexVertexData->gConvexPoints1; + numPoints = convexVertexData->gNumConvexPoints1; + } + + // spu_printf("numPoints = %d\n",numPoints); + + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar newDot,maxDot = btScalar(-1e30); + + btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + + for (int i=0;i maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ()); + + break; + }; + + default: + + //spu_printf("SPU:(type %i) missing support function\n",shapeType); + + +#if __ASSERT + spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType); +#endif // __ASSERT + return btPoint3(0.f, 0.f, 0.f); + } +} + + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp index 71683b231..553269f21 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp @@ -1,342 +1,342 @@ -/* -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 "SpuMinkowskiPenetrationDepthSolver.h" -#include "SpuVoronoiSimplexSolver.h" -#include "SpuGjkPairDetector.h" -#include "SpuContactResult.h" -#include "SpuPreferredPenetrationDirections.h" - - -#include "SpuLocalSupport.h" - -#define NUM_UNITSPHERE_POINTS 42 -static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = -{ -btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), -btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), -btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), -btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), -btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), -btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), -btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), -btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), -btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), -btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), -btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), -btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), -btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), -btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), -btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), -btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), -btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), -btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), -btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), -btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), -btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), -btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), -btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), -btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), -btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), -btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), -btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), -btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), -btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), -btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), -btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), -btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), -btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), -btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), -btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), -btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), -btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), -btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), -btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), -btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), -btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), -btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) -}; - -bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, - void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, - btTransform& transA,const btTransform& transB, - btVector3& v, btPoint3& pa, btPoint3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, - struct SpuConvexPolyhedronVertexData* convexVertexData - ) const -{ - - (void)stackAlloc; - (void)v; - - - struct btIntermediateResult : public SpuContactResult - { - - btIntermediateResult():m_hasResult(false) - { - } - - btVector3 m_normalOnBInWorld; - btVector3 m_pointInWorld; - btScalar m_depth; - bool m_hasResult; - - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) - { - (void)partId0; - (void)index0; - (void)partId1; - (void)index1; - } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) - { - m_normalOnBInWorld = normalOnBInWorld; - m_pointInWorld = pointInWorld; - m_depth = depth; - m_hasResult = true; - } - }; - - //just take fixed number of orientation, and sample the penetration depth in that direction - btScalar minProj = btScalar(1e30); - btVector3 minNorm; - btVector3 minVertex; - btVector3 minA,minB; - btVector3 seperatingAxisInA,seperatingAxisInB; - btVector3 pInA,qInB,pWorld,qWorld,w; - -//#define USE_BATCHED_SUPPORT 1 -#ifdef USE_BATCHED_SUPPORT - - btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - int i; - - int numSampleDirections = NUM_UNITSPHERE_POINTS; - - for (i=0;igetNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); - numSampleDirections++; - } - } - } - - { - int numPDB = convexB->getNumPreferredPenetrationDirections(); - if (numPDB) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); - numSampleDirections++; - } - } - } - - - - convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); - convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); - - for (i=0;ilocalGetSupportingVertexWithoutMargin(seperatingAxisInA); - // qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); - - pWorld = transA(pInA); - qWorld = transB(qInB); - w = qWorld - pWorld; - btScalar delta = norm.dot(w); - //find smallest delta - if (delta < minProj) - { - minProj = delta; - minNorm = norm; - minA = pWorld; - minB = qWorld; - } - } -#endif //USE_BATCHED_SUPPORT - - //add the margins - - minA += minNorm*marginA; - minB -= minNorm*marginB; - //no penetration - if (minProj < btScalar(0.)) - return false; - - minProj += (marginA + marginB); - - - - - -//#define DEBUG_DRAW 1 -#ifdef DEBUG_DRAW - if (debugDraw) - { - btVector3 color(0,1,0); - debugDraw->drawLine(minA,minB,color); - color = btVector3 (1,1,1); - btVector3 vec = minB-minA; - btScalar prj2 = minNorm.dot(vec); - debugDraw->drawLine(minA,minA+(minNorm*minProj),color); - - } -#endif //DEBUG_DRAW - - - - SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0); - - btScalar offsetDist = minProj; - btVector3 offset = minNorm * offsetDist; - - - SpuClosestPointInput input; - input.m_convexVertexData = convexVertexData; - btVector3 newOrg = transA.getOrigin() + offset; - - btTransform displacedTrans = transA; - displacedTrans.setOrigin(newOrg); - - input.m_transformA = displacedTrans; - input.m_transformB = transB; - input.m_maximumDistanceSquared = btScalar(1e30);//minProj; - - btIntermediateResult res; - gjkdet.getClosestPoints(input,res); - - btScalar correctedMinNorm = minProj - res.m_depth; - - - //the penetration depth is over-estimated, relax it - btScalar penetration_relaxation= btScalar(1.); - minNorm*=penetration_relaxation; - - if (res.m_hasResult) - { - - pa = res.m_pointInWorld - minNorm * correctedMinNorm; - pb = res.m_pointInWorld; - -#ifdef DEBUG_DRAW - if (debugDraw) - { - btVector3 color(1,0,0); - debugDraw->drawLine(pa,pb,color); - } -#endif//DEBUG_DRAW - - - } - return res.m_hasResult; -} - - - +/* +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 "SpuMinkowskiPenetrationDepthSolver.h" +#include "SpuVoronoiSimplexSolver.h" +#include "SpuGjkPairDetector.h" +#include "SpuContactResult.h" +#include "SpuPreferredPenetrationDirections.h" + + +#include "SpuLocalSupport.h" + +#define NUM_UNITSPHERE_POINTS 42 +static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = +{ +btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), +btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), +btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), +btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), +btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), +btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), +btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), +btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), +btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), +btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), +btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), +btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), +btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), +btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), +btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), +btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), +btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), +btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), +btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), +btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), +btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), +btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), +btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), +btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), +btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), +btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), +btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), +btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), +btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), +btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), +btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), +btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), +btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), +btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), +btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), +btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), +btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), +btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), +btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), +btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), +btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), +btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) +}; + +bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, + void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, + btTransform& transA,const btTransform& transB, + btVector3& v, btPoint3& pa, btPoint3& pb, + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, + struct SpuConvexPolyhedronVertexData* convexVertexData + ) const +{ + + (void)stackAlloc; + (void)v; + + + struct btIntermediateResult : public SpuContactResult + { + + btIntermediateResult():m_hasResult(false) + { + } + + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_depth; + bool m_hasResult; + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + (void)partId0; + (void)index0; + (void)partId1; + (void)index1; + } + void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; + m_hasResult = true; + } + }; + + //just take fixed number of orientation, and sample the penetration depth in that direction + btScalar minProj = btScalar(1e30); + btVector3 minNorm; + btVector3 minVertex; + btVector3 minA,minB; + btVector3 seperatingAxisInA,seperatingAxisInB; + btVector3 pInA,qInB,pWorld,qWorld,w; + +//#define USE_BATCHED_SUPPORT 1 +#ifdef USE_BATCHED_SUPPORT + + btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + int i; + + int numSampleDirections = NUM_UNITSPHERE_POINTS; + + for (i=0;igetNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;igetPreferredPenetrationDirection(i,norm); + norm = transA.getBasis() * norm; + sPenetrationDirections[numSampleDirections] = norm; + seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + numSampleDirections++; + } + } + } + + { + int numPDB = convexB->getNumPreferredPenetrationDirections(); + if (numPDB) + { + for (int i=0;igetPreferredPenetrationDirection(i,norm); + norm = transB.getBasis() * norm; + sPenetrationDirections[numSampleDirections] = norm; + seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + numSampleDirections++; + } + } + } + + + + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); + convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); + + for (i=0;ilocalGetSupportingVertexWithoutMargin(seperatingAxisInA); + // qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); + + pWorld = transA(pInA); + qWorld = transB(qInB); + w = qWorld - pWorld; + btScalar delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } + } +#endif //USE_BATCHED_SUPPORT + + //add the margins + + minA += minNorm*marginA; + minB -= minNorm*marginB; + //no penetration + if (minProj < btScalar(0.)) + return false; + + minProj += (marginA + marginB); + + + + + +//#define DEBUG_DRAW 1 +#ifdef DEBUG_DRAW + if (debugDraw) + { + btVector3 color(0,1,0); + debugDraw->drawLine(minA,minB,color); + color = btVector3 (1,1,1); + btVector3 vec = minB-minA; + btScalar prj2 = minNorm.dot(vec); + debugDraw->drawLine(minA,minA+(minNorm*minProj),color); + + } +#endif //DEBUG_DRAW + + + + SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0); + + btScalar offsetDist = minProj; + btVector3 offset = minNorm * offsetDist; + + + SpuClosestPointInput input; + input.m_convexVertexData = convexVertexData; + btVector3 newOrg = transA.getOrigin() + offset; + + btTransform displacedTrans = transA; + displacedTrans.setOrigin(newOrg); + + input.m_transformA = displacedTrans; + input.m_transformB = transB; + input.m_maximumDistanceSquared = btScalar(1e30);//minProj; + + btIntermediateResult res; + gjkdet.getClosestPoints(input,res); + + btScalar correctedMinNorm = minProj - res.m_depth; + + + //the penetration depth is over-estimated, relax it + btScalar penetration_relaxation= btScalar(1.); + minNorm*=penetration_relaxation; + + if (res.m_hasResult) + { + + pa = res.m_pointInWorld - minNorm * correctedMinNorm; + pb = res.m_pointInWorld; + +#ifdef DEBUG_DRAW + if (debugDraw) + { + btVector3 color(1,0,0); + debugDraw->drawLine(pa,pb,color); + } +#endif//DEBUG_DRAW + + + } + return res.m_hasResult; +} + + + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h index d5e66bb55..c862713b1 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h @@ -1,46 +1,46 @@ - -/* -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 MINKOWSKI_PENETRATION_DEPTH_SOLVER_H -#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H - - -#include "SpuConvexPenetrationDepthSolver.h" - -class btStackAlloc; -class btIDebugDraw; -class SpuVoronoiSimplexSolver; - -///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. -///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. -class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver -{ -public: - - virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, - void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, - btTransform& transA,const btTransform& transB, - btVector3& v, btPoint3& pa, btPoint3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, - struct SpuConvexPolyhedronVertexData* convexVertexData - ) const; - - -}; - - -#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_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 MINKOWSKI_PENETRATION_DEPTH_SOLVER_H +#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H + + +#include "SpuConvexPenetrationDepthSolver.h" + +class btStackAlloc; +class btIDebugDraw; +class SpuVoronoiSimplexSolver; + +///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. +///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. +class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver +{ +public: + + virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, + void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, + btTransform& transA,const btTransform& transB, + btVector3& v, btPoint3& pa, btPoint3& pb, + class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, + struct SpuConvexPolyhedronVertexData* convexVertexData + ) const; + + +}; + + +#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h index 044e2be88..774a0cb2e 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h @@ -1,70 +1,70 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 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_PREFERRED_PENETRATION_DIRECTIONS_H -#define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H - - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" - -int spuGetNumPreferredPenetrationDirections(int shapeType, void* shape) -{ - switch (shapeType) - { - case TRIANGLE_SHAPE_PROXYTYPE: - { - return 2; - //spu_printf("2\n"); - break; - } - default: - { -#if __ASSERT - spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType); -#endif // __ASSERT - } - } - - return 0; -} - -void spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector) -{ - - - switch (shapeType) - { - case TRIANGLE_SHAPE_PROXYTYPE: - { - btVector3* vertices = (btVector3*)shape; - ///calcNormal - penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); - penetrationVector.normalize(); - if (index) - penetrationVector *= btScalar(-1.); - break; - } - default: - { - -#if __ASSERT - spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType); -#endif // __ASSERT - } - } - -} - -#endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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_PREFERRED_PENETRATION_DIRECTIONS_H +#define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H + + +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" + +int spuGetNumPreferredPenetrationDirections(int shapeType, void* shape) +{ + switch (shapeType) + { + case TRIANGLE_SHAPE_PROXYTYPE: + { + return 2; + //spu_printf("2\n"); + break; + } + default: + { +#if __ASSERT + spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType); +#endif // __ASSERT + } + } + + return 0; +} + +void spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector) +{ + + + switch (shapeType) + { + case TRIANGLE_SHAPE_PROXYTYPE: + { + btVector3* vertices = (btVector3*)shape; + ///calcNormal + penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); + penetrationVector.normalize(); + if (index) + penetrationVector *= btScalar(-1.); + break; + } + default: + { + +#if __ASSERT + spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType); +#endif // __ASSERT + } + } + +} + +#endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp index a4557dfc4..b81e27bf3 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp @@ -1,606 +1,606 @@ - -/* -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. - - Elsevier CDROM license agreements grants nonexclusive license to use the software - for any purpose, commercial or non-commercial as long as the following credit is included - identifying the original source of the software: - - Parts of the source are "from the book Real-Time Collision Detection by - Christer Ericson, published by Morgan Kaufmann Publishers, - (c) 2005 Elsevier Inc." - -*/ - - -#include "SpuVoronoiSimplexSolver.h" -#include -#include - -#define VERTA 0 -#define VERTB 1 -#define VERTC 2 -#define VERTD 3 - -#define CATCH_DEGENERATE_TETRAHEDRON 1 -void SpuVoronoiSimplexSolver::removeVertex(int index) -{ - - assert(m_numVertices>0); - m_numVertices--; - m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; - m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; - m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; -} - -void SpuVoronoiSimplexSolver::reduceVertices (const SpuUsageBitfield& usedVerts) -{ - if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) - removeVertex(3); - - if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) - removeVertex(2); - - if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) - removeVertex(1); - - if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) - removeVertex(0); - -} - - - - - -//clear the simplex, remove all the vertices -void SpuVoronoiSimplexSolver::reset() -{ - m_cachedValidClosest = false; - m_numVertices = 0; - m_needsUpdate = true; - m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - m_cachedBC.reset(); -} - - - - //add a vertex -void SpuVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) -{ - m_lastW = w; - m_needsUpdate = true; - - m_simplexVectorW[m_numVertices] = w; - m_simplexPointsP[m_numVertices] = p; - m_simplexPointsQ[m_numVertices] = q; - - m_numVertices++; -} - -bool SpuVoronoiSimplexSolver::updateClosestVectorAndPoints() -{ - - if (m_needsUpdate) - { - m_cachedBC.reset(); - - m_needsUpdate = false; - - switch (numVertices()) - { - case 0: - m_cachedValidClosest = false; - break; - case 1: - { - m_cachedP1 = m_simplexPointsP[0]; - m_cachedP2 = m_simplexPointsQ[0]; - m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] - m_cachedBC.reset(); - m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); - m_cachedValidClosest = m_cachedBC.isValid(); - break; - }; - case 2: - { - //closest point origin from line segment - const btVector3& from = m_simplexVectorW[0]; - const btVector3& to = m_simplexVectorW[1]; - btVector3 nearest; - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 diff = p - from; - btVector3 v = to - from; - btScalar t = v.dot(diff); - - if (t > 0) { - btScalar dotVV = v.dot(v); - if (t < dotVV) { - t /= dotVV; - diff -= t*v; - m_cachedBC.m_usedVertices.usedVertexA = true; - m_cachedBC.m_usedVertices.usedVertexB = true; - } else { - t = 1; - diff -= v; - //reduce to 1 point - m_cachedBC.m_usedVertices.usedVertexB = true; - } - } else - { - t = 0; - //reduce to 1 point - m_cachedBC.m_usedVertices.usedVertexA = true; - } - m_cachedBC.setBarycentricCoordinates(1-t,t); - nearest = from + t*v; - - m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); - m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); - m_cachedV = m_cachedP1 - m_cachedP2; - - reduceVertices(m_cachedBC.m_usedVertices); - - m_cachedValidClosest = m_cachedBC.isValid(); - break; - } - case 3: - { - //closest point origin from triangle - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; - - closestPtPointTriangle(p,a,b,c,m_cachedBC); - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; - - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; - - m_cachedV = m_cachedP1-m_cachedP2; - - reduceVertices (m_cachedBC.m_usedVertices); - m_cachedValidClosest = m_cachedBC.isValid(); - - break; - } - case 4: - { - - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; - const btVector3& d = m_simplexVectorW[3]; - - bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); - - if (hasSeperation) - { - - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; - - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; - - m_cachedV = m_cachedP1-m_cachedP2; - reduceVertices (m_cachedBC.m_usedVertices); - } else - { -// printf("sub distance got penetration\n"); - - if (m_cachedBC.m_degenerate) - { - m_cachedValidClosest = false; - } else - { - m_cachedValidClosest = true; - //degenerate case == false, penetration = true + zero - m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - break; - } - - m_cachedValidClosest = m_cachedBC.isValid(); - - //closest point origin from tetrahedron - break; - } - default: - { - m_cachedValidClosest = false; - } - }; - } - - return m_cachedValidClosest; - -} - -//return/calculate the closest vertex -bool SpuVoronoiSimplexSolver::closest(btVector3& v) -{ - bool succes = updateClosestVectorAndPoints(); - v = m_cachedV; - return succes; -} - - - -btScalar SpuVoronoiSimplexSolver::maxVertex() -{ - int i, numverts = numVertices(); - btScalar maxV = btScalar(0.); - for (i=0;i= btScalar(0.0) && d4 <= d3) - { - result.m_closestPointOnSimplex = b; - result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(0,1,0); - - return true; // b; // barycentric coordinates (0,1,0) - } - // Check if P in edge region of AB, if so return projection of P onto AB - btScalar vc = d1*d4 - d3*d2; - if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { - btScalar v = d1 / (d1 - d3); - result.m_closestPointOnSimplex = a + v * ab; - result.m_usedVertices.usedVertexA = true; - result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(1-v,v,0); - return true; - //return a + v * ab; // barycentric coordinates (1-v,v,0) - } - - // Check if P in vertex region outside C - btVector3 cp = p - c; - btScalar d5 = ab.dot(cp); - btScalar d6 = ac.dot(cp); - if (d6 >= btScalar(0.0) && d5 <= d6) - { - result.m_closestPointOnSimplex = c; - result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,0,1); - return true;//c; // barycentric coordinates (0,0,1) - } - - // Check if P in edge region of AC, if so return projection of P onto AC - btScalar vb = d5*d2 - d1*d6; - if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { - btScalar w = d2 / (d2 - d6); - result.m_closestPointOnSimplex = a + w * ac; - result.m_usedVertices.usedVertexA = true; - result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-w,0,w); - return true; - //return a + w * ac; // barycentric coordinates (1-w,0,w) - } - - // Check if P in edge region of BC, if so return projection of P onto BC - btScalar va = d3*d6 - d5*d4; - if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { - btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - - result.m_closestPointOnSimplex = b + w * (c - b); - result.m_usedVertices.usedVertexB = true; - result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,1-w,w); - return true; - // return b + w * (c - b); // barycentric coordinates (0,1-w,w) - } - - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - btScalar denom = btScalar(1.0) / (va + vb + vc); - btScalar v = vb * denom; - btScalar w = vc * denom; - - result.m_closestPointOnSimplex = a + ab * v + ac * w; - result.m_usedVertices.usedVertexA = true; - result.m_usedVertices.usedVertexB = true; - result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-v-w,v,w); - - return true; -// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w - -} - - - - - -/// Test if point p and d lie on opposite sides of plane through abc -int SpuVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d) -{ - btVector3 normal = (b-a).cross(c-a); - - btScalar signp = (p - a).dot(normal); // [AP AB AC] - btScalar signd = (d - a).dot( normal); // [AD AB AC] - -#ifdef CATCH_DEGENERATE_TETRAHEDRON -#ifdef BT_USE_DOUBLE_PRECISION -if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) - { - return -1; - } -#else - if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) - { -// printf("affine dependent/degenerate\n");// - return -1; - } -#endif - -#endif - // Points on opposite sides if expression signs are opposite - return signp * signd < btScalar(0.); -} - - -bool SpuVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, SpuSubSimplexClosestResult& finalResult) -{ - SpuSubSimplexClosestResult tempResult; - - // Start out assuming point inside all halfspaces, so closest to itself - finalResult.m_closestPointOnSimplex = p; - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = true; - finalResult.m_usedVertices.usedVertexB = true; - finalResult.m_usedVertices.usedVertexC = true; - finalResult.m_usedVertices.usedVertexD = true; - - int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); - int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); - int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); - int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); - - if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) - { - finalResult.m_degenerate = true; - return false; - } - - if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) - { - return false; - } - - - btScalar bestSqDist = FLT_MAX; - // If point outside face abc then compute closest point on abc - if (pointOutsideABC) - { - closestPtPointTriangle(p, a, b, c,tempResult); - btPoint3 q = tempResult.m_closestPointOnSimplex; - - btScalar sqDist = (q - p).dot( q - p); - // Update best closest point if (squared) distance is less than current best - if (sqDist < bestSqDist) { - bestSqDist = sqDist; - finalResult.m_closestPointOnSimplex = q; - //convert result bitmask! - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; - finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; - finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTB], - tempResult.m_barycentricCoords[VERTC], - 0 - ); - - } - } - - - // Repeat test for face acd - if (pointOutsideACD) - { - closestPtPointTriangle(p, a, c, d,tempResult); - btPoint3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.m_closestPointOnSimplex = q; - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; - finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - 0, - tempResult.m_barycentricCoords[VERTB], - tempResult.m_barycentricCoords[VERTC] - ); - - } - } - // Repeat test for face adb - - - if (pointOutsideADB) - { - closestPtPointTriangle(p, a, d, b,tempResult); - btPoint3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.m_closestPointOnSimplex = q; - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; - finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; - finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - 0, - tempResult.m_barycentricCoords[VERTB] - ); - - } - } - // Repeat test for face bdc - - - if (pointOutsideBDC) - { - closestPtPointTriangle(p, b, d, c,tempResult); - btPoint3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.m_closestPointOnSimplex = q; - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; - finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; - - finalResult.setBarycentricCoordinates( - 0, - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - tempResult.m_barycentricCoords[VERTB] - ); - - } - } - - //help! we ended up full ! - - if (finalResult.m_usedVertices.usedVertexA && - finalResult.m_usedVertices.usedVertexB && - finalResult.m_usedVertices.usedVertexC && - finalResult.m_usedVertices.usedVertexD) - { - return true; - } - - return true; -} - + +/* +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. + + Elsevier CDROM license agreements grants nonexclusive license to use the software + for any purpose, commercial or non-commercial as long as the following credit is included + identifying the original source of the software: + + Parts of the source are "from the book Real-Time Collision Detection by + Christer Ericson, published by Morgan Kaufmann Publishers, + (c) 2005 Elsevier Inc." + +*/ + + +#include "SpuVoronoiSimplexSolver.h" +#include +#include + +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 + +#define CATCH_DEGENERATE_TETRAHEDRON 1 +void SpuVoronoiSimplexSolver::removeVertex(int index) +{ + + assert(m_numVertices>0); + m_numVertices--; + m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; + m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; + m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; +} + +void SpuVoronoiSimplexSolver::reduceVertices (const SpuUsageBitfield& usedVerts) +{ + if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) + removeVertex(3); + + if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) + removeVertex(2); + + if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) + removeVertex(1); + + if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) + removeVertex(0); + +} + + + + + +//clear the simplex, remove all the vertices +void SpuVoronoiSimplexSolver::reset() +{ + m_cachedValidClosest = false; + m_numVertices = 0; + m_needsUpdate = true; + m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_cachedBC.reset(); +} + + + + //add a vertex +void SpuVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) +{ + m_lastW = w; + m_needsUpdate = true; + + m_simplexVectorW[m_numVertices] = w; + m_simplexPointsP[m_numVertices] = p; + m_simplexPointsQ[m_numVertices] = q; + + m_numVertices++; +} + +bool SpuVoronoiSimplexSolver::updateClosestVectorAndPoints() +{ + + if (m_needsUpdate) + { + m_cachedBC.reset(); + + m_needsUpdate = false; + + switch (numVertices()) + { + case 0: + m_cachedValidClosest = false; + break; + case 1: + { + m_cachedP1 = m_simplexPointsP[0]; + m_cachedP2 = m_simplexPointsQ[0]; + m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] + m_cachedBC.reset(); + m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); + m_cachedValidClosest = m_cachedBC.isValid(); + break; + }; + case 2: + { + //closest point origin from line segment + const btVector3& from = m_simplexVectorW[0]; + const btVector3& to = m_simplexVectorW[1]; + btVector3 nearest; + + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 diff = p - from; + btVector3 v = to - from; + btScalar t = v.dot(diff); + + if (t > 0) { + btScalar dotVV = v.dot(v); + if (t < dotVV) { + t /= dotVV; + diff -= t*v; + m_cachedBC.m_usedVertices.usedVertexA = true; + m_cachedBC.m_usedVertices.usedVertexB = true; + } else { + t = 1; + diff -= v; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexB = true; + } + } else + { + t = 0; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexA = true; + } + m_cachedBC.setBarycentricCoordinates(1-t,t); + nearest = from + t*v; + + m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); + m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); + m_cachedV = m_cachedP1 - m_cachedP2; + + reduceVertices(m_cachedBC.m_usedVertices); + + m_cachedValidClosest = m_cachedBC.isValid(); + break; + } + case 3: + { + //closest point origin from triangle + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); + + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; + + closestPtPointTriangle(p,a,b,c,m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedV = m_cachedP1-m_cachedP2; + + reduceVertices (m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); + + break; + } + case 4: + { + + + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); + + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; + const btVector3& d = m_simplexVectorW[3]; + + bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); + + if (hasSeperation) + { + + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedV = m_cachedP1-m_cachedP2; + reduceVertices (m_cachedBC.m_usedVertices); + } else + { +// printf("sub distance got penetration\n"); + + if (m_cachedBC.m_degenerate) + { + m_cachedValidClosest = false; + } else + { + m_cachedValidClosest = true; + //degenerate case == false, penetration = true + zero + m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + } + break; + } + + m_cachedValidClosest = m_cachedBC.isValid(); + + //closest point origin from tetrahedron + break; + } + default: + { + m_cachedValidClosest = false; + } + }; + } + + return m_cachedValidClosest; + +} + +//return/calculate the closest vertex +bool SpuVoronoiSimplexSolver::closest(btVector3& v) +{ + bool succes = updateClosestVectorAndPoints(); + v = m_cachedV; + return succes; +} + + + +btScalar SpuVoronoiSimplexSolver::maxVertex() +{ + int i, numverts = numVertices(); + btScalar maxV = btScalar(0.); + for (i=0;i= btScalar(0.0) && d4 <= d3) + { + result.m_closestPointOnSimplex = b; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(0,1,0); + + return true; // b; // barycentric coordinates (0,1,0) + } + // Check if P in edge region of AB, if so return projection of P onto AB + btScalar vc = d1*d4 - d3*d2; + if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { + btScalar v = d1 / (d1 - d3); + result.m_closestPointOnSimplex = a + v * ab; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(1-v,v,0); + return true; + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + btVector3 cp = p - c; + btScalar d5 = ab.dot(cp); + btScalar d6 = ac.dot(cp); + if (d6 >= btScalar(0.0) && d5 <= d6) + { + result.m_closestPointOnSimplex = c; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0,0,1); + return true;//c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + btScalar vb = d5*d2 - d1*d6; + if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { + btScalar w = d2 / (d2 - d6); + result.m_closestPointOnSimplex = a + w * ac; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-w,0,w); + return true; + //return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + btScalar va = d3*d6 - d5*d4; + if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { + btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + + result.m_closestPointOnSimplex = b + w * (c - b); + result.m_usedVertices.usedVertexB = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0,1-w,w); + return true; + // return b + w * (c - b); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + btScalar denom = btScalar(1.0) / (va + vb + vc); + btScalar v = vb * denom; + btScalar w = vc * denom; + + result.m_closestPointOnSimplex = a + ab * v + ac * w; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-v-w,v,w); + + return true; +// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w + +} + + + + + +/// Test if point p and d lie on opposite sides of plane through abc +int SpuVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d) +{ + btVector3 normal = (b-a).cross(c-a); + + btScalar signp = (p - a).dot(normal); // [AP AB AC] + btScalar signd = (d - a).dot( normal); // [AD AB AC] + +#ifdef CATCH_DEGENERATE_TETRAHEDRON +#ifdef BT_USE_DOUBLE_PRECISION +if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) + { + return -1; + } +#else + if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) + { +// printf("affine dependent/degenerate\n");// + return -1; + } +#endif + +#endif + // Points on opposite sides if expression signs are opposite + return signp * signd < btScalar(0.); +} + + +bool SpuVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, SpuSubSimplexClosestResult& finalResult) +{ + SpuSubSimplexClosestResult tempResult; + + // Start out assuming point inside all halfspaces, so closest to itself + finalResult.m_closestPointOnSimplex = p; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = true; + finalResult.m_usedVertices.usedVertexB = true; + finalResult.m_usedVertices.usedVertexC = true; + finalResult.m_usedVertices.usedVertexD = true; + + int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); + int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); + int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); + int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); + + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) + { + finalResult.m_degenerate = true; + return false; + } + + if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) + { + return false; + } + + + btScalar bestSqDist = FLT_MAX; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC) + { + closestPtPointTriangle(p, a, b, c,tempResult); + btPoint3 q = tempResult.m_closestPointOnSimplex; + + btScalar sqDist = (q - p).dot( q - p); + // Update best closest point if (squared) distance is less than current best + if (sqDist < bestSqDist) { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + //convert result bitmask! + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC], + 0 + ); + + } + } + + + // Repeat test for face acd + if (pointOutsideACD) + { + closestPtPointTriangle(p, a, c, d,tempResult); + btPoint3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + btScalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + 0, + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC] + ); + + } + } + // Repeat test for face adb + + + if (pointOutsideADB) + { + closestPtPointTriangle(p, a, d, b,tempResult); + btPoint3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + btScalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + 0, + tempResult.m_barycentricCoords[VERTB] + ); + + } + } + // Repeat test for face bdc + + + if (pointOutsideBDC) + { + closestPtPointTriangle(p, b, d, c,tempResult); + btPoint3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + btScalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + + finalResult.setBarycentricCoordinates( + 0, + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + tempResult.m_barycentricCoords[VERTB] + ); + + } + } + + //help! we ended up full ! + + if (finalResult.m_usedVertices.usedVertexA && + finalResult.m_usedVertices.usedVertexB && + finalResult.m_usedVertices.usedVertexC && + finalResult.m_usedVertices.usedVertexD) + { + return true; + } + + return true; +} + diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h index 88c6c9a34..b96a29906 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h @@ -1,156 +1,156 @@ - -/* -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 SPUVoronoiSimplexSolver_H -#define SPUVoronoiSimplexSolver_H - -#include -#include - -#define VORONOI_SIMPLEX_MAX_VERTS 5 - -struct SpuUsageBitfield{ - SpuUsageBitfield() - { - reset(); - } - - void reset() - { - usedVertexA = false; - usedVertexB = false; - usedVertexC = false; - usedVertexD = false; - } - unsigned short usedVertexA : 1; - unsigned short usedVertexB : 1; - unsigned short usedVertexC : 1; - unsigned short usedVertexD : 1; - unsigned short unused1 : 1; - unsigned short unused2 : 1; - unsigned short unused3 : 1; - unsigned short unused4 : 1; -}; - - -struct SpuSubSimplexClosestResult -{ - btVector3 m_closestPointOnSimplex; - //MASK for m_usedVertices - //stores the simplex vertex-usage, using the MASK, - // if m_usedVertices & MASK then the related vertex is used - SpuUsageBitfield m_usedVertices; - float m_barycentricCoords[4]; - bool m_degenerate; - - void reset() - { - m_degenerate = false; - setBarycentricCoordinates(); - m_usedVertices.reset(); - } - bool isValid() - { - bool valid = (m_barycentricCoords[0] >= float(0.)) && - (m_barycentricCoords[1] >= float(0.)) && - (m_barycentricCoords[2] >= float(0.)) && - (m_barycentricCoords[3] >= float(0.)); - - - return valid; - } - void setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.)) - { - m_barycentricCoords[0] = a; - m_barycentricCoords[1] = b; - m_barycentricCoords[2] = c; - m_barycentricCoords[3] = d; - } - -}; - -/// SpuVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. -/// Can be used with GJK, as an alternative to Johnson distance algorithm. -class SpuVoronoiSimplexSolver -{ -public: - - int m_numVertices; - - btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; - - int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS]; - int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS]; - - btVector3 m_cachedP1; - btVector3 m_cachedP2; - btVector3 m_cachedV; - btVector3 m_lastW; - bool m_cachedValidClosest; - - SpuSubSimplexClosestResult m_cachedBC; - - bool m_needsUpdate; - - void removeVertex(int index); - void reduceVertices (const SpuUsageBitfield& usedVerts); - bool updateClosestVectorAndPoints(); - - bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, SpuSubSimplexClosestResult& finalResult); - int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); - bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,SpuSubSimplexClosestResult& result); - - int RemoveDegenerateIndices (const int *inArray, int numIndices, int *outArray) const; - -public: - - void reset(); - - void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q); - - - bool closest(btVector3& v); - - btScalar maxVertex(); - - bool fullSimplex() const - { - return (m_numVertices == 4); - } - - int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; - - bool inSimplex(const btVector3& w); - - void backup_closest(btVector3& v) ; - - bool emptySimplex() const ; - - void compute_points(btVector3& p1, btVector3& p2) ; - - int numVertices() const - { - return m_numVertices; - } -}; - - - -#endif //SpuVoronoiSimplexSolver + +/* +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 SPUVoronoiSimplexSolver_H +#define SPUVoronoiSimplexSolver_H + +#include +#include + +#define VORONOI_SIMPLEX_MAX_VERTS 5 + +struct SpuUsageBitfield{ + SpuUsageBitfield() + { + reset(); + } + + void reset() + { + usedVertexA = false; + usedVertexB = false; + usedVertexC = false; + usedVertexD = false; + } + unsigned short usedVertexA : 1; + unsigned short usedVertexB : 1; + unsigned short usedVertexC : 1; + unsigned short usedVertexD : 1; + unsigned short unused1 : 1; + unsigned short unused2 : 1; + unsigned short unused3 : 1; + unsigned short unused4 : 1; +}; + + +struct SpuSubSimplexClosestResult +{ + btVector3 m_closestPointOnSimplex; + //MASK for m_usedVertices + //stores the simplex vertex-usage, using the MASK, + // if m_usedVertices & MASK then the related vertex is used + SpuUsageBitfield m_usedVertices; + float m_barycentricCoords[4]; + bool m_degenerate; + + void reset() + { + m_degenerate = false; + setBarycentricCoordinates(); + m_usedVertices.reset(); + } + bool isValid() + { + bool valid = (m_barycentricCoords[0] >= float(0.)) && + (m_barycentricCoords[1] >= float(0.)) && + (m_barycentricCoords[2] >= float(0.)) && + (m_barycentricCoords[3] >= float(0.)); + + + return valid; + } + void setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.)) + { + m_barycentricCoords[0] = a; + m_barycentricCoords[1] = b; + m_barycentricCoords[2] = c; + m_barycentricCoords[3] = d; + } + +}; + +/// SpuVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. +/// Can be used with GJK, as an alternative to Johnson distance algorithm. +class SpuVoronoiSimplexSolver +{ +public: + + int m_numVertices; + + btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + + int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS]; + int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS]; + + btVector3 m_cachedP1; + btVector3 m_cachedP2; + btVector3 m_cachedV; + btVector3 m_lastW; + bool m_cachedValidClosest; + + SpuSubSimplexClosestResult m_cachedBC; + + bool m_needsUpdate; + + void removeVertex(int index); + void reduceVertices (const SpuUsageBitfield& usedVerts); + bool updateClosestVectorAndPoints(); + + bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, SpuSubSimplexClosestResult& finalResult); + int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); + bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,SpuSubSimplexClosestResult& result); + + int RemoveDegenerateIndices (const int *inArray, int numIndices, int *outArray) const; + +public: + + void reset(); + + void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q); + + + bool closest(btVector3& v); + + btScalar maxVertex(); + + bool fullSimplex() const + { + return (m_numVertices == 4); + } + + int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; + + bool inSimplex(const btVector3& w); + + void backup_closest(btVector3& v) ; + + bool emptySimplex() const ; + + void compute_points(btVector3& p1, btVector3& p2) ; + + int numVertices() const + { + return m_numVertices; + } +}; + + + +#endif //SpuVoronoiSimplexSolver diff --git a/Extras/BulletMultiThreaded/SpuSampleTaskProcess.cpp b/Extras/BulletMultiThreaded/SpuSampleTaskProcess.cpp index 89f3ae40b..743aff5c8 100644 --- a/Extras/BulletMultiThreaded/SpuSampleTaskProcess.cpp +++ b/Extras/BulletMultiThreaded/SpuSampleTaskProcess.cpp @@ -1,196 +1,205 @@ -/* -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. -*/ - -//#define __CELLOS_LV2__ 1 - -#define USE_SAMPLE_PROCESS 1 -#ifdef USE_SAMPLE_PROCESS - - -#include "btThreadSupportInterface.h" - -//#include "SPUAssert.h" -#include - - -#include "SpuSampleTaskProcess.h" - - -#include - - -void SampleThreadFunc(void* userPtr,void* lsMemory) -{ - //do nothing - printf("hello world\n"); -} - -void* SamplelsMemoryFunc() -{ - //don't create local store memory, just return 0 - return 0; -} - - - -extern "C" { - extern char SPU_SAMPLE_ELF_SYMBOL[]; -}; - - - - - -SpuSampleTaskProcess::SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks) -:m_threadInterface(threadInterface), -m_maxNumOutstandingTasks(maxNumOutstandingTasks) -{ - - m_taskBusy.resize(m_maxNumOutstandingTasks); - m_spuSampleTaskDesc.resize(m_maxNumOutstandingTasks); - - for (int i = 0; i < m_maxNumOutstandingTasks; i++) - { - m_taskBusy[i] = false; - } - m_numBusyTasks = 0; - m_currentTask = 0; - - m_initialized = false; - - m_threadInterface->startSPU(); - - -} - -SpuSampleTaskProcess::~SpuSampleTaskProcess() -{ - m_threadInterface->stopSPU(); - -} - - - -void SpuSampleTaskProcess::initialize() -{ -#ifdef DEBUG_SPU_TASK_SCHEDULING - printf("SpuSampleTaskProcess::initialize()\n"); -#endif //DEBUG_SPU_TASK_SCHEDULING - - for (int i = 0; i < m_maxNumOutstandingTasks; i++) - { - m_taskBusy[i] = false; - } - m_numBusyTasks = 0; - m_currentTask = 0; - m_initialized = true; - -} - - -void SpuSampleTaskProcess::issueTask(void* sampleMainMemPtr,int sampleValue) -{ - -#ifdef DEBUG_SPU_TASK_SCHEDULING - printf("SpuSampleTaskProcess::issueTask (m_currentTask= %d\)n", m_currentTask); -#endif //DEBUG_SPU_TASK_SCHEDULING - - m_taskBusy[m_currentTask] = true; - m_numBusyTasks++; - - SpuSampleTaskDesc& taskDesc = m_spuSampleTaskDesc[m_currentTask]; - { - // send task description in event message - // no error checking here... - // but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS. - - taskDesc.m_mainMemoryPtr = reinterpret_cast(sampleMainMemPtr); - taskDesc.m_sampleValue = sampleValue; - - //some bookkeeping to recognize finished tasks - taskDesc.m_taskId = m_currentTask; - } - - - m_threadInterface->sendRequest(CMD_SAMPLE_TASK_COMMAND, (uint32_t) &taskDesc, m_currentTask); - - // if all tasks busy, wait for spu event to clear the task. - - if (m_numBusyTasks >= m_maxNumOutstandingTasks) - { - unsigned int taskId; - unsigned int outputSize; - - m_threadInterface->waitForResponse(&taskId, &outputSize); - - //printf("PPU: after issue, received event: %u %d\n", taskId, outputSize); - - postProcess(taskId, outputSize); - - m_taskBusy[taskId] = false; - - m_numBusyTasks--; - } - - // find new task buffer - for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++) - { - if (!m_taskBusy[i]) - { - m_currentTask = i; - break; - } - } -} - - -///Optional PPU-size post processing for each task -void SpuSampleTaskProcess::postProcess(int taskId, int outputSize) -{ - -} - - -void SpuSampleTaskProcess::flush() -{ -#ifdef DEBUG_SPU_TASK_SCHEDULING - printf("\nSpuCollisionTaskProcess::flush()\n"); -#endif //DEBUG_SPU_TASK_SCHEDULING - - - // all tasks are issued, wait for all tasks to be complete - while(m_numBusyTasks > 0) - { -// Consolidating SPU code - unsigned int taskId; - unsigned int outputSize; - - { - - m_threadInterface->waitForResponse(&taskId, &outputSize); - } - - //printf("PPU: flushing, received event: %u %d\n", taskId, outputSize); - - postProcess(taskId, outputSize); - - m_taskBusy[taskId] = false; - - m_numBusyTasks--; - } - - -} -#endif //USE_SAMPLE_PROCESS +/* +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. +*/ + +//#define __CELLOS_LV2__ 1 + +#define USE_SAMPLE_PROCESS 1 +#ifdef USE_SAMPLE_PROCESS + + +#include "SpuSampleTaskProcess.h" +#include + +#ifdef __SPU__ + + + +void SampleThreadFunc(void* userPtr,void* lsMemory) +{ + //do nothing + printf("hello world\n"); +} + + +void* SamplelsMemoryFunc() +{ + //don't create local store memory, just return 0 + return 0; +} + + +#else + + +#include "btThreadSupportInterface.h" + +//# include "SPUAssert.h" +#include + + + +extern "C" { + extern char SPU_SAMPLE_ELF_SYMBOL[]; +}; + + + + + +SpuSampleTaskProcess::SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks) +:m_threadInterface(threadInterface), +m_maxNumOutstandingTasks(maxNumOutstandingTasks) +{ + + m_taskBusy.resize(m_maxNumOutstandingTasks); + m_spuSampleTaskDesc.resize(m_maxNumOutstandingTasks); + + for (int i = 0; i < m_maxNumOutstandingTasks; i++) + { + m_taskBusy[i] = false; + } + m_numBusyTasks = 0; + m_currentTask = 0; + + m_initialized = false; + + m_threadInterface->startSPU(); + + +} + +SpuSampleTaskProcess::~SpuSampleTaskProcess() +{ + m_threadInterface->stopSPU(); + +} + + + +void SpuSampleTaskProcess::initialize() +{ +#ifdef DEBUG_SPU_TASK_SCHEDULING + printf("SpuSampleTaskProcess::initialize()\n"); +#endif //DEBUG_SPU_TASK_SCHEDULING + + for (int i = 0; i < m_maxNumOutstandingTasks; i++) + { + m_taskBusy[i] = false; + } + m_numBusyTasks = 0; + m_currentTask = 0; + m_initialized = true; + +} + + +void SpuSampleTaskProcess::issueTask(void* sampleMainMemPtr,int sampleValue) +{ + +#ifdef DEBUG_SPU_TASK_SCHEDULING + printf("SpuSampleTaskProcess::issueTask (m_currentTask= %d\)n", m_currentTask); +#endif //DEBUG_SPU_TASK_SCHEDULING + + m_taskBusy[m_currentTask] = true; + m_numBusyTasks++; + + SpuSampleTaskDesc& taskDesc = m_spuSampleTaskDesc[m_currentTask]; + { + // send task description in event message + // no error checking here... + // but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS. + + taskDesc.m_mainMemoryPtr = reinterpret_cast(sampleMainMemPtr); + taskDesc.m_sampleValue = sampleValue; + + //some bookkeeping to recognize finished tasks + taskDesc.m_taskId = m_currentTask; + } + + + m_threadInterface->sendRequest(CMD_SAMPLE_TASK_COMMAND, (uint32_t) &taskDesc, m_currentTask); + + // if all tasks busy, wait for spu event to clear the task. + + if (m_numBusyTasks >= m_maxNumOutstandingTasks) + { + unsigned int taskId; + unsigned int outputSize; + + m_threadInterface->waitForResponse(&taskId, &outputSize); + + //printf("PPU: after issue, received event: %u %d\n", taskId, outputSize); + + postProcess(taskId, outputSize); + + m_taskBusy[taskId] = false; + + m_numBusyTasks--; + } + + // find new task buffer + for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++) + { + if (!m_taskBusy[i]) + { + m_currentTask = i; + break; + } + } +} + + +///Optional PPU-size post processing for each task +void SpuSampleTaskProcess::postProcess(int taskId, int outputSize) +{ + +} + + +void SpuSampleTaskProcess::flush() +{ +#ifdef DEBUG_SPU_TASK_SCHEDULING + printf("\nSpuCollisionTaskProcess::flush()\n"); +#endif //DEBUG_SPU_TASK_SCHEDULING + + + // all tasks are issued, wait for all tasks to be complete + while(m_numBusyTasks > 0) + { +// Consolidating SPU code + unsigned int taskId; + unsigned int outputSize; + + { + + m_threadInterface->waitForResponse(&taskId, &outputSize); + } + + //printf("PPU: flushing, received event: %u %d\n", taskId, outputSize); + + postProcess(taskId, outputSize); + + m_taskBusy[taskId] = false; + + m_numBusyTasks--; + } + + +} + +#endif + + +#endif //USE_SAMPLE_PROCESS diff --git a/Extras/BulletMultiThreaded/SpuSampleTaskProcess.h b/Extras/BulletMultiThreaded/SpuSampleTaskProcess.h index fffd55785..24f380713 100644 --- a/Extras/BulletMultiThreaded/SpuSampleTaskProcess.h +++ b/Extras/BulletMultiThreaded/SpuSampleTaskProcess.h @@ -1,87 +1,159 @@ -/* -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. -*/ - -#ifndef SPU_SAMPLE_TASK_PROCESS_H -#define SPU_SAMPLE_TASK_PROCESS_H - -#include - - -#include "PlatformDefinitions.h" - -#include - -#include "LinearMath/btAlignedObjectArray.h" - - -///SpuSampleTaskDesc -struct SpuSampleTaskDesc -{ - uint64_t m_mainMemoryPtr; - int m_sampleValue; - - uint16_t m_taskId; -} -#ifdef __CELLOS_LV2__ -__attribute__ ((aligned (16))) -#endif -; - -//just add your commands here, try to keep them globally unique for debugging purposes -#define CMD_SAMPLE_TASK_COMMAND 10 - - - -/// SpuSampleTaskProcess handles SPU processing of collision pairs. -/// When PPU issues a task, it will look for completed task buffers -/// PPU will do postprocessing, dependent on workunit output (not likely) -class SpuSampleTaskProcess -{ - // track task buffers that are being used, and total busy tasks - btAlignedObjectArray m_taskBusy; - btAlignedObjectArraym_spuSampleTaskDesc; - - unsigned int m_numBusyTasks; - - // the current task and the current entry to insert a new work unit - unsigned int m_currentTask; - - bool m_initialized; - - void postProcess(int taskId, int outputSize); - - class btThreadSupportInterface* m_threadInterface; - - unsigned int m_maxNumOutstandingTasks; - - - -public: - SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks); - - ~SpuSampleTaskProcess(); - - ///call initialize in the beginning of the frame, before addCollisionPairToTask - void initialize(); - - void issueTask(void* sampleMainMemPtr,int sampleValue); - - ///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished - void flush(); -}; - - -#endif // SPU_SAMPLE_TASK_PROCESS_H - +/* +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. +*/ + +#ifndef SPU_SAMPLE_TASK_PROCESS_H +#define SPU_SAMPLE_TASK_PROCESS_H + +#include + + +#include "PlatformDefinitions.h" + +#include + +#include "LinearMath/btAlignedObjectArray.h" + + +///SpuSampleTaskDesc +struct SpuSampleTaskDesc +{ + uint64_t m_mainMemoryPtr; + int m_sampleValue; + + uint16_t m_taskId; +} +#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) +__attribute__ ((aligned (16))) +#endif +; + +//just add your commands here, try to keep them globally unique for debugging purposes +#define CMD_SAMPLE_TASK_COMMAND 10 + + + +/// SpuSampleTaskProcess handles SPU processing of collision pairs. +/// When PPU issues a task, it will look for completed task buffers +/// PPU will do postprocessing, dependent on workunit output (not likely) +class SpuSampleTaskProcess +{ + // track task buffers that are being used, and total busy tasks + btAlignedObjectArray m_taskBusy; + btAlignedObjectArraym_spuSampleTaskDesc; + + unsigned int m_numBusyTasks; + + // the current task and the current entry to insert a new work unit + unsigned int m_currentTask; + + bool m_initialized; + + void postProcess(int taskId, int outputSize); + + class btThreadSupportInterface* m_threadInterface; + + unsigned int m_maxNumOutstandingTasks; + + + +public: + SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks); + + ~SpuSampleTaskProcess(); + + ///call initialize in the beginning of the frame, before addCollisionPairToTask + void initialize(); + + void issueTask(void* sampleMainMemPtr,int sampleValue); + + ///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished + void flush(); +}; + + +#if defined(USE_LIBSPE2) && defined(__SPU__) +////////////////////MAIN///////////////////////////// +#include "../SpuLibspe2Support.h" +#include +#include +#include + +void * SamplelsMemoryFunc(); +void SampleThreadFunc(void* userPtr,void* lsMemory); + +int main(unsigned long long speid, addr64 argp, addr64 envp) +{ + printf("SPU is up \n"); + + ATTRIBUTE_ALIGNED128(btSpuStatus status); + ATTRIBUTE_ALIGNED16( SpuSampleTaskDesc taskDesc ) ; + unsigned int received_message = Spu_Mailbox_Event_Nothing; + bool shutdown = false; + + cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + status.m_status = Spu_Status_Free; + status.m_lsMemory.p = SamplelsMemoryFunc(); + + cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + + while (!shutdown) + { + received_message = spu_read_in_mbox(); + + + + switch(received_message) + { + case Spu_Mailbox_Event_Shutdown: + shutdown = true; + break; + case Spu_Mailbox_Event_Task: + // refresh the status + printf("SPU recieved Task \n"); + cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + btAssert(status.m_status==Spu_Status_Occupied); + + cellDmaGet(&taskDesc, status.m_taskDesc.p, sizeof(SpuSampleTaskDesc), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + SampleThreadFunc((void*)&taskDesc, reinterpret_cast (taskDesc.m_mainMemoryPtr) ); + break; + case Spu_Mailbox_Event_Nothing: + default: + break; + } + + // set to status free and wait for next task + status.m_status = Spu_Status_Free; + cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(3)); + + + } + return 0; +} +////////////////////////////////////////////////////// +#endif + + + +#endif // SPU_SAMPLE_TASK_PROCESS_H + diff --git a/Extras/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h b/Extras/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h index 62f81f602..769eeb936 100644 --- a/Extras/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h +++ b/Extras/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h @@ -25,16 +25,21 @@ Written by: Marten Svanfeldt #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "../SpuSync.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "LinearMath/btAlignedAllocator.h" ATTRIBUTE_ALIGNED16(struct) ManifoldCellHolder { + BT_DECLARE_ALIGNED_ALLOCATOR(); + uint32_t m_hashCellIndex; class btPersistentManifold* m_manifold; }; ATTRIBUTE_ALIGNED16(struct) ConstraintCellHolder { + BT_DECLARE_ALIGNED_ALLOCATOR(); + uint32_t m_hashCellIndex; uint32_t m_constraintType; class btTypedConstraint* m_constraint; @@ -103,6 +108,8 @@ inline unsigned int spuGetHashCellIndex(int x, int y, int z) ATTRIBUTE_ALIGNED16(struct) SpuSolverBody { + BT_DECLARE_ALIGNED_ALLOCATOR(); + btVector3 m_linearVelocity; btVector3 m_angularVelocity; @@ -113,6 +120,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverBody ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint { + BT_DECLARE_ALIGNED_ALLOCATOR(); + uint32_t m_localOffsetBodyA; uint32_t m_localOffsetBodyB; @@ -135,6 +144,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint { + BT_DECLARE_ALIGNED_ALLOCATOR(); + uint16_t m_localOffsetBodyA; uint16_t m_localOffsetBodyB; @@ -204,6 +215,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc { + BT_DECLARE_ALIGNED_ALLOCATOR(); + SpuSolverHash* m_solverHash; SpuSolverBody* m_solverBodyList; SpuSolverInternalConstraint* m_solverInternalConstraintList; @@ -214,6 +227,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc ATTRIBUTE_ALIGNED16(struct) SpuSolverTaskDesc { + BT_DECLARE_ALIGNED_ALLOCATOR(); + uint32_t m_solverCommand; uint32_t m_taskId; SpuSolverDataDesc m_solverData;