diff --git a/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp index 2bc17e61d..083d60b1b 100644 --- a/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -34,7 +34,7 @@ void btConvexInternalShape::setLocalScaling(const btVector3& scaling) void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const { - +#ifndef __SPU__ //use localGetSupportingVertexWithoutMargin? btScalar margin = getMargin(); for (int i=0;i<3;i++) @@ -50,6 +50,7 @@ void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAa tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); minAabb[i] = tmp[i]-margin; } +#endif } diff --git a/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h index 2f68ee7ac..54b5afac3 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h +++ b/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -29,6 +29,14 @@ ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAa public: BT_DECLARE_ALIGNED_ALLOCATOR(); + btConvexPointCloudShape() + { + m_localScaling.setValue(1.f,1.f,1.f); + m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; + m_unscaledPoints = 0; + m_numPoints = 0; + } + btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) { m_localScaling = localScaling; @@ -40,10 +48,11 @@ public: recalcLocalAabb(); } - void setPoints (btVector3* points, int numPoints, bool computeAabb = true) + void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f)) { m_unscaledPoints = points; m_numPoints = numPoints; + m_localScaling = localScaling; if (computeAabb) recalcLocalAabb(); diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 347f514a1..05c3a15e9 100644 --- a/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -21,6 +21,18 @@ subject to the following restrictions: #include "btConvexHullShape.h" #include "btConvexPointCloudShape.h" +///not supported on IBM SDK, until we fix the alignment of btVector3 +#if defined (__CELLOS_LV2__) && defined (__SPU__) +#include +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__ + btConvexShape::btConvexShape () { } @@ -32,35 +44,71 @@ btConvexShape::~btConvexShape() -static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints, const btVector3& localScaling) -{ - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT); +static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) +{ - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; + btVector3 vec = localDirOrg * localScaling; + +#if defined (__CELLOS_LV2__) && defined (__SPU__) + + btVector3 localDir = vec; + + vec_float4 v_distMax = {-FLT_MAX,0,0,0}; + vec_int4 v_idxMax = {-999,0,0,0}; + int v=0; + int numverts = numPoints; + + for(;v<(int)numverts-4;v+=4) { + vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); + vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); + vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); + vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); + const vec_int4 i0 = {v ,0,0,0}; + const vec_int4 i1 = {v+1,0,0,0}; + const vec_int4 i2 = {v+2,0,0,0}; + const vec_int4 i3 = {v+3,0,0,0}; + vec_uint4 retGt01 = spu_cmpgt(p0,p1); + vec_float4 pmax01 = spu_sel(p1,p0,retGt01); + vec_int4 imax01 = spu_sel(i1,i0,retGt01); + vec_uint4 retGt23 = spu_cmpgt(p2,p3); + vec_float4 pmax23 = spu_sel(p3,p2,retGt23); + vec_int4 imax23 = spu_sel(i3,i2,retGt23); + vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); + vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); + vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); + vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); + v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); + v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); } + for(;v<(int)numverts;v++) { + vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); + const vec_int4 i = {v,0,0,0}; + vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); + v_distMax = spu_sel(p,v_distMax,retGtMax); + v_idxMax = spu_sel(i,v_idxMax,retGtMax); + } + int ptIndex = spu_extract(v_idxMax,0); + const btVector3& supVec= points[ptIndex] * localScaling; + return supVec; +#else + btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT); + int ptIndex = -1; for (int i=0;i maxDot) { maxDot = newDot; - supVec = vtx; + ptIndex = i; } } - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); + btAssert(ptIndex >= 0); + btVector3 supVec = points[ptIndex] * localScaling; + return supVec; +#endif //__SPU__ } btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 711953e77..d464439e8 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -23,9 +23,10 @@ btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape() btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const { + int i; btVector3 supVec(0,0,0); - +#ifndef __SPU__ btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); btVector3 vec = vec0; @@ -54,11 +55,14 @@ btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const b } return supVec; - +#endif //__SPU__ } + + void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const { +#ifndef __SPU__ int i; btVector3 vtx; @@ -86,12 +90,14 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( } } } +#endif //__SPU__ } void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { +#ifndef __SPU__ //not yet, return box inertia btScalar margin = getMargin(); @@ -111,7 +117,7 @@ void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine const btScalar scaledmass = mass * btScalar(0.08333333); inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); - +#endif //__SPU__ } diff --git a/src/BulletCollision/CollisionShapes/btTriangleShape.h b/src/BulletCollision/CollisionShapes/btTriangleShape.h index 09a769515..69b7dbbb7 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -19,7 +19,7 @@ subject to the following restrictions: #include "btConvexShape.h" #include "btBoxShape.h" -class btTriangleShape : public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape { diff --git a/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 19f771296..9ee83e7d5 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -96,7 +96,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( { - btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; //we don't use margins during CCD diff --git a/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 412aace21..7e3fde8e2 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef CONVEX_PENETRATION_DEPTH_H -#define CONVEX_PENETRATION_DEPTH_H +#ifndef __CONVEX_PENETRATION_DEPTH_H +#define __CONVEX_PENETRATION_DEPTH_H class btStackAlloc; class btVector3; diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 76a91c36d..81d2420fc 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -68,7 +68,43 @@ namespace gjkepa2_impl const btConvexShape* m_shapes[2]; btMatrix3x3 m_toshape1; btTransform m_toshape0; +#ifdef __SPU__ + bool m_enableMargin; +#else btVector3 (btConvexShape::*Ls)(const btVector3&) const; +#endif//__SPU__ + + + MinkowskiDiff() + { + + } +#ifdef __SPU__ + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + if (m_enableMargin) + { + return m_shapes[0]->localGetSupportVertexNonVirtual(d); + } else + { + return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); + } + } + inline btVector3 Support1(const btVector3& d) const + { + if (m_enableMargin) + { + return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d)); + } else + { + return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d)); + } + } +#else void EnableMargin(bool enable) { if(enable) @@ -84,6 +120,8 @@ namespace gjkepa2_impl { return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); } +#endif //__SPU__ + inline btVector3 Support(const btVector3& d) const { return(Support0(d)-Support1(-d)); @@ -858,6 +896,7 @@ bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, return(false); } +#ifndef __SPU__ // btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, btScalar margin, @@ -923,6 +962,7 @@ bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, else return(true); } +#endif //__SPU__ /* Symbols cleanup */ diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 68dbc5665..a49689a15 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -25,6 +25,10 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver { public : + btGjkEpaPenetrationDepthSolver() + { + } + bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index a7bdd524d..8f58df390 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -38,20 +38,48 @@ int gNumDeepPenetrationChecks = 0; int gNumGjkChecks = 0; - btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) +:m_penetrationDepthSolver(penetrationDepthSolver), +m_simplexSolver(simplexSolver), +m_minkowskiA(objectA), +m_minkowskiB(objectB), +m_shapeTypeA(objectA->getShapeType()), +m_shapeTypeB(objectB->getShapeType()), +m_marginA(objectA->getMargin()), +m_marginB(objectB->getMargin()), +m_ignoreMargin(false), +m_lastUsedMethod(-1), +m_catchDegeneracies(1) +{ +} +btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) :m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)), m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), m_minkowskiB(objectB), +m_shapeTypeA(shapeTypeA), +m_shapeTypeB(shapeTypeB), +m_marginA(marginA), +m_marginB(marginB), m_ignoreMargin(false), m_lastUsedMethod(-1), m_catchDegeneracies(1) { } -void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +{ + (void)swapResults; + + getClosestPointsNonVirtual(input,output,debugDraw); +} + +#ifdef __SPU__ +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +#else +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +#endif { m_cachedSeparatingDistance = 0.f; @@ -64,21 +92,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; -#ifdef __SPU__ - btScalar marginA = m_minkowskiA->getMarginNonVirtual(); - btScalar marginB = m_minkowskiB->getMarginNonVirtual(); -#else - btScalar marginA = m_minkowskiA->getMargin(); - btScalar marginB = m_minkowskiB->getMargin(); -#ifdef TEST_NON_VIRTUAL - btScalar marginAv = m_minkowskiA->getMarginNonVirtual(); - btScalar marginBv = m_minkowskiB->getMarginNonVirtual(); - btAssert(marginA == marginAv); - btAssert(marginB == marginBv); -#endif //TEST_NON_VIRTUAL -#endif - + btScalar marginA = m_marginA; + btScalar marginB = m_marginB; gNumGjkChecks++; @@ -123,6 +139,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); +#if 1 + + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + +// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA); +// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB); + +#else #ifdef __SPU__ btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); @@ -136,6 +161,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btAssert((qInBv-qInB).length() < 0.0001); #endif // #endif //__SPU__ +#endif + btVector3 pWorld = localTransA(pInA); btVector3 qWorld = localTransB(qInB); @@ -291,7 +318,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) { //penetration case - + //if there is no way to handle penetrations, bail out if (m_penetrationDepthSolver) { @@ -373,6 +400,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& } } + } } diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index 0ad4aab8a..cc6287c86 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -36,6 +36,11 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface btSimplexSolverInterface* m_simplexSolver; const btConvexShape* m_minkowskiA; const btConvexShape* m_minkowskiB; + int m_shapeTypeA; + int m_shapeTypeB; + btScalar m_marginA; + btScalar m_marginB; + bool m_ignoreMargin; btScalar m_cachedSeparatingDistance; @@ -50,10 +55,14 @@ public: btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); + btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); virtual ~btGjkPairDetector() {}; virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); + + void setMinkowskiA(btConvexShape* minkA) { m_minkowskiA = minkA; diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index d4e29882d..924a8af87 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -25,7 +25,8 @@ ContactProcessedCallback gContactProcessedCallback = 0; btPersistentManifold::btPersistentManifold() -:m_body0(0), +:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), +m_body0(0), m_body1(0), m_cachedPoints (0), m_index1a(0) diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 0b3c734d1..7a30f92c5 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -32,7 +32,11 @@ typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* b extern ContactDestroyedCallback gContactDestroyedCallback; - +enum btContactManifoldTypes +{ + BT_PERSISTENT_MANIFOLD_TYPE = 1, + MAX_CONTACT_MANIFOLD_TYPE +}; #define MANIFOLD_CACHE_SIZE 4 @@ -43,7 +47,7 @@ extern ContactDestroyedCallback gContactDestroyedCallback; ///reduces the cache to 4 points, when more then 4 points are added, using following rules: ///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points ///note that some pairs of objects might have more then one contact manifold. -ATTRIBUTE_ALIGNED16( class) btPersistentManifold +ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject { btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; @@ -72,11 +76,11 @@ public: btPersistentManifold(); btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) - : m_body0(body0),m_body1(body1),m_cachedPoints(0), + : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), + m_body0(body0),m_body1(body1),m_cachedPoints(0), m_contactBreakingThreshold(contactBreakingThreshold), m_contactProcessingThreshold(contactProcessingThreshold) { - } SIMD_FORCE_INLINE void* getBody0() { return m_body0;} diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 996e93fca..235be5871 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -22,10 +22,10 @@ static btRigidBody s_fixed(0, 0,0); #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) btTypedConstraint::btTypedConstraint(btTypedConstraintType type) -:m_userConstraintType(-1), +:btTypedObject(type), +m_userConstraintType(-1), m_userConstraintId(-1), m_needsFeedback(false), -m_constraintType (type), m_rbA(s_fixed), m_rbB(s_fixed), m_appliedImpulse(btScalar(0.)), @@ -34,31 +34,30 @@ m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) -:m_userConstraintType(-1), +:btTypedObject(type), +m_userConstraintType(-1), m_userConstraintId(-1), m_needsFeedback(false), -m_constraintType (type), m_rbA(rbA), m_rbB(s_fixed), m_appliedImpulse(btScalar(0.)), m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) -:m_userConstraintType(-1), +:btTypedObject(type), +m_userConstraintType(-1), m_userConstraintId(-1), m_needsFeedback(false), -m_constraintType (type), m_rbA(rbA), m_rbB(rbB), m_appliedImpulse(btScalar(0.)), m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 6c9179378..430e16285 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -19,14 +19,12 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btScalar.h" #include "btSolverConstraint.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" struct btSolverBody; - - - enum btTypedConstraintType { - POINT2POINT_CONSTRAINT_TYPE, + POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1, HINGE_CONSTRAINT_TYPE, CONETWIST_CONSTRAINT_TYPE, D6_CONSTRAINT_TYPE, @@ -35,14 +33,12 @@ enum btTypedConstraintType }; ///TypedConstraint is the baseclass for Bullet constraints and vehicles -class btTypedConstraint +class btTypedConstraint : public btTypedObject { int m_userConstraintType; int m_userConstraintId; bool m_needsFeedback; - btTypedConstraintType m_constraintType; - btTypedConstraint& operator=(btTypedConstraint& other) { btAssert(0); @@ -231,7 +227,7 @@ public: btTypedConstraintType getConstraintType () const { - return m_constraintType; + return btTypedConstraintType(m_objectType); } void setDbgDrawSize(btScalar dbgDrawSize) diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp index 5fe2b079b..b3c8f3d2f 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp @@ -28,253 +28,6 @@ static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) } #endif //__SPU__ -btVector3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex) -{ - switch (shapeType) - { - case SPHERE_SHAPE_PROXYTYPE: - { - return btVector3(0,0,0); - } - case BOX_SHAPE_PROXYTYPE: - { -// spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n"); - btConvexInternalShape* convexShape = (btConvexInternalShape*)shape; - const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); - - return btVector3( - localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(), - localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(), - localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()); - } - - case TRIANGLE_SHAPE_PROXYTYPE: - { - - btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3* vertices = (btVector3*)shape; - btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); - btVector3 sup = vertices[dots.maxAxis()]; - return btVector3(sup.getX(),sup.getY(),sup.getZ()); - break; - } - - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)shape; - - //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) - - btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); - btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); - - int cylinderUpAxis = cylShape->getUpAxis(); - int XX(1),YY(0),ZZ(2); - - switch (cylinderUpAxis) - { - case 0: - { - XX = 1; - YY = 0; - ZZ = 2; - break; - } - case 1: - { - XX = 0; - YY = 1; - ZZ = 2; - break; - } - case 2: - { - XX = 0; - YY = 2; - ZZ = 1; - break; - } - default: - btAssert(0); - //printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n"); - }; - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; - - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); - } - else - { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); - } - } - - case CAPSULE_SHAPE_PROXYTYPE: - { - //spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n"); - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - - btCapsuleShape* capsuleShape = (btCapsuleShape*)shape; - btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - int capsuleUpAxis = capsuleShape->getUpAxis(); - - btScalar radius = capsuleShape->getRadius(); - btVector3 supVec(0,0,0); - - btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - btVector3 vtx; - btScalar newDot; - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = halfHeight; - - vtx = pos +vec*(radius); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = -halfHeight; - - vtx = pos +vec*(radius); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); - break; - }; - - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - //spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n"); - -#if defined (__CELLOS_LV2__) && defined (__SPU__) - vec_float4 v_distMax = {-FLT_MAX,0,0,0}; - vec_int4 v_idxMax = {-999,0,0,0}; - int v=0; - int numverts = convexVertexData->gNumConvexPoints; - btVector3* points = convexVertexData->gConvexPoints; - - for(;v<(int)numverts-4;v+=4) { - vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); - vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); - vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); - vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); - const vec_int4 i0 = {v ,0,0,0}; - const vec_int4 i1 = {v+1,0,0,0}; - const vec_int4 i2 = {v+2,0,0,0}; - const vec_int4 i3 = {v+3,0,0,0}; - vec_uint4 retGt01 = spu_cmpgt(p0,p1); - vec_float4 pmax01 = spu_sel(p1,p0,retGt01); - vec_int4 imax01 = spu_sel(i1,i0,retGt01); - vec_uint4 retGt23 = spu_cmpgt(p2,p3); - vec_float4 pmax23 = spu_sel(p3,p2,retGt23); - vec_int4 imax23 = spu_sel(i3,i2,retGt23); - vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); - vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); - vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); - vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); - v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); - v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); - } - for(;v<(int)numverts;v++) { - vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); - const vec_int4 i = {v,0,0,0}; - vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); - v_distMax = spu_sel(p,v_distMax,retGtMax); - v_idxMax = spu_sel(i,v_idxMax,retGtMax); - } - int ptIndex = spu_extract(v_idxMax,0); - const btVector3& supVec= points[ptIndex]; -#else - - btVector3* points = 0; - int numPoints = 0; - points = convexVertexData->gConvexPoints; - numPoints = convexVertexData->gNumConvexPoints; - - // spu_printf("numPoints = %d\n",numPoints); - - int ptIndex = 0; - btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT); - - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - - for (int i=0;i maxDot) - { - maxDot = newDot; - ptIndex = i; - } - } - const btVector3& supVec= points[ptIndex]; - -#endif - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); - - break; - }; - - default: - - //spu_printf("SPU:(type %i) missing support function\n",shapeType); - - -#if __ASSERT - // spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType); -#endif // __ASSERT - return btVector3(0.f, 0.f, 0.f); - } -} void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform) { diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h index c5734b96e..30c110c4b 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h @@ -30,12 +30,15 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" + +class btCollisionShape; struct SpuCollisionPairInput { ppu_address_t m_collisionShapes[2]; - void* m_spuCollisionShapes[2]; + btCollisionShape* m_spuCollisionShapes[2]; ppu_address_t m_persistentManifoldPtr; btVector3 m_primitiveDimensions0; @@ -53,24 +56,14 @@ struct SpuCollisionPairInput }; -struct SpuClosestPointInput +struct SpuClosestPointInput : public btDiscreteCollisionDetectorInterface::ClosestPointInput { - SpuClosestPointInput() - :m_maximumDistanceSquared(float(BT_LARGE_FLOAT)), - m_stackAlloc(0) - { - } - - btTransform m_transformA; - btTransform m_transformB; - float m_maximumDistanceSquared; - class btStackAlloc* m_stackAlloc; 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 +class SpuContactResult : public btDiscreteCollisionDetectorInterface::Result { btTransform m_rootWorldTransform0; btTransform m_rootWorldTransform1; diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h index 1509456c9..bdbab5e53 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h @@ -15,8 +15,8 @@ subject to the following restrictions: */ -#ifndef CONVEX_PENETRATION_DEPTH_H -#define CONVEX_PENETRATION_DEPTH_H +#ifndef SPU_CONVEX_PENETRATION_DEPTH_H +#define SPU_CONVEX_PENETRATION_DEPTH_H @@ -47,5 +47,5 @@ public: -#endif //CONVEX_PENETRATION_DEPTH_H +#endif //SPU_CONVEX_PENETRATION_DEPTH_H diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp deleted file mode 100644 index 7be8757f2..000000000 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuEpaPenetrationDepthSolver.h" -#include "SpuVoronoiSimplexSolver.h" -#include "SpuGjkPairDetector.h" -#include "SpuContactResult.h" -#include "SpuGjkEpa2.h" - -bool SpuEpaPenetrationDepthSolver::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 -{ - bool r; - SpuGjkEpaSolver2::sResults results; - r = SpuGjkEpaSolver2::Penetration (convexA, convexVertexDataA, shapeTypeA, marginA, transA, convexB, convexVertexDataB, shapeTypeB, marginB, transB, btVector3(1.0f, 0.0f, 0.0f), results); - pa = results.witnesses[0]; - pb = results.witnesses[1]; - return r; -} diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h deleted file mode 100644 index 3c25d0474..000000000 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h +++ /dev/null @@ -1,47 +0,0 @@ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SPU_EPA_PENETRATION_DEPTH_SOLVER_H -#define SPU_EPA_PENETRATION_DEPTH_SOLVER_H - - -#include "SpuConvexPenetrationDepthSolver.h" - -class btStackAlloc; -class btIDebugDraw; -class SpuVoronoiSimplexSolver; - -///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. -///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. -class SpuEpaPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver -{ -public: - - virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, - void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, - btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, - struct SpuConvexPolyhedronVertexData* convexVertexDataA, - struct SpuConvexPolyhedronVertexData* convexVertexDataB - ) const; - - -}; - - -#endif //SPU_EPA_PENETRATION_DEPTH_SOLVER_H - diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp index 09ef3350f..d92710238 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp @@ -28,6 +28,7 @@ subject to the following restrictions: #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" @@ -37,13 +38,16 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "SpuMinkowskiPenetrationDepthSolver.h" -#include "SpuEpaPenetrationDepthSolver.h" -#include "SpuGjkPairDetector.h" -#include "SpuVoronoiSimplexSolver.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 @@ -351,11 +355,12 @@ public: - //btTriangleShape tmpTriangleShape(spuTriangleVertices[0],spuTriangleVertices[1],spuTriangleVertices[2]); + ATTRIBUTE_ALIGNED16(btTriangleShape) tmpTriangleShape(spuTriangleVertices[0],spuTriangleVertices[1],spuTriangleVertices[2]); SpuCollisionPairInput triangleConcaveInput(*m_wuInput); - triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0]; +// triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0]; + triangleConcaveInput.m_spuCollisionShapes[1] = &tmpTriangleShape; triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE; m_spuContacts.setShapeIdentifiers(-1,-1,subPart,triangleIndex); @@ -496,9 +501,17 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa { //try generic GJK + + + //SpuConvexPenetrationDepthSolver* penetrationSolver=0; + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver epaPenetrationSolver2; + + btConvexPenetrationDepthSolver* penetrationSolver = (btConvexPenetrationDepthSolver*)&epaPenetrationSolver2; + +#if 0 SpuVoronoiSimplexSolver vsSolver; SpuMinkowskiPenetrationDepthSolver minkowskiPenetrationSolver; - SpuConvexPenetrationDepthSolver* penetrationSolver; #ifdef ENABLE_EPA SpuEpaPenetrationDepthSolver epaPenetrationSolver; if (gUseEpa) @@ -509,6 +522,7 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa { penetrationSolver = &minkowskiPenetrationSolver; } +#endif ///DMA in the vertices for convex shapes ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); @@ -549,21 +563,33 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa 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; + } - void* shape0Ptr = wuInput->m_spuCollisionShapes[0]; - void* shape1Ptr = wuInput->m_spuCollisionShapes[1]; + 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; @@ -588,8 +614,8 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa wuInput->m_isSwapped); { - SpuGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&vsSolver,penetrationSolver); - gjk.getClosestPoints(cpInput,spuContacts);//,debugDraw); + btGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&simplexSolver,penetrationSolver);//&vsSolver,penetrationSolver); + gjk.getClosestPoints(cpInput,spuContacts,0);//,debugDraw); #ifdef USE_SEPDISTANCE_UTIL btScalar sepDist = gjk.getCachedSeparatingDistance()+spuManifold->getContactBreakingThreshold(); lsMemPtr->getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(gjk.getCachedSeparatingAxis(),sepDist,wuInput->m_worldTransform0,wuInput->m_worldTransform1); @@ -987,8 +1013,9 @@ void processCollisionTask(void* userPtr, void* lsMemPtr) #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) @@ -1118,6 +1145,7 @@ void processCollisionTask(void* userPtr, void* lsMemPtr) } else +#endif //USE_DEDICATED_BOX_BOX { if ( #ifdef USE_SEPDISTANCE_UTIL diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp deleted file mode 100644 index 5bf0d9379..000000000 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp +++ /dev/null @@ -1,851 +0,0 @@ -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "SpuCollisionShapes.h" -#include "SpuGjkEpa2.h" - -#if defined(DEBUG) || defined (_DEBUG) -#include //for debug printf -#ifdef __SPU__ -#include -#define printf spu_printf -#endif //__SPU__ -#endif - -namespace gjkepa2_spu_impl -{ - -// Config - - /* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((btScalar)0.0001) -#define GJK_MIN_DISTANCE ((btScalar)0.0001) -#define GJK_DUPLICATED_EPS ((btScalar)0.0001) -#define GJK_SIMPLEX2_EPS ((btScalar)0.0) -#define GJK_SIMPLEX3_EPS ((btScalar)0.0) -#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - - /* EPA */ -#define EPA_MAX_VERTICES 64 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((btScalar)0.0001) -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((btScalar)0.00001) -#define EPA_INSIDE_EPS ((btScalar)0.01) - - -// Shorthands -typedef unsigned int U; -typedef unsigned char U1; - -struct convexShape -{ - void* shape; - SpuConvexPolyhedronVertexData* convexData; - int shapeType; - float margin; -}; - -// MinkowskiDiff -struct MinkowskiDiff - { - convexShape m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; - btVector3 (btConvexShape::*Ls)(const btVector3&) const; - void EnableMargin(bool enable) - { -#if 0 - if(enable) - Ls=&btConvexShape::localGetSupportingVertex; - else - Ls=&btConvexShape::localGetSupportingVertexWithoutMargin; -#endif - } - inline btVector3 Support0(const btVector3& d) const - { - btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[0].shapeType, m_shapes[0].shape, d, m_shapes[0].convexData); - btVector3 ud = d; - ud.normalize(); - sp += ud * m_shapes[0].margin; - return sp; - // return(((m_shapes[0])->*(Ls))(d)); - } - inline btVector3 Support1(const btVector3& d) const - { - btVector3 nd = m_toshape1*d; - btVector3 ud = nd; - ud.normalize (); - btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[1].shapeType, m_shapes[1].shape, nd, m_shapes[1].convexData); - sp += ud * m_shapes[1].margin; - return m_toshape0 * sp; - // return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); - } - inline btVector3 Support(const btVector3& d) const - { - return(Support0(d)-Support1(-d)); - } - btVector3 Support(const btVector3& d,U index) const - { - - if(index) - return(Support1(d)); - else - return(Support0(d)); - - } - }; - -typedef MinkowskiDiff tShape; - -// GJK -struct GJK -{ -/* Types */ -struct sSV - { - btVector3 d,w; - }; -struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; -struct eStatus { enum _ { - Valid, - Inside, - Failed };}; -/* Fields */ -tShape m_shape; -btVector3 m_ray; -btScalar m_distance; -sSimplex m_simplices[2]; -sSV m_store[4]; -sSV* m_free[4]; -U m_nfree; -U m_current; -sSimplex* m_simplex; -eStatus::_ m_status; -/* Methods */ - GJK() - { - Initialize(); - } -void Initialize() - { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } -eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) - { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(rlw; - bool found=false; - for(U i=0;i<4;++i) - { - if((w-lastw[i]).length2()w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; - } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = btVector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;iw*weights[i]; - } - else - { - m_free[m_nfree++] = cs.c[i]; - } - } - if(mask==15) m_status=eStatus::Inside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)rank) - { - case 1: - { - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - appendvertice(*m_simplex, axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 2: - { - const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - if(btFabs(btDot(axis,d))>0) - { - const btVector3 p=btCross(d,axis); - appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - } - break; - case 3: - { - const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - const btScalar l=n.length(); - if(l>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 4: - { - if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, - m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) - return(true); - } - break; - } - return(false); - } -/* Internals */ -void getsupport(const btVector3& d,sSV& sv) const - { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); - } -void removevertice(sSimplex& simplex) - { - m_free[m_nfree++]=simplex.c[--simplex.rank]; - } -void appendvertice(sSimplex& simplex,const btVector3& v) - { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); - } -static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) - { - return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- - a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ - a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) - { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-btDot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } - } - return(-1); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) - { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c}; - const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=btCross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) - { - btScalar mindist=-1; - btScalar subw[2] = { btScalar(0.0f), btScalar(0.0f) }; - U subm; - for(U i=0;i<3;++i) - { - if(btDot(*vt[i],btCross(dl[i],n))>0) - { - const U j=imd3[i]; - const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subdGJK_SIMPLEX4_EPS)) - { - btScalar mindist=-1; - btScalar subw[3]; - U subm; - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const btScalar s=vl*btDot(d,btCross(dl[i],dl[j])); - if(s>0) - { - const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd1)&&gjk.EncloseOrigin()) - { - /* Clean up */ - while(m_hull.root) - { - sFace* f(m_hull.root); - remove(m_hull,f); - append(m_stock,f); - } - m_status = eStatus::Valid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) - { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(simplex.p[0],simplex.p[1]); - } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), - newface(simplex.c[1],simplex.c[0],simplex.c[3],true), - newface(simplex.c[2],simplex.c[1],simplex.c[3],true), - newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterationspass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=btDot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) - { - for(U j=0;(j<3)&&valid;++j) - { - valid&=expand( pass,w, - best->f[j],best->e[j], - horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - if(best->p>=outer.p) outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } - } - const btVector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = btCross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = btCross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = btCross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); - } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } -sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = btCross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; - face->p = btMin(btMin( - btDot(a->w,btCross(face->n,a->w-b->w)), - btDot(b->w,btCross(face->n,b->w-c->w))), - btDot(c->w,btCross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) - { - face->d = btDot(a->w,face->n)/l; - face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) - { - return(face); - } else m_status=eStatus::NonConvex; - } else m_status=eStatus::Degenerated; - remove(m_hull,face); - append(m_stock,face); - return(0); - } - m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; - return(0); - } -sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - btScalar maxp=minf->p; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if((f->p>=maxp)&&(sqdp; - } - } - return(minf); - } -bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) - { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) - { - const U e1=i1m3[e]; - if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) - { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) - { - bind(nf,0,f,e); - if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; - horizon.cf=nf; - ++horizon.nf; - return(true); - } - } - else - { - const U e2=i2m3[e]; - f->pass = (U1)pass; - if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& - expand(pass,w,f->f[e2],f->e[e2],horizon)) - { - remove(m_hull,f); - append(m_stock,f); - return(true); - } - } - } - return(false); - } -static inline void bind(sFace* fa,U ea,sFace* fb,U eb) - { - fa->e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } -static inline void append(sList& list,sFace* face) - { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; - } -static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; - } -}; - -// -static void Initialize(void* shapeA, - SpuConvexPolyhedronVertexData* convexDataA, - int shapeTypeA, - float marginA, - const btTransform& wtrs0, - void* shapeB, - SpuConvexPolyhedronVertexData* convexDataB, - int shapeTypeB, - float marginB, - const btTransform& wtrs1, - SpuGjkEpaSolver2::sResults& results, - tShape& shape, - bool withmargins) -{ -/* Results */ -results.witnesses[0] = -results.witnesses[1] = btVector3(0,0,0); -results.status = SpuGjkEpaSolver2::sResults::Separated; -/* Shape */ -shape.m_shapes[0].margin = marginA; -shape.m_shapes[0].shape = shapeA; -shape.m_shapes[0].shapeType = shapeTypeA; -shape.m_shapes[0].convexData = convexDataA; -shape.m_shapes[1].margin = marginB; -shape.m_shapes[1].shape = shapeB; -shape.m_shapes[1].shapeType = shapeTypeB; -shape.m_shapes[1].convexData = convexDataB; -shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); -shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); -shape.EnableMargin(withmargins); -} - -} - -// -// Api -// - -using namespace gjkepa2_spu_impl; - -// -int SpuGjkEpaSolver2::StackSizeRequirement() -{ -return(sizeof(GJK)+sizeof(EPA)); -} - -// -bool SpuGjkEpaSolver2::Penetration(void* shapeA, - SpuConvexPolyhedronVertexData* convexDataA, - int shapeTypeA, - float marginA, - const btTransform& wtrs0, - void* shapeB, - SpuConvexPolyhedronVertexData* convexDataB, - int shapeTypeB, - float marginB, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) -{ -tShape shape; -Initialize(shapeA, convexDataA, shapeTypeA, marginA, wtrs0, shapeB, convexDataB, shapeTypeB, marginB, wtrs1, results,shape,true); -GJK gjk; -GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); -switch(gjk_status) - { - case GJK::eStatus::Inside: - { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) - { - btVector3 w0=btVector3(0,0,0); - for(U i=0;id,0)*epa.m_result.p[i]; - } - results.status = sResults::Penetrating; - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); - return(true); - } else results.status=sResults::EPA_Failed; - } - break; - case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; - } -return(false); -} - -/* Symbols cleanup */ - -#undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY -#undef GJK_MIN_DISTANCE -#undef GJK_DUPLICATED_EPS -#undef GJK_SIMPLEX2_EPS -#undef GJK_SIMPLEX3_EPS -#undef GJK_SIMPLEX4_EPS - -#undef EPA_MAX_VERTICES -#undef EPA_MAX_FACES -#undef EPA_MAX_ITERATIONS -#undef EPA_ACCURACY -#undef EPA_FALLBACK -#undef EPA_PLANE_EPS -#undef EPA_INSIDE_EPS diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h deleted file mode 100644 index 1cd992b07..000000000 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ -#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ -#include "BulletCollision/CollisionShapes/btConvexShape.h" - -///btGjkEpaSolver contributed under zlib by Nathanael Presson -struct SpuGjkEpaSolver2 -{ -struct sResults - { - enum eStatus - { - Separated, /* Shapes doesnt penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - btVector3 witnesses[2]; - btVector3 normal; - }; - -static int StackSizeRequirement(); - - -static bool Penetration(void* shapeA, - SpuConvexPolyhedronVertexData* convexDataA, - int shapeTypeA, - float marginA, - const btTransform& xformA, - void* shapeB, - SpuConvexPolyhedronVertexData* convexDataB, - int shapeTypeB, - float marginB, - const btTransform& xformB, - const btVector3& guess, - sResults& results); -}; - -#endif diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp deleted file mode 100644 index 87419f56d..000000000 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp +++ /dev/null @@ -1,316 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuGjkPairDetector.h" -#include "SpuConvexPenetrationDepthSolver.h" -#include "SpuCollisionShapes.h" - - - -#if defined(DEBUG) || defined (_DEBUG) -#include //for debug printf -#ifdef __SPU__ -#include -#define printf spu_printf -#endif //__SPU__ -#endif - -//must be above the machine epsilon -#define REL_ERROR2 btScalar(1.0e-6) - -//temp globals, to improve GJK/EPA/penetration calculations -int gSpuNumDeepPenetrationChecks = 0; -int gSpuNumGjkChecks = 0; - - - -SpuGjkPairDetector::SpuGjkPairDetector(void* objectA,void* objectB,int shapeTypeA, int shapeTypeB, float marginA,float marginB,SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(float(0.),float(0.),float(1.)), -m_cachedSeparatingDistance(0.f), -m_penetrationDepthSolver(penetrationDepthSolver), -m_simplexSolver(simplexSolver), -m_minkowskiA(objectA), -m_minkowskiB(objectB), -m_shapeTypeA(shapeTypeA), -m_shapeTypeB(shapeTypeB), -m_marginA(marginA), -m_marginB(marginB), -m_ignoreMargin(false), -m_lastUsedMethod(-1), -m_catchDegeneracies(1) -{ -} - -void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output) -{ - m_cachedSeparatingDistance = 0.f; - - btScalar distance=btScalar(0.); - btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 pointOnA,pointOnB; - btTransform localTransA = input.m_transformA; - btTransform localTransB = input.m_transformB; - btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); - localTransA.getOrigin() -= positionOffset; - localTransB.getOrigin() -= positionOffset; - - btScalar marginA = m_marginA; - btScalar marginB = m_marginB; - - gSpuNumGjkChecks++; - - //for CCD we don't use margins - if (m_ignoreMargin) - { - marginA = btScalar(0.); - marginB = btScalar(0.); - } - - m_curIter = 0; - int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? - m_cachedSeparatingAxis.setValue(0,1,0); - - bool isValid = false; - bool checkSimplex = false; - bool checkPenetration = true; - m_degenerateSimplex = 0; - - m_lastUsedMethod = -1; - - { - btScalar squaredDistance = SIMD_INFINITY; - btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - - m_simplexSolver->reset(); - - for ( ; ; ) - //while (true) - { - - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); - -// btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); -// btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); - - btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA); - btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB); - - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); - - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) - { - checkPenetration = false; - break; - } - - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (m_simplexSolver->inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * REL_ERROR2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) - { - m_degenerateSimplex = 2; - } - checkSimplex = true; - break; - } - //add current vertex to simplex - m_simplexSolver->addVertex(w, pWorld, qWorld); - - //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = m_cachedSeparatingAxis.length2(); - - //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); - - //are we getting any closer ? - if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) - { - m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - checkSimplex = true; - break; - } - - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { - #if defined(DEBUG) || defined (_DEBUG) - - printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance, - m_shapeTypeA, - m_shapeTypeB); - - #endif - break; - - } - - - bool check = (!m_simplexSolver->fullSimplex()); - //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); - - if (!check) - { - //do we need this backup_closest here ? - m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - break; - } - } - - if (checkSimplex) - { - m_simplexSolver->compute_points(pointOnA, pointOnB); - normalInB = pointOnA-pointOnB; - btScalar lenSqr = m_cachedSeparatingAxis.length2(); - //valid normal - if (lenSqr < 0.0001) - { - m_degenerateSimplex = 5; - } - if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - normalInB *= rlen; //normalize - btScalar s = btSqrt(squaredDistance); - - btAssert(s > btScalar(0.0)); - pointOnA -= m_cachedSeparatingAxis * (marginA / s); - pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((btScalar(1.)/rlen) - margin); - isValid = true; - - m_lastUsedMethod = 1; - } else - { - m_lastUsedMethod = 2; - } - } - - bool catchDegeneratePenetrationCase = - (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); - - //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) - { - //penetration case - - //if there is no way to handle penetrations, bail out - if (m_penetrationDepthSolver) - { - // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - gSpuNumDeepPenetrationChecks++; - - bool isValid2 = m_penetrationDepthSolver->calcPenDepth( - *m_simplexSolver, - m_minkowskiA,m_minkowskiB, - m_shapeTypeA, m_shapeTypeB, - marginA, marginB, - localTransA,localTransB, - m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - 0,input.m_stackAlloc,input.m_convexVertexData[0], input.m_convexVertexData[1] - ); - - if (isValid2) - { - btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); - //only replace valid penetrations when the result is deeper (check) - if (!isValid || (distance2 < distance)) - { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - isValid = true; - m_lastUsedMethod = 3; - } else - { - - } - } else - { - //isValid = false; - m_lastUsedMethod = 4; - } - } else - { - m_lastUsedMethod = 5; - } - - } - } - } - - if (isValid) - { -#ifdef __SPU__ - //spu_printf("distance\n"); -#endif //__SPU__ - - m_cachedSeparatingDistance = distance; - m_cachedSeparatingAxis = normalInB; - - output.addContactPoint( - normalInB, - pointOnB+positionOffset, - distance); - //printf("gjk add:%f",distance); - } - - -} - - - - - diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h deleted file mode 100644 index 934e2944d..000000000 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h +++ /dev/null @@ -1,103 +0,0 @@ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - - -#ifndef SPU_GJK_PAIR_DETECTOR_H -#define SPU_GJK_PAIR_DETECTOR_H - - - -#include "SpuContactResult.h" - - -#include "SpuVoronoiSimplexSolver.h" -class SpuConvexPenetrationDepthSolver; - -/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface -class SpuGjkPairDetector -{ - - - btVector3 m_cachedSeparatingAxis; - btScalar m_cachedSeparatingDistance; - const SpuConvexPenetrationDepthSolver* m_penetrationDepthSolver; - SpuVoronoiSimplexSolver* m_simplexSolver; - void* m_minkowskiA; - void* m_minkowskiB; - int m_shapeTypeA; - int m_shapeTypeB; - float m_marginA; - float m_marginB; - bool m_ignoreMargin; - - -public: - - //some debugging to fix degeneracy problems - int m_lastUsedMethod; - int m_curIter; - int m_degenerateSimplex; - int m_catchDegeneracies; - - - SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver); - virtual ~SpuGjkPairDetector() {}; - - virtual void getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output); - - void setMinkowskiA(void* minkA) - { - m_minkowskiA = minkA; - } - - void setMinkowskiB(void* minkB) - { - m_minkowskiB = minkB; - } - - void setCachedSeperatingAxis(const btVector3& seperatingAxis) - { - m_cachedSeparatingAxis = seperatingAxis; - } - - const btVector3& getCachedSeparatingAxis() const - { - return m_cachedSeparatingAxis; - } - btScalar getCachedSeparatingDistance() const - { - return m_cachedSeparatingDistance; - } - - void setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver* penetrationDepthSolver) - { - m_penetrationDepthSolver = penetrationDepthSolver; - } - - ///don't use setIgnoreMargin, it's for Bullet's internal use - void setIgnoreMargin(bool ignoreMargin) - { - m_ignoreMargin = ignoreMargin; - } - - -}; - - - -#endif //SPU_GJK_PAIR_DETECTOR_H diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp index 4eddfefc2..70a37eee5 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp @@ -14,12 +14,10 @@ subject to the following restrictions: */ #include "SpuMinkowskiPenetrationDepthSolver.h" -#include "SpuVoronoiSimplexSolver.h" -#include "SpuGjkPairDetector.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 @@ -69,8 +67,8 @@ btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) }; -bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, - void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, +bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( btVoronoiSimplexSolver& simplexSolver, + btConvexShape* convexA,btConvexShape* 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, @@ -292,8 +290,7 @@ bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& #endif //DEBUG_DRAW - - SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0); + btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); btScalar offsetDist = minProj; btVector3 offset = minNorm * offsetDist; @@ -312,7 +309,7 @@ bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; btIntermediateResult res; - gjkdet.getClosestPoints(input,res); + gjkdet.getClosestPoints(input,res,0); btScalar correctedMinNorm = minProj - res.m_depth; diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h index 88e1adc9d..3d6822920 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h @@ -22,7 +22,8 @@ subject to the following restrictions: class btStackAlloc; class btIDebugDraw; -class SpuVoronoiSimplexSolver; +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. @@ -30,8 +31,8 @@ class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolve { public: - virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, - void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, + virtual bool calcPenDepth( btVoronoiSimplexSolver& simplexSolver, + btConvexShape* convexA,btConvexShape* 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, diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp deleted file mode 100644 index a9dc55bfb..000000000 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp +++ /dev/null @@ -1,605 +0,0 @@ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - - Elsevier CDROM license agreements grants nonexclusive license to use the software - for any purpose, commercial or non-commercial as long as the following credit is included - identifying the original source of the software: - - Parts of the source are "from the book Real-Time Collision Detection by - Christer Ericson, published by Morgan Kaufmann Publishers, - (c) 2005 Elsevier Inc." - -*/ - - -#include "SpuVoronoiSimplexSolver.h" -#include - -#define VERTA 0 -#define VERTB 1 -#define VERTC 2 -#define VERTD 3 - -#define CATCH_DEGENERATE_TETRAHEDRON 1 -void SpuVoronoiSimplexSolver::removeVertex(int index) -{ - - btAssert(m_numVertices>0); - m_numVertices--; - m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; - m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; - m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; -} - -void SpuVoronoiSimplexSolver::reduceVertices (const SpuUsageBitfield& usedVerts) -{ - if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) - removeVertex(3); - - if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) - removeVertex(2); - - if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) - removeVertex(1); - - if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) - removeVertex(0); - -} - - - - - -//clear the simplex, remove all the vertices -void SpuVoronoiSimplexSolver::reset() -{ - m_cachedValidClosest = false; - m_numVertices = 0; - m_needsUpdate = true; - m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_cachedBC.reset(); -} - - - - //add a vertex -void SpuVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) -{ - m_lastW = w; - m_needsUpdate = true; - - m_simplexVectorW[m_numVertices] = w; - m_simplexPointsP[m_numVertices] = p; - m_simplexPointsQ[m_numVertices] = q; - - m_numVertices++; -} - -bool SpuVoronoiSimplexSolver::updateClosestVectorAndPoints() -{ - - if (m_needsUpdate) - { - m_cachedBC.reset(); - - m_needsUpdate = false; - - switch (numVertices()) - { - case 0: - m_cachedValidClosest = false; - break; - case 1: - { - m_cachedP1 = m_simplexPointsP[0]; - m_cachedP2 = m_simplexPointsQ[0]; - m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] - m_cachedBC.reset(); - m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); - m_cachedValidClosest = m_cachedBC.isValid(); - break; - }; - case 2: - { - //closest point origin from line segment - const btVector3& from = m_simplexVectorW[0]; - const btVector3& to = m_simplexVectorW[1]; - btVector3 nearest; - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 diff = p - from; - btVector3 v = to - from; - btScalar t = v.dot(diff); - - if (t > 0) { - btScalar dotVV = v.dot(v); - if (t < dotVV) { - t /= dotVV; - diff -= t*v; - m_cachedBC.m_usedVertices.usedVertexA = true; - m_cachedBC.m_usedVertices.usedVertexB = true; - } else { - t = 1; - diff -= v; - //reduce to 1 point - m_cachedBC.m_usedVertices.usedVertexB = true; - } - } else - { - t = 0; - //reduce to 1 point - m_cachedBC.m_usedVertices.usedVertexA = true; - } - m_cachedBC.setBarycentricCoordinates(1-t,t); - nearest = from + t*v; - - m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); - m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); - m_cachedV = m_cachedP1 - m_cachedP2; - - reduceVertices(m_cachedBC.m_usedVertices); - - m_cachedValidClosest = m_cachedBC.isValid(); - break; - } - case 3: - { - //closest point origin from triangle - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; - - closestPtPointTriangle(p,a,b,c,m_cachedBC); - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; - - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; - - m_cachedV = m_cachedP1-m_cachedP2; - - reduceVertices (m_cachedBC.m_usedVertices); - m_cachedValidClosest = m_cachedBC.isValid(); - - break; - } - case 4: - { - - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; - const btVector3& d = m_simplexVectorW[3]; - - bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); - - if (hasSeperation) - { - - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; - - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; - - m_cachedV = m_cachedP1-m_cachedP2; - reduceVertices (m_cachedBC.m_usedVertices); - } else - { -// printf("sub distance got penetration\n"); - - if (m_cachedBC.m_degenerate) - { - m_cachedValidClosest = false; - } else - { - m_cachedValidClosest = true; - //degenerate case == false, penetration = true + zero - m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - break; - } - - m_cachedValidClosest = m_cachedBC.isValid(); - - //closest point origin from tetrahedron - break; - } - default: - { - m_cachedValidClosest = false; - } - }; - } - - return m_cachedValidClosest; - -} - -//return/calculate the closest vertex -bool SpuVoronoiSimplexSolver::closest(btVector3& v) -{ - bool succes = updateClosestVectorAndPoints(); - v = m_cachedV; - return succes; -} - - - -btScalar SpuVoronoiSimplexSolver::maxVertex() -{ - int i, numverts = numVertices(); - btScalar maxV = btScalar(0.); - for (i=0;i= btScalar(0.0) && d4 <= d3) - { - result.m_closestPointOnSimplex = b; - result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(0,1,0); - - return true; // b; // barycentric coordinates (0,1,0) - } - // Check if P in edge region of AB, if so return projection of P onto AB - btScalar vc = d1*d4 - d3*d2; - if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { - btScalar v = d1 / (d1 - d3); - result.m_closestPointOnSimplex = a + v * ab; - result.m_usedVertices.usedVertexA = true; - result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(1-v,v,0); - return true; - //return a + v * ab; // barycentric coordinates (1-v,v,0) - } - - // Check if P in vertex region outside C - btVector3 cp = p - c; - btScalar d5 = ab.dot(cp); - btScalar d6 = ac.dot(cp); - if (d6 >= btScalar(0.0) && d5 <= d6) - { - result.m_closestPointOnSimplex = c; - result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,0,1); - return true;//c; // barycentric coordinates (0,0,1) - } - - // Check if P in edge region of AC, if so return projection of P onto AC - btScalar vb = d5*d2 - d1*d6; - if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { - btScalar w = d2 / (d2 - d6); - result.m_closestPointOnSimplex = a + w * ac; - result.m_usedVertices.usedVertexA = true; - result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-w,0,w); - return true; - //return a + w * ac; // barycentric coordinates (1-w,0,w) - } - - // Check if P in edge region of BC, if so return projection of P onto BC - btScalar va = d3*d6 - d5*d4; - if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { - btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - - result.m_closestPointOnSimplex = b + w * (c - b); - result.m_usedVertices.usedVertexB = true; - result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,1-w,w); - return true; - // return b + w * (c - b); // barycentric coordinates (0,1-w,w) - } - - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - btScalar denom = btScalar(1.0) / (va + vb + vc); - btScalar v = vb * denom; - btScalar w = vc * denom; - - result.m_closestPointOnSimplex = a + ab * v + ac * w; - result.m_usedVertices.usedVertexA = true; - result.m_usedVertices.usedVertexB = true; - result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-v-w,v,w); - - return true; -// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w - -} - - - - - -/// Test if point p and d lie on opposite sides of plane through abc -int SpuVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d) -{ - btVector3 normal = (b-a).cross(c-a); - - btScalar signp = (p - a).dot(normal); // [AP AB AC] - btScalar signd = (d - a).dot( normal); // [AD AB AC] - -#ifdef CATCH_DEGENERATE_TETRAHEDRON -#ifdef BT_USE_DOUBLE_PRECISION -if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) - { - return -1; - } -#else - if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) - { -// printf("affine dependent/degenerate\n");// - return -1; - } -#endif - -#endif - // Points on opposite sides if expression signs are opposite - return signp * signd < btScalar(0.); -} - - -bool SpuVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, SpuSubSimplexClosestResult& finalResult) -{ - SpuSubSimplexClosestResult tempResult; - - // Start out assuming point inside all halfspaces, so closest to itself - finalResult.m_closestPointOnSimplex = p; - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = true; - finalResult.m_usedVertices.usedVertexB = true; - finalResult.m_usedVertices.usedVertexC = true; - finalResult.m_usedVertices.usedVertexD = true; - - int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); - int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); - int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); - int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); - - if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) - { - finalResult.m_degenerate = true; - return false; - } - - if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) - { - return false; - } - - - btScalar bestSqDist = FLT_MAX; - // If point outside face abc then compute closest point on abc - if (pointOutsideABC) - { - closestPtPointTriangle(p, a, b, c,tempResult); - btVector3 q = tempResult.m_closestPointOnSimplex; - - btScalar sqDist = (q - p).dot( q - p); - // Update best closest point if (squared) distance is less than current best - if (sqDist < bestSqDist) { - bestSqDist = sqDist; - finalResult.m_closestPointOnSimplex = q; - //convert result bitmask! - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; - finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; - finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTB], - tempResult.m_barycentricCoords[VERTC], - 0 - ); - - } - } - - - // Repeat test for face acd - if (pointOutsideACD) - { - closestPtPointTriangle(p, a, c, d,tempResult); - btVector3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.m_closestPointOnSimplex = q; - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; - finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - 0, - tempResult.m_barycentricCoords[VERTB], - tempResult.m_barycentricCoords[VERTC] - ); - - } - } - // Repeat test for face adb - - - if (pointOutsideADB) - { - closestPtPointTriangle(p, a, d, b,tempResult); - btVector3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.m_closestPointOnSimplex = q; - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; - finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; - finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - 0, - tempResult.m_barycentricCoords[VERTB] - ); - - } - } - // Repeat test for face bdc - - - if (pointOutsideBDC) - { - closestPtPointTriangle(p, b, d, c,tempResult); - btVector3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.m_closestPointOnSimplex = q; - finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; - finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; - - finalResult.setBarycentricCoordinates( - 0, - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - tempResult.m_barycentricCoords[VERTB] - ); - - } - } - - //help! we ended up full ! - - if (finalResult.m_usedVertices.usedVertexA && - finalResult.m_usedVertices.usedVertexB && - finalResult.m_usedVertices.usedVertexC && - finalResult.m_usedVertices.usedVertexD) - { - return true; - } - - return true; -} - diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h deleted file mode 100644 index 9e1af9da5..000000000 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h +++ /dev/null @@ -1,155 +0,0 @@ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef SPUVoronoiSimplexSolver_H -#define SPUVoronoiSimplexSolver_H - -#include "LinearMath/btTransform.h" - -#define VORONOI_SIMPLEX_MAX_VERTS 5 - -struct SpuUsageBitfield{ - SpuUsageBitfield() - { - reset(); - } - - void reset() - { - usedVertexA = false; - usedVertexB = false; - usedVertexC = false; - usedVertexD = false; - } - unsigned short usedVertexA : 1; - unsigned short usedVertexB : 1; - unsigned short usedVertexC : 1; - unsigned short usedVertexD : 1; - unsigned short unused1 : 1; - unsigned short unused2 : 1; - unsigned short unused3 : 1; - unsigned short unused4 : 1; -}; - - -struct SpuSubSimplexClosestResult -{ - btVector3 m_closestPointOnSimplex; - //MASK for m_usedVertices - //stores the simplex vertex-usage, using the MASK, - // if m_usedVertices & MASK then the related vertex is used - SpuUsageBitfield m_usedVertices; - float m_barycentricCoords[4]; - bool m_degenerate; - - void reset() - { - m_degenerate = false; - setBarycentricCoordinates(); - m_usedVertices.reset(); - } - bool isValid() - { - bool valid = (m_barycentricCoords[0] >= float(0.)) && - (m_barycentricCoords[1] >= float(0.)) && - (m_barycentricCoords[2] >= float(0.)) && - (m_barycentricCoords[3] >= float(0.)); - - - return valid; - } - void setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.)) - { - m_barycentricCoords[0] = a; - m_barycentricCoords[1] = b; - m_barycentricCoords[2] = c; - m_barycentricCoords[3] = d; - } - -}; - -/// SpuVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. -/// Can be used with GJK, as an alternative to Johnson distance algorithm. -class SpuVoronoiSimplexSolver -{ -public: - - int m_numVertices; - - btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; - - int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS]; - int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS]; - - btVector3 m_cachedP1; - btVector3 m_cachedP2; - btVector3 m_cachedV; - btVector3 m_lastW; - bool m_cachedValidClosest; - - SpuSubSimplexClosestResult m_cachedBC; - - bool m_needsUpdate; - - void removeVertex(int index); - void reduceVertices (const SpuUsageBitfield& usedVerts); - bool updateClosestVectorAndPoints(); - - bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, SpuSubSimplexClosestResult& finalResult); - int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); - bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,SpuSubSimplexClosestResult& result); - - int RemoveDegenerateIndices (const int *inArray, int numIndices, int *outArray) const; - -public: - - void reset(); - - void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); - - - bool closest(btVector3& v); - - btScalar maxVertex(); - - bool fullSimplex() const - { - return (m_numVertices == 4); - } - - int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; - - bool inSimplex(const btVector3& w); - - void backup_closest(btVector3& v) ; - - bool emptySimplex() const ; - - void compute_points(btVector3& p1, btVector3& p2) ; - - int numVertices() const - { - return m_numVertices; - } -}; - - - -#endif //SpuVoronoiSimplexSolver diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 94d41a6df..67dce56e9 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -479,4 +479,17 @@ SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) } } +///rudimentary class to provide type info +struct btTypedObject +{ + btTypedObject(int objectType) + :m_objectType(objectType) + { + } + int m_objectType; + inline int getObjectType() const + { + return m_objectType; + } +}; #endif //SIMD___SCALAR_H