diff options
Diffstat (limited to 'extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask')
15 files changed, 0 insertions, 4227 deletions
diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/Box.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/Box.h deleted file mode 100644 index 9bc2ebf51ec..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/Box.h +++ /dev/null @@ -1,172 +0,0 @@ -/* - Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. - All rights reserved. - -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 __BOX_H__ -#define __BOX_H__ - - -#ifndef PE_REF -#define PE_REF(a) a& -#endif - -#include <math.h> - -///only use a system-wide vectormath_aos.h on CELLOS_LV2 or if USE_SYSTEM_VECTORMATH -#if defined(__CELLOS_LV2__) || defined (USE_SYSTEM_VECTORMATH) -#include <vectormath_aos.h> -#else -#include "BulletMultiThreaded/vectormath/scalar/cpp/vectormath_aos.h" -#endif - - - -using namespace Vectormath::Aos; - -enum FeatureType { F, E, V }; - -//---------------------------------------------------------------------------- -// Box -//---------------------------------------------------------------------------- -///The Box is an internal class used by the boxBoxDistance calculation. -class Box -{ -public: - Vector3 half; - - inline Box() - {} - inline Box(PE_REF(Vector3) half_); - inline Box(float hx, float hy, float hz); - - inline void Set(PE_REF(Vector3) half_); - inline void Set(float hx, float hy, float hz); - - inline Vector3 GetAABB(const Matrix3& rotation) const; -}; - -inline -Box::Box(PE_REF(Vector3) half_) -{ - Set(half_); -} - -inline -Box::Box(float hx, float hy, float hz) -{ - Set(hx, hy, hz); -} - -inline -void -Box::Set(PE_REF(Vector3) half_) -{ - half = half_; -} - -inline -void -Box::Set(float hx, float hy, float hz) -{ - half = Vector3(hx, hy, hz); -} - -inline -Vector3 -Box::GetAABB(const Matrix3& rotation) const -{ - return absPerElem(rotation) * half; -} - -//------------------------------------------------------------------------------------------------- -// BoxPoint -//------------------------------------------------------------------------------------------------- - -///The BoxPoint class is an internally used class to contain feature information for boxBoxDistance calculation. -class BoxPoint -{ -public: - BoxPoint() : localPoint(0.0f) {} - - Point3 localPoint; - FeatureType featureType; - int featureIdx; - - inline void setVertexFeature(int plusX, int plusY, int plusZ); - inline void setEdgeFeature(int dim0, int plus0, int dim1, int plus1); - inline void setFaceFeature(int dim, int plus); - - inline void getVertexFeature(int & plusX, int & plusY, int & plusZ) const; - inline void getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const; - inline void getFaceFeature(int & dim, int & plus) const; -}; - -inline -void -BoxPoint::setVertexFeature(int plusX, int plusY, int plusZ) -{ - featureType = V; - featureIdx = plusX << 2 | plusY << 1 | plusZ; -} - -inline -void -BoxPoint::setEdgeFeature(int dim0, int plus0, int dim1, int plus1) -{ - featureType = E; - - if (dim0 > dim1) { - featureIdx = plus1 << 5 | dim1 << 3 | plus0 << 2 | dim0; - } else { - featureIdx = plus0 << 5 | dim0 << 3 | plus1 << 2 | dim1; - } -} - -inline -void -BoxPoint::setFaceFeature(int dim, int plus) -{ - featureType = F; - featureIdx = plus << 2 | dim; -} - -inline -void -BoxPoint::getVertexFeature(int & plusX, int & plusY, int & plusZ) const -{ - plusX = featureIdx >> 2; - plusY = featureIdx >> 1 & 1; - plusZ = featureIdx & 1; -} - -inline -void -BoxPoint::getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const -{ - plus0 = featureIdx >> 5; - dim0 = featureIdx >> 3 & 3; - plus1 = featureIdx >> 2 & 1; - dim1 = featureIdx & 3; -} - -inline -void -BoxPoint::getFaceFeature(int & dim, int & plus) const -{ - plus = featureIdx >> 2; - dim = featureIdx & 3; -} - -#endif /* __BOX_H__ */ diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp deleted file mode 100644 index dfcd8426695..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp +++ /dev/null @@ -1,302 +0,0 @@ -/* -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 "SpuCollisionShapes.h" - -///not supported on IBM SDK, until we fix the alignment of btVector3 -#if defined (__CELLOS_LV2__) && defined (__SPU__) -#include <spu_intrinsics.h> -static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) -{ - vec_float4 result; - result = spu_mul( vec0, vec1 ); - result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); - return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); -} -#endif //__SPU__ - - -void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform) -{ - //calculate the aabb, given the types... - switch (shapeType) - { - case CYLINDER_SHAPE_PROXYTYPE: - /* fall through */ - case BOX_SHAPE_PROXYTYPE: - { - btScalar margin=convexShape->getMarginNV(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - halfExtents += btVector3(margin,margin,margin); - const btTransform& t = xform; - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); - - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - btScalar margin=convexShape->getMarginNV(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - //add the radius to y-axis to get full height - btScalar radius = halfExtents[0]; - halfExtents[1] += radius; - halfExtents += btVector3(margin,margin,margin); -#if 0 - int capsuleUpAxis = convexShape->getUpAxis(); - btScalar halfHeight = convexShape->getHalfHeight(); - btScalar radius = convexShape->getRadius(); - halfExtents[capsuleUpAxis] = radius + halfHeight; -#endif - const btTransform& t = xform; - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); - - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - case SPHERE_SHAPE_PROXYTYPE: - { - btScalar radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); - btScalar margin = radius + convexShape->getMarginNV(); - const btTransform& t = xform; - const btVector3& center = t.getOrigin(); - btVector3 extent(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); - cellDmaGet(&convexHullShape0, convexShapePtr , sizeof(btConvexHullShape), DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; - const btTransform& t = xform; - 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"); - } - }; -} - -void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape) -{ - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - - dmaSize = sizeof(btTriangleIndexVertexArray); - dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getMeshInterface()); - // spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2); -#ifdef __SPU__ - cellDmaGet(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - bvhMeshShape->gTriangleMeshInterfacePtr = &bvhMeshShape->gTriangleMeshInterfaceStorage; -#else - bvhMeshShape->gTriangleMeshInterfacePtr = (btTriangleIndexVertexArray*)cellDmaGetReadOnly(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); -#endif - - //cellDmaWaitTagStatusAll(DMA_MASK(1)); - - ///now DMA over the BVH - - dmaSize = sizeof(btOptimizedBvh); - dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getOptimizedBvh()); - //spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2); - cellDmaGet(&bvhMeshShape->gOptimizedBvh, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); - //cellDmaWaitTagStatusAll(DMA_MASK(2)); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); -} - -void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag) -{ - cellDmaGet(IndexMesh, (ppu_address_t)&indexArray[index] , sizeof(btIndexedMesh), DMA_TAG(dmaTag), 0, 0); - -} - -void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag) -{ - cellDmaGet(subTreeHeaders, subTreePtr, batchSize * sizeof(btBvhSubtreeInfo), DMA_TAG(dmaTag), 0, 0); -} - -void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag) -{ - cellDmaGet(nodes, reinterpret_cast<ppu_address_t>(&nodeArray[subtree.m_rootNodeIndex]) , subtree.m_subtreeSize* sizeof(btQuantizedBvhNode), DMA_TAG(2), 0, 0); -} - -///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; - } - case STATIC_PLANE_PROXYTYPE: - { - int shapeSize = sizeof(btStaticPlaneShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - - default: - btAssert(0); - //unsupported shapetype, please add here - return 0; - } -} - -void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU) -{ - convexVertexData->gNumConvexPoints = convexShapeSPU->getNumPoints(); - if (convexVertexData->gNumConvexPoints>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,convexVertexData->gNumConvexPoints); - return; - } - - register int dmaSize = convexVertexData->gNumConvexPoints*sizeof(btVector3); - ppu_address_t pointsPPU = (ppu_address_t) convexShapeSPU->getUnscaledPoints(); - cellDmaGet(&convexVertexData->g_convexPointBuffer[0], pointsPPU , dmaSize, DMA_TAG(2), 0, 0); -} - -void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType) -{ - register int dmaSize = getShapeTypeSize(shapeType); - cellDmaGet(collisionShapeLocation, collisionShapePtr , dmaSize, DMA_TAG(dmaTag), 0, 0); - //cellDmaGetReadOnly(collisionShapeLocation, collisionShapePtr , dmaSize, DMA_TAG(dmaTag), 0, 0); - //cellDmaWaitTagStatusAll(DMA_MASK(dmaTag)); -} - -void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag) -{ - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - int childShapeCount = spuCompoundShape->getNumChildShapes(); - dmaSize = childShapeCount * sizeof(btCompoundShapeChild); - dmaPpuAddress2 = (ppu_address_t)spuCompoundShape->getChildList(); - cellDmaGet(&compoundShapeLocation->gSubshapes[0], dmaPpuAddress2, dmaSize, DMA_TAG(dmaTag), 0, 0); -} - -void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag) -{ - int childShapeCount = spuCompoundShape->getNumChildShapes(); - int i; - // DMA all the subshapes - for ( i = 0; i < childShapeCount; ++i) - { - btCompoundShapeChild& childShape = compoundShapeLocation->gSubshapes[i]; - dmaCollisionShape (&compoundShapeLocation->gSubshapeShape[i],(ppu_address_t)childShape.m_childShape, dmaTag, childShape.m_childShapeType); - } -} - - -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; -#ifdef BT_DEBUG - int subTreeSize = endNodeIndex - startNodeIndex; -#endif - - int escapeIndex; - - unsigned int aabbOverlap, isLeafNode; - - while (curIndex < endNodeIndex) - { - //catch bugs in tree data - btAssert (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; - } - } - -} diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h deleted file mode 100644 index d369395e160..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h +++ /dev/null @@ -1,126 +0,0 @@ -/* -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_COLLISION_SHAPES_H -#define __SPU_COLLISION_SHAPES_H - -#include "../SpuDoubleBuffer.h" - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.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" - -#define MAX_NUM_SPU_CONVEX_POINTS 128 - -ATTRIBUTE_ALIGNED16(struct) SpuConvexPolyhedronVertexData -{ - void* gSpuConvexShapePtr; - btVector3* gConvexPoints; - int gNumConvexPoints; - int unused; - ATTRIBUTE_ALIGNED16(btVector3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]); -}; - -#define MAX_SHAPE_SIZE 256 - -ATTRIBUTE_ALIGNED16(struct) CollisionShape_LocalStoreMemory -{ - ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]); -}; - -ATTRIBUTE_ALIGNED16(struct) CompoundShape_LocalStoreMemory -{ - // Compound data -#define MAX_SPU_COMPOUND_SUBSHAPES 16 - ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]); - ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]); -}; - -ATTRIBUTE_ALIGNED16(struct) bvhMeshShape_LocalStoreMemory -{ - //ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh); - ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]); - btOptimizedBvh* getOptimizedBvh() - { - return (btOptimizedBvh*) gOptimizedBvh; - } - - ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterfaceStorage); - btTriangleIndexVertexArray* gTriangleMeshInterfacePtr; - ///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)]); -}; - - -void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform); -void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape); -void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag); -void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag); -void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag); - -int getShapeTypeSize(int shapeType); -void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU); -void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType); -void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag); -void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag); - - -#define USE_BRANCHFREE_TEST 1 -#ifdef USE_BRANCHFREE_TEST -SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) -{ -#if defined(__CELLOS_LV2__) && defined (__SPU__) - vec_ushort8 vecMin = {aabbMin1[0],aabbMin2[0],aabbMin1[2],aabbMin2[2],aabbMin1[1],aabbMin2[1],0,0}; - vec_ushort8 vecMax = {aabbMax2[0],aabbMax1[0],aabbMax2[2],aabbMax1[2],aabbMax2[1],aabbMax1[1],0,0}; - vec_ushort8 isGt = spu_cmpgt(vecMin,vecMax); - return spu_extract(spu_gather(isGt),0)==0; - -#else - 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); -#endif -} -#else - -SIMD_FORCE_INLINE 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); - -#endif diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp deleted file mode 100644 index 8e540d9297b..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuContactResult.h" - -//#define DEBUG_SPU_COLLISION_DETECTION 1 - -#ifdef DEBUG_SPU_COLLISION_DETECTION -#ifndef __SPU__ -#include <stdio.h> -#define spu_printf printf -#endif -#endif DEBUG_SPU_COLLISION_DETECTION - -SpuContactResult::SpuContactResult() -{ - m_manifoldAddress = 0; - m_spuManifold = NULL; - m_RequiresWriteBack = false; -} - - SpuContactResult::~SpuContactResult() -{ - g_manifoldDmaExport.swapBuffers(); -} - - ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; -inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1) -{ - btScalar friction = friction0*friction1; - - const btScalar MAX_FRICTION = btScalar(10.); - - if (friction < -MAX_FRICTION) - friction = -MAX_FRICTION; - if (friction > MAX_FRICTION) - friction = MAX_FRICTION; - return friction; - -} - -inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1) -{ - return restitution0*restitution1; -} - - - - void SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, ppu_address_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction1, bool isSwapped) - { - //spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress); - m_rootWorldTransform0 = worldTrans0; - m_rootWorldTransform1 = worldTrans1; - m_manifoldAddress = manifoldAddress; - m_spuManifold = spuManifold; - - m_combinedFriction = calculateCombinedFriction(friction0,friction1); - m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1); - m_isSwapped = isSwapped; - } - - void SpuContactResult::setShapeIdentifiersA(int partId0,int index0) - { - - } - - void SpuContactResult::setShapeIdentifiersB(int partId1,int index1) - { - - } - - - - ///return true if it requires a dma transfer back -bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld, - const btVector3& pointInWorld, - float depth, - btPersistentManifold* manifoldPtr, - btTransform& transA, - btTransform& transB, - btScalar combinedFriction, - btScalar combinedRestitution, - bool isSwapped) -{ - -// float contactTreshold = manifoldPtr->getContactBreakingThreshold(); - - //spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr); - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: contactTreshold %f\n",contactTreshold); -#endif //DEBUG_SPU_COLLISION_DETECTION - if (depth > manifoldPtr->getContactBreakingThreshold()) - return false; - - btVector3 pointA; - btVector3 localA; - btVector3 localB; - btVector3 normal; - - - if (isSwapped) - { - normal = normalOnBInWorld * -1; - pointA = pointInWorld + normal * depth; - localA = transA.invXform(pointA ); - localB = transB.invXform(pointInWorld); - } - else - { - normal = normalOnBInWorld; - pointA = pointInWorld + normal * depth; - localA = transA.invXform(pointA ); - localB = transB.invXform(pointInWorld); - } - - btManifoldPoint newPt(localA,localB,normal,depth); - newPt.m_positionWorldOnA = pointA; - newPt.m_positionWorldOnB = pointInWorld; - - newPt.m_combinedFriction = combinedFriction; - newPt.m_combinedRestitution = combinedRestitution; - - - int insertIndex = manifoldPtr->getCacheEntry(newPt); - if (insertIndex >= 0) - { - // we need to replace the current contact point, otherwise small errors will accumulate (spheres start rolling etc) - manifoldPtr->replaceContactPoint(newPt,insertIndex); - return true; - - } else - { - - /* - ///@todo: SPU callbacks, either immediate (local on the SPU), or deferred - //User can override friction and/or restitution - if (gContactAddedCallback && - //and if either of the two bodies requires custom material - ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) || - (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback))) - { - //experimental feature info, for per-triangle material etc. - (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1); - } - */ - manifoldPtr->addManifoldPoint(newPt); - return true; - - } - return false; - -} - - -void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold) -{ - ///only write back the contact information on SPU. Other platforms avoid copying, and use the data in-place - ///see SpuFakeDma.cpp 'cellDmaLargeGetReadOnly' -#if defined (__SPU__) || defined (USE_LIBSPE2) - memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold)); - - g_manifoldDmaExport.swapBuffers(); - ppu_address_t mmAddr = (ppu_address_t)mmManifold; - g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9)); - // Should there be any kind of wait here? What if somebody tries to use this tag again? What if we call this function again really soon? - //no, the swapBuffers does the wait -#endif -} - -void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) -{ -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth); - spu_printf("*** normal = %f,%f,%f\n",normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ()); - spu_printf("*** position = %f,%f,%f\n",pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ()); -#endif //DEBUG_SPU_COLLISION_DETECTION - - -#ifdef DEBUG_SPU_COLLISION_DETECTION - // int sman = sizeof(rage::phManifold); -// spu_printf("sizeof_manifold = %i\n",sman); -#endif //DEBUG_SPU_COLLISION_DETECTION - - btPersistentManifold* localManifold = m_spuManifold; - - btVector3 normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ()); - btVector3 pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ()); - - //process the contact point - const bool retVal = ManifoldResultAddContactPoint(normalB, - pointWrld, - depth, - localManifold, - m_rootWorldTransform0, - m_rootWorldTransform1, - m_combinedFriction, - m_combinedRestitution, - m_isSwapped); - m_RequiresWriteBack = m_RequiresWriteBack || retVal; -} - -void SpuContactResult::flush() -{ - - if (m_spuManifold && m_spuManifold->getNumContacts()) - { - m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1); - m_RequiresWriteBack = true; - } - - - if (m_RequiresWriteBack) - { -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n"); - spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts()); - spu_printf("Manifold address: %llu\n", m_manifoldAddress); -#endif //DEBUG_SPU_COLLISION_DETECTION - // spu_printf("writeDoubleBufferedManifold\n"); - writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress); -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: Finished (Put) DMA\n"); -#endif //DEBUG_SPU_COLLISION_DETECTION - } - m_spuManifold = NULL; - m_RequiresWriteBack = false; -} - - diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h deleted file mode 100644 index 394f56dcbd1..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h +++ /dev/null @@ -1,106 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SPU_CONTACT_RESULT2_H -#define SPU_CONTACT_RESULT2_H - - -#ifndef _WIN32 -#include <stdint.h> -#endif - - - -#include "../SpuDoubleBuffer.h" - - -#include "LinearMath/btTransform.h" - - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" - -class btCollisionShape; - - -struct SpuCollisionPairInput -{ - ppu_address_t m_collisionShapes[2]; - btCollisionShape* m_spuCollisionShapes[2]; - - ppu_address_t m_persistentManifoldPtr; - btVector3 m_primitiveDimensions0; - btVector3 m_primitiveDimensions1; - int m_shapeType0; - int m_shapeType1; - float m_collisionMargin0; - float m_collisionMargin1; - - btTransform m_worldTransform0; - btTransform m_worldTransform1; - - bool m_isSwapped; - bool m_useEpa; -}; - - -struct SpuClosestPointInput : public btDiscreteCollisionDetectorInterface::ClosestPointInput -{ - struct SpuConvexPolyhedronVertexData* m_convexVertexData[2]; -}; - -///SpuContactResult exports the contact points using double-buffered DMA transfers, only when needed -///So when an existing contact point is duplicated, no transfer/refresh is performed. -class SpuContactResult : public btDiscreteCollisionDetectorInterface::Result -{ - btTransform m_rootWorldTransform0; - btTransform m_rootWorldTransform1; - ppu_address_t m_manifoldAddress; - - btPersistentManifold* m_spuManifold; - bool m_RequiresWriteBack; - btScalar m_combinedFriction; - btScalar m_combinedRestitution; - - bool m_isSwapped; - - DoubleBuffer<btPersistentManifold, 1> g_manifoldDmaExport; - - public: - SpuContactResult(); - virtual ~SpuContactResult(); - - btPersistentManifold* GetSpuManifold() const - { - return m_spuManifold; - } - - virtual void setShapeIdentifiersA(int partId0,int index0); - virtual void setShapeIdentifiersB(int partId1,int index1); - - void setContactInfo(btPersistentManifold* spuManifold, ppu_address_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction01, bool isSwapped); - - - void writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold); - - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); - - void flush(); -}; - - - -#endif //SPU_CONTACT_RESULT2_H - diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h deleted file mode 100644 index 449f19288c4..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h +++ /dev/null @@ -1,51 +0,0 @@ - -/* -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_CONVEX_PENETRATION_DEPTH_H -#define SPU_CONVEX_PENETRATION_DEPTH_H - - - -class btStackAlloc; -class btIDebugDraw; -#include "BulletCollision/NarrowphaseCollision/btConvexPenetrationDepthSolver.h" - -#include "LinearMath/btTransform.h" - - -///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. -class SpuConvexPenetrationDepthSolver : public btConvexPenetrationDepthSolver -{ -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, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, - struct SpuConvexPolyhedronVertexData* convexVertexDataA, - struct SpuConvexPolyhedronVertexData* convexVertexDataB - ) const = 0; - - -}; - - - -#endif //SPU_CONVEX_PENETRATION_DEPTH_H - diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp deleted file mode 100644 index c3dfaa793e3..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp +++ /dev/null @@ -1,1381 +0,0 @@ -/* -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 "SpuGatheringCollisionTask.h" - -//#define DEBUG_SPU_COLLISION_DETECTION 1 -#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/btConvexPointCloudShape.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 "SpuEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" - - -#include "boxBoxDistance.h" -#include "BulletMultiThreaded/vectormath2bullet.h" -#include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData -#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" - -#ifdef __SPU__ -///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases -#ifndef USE_LIBSPE2 -#define USE_SOFTWARE_CACHE 1 -#endif -#endif //__SPU__ - -int gSkippedCol = 0; -int gProcessedCol = 0; - -//////////////////////////////////////////////// -/// software caching -#if USE_SOFTWARE_CACHE -#include <spu_intrinsics.h> -#include <sys/spu_thread.h> -#include <sys/spu_event.h> -#include <stdint.h> -#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 "../Extras/software_cache/cache/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 - -bool gUseEpa = false; - -#ifdef USE_SN_TUNER -#include <LibSN_SPU.h> -#endif //USE_SN_TUNER - -#if defined (__SPU__) && !defined (USE_LIBSPE2) -#include <spu_printf.h> -#elif defined (USE_LIBSPE2) -#define spu_printf(a) -#else -#define IGNORE_ALIGNMENT 1 -#include <stdio.h> -#include <stdlib.h> -#define spu_printf printf - -#endif - -//int gNumConvexPoints0=0; - -///Make sure no destructors are called on this memory -struct CollisionTask_LocalStoreMemory -{ - ///This CollisionTask_LocalStoreMemory is mainly used for the SPU version, using explicit DMA - ///Other platforms can use other memory programming models. - - ATTRIBUTE_ALIGNED16(btBroadphasePair gBroadphasePairsBuffer[SPU_BATCHSIZE_BROADPHASE_PAIRS]); - DoubleBuffer<unsigned char, MIDPHASE_WORKUNIT_PAGE_SIZE> g_workUnitTaskBuffers; - ATTRIBUTE_ALIGNED16(char gSpuContactManifoldAlgoBuffer [sizeof(SpuContactManifoldCollisionAlgorithm)+16]); - ATTRIBUTE_ALIGNED16(char gColObj0Buffer [sizeof(btCollisionObject)+16]); - ATTRIBUTE_ALIGNED16(char gColObj1Buffer [sizeof(btCollisionObject)+16]); - ///we reserve 32bit integer indices, even though they might be 16bit - ATTRIBUTE_ALIGNED16(int spuIndices[16]); - btPersistentManifold gPersistentManifoldBuffer; - CollisionShape_LocalStoreMemory gCollisionShapes[2]; - bvhMeshShape_LocalStoreMemory bvhShapeData; - SpuConvexPolyhedronVertexData convexVertexData[2]; - CompoundShape_LocalStoreMemory compoundShapeData[2]; - - ///The following pointers might either point into this local store memory, or to the original/other memory locations. - ///See SpuFakeDma for implementation of cellDmaSmallGetReadOnly. - btCollisionObject* m_lsColObj0Ptr; - btCollisionObject* m_lsColObj1Ptr; - btBroadphasePair* m_pairsPointer; - btPersistentManifold* m_lsManifoldPtr; - SpuContactManifoldCollisionAlgorithm* m_lsCollisionAlgorithmPtr; - - bool needsDmaPutContactManifoldAlgo; - - btCollisionObject* getColObj0() - { - return m_lsColObj0Ptr; - } - btCollisionObject* getColObj1() - { - return m_lsColObj1Ptr; - } - - - btBroadphasePair* getBroadphasePairPtr() - { - return m_pairsPointer; - } - - SpuContactManifoldCollisionAlgorithm* getlocalCollisionAlgorithm() - { - return m_lsCollisionAlgorithmPtr; - } - - btPersistentManifold* getContactManifoldPtr() - { - return m_lsManifoldPtr; - } -}; - - -#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) - -ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory gLocalStoreMemory); - -void* createCollisionLocalStoreMemory() -{ - return &gLocalStoreMemory; -} -#else -void* createCollisionLocalStoreMemory() -{ - return new CollisionTask_LocalStoreMemory; -} - -#endif - -void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts); - - -SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_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 -} - -SIMD_FORCE_INLINE void small_cache_read_triple( void* ls0, ppu_address_t ea0, - void* ls1, ppu_address_t ea1, - void* ls2, ppu_address_t ea2, - size_t size) -{ - 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; -#ifdef __SPU__ - cellDmaSmallGet(tmpTarget0,ea0,size,DMA_TAG(1),0,0); -#else - tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0); -#endif - - - char* localStore1 = (char*)ls1; - last4BitsOffset = ea1 & 0x0f; - char* tmpTarget1 = tmpBuffer1 + last4BitsOffset; -#ifdef __SPU__ - cellDmaSmallGet(tmpTarget1,ea1,size,DMA_TAG(1),0,0); -#else - tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0); -#endif - - char* localStore2 = (char*)ls2; - last4BitsOffset = ea2 & 0x0f; - char* tmpTarget2 = tmpBuffer2 + last4BitsOffset; -#ifdef __SPU__ - cellDmaSmallGet(tmpTarget2,ea2,size,DMA_TAG(1),0,0); -#else - tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0); -#endif - - - cellDmaWaitTagStatusAll( DMA_MASK(1) ); - - //this is slowish, perhaps memcpy on SPU is smarter? - for (i=0; btLikely( i<size );i++) - { - localStore0[i] = tmpTarget0[i]; - localStore1[i] = tmpTarget1[i]; - localStore2[i] = tmpTarget2[i]; - } - - -} - - - - -class spuNodeCallback : public btNodeOverlapCallback -{ - SpuCollisionPairInput* m_wuInput; - SpuContactResult& m_spuContacts; - CollisionTask_LocalStoreMemory* m_lsMemPtr; - ATTRIBUTE_ALIGNED16(btTriangleShape) m_tmpTriangleShape; - - ATTRIBUTE_ALIGNED16(btVector3 spuTriangleVertices[3]); - ATTRIBUTE_ALIGNED16(btScalar spuUnscaledVertex[4]); - - - -public: - spuNodeCallback(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts) - : m_wuInput(wuInput), - m_spuContacts(spuContacts), - m_lsMemPtr(lsMemPtr) - { - } - - 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); - - if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT) - { - unsigned short int* indexBasePtr = (unsigned short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); - ATTRIBUTE_ALIGNED16(unsigned short int tmpIndices[3]); - - small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0], - &tmpIndices[1],(ppu_address_t)&indexBasePtr[1], - &tmpIndices[2],(ppu_address_t)&indexBasePtr[2], - sizeof(unsigned short int)); - - m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]); - m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]); - m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]); - } else - { - unsigned int* indexBasePtr = (unsigned int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); - - small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0], - &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1], - &m_lsMemPtr->spuIndices[2],(ppu_address_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->bvhShapeData.gTriangleMeshInterfacePtr->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->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride); - // spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); - - - ///handle un-aligned vertices... - - //another DMA for each vertex - small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0], - &spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1], - &spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2], - sizeof(btScalar)); - - m_tmpTriangleShape.getVertexPtr(j).setValue(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()); - } - - - SpuCollisionPairInput triangleConcaveInput(*m_wuInput); -// triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0]; - triangleConcaveInput.m_spuCollisionShapes[1] = &m_tmpTriangleShape; - triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE; - - m_spuContacts.setShapeIdentifiersB(subPart,triangleIndex); - - // m_spuContacts.flush(); - - ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts); - ///this flush should be automatic - // m_spuContacts.flush(); - } - -}; - - - -void btConvexPlaneCollideSingleContact (SpuCollisionPairInput* wuInput,CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts) -{ - - btConvexShape* convexShape = (btConvexShape*) wuInput->m_spuCollisionShapes[0]; - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) wuInput->m_spuCollisionShapes[1]; - - bool hasCollision = false; - const btVector3& planeNormal = planeShape->getPlaneNormal(); - const btScalar& planeConstant = planeShape->getPlaneConstant(); - - - btTransform convexWorldTransform = wuInput->m_worldTransform0; - btTransform convexInPlaneTrans; - convexInPlaneTrans= wuInput->m_worldTransform1.inverse() * convexWorldTransform; - btTransform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * wuInput->m_worldTransform1; - - //btVector3 vtx = convexShape->localGetSupportVertexWithoutMarginNonVirtual(planeInConvex.getBasis()*-planeNormal); - btVector3 vtx = convexShape->localGetSupportVertexNonVirtual(planeInConvex.getBasis()*-planeNormal); - - btVector3 vtxInPlane = convexInPlaneTrans(vtx); - btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; - btVector3 vtxInPlaneWorld = wuInput->m_worldTransform1 * vtxInPlaneProjected; - - hasCollision = distance < lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold(); - //resultOut->setPersistentManifold(m_manifoldPtr); - if (hasCollision) - { - /// report a contact. internally this will be kept persistent, and contact reduction is done - btVector3 normalOnSurfaceB =wuInput->m_worldTransform1.getBasis() * planeNormal; - btVector3 pOnB = vtxInPlaneWorld; - spuContacts.addContactPoint(normalOnSurfaceB,pOnB,distance); - } -} - -void ProcessConvexPlaneSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) -{ - - register int dmaSize = 0; - register ppu_address_t dmaPpuAddress2; - btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr; - - ///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); - dmaPpuAddress2 = wuInput->m_collisionShapes[0]; - - 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); - dmaPpuAddress2 = wuInput->m_collisionShapes[1]; - 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)); - dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0); - lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0]; - } - - - if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(1)); - dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1); - lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1]; - } - - - btConvexPointCloudShape cpc0,cpc1; - - if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(2)); - lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0]; - btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0]; - const btVector3& localScaling = ch->getLocalScalingNV(); - cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling); - wuInput->m_spuCollisionShapes[0] = &cpc0; - } - - if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(2)); - lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0]; - btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1]; - const btVector3& localScaling = ch->getLocalScalingNV(); - cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling); - wuInput->m_spuCollisionShapes[1] = &cpc1; - - } - - -// const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0]; -// const btConvexShape* shape1Ptr = (const btConvexShape*)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[0] = &lsMemPtr->convexVertexData[0]; - cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1]; - cpInput.m_transformA = wuInput->m_worldTransform0; - cpInput.m_transformB = wuInput->m_worldTransform1; - float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold()); - cpInput.m_maximumDistanceSquared = sumMargin * sumMargin; - - ppu_address_t manifoldAddress = (ppu_address_t)manifold; - - btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr(); - //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped); - spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(), - lsMemPtr->getColObj1()->getWorldTransform(), - lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(), - lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(), - wuInput->m_isSwapped); - - - btConvexPlaneCollideSingleContact(wuInput,lsMemPtr,spuContacts); - - - - -} - - - - -//////////////////////// -/// 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 - dmaBvhShapeData (&lsMemPtr->bvhShapeData, trimeshShape); - - 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]; - - computeAabb (aabbMin, aabbMax, convexShape, wuInput->m_collisionShapes[0], wuInput->m_shapeType0, convexInTriangleSpace); - - - //CollisionShape* triangleShape = static_cast<btCollisionShape*>(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->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0); - lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1); - - QuantizedNodeArray& nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray(); - //spu_printf("SPU: numNodes = %d\n",nodeArray.size()); - - BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray(); - - - spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts); - IndexedMeshArray& indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->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 - dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - //display the headers - int numBatch = subTrees.size(); - for (int i=0;i<numBatch;) - { - //@todo- can reorder DMA transfers for less stall - int remaining = subTrees.size() - i; - int nextBatch = remaining < MAX_SPU_SUBTREE_HEADERS ? remaining : MAX_SPU_SUBTREE_HEADERS; - - dmaBvhSubTreeHeaders (&lsMemPtr->bvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - // spu_printf("nextBatch = %d\n",nextBatch); - - for (int j=0;j<nextBatch;j++) - { - const btBvhSubtreeInfo& subtree = lsMemPtr->bvhShapeData.gSubtreeHeaders[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 - dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2); - cellDmaWaitTagStatusAll(DMA_MASK(2)); - - /* Walk this subtree */ - spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, - &lsMemPtr->bvhShapeData.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 - } - -} - - -int stats[11]={0,0,0,0,0,0,0,0,0,0,0}; -int degenerateStats[11]={0,0,0,0,0,0,0,0,0,0,0}; - - -//////////////////////// -/// 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; - register ppu_address_t dmaPpuAddress2; - -#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 - - - - //SpuConvexPenetrationDepthSolver* penetrationSolver=0; - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver epaPenetrationSolver2; - - btConvexPenetrationDepthSolver* penetrationSolver = &epaPenetrationSolver2; - - //SpuMinkowskiPenetrationDepthSolver minkowskiPenetrationSolver; -#ifdef ENABLE_EPA - if (gUseEpa) - { - penetrationSolver = &epaPenetrationSolver2; - } else -#endif - { - //penetrationSolver = &minkowskiPenetrationSolver; - } - - - ///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); - dmaPpuAddress2 = wuInput->m_collisionShapes[0]; - - 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); - dmaPpuAddress2 = wuInput->m_collisionShapes[1]; - 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)); - dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0); - lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0]; - } - - - if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(1)); - dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1); - lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1]; - } - - - btConvexPointCloudShape cpc0,cpc1; - - if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(2)); - lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0]; - btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0]; - const btVector3& localScaling = ch->getLocalScalingNV(); - cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling); - wuInput->m_spuCollisionShapes[0] = &cpc0; - } - - if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(2)); - lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0]; - btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1]; - const btVector3& localScaling = ch->getLocalScalingNV(); - cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling); - wuInput->m_spuCollisionShapes[1] = &cpc1; - - } - - - const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0]; - const btConvexShape* shape1Ptr = (const btConvexShape*)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[0] = &lsMemPtr->convexVertexData[0]; - cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1]; - cpInput.m_transformA = wuInput->m_worldTransform0; - cpInput.m_transformB = wuInput->m_worldTransform1; - float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold()); - cpInput.m_maximumDistanceSquared = sumMargin * sumMargin; - - ppu_address_t manifoldAddress = (ppu_address_t)manifold; - - btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr(); - //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped); - spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(), - lsMemPtr->getColObj1()->getWorldTransform(), - lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(), - lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(), - wuInput->m_isSwapped); - - { - btGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&simplexSolver,penetrationSolver);//&vsSolver,penetrationSolver); - gjk.getClosestPoints(cpInput,spuContacts,0);//,debugDraw); - - stats[gjk.m_lastUsedMethod]++; - degenerateStats[gjk.m_degenerateSimplex]++; - -#ifdef USE_SEPDISTANCE_UTIL - btScalar sepDist = gjk.getCachedSeparatingDistance()+spuManifold->getContactBreakingThreshold(); - lsMemPtr->getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(gjk.getCachedSeparatingAxis(),sepDist,wuInput->m_worldTransform0,wuInput->m_worldTransform1); - lsMemPtr->needsDmaPutContactManifoldAlgo = true; -#endif //USE_SEPDISTANCE_UTIL - - } - - } - - -} - - -template<typename T> 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; - register ppu_address_t dmaPpuAddress2; - - dmaSize = sizeof(btCollisionObject);//btTransform); - dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject0(); - lsMem.m_lsColObj0Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj0Buffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - - dmaSize = sizeof(btCollisionObject);//btTransform); - dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject1(); - lsMem.m_lsColObj1Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj1Buffer, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); - - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - - btCollisionObject* ob0 = lsMem.getColObj0(); - btCollisionObject* ob1 = lsMem.getColObj1(); - - collisionPairInput.m_worldTransform0 = ob0->getWorldTransform(); - collisionPairInput.m_worldTransform1 = ob1->getWorldTransform(); -} - - - -void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem, - SpuContactResult &spuContacts, - ppu_address_t collisionShape0Ptr, void* collisionShape0Loc, - ppu_address_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true) -{ - - if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) - && btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) - { - if (dmaShapes) - { - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - 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(); - - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - - // Both are compounds, do N^2 CD for now - ///@todo: add some AABB-based pruning (probably not -> slower) - - btCompoundShape* spuCompoundShape0 = (btCompoundShape*)collisionShape0Loc; - btCompoundShape* spuCompoundShape1 = (btCompoundShape*)collisionShape1Loc; - - dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape0, 1); - dmaCompoundShapeInfo (&lsMem.compoundShapeData[1], spuCompoundShape1, 2); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - - - dmaCompoundSubShapes (&lsMem.compoundShapeData[0], spuCompoundShape0, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - dmaCompoundSubShapes (&lsMem.compoundShapeData[1], spuCompoundShape1, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - int childShapeCount0 = spuCompoundShape0->getNumChildShapes(); - int childShapeCount1 = spuCompoundShape1->getNumChildShapes(); - - // Start the N^2 - for (int i = 0; i < childShapeCount0; ++i) - { - btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].gSubshapes[i]; - btAssert(!btBroadphaseProxy::isCompound(childShape0.m_childShapeType)); - - for (int j = 0; j < childShapeCount1; ++j) - { - btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].gSubshapes[j]; - btAssert(!btBroadphaseProxy::isCompound(childShape1.m_childShapeType)); - - - /* Create a new collision pair input struct using the two child shapes */ - 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; - /* Recursively call handleCollisionPair () with new collision pair input */ - - handleCollisionPair(cinput, lsMem, spuContacts, - (ppu_address_t)childShape0.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], - (ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false); - } - } - } - else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) ) - { - //snPause(); - - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - - // object 0 compound, object 1 non-compound - btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc; - dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - int childShapeCount = spuCompoundShape->getNumChildShapes(); - - for (int i = 0; i < childShapeCount; ++i) - { - btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i]; - btAssert(!btBroadphaseProxy::isCompound(childShape.m_childShapeType)); - // Dma the child shape - dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType); - 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, - (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], - collisionShape1Ptr, collisionShape1Loc, false); - } - } - else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) ) - { - //snPause(); - - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - // object 0 non-compound, object 1 compound - btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc; - dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - int childShapeCount = spuCompoundShape->getNumChildShapes(); - - for (int i = 0; i < childShapeCount; ++i) - { - btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i]; - btAssert(!btBroadphaseProxy::isCompound(childShape.m_childShapeType)); - // Dma the child shape - dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType); - 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, - (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].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) - { - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - } - - if (collisionPairInput.m_shapeType1 == STATIC_PLANE_PROXYTYPE) - { - btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc; - btStaticPlaneShape* planeShape= (btStaticPlaneShape*)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] = planeShape; - - ProcessConvexPlaneSpuCollision(&collisionPairInput,&lsMem,spuContacts); - } else - { - 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); - - gUseEpa = taskDesc.m_useEpa; - - // spu_printf("taskDescPtr=%llx\n",taskDescPtr); - - SpuContactResult spuContacts; - - //////////////////// - - ppu_address_t dmaInPtr = taskDesc.m_inPairPtr; - 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; - register ppu_address_t dmaPpuAddress; - register ppu_address_t dmaPpuAddress2; - - 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<SpuGatherAndProcessWorkUnitInput *>(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); - dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair); - lsMem.m_pairsPointer = (btBroadphasePair*)cellDmaGetReadOnly(&lsMem.gBroadphasePairsBuffer, dmaPpuAddress , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - for (p=0;p<numPairs;p++) - { - - //for each broadphase pair, do something - - btBroadphasePair& pair = lsMem.getBroadphasePairPtr()[p]; -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("pair->m_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 - - if (pair.m_internalTmpValue == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1) - { - dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm); - dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm; - lsMem.m_lsCollisionAlgorithmPtr = (SpuContactManifoldCollisionAlgorithm*)cellDmaGetReadOnly(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - lsMem.needsDmaPutContactManifoldAlgo = false; - - collisionPairInput.m_persistentManifoldPtr = (ppu_address_t) lsMem.getlocalCollisionAlgorithm()->getContactManifoldPtr(); - collisionPairInput.m_isSwapped = false; - - 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; - lsMem.m_lsManifoldPtr = (btPersistentManifold*)cellDmaGetReadOnly(&lsMem.gPersistentManifoldBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - - collisionPairInput.m_shapeType0 = lsMem.getlocalCollisionAlgorithm()->getShapeType0(); - collisionPairInput.m_shapeType1 = lsMem.getlocalCollisionAlgorithm()->getShapeType1(); - collisionPairInput.m_collisionMargin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); - collisionPairInput.m_collisionMargin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); - - - - //??cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - if (1) - { - //snPause(); - - // Get the collision objects - dmaAndSetupCollisionObjects(collisionPairInput, lsMem); - - if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive()) - { - - lsMem.needsDmaPutContactManifoldAlgo = true; -#ifdef USE_SEPDISTANCE_UTIL - lsMem.getlocalCollisionAlgorithm()->m_sepDistance.updateSeparatingDistance(collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1); -#endif //USE_SEPDISTANCE_UTIL - -#define USE_DEDICATED_BOX_BOX 1 -#ifdef USE_DEDICATED_BOX_BOX - bool boxbox = ((lsMem.getlocalCollisionAlgorithm()->getShapeType0()==BOX_SHAPE_PROXYTYPE)&& - (lsMem.getlocalCollisionAlgorithm()->getShapeType1()==BOX_SHAPE_PROXYTYPE)); - if (boxbox) - { - //spu_printf("boxbox dist = %f\n",distance); - btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr(); - btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr; - ppu_address_t manifoldAddress = (ppu_address_t)manifold; - - spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(), - lsMem.getColObj1()->getWorldTransform(), - lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(), - lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(), - collisionPairInput.m_isSwapped); - - - //float distance=0.f; - btVector3 normalInB; - - - if (//!gUseEpa && -#ifdef USE_SEPDISTANCE_UTIL - lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f -#else - 1 -#endif - ) - { -//#define USE_PE_BOX_BOX 1 -#ifdef USE_PE_BOX_BOX - { - - //getCollisionMargin0 - btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); - btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); - btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0); - btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1); - - Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ()); - Vector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin()); - Vector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin()); - Matrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis()); - Matrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis()); - - Transform3 transformA(vmMatrix0,vmPos0); - Box boxB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ()); - Transform3 transformB(vmMatrix1,vmPos1); - BoxPoint resultClosestBoxPointA; - BoxPoint resultClosestBoxPointB; - Vector3 resultNormal; -#ifdef USE_SEPDISTANCE_UTIL - float distanceThreshold = FLT_MAX -#else - float distanceThreshold = 0.f; -#endif - - - distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB, boxA, transformA, boxB,transformB,distanceThreshold); - - normalInB = -getBtVector3(resultNormal); - - if(distance < spuManifold->getContactBreakingThreshold()) - { - btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(resultClosestBoxPointB.localPoint)); - - spuContacts.addContactPoint( - normalInB, - pointOnB, - distance); - } - } -#else - { - - btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); - btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); - btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0); - btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1); - - - btBoxShape box0(shapeDim0); - btBoxShape box1(shapeDim1); - - struct SpuBridgeContactCollector : public btDiscreteCollisionDetectorInterface::Result - { - SpuContactResult& m_spuContacts; - - virtual void setShapeIdentifiersA(int partId0,int index0) - { - m_spuContacts.setShapeIdentifiersA(partId0,index0); - } - virtual void setShapeIdentifiersB(int partId1,int index1) - { - m_spuContacts.setShapeIdentifiersB(partId1,index1); - } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) - { - m_spuContacts.addContactPoint(normalOnBInWorld,pointInWorld,depth); - } - - SpuBridgeContactCollector(SpuContactResult& spuContacts) - :m_spuContacts(spuContacts) - { - - } - }; - - SpuBridgeContactCollector bridgeOutput(spuContacts); - - btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = BT_LARGE_FLOAT; - input.m_transformA = collisionPairInput.m_worldTransform0; - input.m_transformB = collisionPairInput.m_worldTransform1; - - btBoxBoxDetector detector(&box0,&box1); - - detector.getClosestPoints(input,bridgeOutput,0); - - } -#endif //USE_PE_BOX_BOX - - lsMem.needsDmaPutContactManifoldAlgo = true; -#ifdef USE_SEPDISTANCE_UTIL - btScalar sepDist2 = distance+spuManifold->getContactBreakingThreshold(); - lsMem.getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(normalInB,sepDist2,collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1); -#endif //USE_SEPDISTANCE_UTIL - gProcessedCol++; - } else - { - gSkippedCol++; - } - - spuContacts.flush(); - - - } else -#endif //USE_DEDICATED_BOX_BOX - { - if ( -#ifdef USE_SEPDISTANCE_UTIL - lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f -#else - 1 -#endif //USE_SEPDISTANCE_UTIL - ) - { - handleCollisionPair(collisionPairInput, lsMem, spuContacts, - (ppu_address_t)lsMem.getColObj0()->getRootCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape, - (ppu_address_t)lsMem.getColObj1()->getRootCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape); - } else - { - //spu_printf("boxbox dist = %f\n",distance); - btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr(); - btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr; - ppu_address_t manifoldAddress = (ppu_address_t)manifold; - - spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(), - lsMem.getColObj1()->getWorldTransform(), - lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(), - lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(), - collisionPairInput.m_isSwapped); - - spuContacts.flush(); - } - } - - } - - } - } - -#ifdef USE_SEPDISTANCE_UTIL -#if defined (__SPU__) || defined (USE_LIBSPE2) - if (lsMem.needsDmaPutContactManifoldAlgo) - { - dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm); - dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm; - cellDmaLargePut(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } -#endif -#endif //#ifdef USE_SEPDISTANCE_UTIL - - } - } - } - } //end for (j = 0; j < numOnPage; j++) - - }// for - - - - return; -} - - diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h deleted file mode 100644 index bbaa555ee1b..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h +++ /dev/null @@ -1,140 +0,0 @@ -/* -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 -{ - ppu_address_t m_inPairPtr;//m_pairArrayPtr; - //mutex variable - uint32_t m_someMutexVariableInMainMemory; - - ppu_address_t m_dispatcher; - - uint32_t numOnLastPage; - - uint16_t numPages; - uint16_t taskId; - bool m_useEpa; - - 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 <spu_intrinsics.h> -#include <spu_mfcio.h> -#include <SpuFakeDma.h> - -//#define DEBUG_LIBSPE2_SPU_TASK - - - -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 )) - { -#ifdef DEBUG_LIBSPE2_SPU_TASK - printf("SPU: received Spu_Mailbox_Event_Task\n"); -#endif //DEBUG_LIBSPE2_SPU_TASK - - // 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)); -#ifdef DEBUG_LIBSPE2_SPU_TASK - printf("SPU:processCollisionTask\n"); -#endif //DEBUG_LIBSPE2_SPU_TASK - processCollisionTask((void*)&taskDesc, taskDesc.m_lsMemory); - -#ifdef DEBUG_LIBSPE2_SPU_TASK - printf("SPU:finished processCollisionTask\n"); -#endif //DEBUG_LIBSPE2_SPU_TASK - } - else - { -#ifdef DEBUG_LIBSPE2_SPU_TASK - printf("SPU: received ShutDown\n"); -#endif //DEBUG_LIBSPE2_SPU_TASK - 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/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h deleted file mode 100644 index 8b89de03f59..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h +++ /dev/null @@ -1,19 +0,0 @@ -/* -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. -*/ - - - - - diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp deleted file mode 100644 index 9f7e64dd1b3..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp +++ /dev/null @@ -1,348 +0,0 @@ -/* -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 "SpuContactResult.h" -#include "SpuPreferredPenetrationDirections.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "SpuCollisionShapes.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( btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc) -{ -#if 0 - (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 setShapeIdentifiersA(int partId0,int index0) - { - (void)partId0; - (void)index0; - } - - virtual void setShapeIdentifiersB(int partId1,int index1) - { - (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(BT_LARGE_FLOAT); - btVector3 minNorm(0.f,0.f,0.f); - 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;i<numSampleDirections;i++) - { - const btVector3& norm = sPenetrationDirections[i]; - seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; - seperatingAxisInBBatch[i] = norm * transB.getBasis() ; - } - - { - int numPDA = convexA->getNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i=0;i<numPDA;i++) - { - btVector3 norm; - convexA->getPreferredPenetrationDirection(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;i<numPDB;i++) - { - btVector3 norm; - convexB->getPreferredPenetrationDirection(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;i<numSampleDirections;i++) - { - const btVector3& norm = sPenetrationDirections[i]; - seperatingAxisInA = seperatingAxisInABatch[i]; - seperatingAxisInB = seperatingAxisInBBatch[i]; - - pInA = supportVerticesABatch[i]; - qInB = supportVerticesBBatch[i]; - - 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; - } - } -#else - - int numSampleDirections = NUM_UNITSPHERE_POINTS; - -///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh -#define DO_PREFERRED_DIRECTIONS 1 -#ifdef DO_PREFERRED_DIRECTIONS - { - int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA); - if (numPDA) - { - for (int i=0;i<numPDA;i++) - { - btVector3 norm; - spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm); - norm = transA.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - - { - int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB); - if (numPDB) - { - for (int i=0;i<numPDB;i++) - { - btVector3 norm; - spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm); - norm = transB.getBasis() * norm; - sPenetrationDirections[numSampleDirections] = norm; - numSampleDirections++; - } - } - } -#endif //DO_PREFERRED_DIRECTIONS - - for (int i=0;i<numSampleDirections;i++) - { - const btVector3& norm = sPenetrationDirections[i]; - seperatingAxisInA = (-norm)* transA.getBasis(); - seperatingAxisInB = norm* transB.getBasis(); - - pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual( seperatingAxisInA);//, NULL); - qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);//, NULL); - - // pInA = convexA->localGetSupportingVertexWithoutMargin(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) + btScalar(1.00); - - - - - -//#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 - - - btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); - - btScalar offsetDist = minProj; - btVector3 offset = minNorm * offsetDist; - - - SpuClosestPointInput input; - input.m_convexVertexData[0] = convexVertexDataA; - input.m_convexVertexData[1] = convexVertexDataB; - btVector3 newOrg = transA.getOrigin() + offset; - - btTransform displacedTrans = transA; - displacedTrans.setOrigin(newOrg); - - input.m_transformA = displacedTrans; - input.m_transformB = transB; - input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; - - btIntermediateResult res; - gjkdet.getClosestPoints(input,res,0); - - 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 - - - } else { - // could not seperate shapes - //btAssert (false); - } - return res.m_hasResult; -#endif - return false; -} - - - diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h deleted file mode 100644 index 18ad223ed36..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h +++ /dev/null @@ -1,48 +0,0 @@ - -/* -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 "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" - -class btStackAlloc; -class btIDebugDraw; -class btVoronoiSimplexSolver; -class btConvexShape; - -///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 btConvexPenetrationDepthSolver -{ -public: - SpuMinkowskiPenetrationDepthSolver() {} - virtual ~SpuMinkowskiPenetrationDepthSolver() {}; - - virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc - ); - - -}; - - -#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H - diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h deleted file mode 100644 index 774a0cb2eb1..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h +++ /dev/null @@ -1,70 +0,0 @@ -/* -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/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp deleted file mode 100644 index 30642a39294..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp +++ /dev/null @@ -1,1155 +0,0 @@ -/* - Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. - All rights reserved. - -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 "Box.h" - -static inline float sqr( float a ) -{ - return (a * a); -} - -enum BoxSepAxisType -{ - A_AXIS, B_AXIS, CROSS_AXIS -}; - -//------------------------------------------------------------------------------------------------- -// voronoiTol: bevels Voronoi planes slightly which helps when features are parallel. -//------------------------------------------------------------------------------------------------- - -static const float voronoiTol = -1.0e-5f; - -//------------------------------------------------------------------------------------------------- -// separating axis tests: gaps along each axis are computed, and the axis with the maximum -// gap is stored. cross product axes are normalized. -//------------------------------------------------------------------------------------------------- - -#define AaxisTest( dim, letter, first ) \ -{ \ - if ( first ) \ - { \ - maxGap = gap = gapsA.get##letter(); \ - if ( gap > distanceThreshold ) return gap; \ - axisType = A_AXIS; \ - faceDimA = dim; \ - axisA = identity.getCol##dim(); \ - } \ - else \ - { \ - gap = gapsA.get##letter(); \ - if ( gap > distanceThreshold ) return gap; \ - else if ( gap > maxGap ) \ - { \ - maxGap = gap; \ - axisType = A_AXIS; \ - faceDimA = dim; \ - axisA = identity.getCol##dim(); \ - } \ - } \ -} - - -#define BaxisTest( dim, letter ) \ -{ \ - gap = gapsB.get##letter(); \ - if ( gap > distanceThreshold ) return gap; \ - else if ( gap > maxGap ) \ - { \ - maxGap = gap; \ - axisType = B_AXIS; \ - faceDimB = dim; \ - axisB = identity.getCol##dim(); \ - } \ -} - -#define CrossAxisTest( dima, dimb, letterb ) \ -{ \ - const float lsqr_tolerance = 1.0e-30f; \ - float lsqr; \ - \ - lsqr = lsqrs.getCol##dima().get##letterb(); \ - \ - if ( lsqr > lsqr_tolerance ) \ - { \ - float l_recip = 1.0f / sqrtf( lsqr ); \ - gap = float(gapsAxB.getCol##dima().get##letterb()) * l_recip; \ - \ - if ( gap > distanceThreshold ) \ - { \ - return gap; \ - } \ - \ - if ( gap > maxGap ) \ - { \ - maxGap = gap; \ - axisType = CROSS_AXIS; \ - edgeDimA = dima; \ - edgeDimB = dimb; \ - axisA = cross(identity.getCol##dima(),matrixAB.getCol##dimb()) * l_recip; \ - } \ - } \ -} - -//------------------------------------------------------------------------------------------------- -// tests whether a vertex of box B and a face of box A are the closest features -//------------------------------------------------------------------------------------------------- - -inline -float -VertexBFaceATest( - bool & inVoronoi, - float & t0, - float & t1, - const Vector3 & hA, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesB ) -{ - // compute a corner of box B in A's coordinate system - - Vector3 corner = - Vector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() ); - - // compute the parameters of the point on A, closest to this corner - - t0 = corner[0]; - t1 = corner[1]; - - if ( t0 > hA[0] ) - t0 = hA[0]; - else if ( t0 < -hA[0] ) - t0 = -hA[0]; - if ( t1 > hA[1] ) - t1 = hA[1]; - else if ( t1 < -hA[1] ) - t1 = -hA[1]; - - // do the Voronoi test: already know the point on B is in the Voronoi region of the - // point on A, check the reverse. - - Vector3 facePointB = - Vector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) ); - - inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) && - ( facePointB[1] >= voronoiTol * facePointB[0] ) && - ( facePointB[2] >= voronoiTol * facePointB[1] ) ); - - return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] )); -} - -#define VertexBFaceA_SetNewMin() \ -{ \ - minDistSqr = distSqr; \ - localPointA.setX(t0); \ - localPointA.setY(t1); \ - localPointB.setX( scalesB.getX() ); \ - localPointB.setY( scalesB.getY() ); \ - featureA = F; \ - featureB = V; \ -} - -void -VertexBFaceATests( - bool & done, - float & minDistSqr, - Point3 & localPointA, - Point3 & localPointB, - FeatureType & featureA, - FeatureType & featureB, - const Vector3 & hA, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesB, - bool first ) -{ - - float t0, t1; - float distSqr; - - distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsB, scalesB ); - - if ( first ) { - VertexBFaceA_SetNewMin(); - } else { - if ( distSqr < minDistSqr ) { - VertexBFaceA_SetNewMin(); - } - } - - if ( done ) - return; - - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsB, scalesB ); - - if ( distSqr < minDistSqr ) { - VertexBFaceA_SetNewMin(); - } - - if ( done ) - return; - - signsB.setY( -signsB.getY() ); - scalesB.setY( -scalesB.getY() ); - - distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsB, scalesB ); - - if ( distSqr < minDistSqr ) { - VertexBFaceA_SetNewMin(); - } - - if ( done ) - return; - - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsB, scalesB ); - - if ( distSqr < minDistSqr ) { - VertexBFaceA_SetNewMin(); - } -} - -//------------------------------------------------------------------------------------------------- -// VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features -//------------------------------------------------------------------------------------------------- - -inline -float -VertexAFaceBTest( - bool & inVoronoi, - float & t0, - float & t1, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) scalesA ) -{ - Vector3 corner = - Vector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() ); - - t0 = corner[0]; - t1 = corner[1]; - - if ( t0 > hB[0] ) - t0 = hB[0]; - else if ( t0 < -hB[0] ) - t0 = -hB[0]; - if ( t1 > hB[1] ) - t1 = hB[1]; - else if ( t1 < -hB[1] ) - t1 = -hB[1]; - - Vector3 facePointA = - Vector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) ); - - inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) && - ( facePointA[1] >= voronoiTol * facePointA[0] ) && - ( facePointA[2] >= voronoiTol * facePointA[1] ) ); - - return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] )); -} - -#define VertexAFaceB_SetNewMin() \ -{ \ - minDistSqr = distSqr; \ - localPointB.setX(t0); \ - localPointB.setY(t1); \ - localPointA.setX( scalesA.getX() ); \ - localPointA.setY( scalesA.getY() ); \ - featureA = V; \ - featureB = F; \ -} - -void -VertexAFaceBTests( - bool & done, - float & minDistSqr, - Point3 & localPointA, - Point3 & localPointB, - FeatureType & featureA, - FeatureType & featureB, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) scalesA, - bool first ) -{ - float t0, t1; - float distSqr; - - distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, scalesA ); - - if ( first ) { - VertexAFaceB_SetNewMin(); - } else { - if ( distSqr < minDistSqr ) { - VertexAFaceB_SetNewMin(); - } - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, scalesA ); - - if ( distSqr < minDistSqr ) { - VertexAFaceB_SetNewMin(); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, scalesA ); - - if ( distSqr < minDistSqr ) { - VertexAFaceB_SetNewMin(); - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, scalesA ); - - if ( distSqr < minDistSqr ) { - VertexAFaceB_SetNewMin(); - } -} - -//------------------------------------------------------------------------------------------------- -// EdgeEdgeTest: -// -// tests whether a pair of edges are the closest features -// -// note on the shorthand: -// 'a' & 'b' refer to the edges. -// 'c' is the dimension of the axis that points from the face center to the edge Center -// 'd' is the dimension of the edge Direction -// the dimension of the face normal is 2 -//------------------------------------------------------------------------------------------------- - -#define EdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \ -{ \ - Vector3 edgeOffsetAB; \ - Vector3 edgeOffsetBA; \ - \ - edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter(); \ - edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() ); \ - \ - edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter(); \ - edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() ); \ - \ - float dirDot = matrixAB.getCol##bd().get##ad_letter(); \ - float denom = 1.0f - dirDot*dirDot; \ - float edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter(); \ - float edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter(); \ - \ - if ( denom == 0.0f ) \ - { \ - tA = 0.0f; \ - } \ - else \ - { \ - tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom; \ - } \ - \ - if ( tA < -hA[ad] ) tA = -hA[ad]; \ - else if ( tA > hA[ad] ) tA = hA[ad]; \ - \ - tB = tA * dirDot + edgeOffsetBA_bd; \ - \ - if ( tB < -hB[bd] ) \ - { \ - tB = -hB[bd]; \ - tA = tB * dirDot + edgeOffsetAB_ad; \ - \ - if ( tA < -hA[ad] ) tA = -hA[ad]; \ - else if ( tA > hA[ad] ) tA = hA[ad]; \ - } \ - else if ( tB > hB[bd] ) \ - { \ - tB = hB[bd]; \ - tA = tB * dirDot + edgeOffsetAB_ad; \ - \ - if ( tA < -hA[ad] ) tA = -hA[ad]; \ - else if ( tA > hA[ad] ) tA = hA[ad]; \ - } \ - \ - Vector3 edgeOffAB = Vector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\ - Vector3 edgeOffBA = Vector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\ - \ - inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) && \ - ( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) && \ - ( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) && \ - ( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] ); \ - \ - edgeOffAB[ad] -= tA; \ - edgeOffBA[bd] -= tB; \ - \ - return dot(edgeOffAB,edgeOffAB); \ -} - -float -EdgeEdgeTest_0101( - bool & inVoronoi, - float & tA, - float & tB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB ) -{ - EdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y ); -} - -float -EdgeEdgeTest_0110( - bool & inVoronoi, - float & tA, - float & tB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB ) -{ - EdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X ); -} - -float -EdgeEdgeTest_1001( - bool & inVoronoi, - float & tA, - float & tB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB ) -{ - EdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y ); -} - -float -EdgeEdgeTest_1010( - bool & inVoronoi, - float & tA, - float & tB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB ) -{ - EdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X ); -} - -#define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter ) \ -{ \ - minDistSqr = distSqr; \ - localPointA.set##ac_letter(scalesA.get##ac_letter()); \ - localPointA.set##ad_letter(tA); \ - localPointB.set##bc_letter(scalesB.get##bc_letter()); \ - localPointB.set##bd_letter(tB); \ - otherFaceDimA = testOtherFaceDimA; \ - otherFaceDimB = testOtherFaceDimB; \ - featureA = E; \ - featureB = E; \ -} - -void -EdgeEdgeTests( - bool & done, - float & minDistSqr, - Point3 & localPointA, - Point3 & localPointB, - int & otherFaceDimA, - int & otherFaceDimB, - FeatureType & featureA, - FeatureType & featureB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB, - bool first ) -{ - - float distSqr; - float tA, tB; - - int testOtherFaceDimA, testOtherFaceDimB; - - testOtherFaceDimA = 0; - testOtherFaceDimB = 0; - - distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( first ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } else { - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } - - if ( done ) - return; - - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } - - if ( done ) - return; - - testOtherFaceDimA = 1; - testOtherFaceDimB = 0; - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, X, Y ); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, X, Y ); - } - - if ( done ) - return; - - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, X, Y ); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, X, Y ); - } - - if ( done ) - return; - - testOtherFaceDimA = 0; - testOtherFaceDimB = 1; - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, Y, X ); - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, Y, X ); - } - - if ( done ) - return; - - signsB.setY( -signsB.getY() ); - scalesB.setY( -scalesB.getY() ); - - distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, Y, X ); - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, Y, X ); - } - - if ( done ) - return; - - testOtherFaceDimA = 1; - testOtherFaceDimB = 1; - signsB.setY( -signsB.getY() ); - scalesB.setY( -scalesB.getY() ); - - distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, Y, X ); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, Y, X ); - } - - if ( done ) - return; - - signsB.setY( -signsB.getY() ); - scalesB.setY( -scalesB.getY() ); - - distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, Y, X ); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, Y, X ); - } -} - -float -boxBoxDistance( - Vector3& normal, - BoxPoint& boxPointA, - BoxPoint& boxPointB, - PE_REF(Box) boxA, const Transform3& transformA, - PE_REF(Box) boxB, const Transform3& transformB, - float distanceThreshold ) -{ - Matrix3 identity; - identity = Matrix3::identity(); - Vector3 ident[3]; - ident[0] = identity.getCol0(); - ident[1] = identity.getCol1(); - ident[2] = identity.getCol2(); - - // get relative transformations - - Transform3 transformAB, transformBA; - Matrix3 matrixAB, matrixBA; - Vector3 offsetAB, offsetBA; - - transformAB = orthoInverse(transformA) * transformB; - transformBA = orthoInverse(transformAB); - - matrixAB = transformAB.getUpper3x3(); - offsetAB = transformAB.getTranslation(); - matrixBA = transformBA.getUpper3x3(); - offsetBA = transformBA.getTranslation(); - - Matrix3 absMatrixAB = absPerElem(matrixAB); - Matrix3 absMatrixBA = absPerElem(matrixBA); - - // find separating axis with largest gap between projections - - BoxSepAxisType axisType; - Vector3 axisA(0.0f), axisB(0.0f); - float gap, maxGap; - int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0; - - // face axes - - Vector3 gapsA = absPerElem(offsetAB) - boxA.half - absMatrixAB * boxB.half; - - AaxisTest(0,X,true); - AaxisTest(1,Y,false); - AaxisTest(2,Z,false); - - Vector3 gapsB = absPerElem(offsetBA) - boxB.half - absMatrixBA * boxA.half; - - BaxisTest(0,X); - BaxisTest(1,Y); - BaxisTest(2,Z); - - // cross product axes - - // 外積が０のときの対策 - absMatrixAB += Matrix3(1.0e-5f); - absMatrixBA += Matrix3(1.0e-5f); - - Matrix3 lsqrs, projOffset, projAhalf, projBhalf; - - lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) + - mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) ); - lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) + - mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) ); - lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) + - mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) ); - - projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY()); - projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ()); - projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX()); - - projAhalf.setCol0(absMatrixBA.getCol1() * boxA.half.getZ() + absMatrixBA.getCol2() * boxA.half.getY()); - projAhalf.setCol1(absMatrixBA.getCol2() * boxA.half.getX() + absMatrixBA.getCol0() * boxA.half.getZ()); - projAhalf.setCol2(absMatrixBA.getCol0() * boxA.half.getY() + absMatrixBA.getCol1() * boxA.half.getX()); - - projBhalf.setCol0(absMatrixAB.getCol1() * boxB.half.getZ() + absMatrixAB.getCol2() * boxB.half.getY()); - projBhalf.setCol1(absMatrixAB.getCol2() * boxB.half.getX() + absMatrixAB.getCol0() * boxB.half.getZ()); - projBhalf.setCol2(absMatrixAB.getCol0() * boxB.half.getY() + absMatrixAB.getCol1() * boxB.half.getX()); - - Matrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf); - - CrossAxisTest(0,0,X); - CrossAxisTest(0,1,Y); - CrossAxisTest(0,2,Z); - CrossAxisTest(1,0,X); - CrossAxisTest(1,1,Y); - CrossAxisTest(1,2,Z); - CrossAxisTest(2,0,X); - CrossAxisTest(2,1,Y); - CrossAxisTest(2,2,Z); - - // need to pick the face on each box whose normal best matches the separating axis. - // will transform vectors to be in the coordinate system of this face to simplify things later. - // for this, a permutation matrix can be used, which the next section computes. - - int dimA[3], dimB[3]; - - if ( axisType == A_AXIS ) { - if ( dot(axisA,offsetAB) < 0.0f ) - axisA = -axisA; - axisB = matrixBA * -axisA; - - Vector3 absAxisB = Vector3(absPerElem(axisB)); - - if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) ) - faceDimB = 0; - else if ( absAxisB[1] > absAxisB[2] ) - faceDimB = 1; - else - faceDimB = 2; - } else if ( axisType == B_AXIS ) { - if ( dot(axisB,offsetBA) < 0.0f ) - axisB = -axisB; - axisA = matrixAB * -axisB; - - Vector3 absAxisA = Vector3(absPerElem(axisA)); - - if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) ) - faceDimA = 0; - else if ( absAxisA[1] > absAxisA[2] ) - faceDimA = 1; - else - faceDimA = 2; - } - - if ( axisType == CROSS_AXIS ) { - if ( dot(axisA,offsetAB) < 0.0f ) - axisA = -axisA; - axisB = matrixBA * -axisA; - - Vector3 absAxisA = Vector3(absPerElem(axisA)); - Vector3 absAxisB = Vector3(absPerElem(axisB)); - - dimA[1] = edgeDimA; - dimB[1] = edgeDimB; - - if ( edgeDimA == 0 ) { - if ( absAxisA[1] > absAxisA[2] ) { - dimA[0] = 2; - dimA[2] = 1; - } else { - dimA[0] = 1; - dimA[2] = 2; - } - } else if ( edgeDimA == 1 ) { - if ( absAxisA[2] > absAxisA[0] ) { - dimA[0] = 0; - dimA[2] = 2; - } else { - dimA[0] = 2; - dimA[2] = 0; - } - } else { - if ( absAxisA[0] > absAxisA[1] ) { - dimA[0] = 1; - dimA[2] = 0; - } else { - dimA[0] = 0; - dimA[2] = 1; - } - } - - if ( edgeDimB == 0 ) { - if ( absAxisB[1] > absAxisB[2] ) { - dimB[0] = 2; - dimB[2] = 1; - } else { - dimB[0] = 1; - dimB[2] = 2; - } - } else if ( edgeDimB == 1 ) { - if ( absAxisB[2] > absAxisB[0] ) { - dimB[0] = 0; - dimB[2] = 2; - } else { - dimB[0] = 2; - dimB[2] = 0; - } - } else { - if ( absAxisB[0] > absAxisB[1] ) { - dimB[0] = 1; - dimB[2] = 0; - } else { - dimB[0] = 0; - dimB[2] = 1; - } - } - } else { - dimA[2] = faceDimA; - dimA[0] = (faceDimA+1)%3; - dimA[1] = (faceDimA+2)%3; - dimB[2] = faceDimB; - dimB[0] = (faceDimB+1)%3; - dimB[1] = (faceDimB+2)%3; - } - - Matrix3 aperm_col, bperm_col; - - aperm_col.setCol0(ident[dimA[0]]); - aperm_col.setCol1(ident[dimA[1]]); - aperm_col.setCol2(ident[dimA[2]]); - - bperm_col.setCol0(ident[dimB[0]]); - bperm_col.setCol1(ident[dimB[1]]); - bperm_col.setCol2(ident[dimB[2]]); - - Matrix3 aperm_row, bperm_row; - - aperm_row = transpose(aperm_col); - bperm_row = transpose(bperm_col); - - // permute all box parameters to be in the face coordinate systems - - Matrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col; - Matrix3 matrixBA_perm = transpose(matrixAB_perm); - - Vector3 offsetAB_perm, offsetBA_perm; - - offsetAB_perm = aperm_row * offsetAB; - offsetBA_perm = bperm_row * offsetBA; - - Vector3 halfA_perm, halfB_perm; - - halfA_perm = aperm_row * boxA.half; - halfB_perm = bperm_row * boxB.half; - - // compute the vector between the centers of each face, in each face's coordinate frame - - Vector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm; - - signsA_perm = copySignPerElem(Vector3(1.0f),aperm_row * axisA); - signsB_perm = copySignPerElem(Vector3(1.0f),bperm_row * axisB); - scalesA_perm = mulPerElem( signsA_perm, halfA_perm ); - scalesB_perm = mulPerElem( signsB_perm, halfB_perm ); - - faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ(); - faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() ); - - faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ(); - faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() ); - - if ( maxGap < 0.0f ) { - // if boxes overlap, this will separate the faces for finding points of penetration. - - faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f; - faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f; - } - - // for each vertex/face or edge/edge pair of the two faces, find the closest points. - // - // these points each have an associated box feature (vertex, edge, or face). if each - // point is in the external Voronoi region of the other's feature, they are the - // closest points of the boxes, and the algorithm can exit. - // - // the feature pairs are arranged so that in the general case, the first test will - // succeed. degenerate cases (parallel faces) may require up to all tests in the - // worst case. - // - // if for some reason no case passes the Voronoi test, the features with the minimum - // distance are returned. - - Point3 localPointA_perm, localPointB_perm; - float minDistSqr; - bool done; - - Vector3 hA_perm( halfA_perm ), hB_perm( halfB_perm ); - - localPointA_perm.setZ( scalesA_perm.getZ() ); - localPointB_perm.setZ( scalesB_perm.getZ() ); - scalesA_perm.setZ(0.0f); - scalesB_perm.setZ(0.0f); - - int otherFaceDimA, otherFaceDimB; - FeatureType featureA, featureB; - - if ( axisType == CROSS_AXIS ) { - EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, - otherFaceDimA, otherFaceDimB, featureA, featureB, - hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, - scalesA_perm, scalesB_perm, true ); - - if ( !done ) { - VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false ); - - if ( !done ) { - VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false ); - } - } - } else if ( axisType == B_AXIS ) { - VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true ); - - if ( !done ) { - VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false ); - - if ( !done ) { - EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, - otherFaceDimA, otherFaceDimB, featureA, featureB, - hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, - scalesA_perm, scalesB_perm, false ); - } - } - } else { - VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true ); - - if ( !done ) { - VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false ); - - if ( !done ) { - EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, - otherFaceDimA, otherFaceDimB, featureA, featureB, - hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, - scalesA_perm, scalesB_perm, false ); - } - } - } - - // convert local points from face-local to box-local coordinate system - - boxPointA.localPoint = Point3( aperm_col * Vector3( localPointA_perm ) ); - boxPointB.localPoint = Point3( bperm_col * Vector3( localPointB_perm ) ); - - // find which features of the boxes are involved. - // the only feature pairs which occur in this function are VF, FV, and EE, even though the - // closest points might actually lie on sub-features, as in a VF contact might be used for - // what's actually a VV contact. this means some feature pairs could possibly seem distinct - // from others, although their contact positions are the same. don't know yet whether this - // matters. - - int sA[3], sB[3]; - - sA[0] = boxPointA.localPoint.getX() > 0.0f; - sA[1] = boxPointA.localPoint.getY() > 0.0f; - sA[2] = boxPointA.localPoint.getZ() > 0.0f; - - sB[0] = boxPointB.localPoint.getX() > 0.0f; - sB[1] = boxPointB.localPoint.getY() > 0.0f; - sB[2] = boxPointB.localPoint.getZ() > 0.0f; - - if ( featureA == F ) { - boxPointA.setFaceFeature( dimA[2], sA[dimA[2]] ); - } else if ( featureA == E ) { - boxPointA.setEdgeFeature( dimA[2], sA[dimA[2]], dimA[otherFaceDimA], sA[dimA[otherFaceDimA]] ); - } else { - boxPointA.setVertexFeature( sA[0], sA[1], sA[2] ); - } - - if ( featureB == F ) { - boxPointB.setFaceFeature( dimB[2], sB[dimB[2]] ); - } else if ( featureB == E ) { - boxPointB.setEdgeFeature( dimB[2], sB[dimB[2]], dimB[otherFaceDimB], sB[dimB[otherFaceDimB]] ); - } else { - boxPointB.setVertexFeature( sB[0], sB[1], sB[2] ); - } - - normal = transformA * axisA; - - if ( maxGap < 0.0f ) { - return (maxGap); - } else { - return (sqrtf( minDistSqr )); - } -} diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h deleted file mode 100644 index c58e257c026..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. - All rights reserved. - -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 __BOXBOXDISTANCE_H__ -#define __BOXBOXDISTANCE_H__ - - -#include "Box.h" - -using namespace Vectormath::Aos; - -//--------------------------------------------------------------------------- -// boxBoxDistance: -// -// description: -// this computes info that can be used for the collision response of two boxes. when the boxes -// do not overlap, the points are set to the closest points of the boxes, and a positive -// distance between them is returned. if the boxes do overlap, a negative distance is returned -// and the points are set to two points that would touch after the boxes are translated apart. -// the contact normal gives the direction to repel or separate the boxes when they touch or -// overlap (it's being approximated here as one of the 15 "separating axis" directions). -// -// returns: -// positive or negative distance between two boxes. -// -// args: -// Vector3& normal: set to a unit contact normal pointing from box A to box B. -// -// BoxPoint& boxPointA, BoxPoint& boxPointB: -// set to a closest point or point of penetration on each box. -// -// Box boxA, Box boxB: -// boxes, represented as 3 half-widths -// -// const Transform3& transformA, const Transform3& transformB: -// box transformations, in world coordinates -// -// float distanceThreshold: -// the algorithm will exit early if it finds that the boxes are more distant than this -// threshold, and not compute a contact normal or points. if this distance returned -// exceeds the threshold, all the other output data may not have been computed. by -// default, this is set to MAX_FLOAT so it will have no effect. -// -//--------------------------------------------------------------------------- - -float -boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB, - PE_REF(Box) boxA, const Transform3 & transformA, PE_REF(Box) boxB, - const Transform3 & transformB, - float distanceThreshold = FLT_MAX ); - -#endif /* __BOXBOXDISTANCE_H__ */ diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt deleted file mode 100644 index 5b4a907058f..00000000000 --- a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt +++ /dev/null @@ -1 +0,0 @@ -Empty placeholder for future Libspe2 SPU task |