diff options
Diffstat (limited to 'extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask')
15 files changed, 4227 insertions, 0 deletions
diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/Box.h b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/Box.h new file mode 100644 index 00000000000..9bc2ebf51ec --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/Box.h @@ -0,0 +1,172 @@ +/* + 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 new file mode 100644 index 00000000000..dfcd8426695 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp @@ -0,0 +1,302 @@ +/* +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 new file mode 100644 index 00000000000..d369395e160 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h @@ -0,0 +1,126 @@ +/* +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 new file mode 100644 index 00000000000..8e540d9297b --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp @@ -0,0 +1,242 @@ +/* +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 new file mode 100644 index 00000000000..394f56dcbd1 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h @@ -0,0 +1,106 @@ +/* +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 new file mode 100644 index 00000000000..449f19288c4 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h @@ -0,0 +1,51 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef 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 new file mode 100644 index 00000000000..c3dfaa793e3 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp @@ -0,0 +1,1381 @@ +/* +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 new file mode 100644 index 00000000000..bbaa555ee1b --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h @@ -0,0 +1,140 @@ +/* +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 new file mode 100644 index 00000000000..8b89de03f59 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h @@ -0,0 +1,19 @@ +/* +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 new file mode 100644 index 00000000000..9f7e64dd1b3 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp @@ -0,0 +1,348 @@ +/* +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 new file mode 100644 index 00000000000..18ad223ed36 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h @@ -0,0 +1,48 @@ + +/* +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 new file mode 100644 index 00000000000..774a0cb2eb1 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h @@ -0,0 +1,70 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef _SPU_PREFERRED_PENETRATION_DIRECTIONS_H +#define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H + + +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" + +int spuGetNumPreferredPenetrationDirections(int shapeType, void* shape) +{ + switch (shapeType) + { + case TRIANGLE_SHAPE_PROXYTYPE: + { + return 2; + //spu_printf("2\n"); + break; + } + default: + { +#if __ASSERT + spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType); +#endif // __ASSERT + } + } + + return 0; +} + +void spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector) +{ + + + switch (shapeType) + { + case TRIANGLE_SHAPE_PROXYTYPE: + { + btVector3* vertices = (btVector3*)shape; + ///calcNormal + penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); + penetrationVector.normalize(); + if (index) + penetrationVector *= btScalar(-1.); + break; + } + default: + { + +#if __ASSERT + spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType); +#endif // __ASSERT + } + } + +} + +#endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H diff --git a/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp new file mode 100644 index 00000000000..30642a39294 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp @@ -0,0 +1,1155 @@ +/* + 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 new file mode 100644 index 00000000000..c58e257c026 --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h @@ -0,0 +1,66 @@ +/* + 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 new file mode 100644 index 00000000000..5b4a907058f --- /dev/null +++ b/extern/bullet2/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt @@ -0,0 +1 @@ +Empty placeholder for future Libspe2 SPU task |