Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Share btGjkPairDetector, btGjkEpa2, btVoronoiSimplexSolver with SPU/Multithreaded implementation (remove duplicate code)

Browse Source 
Make btTypedConstraint and btPersistentManifold both derive from btTypedObject to make SPU-side generic constraint solver easier.

Note: all build systems need to be updated: remove SpuVoronoiSimplexSolver.cpp, SpuGjkPairDetector.cpp, SpuEpaPenetrationDepthSolver.cpp, SpuGjkEpa2.cpp
This commit is contained in:
erwin.coumans
2009-08-07 08:57:56 +00:00
parent 5d2cf447e4
commit aef97d6015
30 changed files with 283 additions and 2505 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
View File

@@ -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
}

11
src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
View File

@@ -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();

82
src/BulletCollision/CollisionShapes/btConvexShape.cpp
View File

@@ -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 <spu_intrinsics.h>
static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
{
vec_float4 result;
result = spu_mul( vec0, vec1 );
result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result );
return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result );
}
#endif //__SPU__
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<numPoints;i++)
{
btVector3 vtx = points[i] * localScaling;
newDot = vec.dot(vtx);
newDot = vec.dot(points[i]);
if (newDot > 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

12
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
View File

@@ -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__
}

2
src/BulletCollision/CollisionShapes/btTriangleShape.h
View File

@@ -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
{

2
src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
View File

@@ -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

4
src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
View File

@@ -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;

40
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
View File

@@ -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 */

4
src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
View File

@@ -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,

62
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
View File

@@ -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&
}
}
}
}

9
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
View File

@@ -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;

3
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -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)

12
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -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;}

17
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -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.)));
}

12
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View File

@@ -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)

247
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp
View File

@@ -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<numPoints;i++)
{
const btVector3& vtx = points[i];// * m_localScaling;
newDot = vec.dot(vtx);
if (newDot > 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)
{

19
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h
View File

@@ -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;

6
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h
View File

@@ -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

37
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp
View File

@@ -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;
}

47
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h
View File

@@ -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

54
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp
View File

@@ -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

851
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp
View File

@@ -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 <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#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(rl<GJK_MIN_DISTANCE)
{/* Touching or inside */
m_status=eStatus::Inside;
break;
}
/* Append new vertice in -'v' direction */
appendvertice(cs,-m_ray);
const btVector3& w=cs.c[cs.rank-1]->w;
bool found=false;
for(U i=0;i<4;++i)
{
if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
{ found=true;break; }
}
if(found)
{/* Return old simplex */
removevertice(m_simplices[m_current]);
break;
}
else
{/* Update lastw */
lastw[clastw=(clastw+1)&3]=w;
}
/* Check for termination */
const btScalar omega=btDot(m_ray,w)/rl;
alpha=btMax(omega,alpha);
if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
{/* Return old simplex */
removevertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
btScalar weights[4];
U mask=0;
switch(cs.rank)
{
case 2: sqdist=projectorigin( cs.c[0]->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;i<ni;++i)
{
if(mask&(1<<i))
{
ns.c[ns.rank] = cs.c[i];
ns.p[ns.rank++] = weights[i];
m_ray += cs.c[i]->w*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)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
} while(m_status==eStatus::Valid);
m_simplex=&m_simplices[m_current];
switch(m_status)
{
case eStatus::Valid: m_distance=m_ray.length();break;
case eStatus::Inside: m_distance=0;break;
}
return(m_status);
}
bool EncloseOrigin()
{
switch(m_simplex->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)||(subd<mindist))
{
mindist = subd;
m = ((subm&1)?1<<i:0)+((subm&2)?1<<j:0);
w[i] = subw[0];
w[j] = subw[1];
w[imd3[j]] = 0;
}
}
}
if(mindist<0)
{
const btScalar d=btDot(a,n);
const btScalar s=btSqrt(l);
const btVector3 p=n*(d/l);
mindist = p.length2();
m = 7;
w[0] = (btCross(dl[1],b-p)).length()/s;
w[1] = (btCross(dl[2],c-p)).length()/s;
w[2] = 1-(w[0]+w[1]);
}
return(mindist);
}
return(-1);
}
static btScalar projectorigin( const btVector3& a,
const btVector3& b,
const btVector3& c,
const btVector3& d,
btScalar* w,U& m)
{
static const U imd3[]={1,2,0};
const btVector3* vt[]={&a,&b,&c,&d};
const btVector3 dl[]={a-d,b-d,c-d};
const btScalar vl=det(dl[0],dl[1],dl[2]);
const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
if(ng&&(btFabs(vl)>GJK_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)||(subd<mindist))
{
mindist = subd;
m = (subm&1?1<<i:0)+
(subm&2?1<<j:0)+
(subm&4?8:0);
w[i] = subw[0];
w[j] = subw[1];
w[imd3[j]] = 0;
w[3] = subw[2];
}
}
}
if(mindist<0)
{
mindist = 0;
m = 15;
w[0] = det(c,b,d)/vl;
w[1] = det(a,c,d)/vl;
w[2] = det(b,a,d)/vl;
w[3] = 1-(w[0]+w[1]+w[2]);
}
return(mindist);
}
return(-1);
}
};
// EPA
struct EPA
{
/* Types */
typedef GJK::sSV sSV;
struct sFace
{
btVector3 n;
btScalar d;
btScalar p;
sSV* c[3];
sFace* f[3];
sFace* l[2];
U1 e[3];
U1 pass;
};
struct sList
{
sFace* root;
U count;
sList() : root(0),count(0) {}
};
struct sHorizon
{
sFace* cf;
sFace* ff;
U nf;
sHorizon() : cf(0),ff(0),nf(0) {}
};
struct eStatus { enum _ {
Valid,
Touching,
Degenerated,
NonConvex,
InvalidHull,
OutOfFaces,
OutOfVertices,
AccuraryReached,
FallBack,
Failed
};};
/* Fields */
eStatus::_ m_status;
GJK::sSimplex m_result;
btVector3 m_normal;
btScalar m_depth;
sSV m_sv_store[EPA_MAX_VERTICES];
sFace m_fc_store[EPA_MAX_FACES];
U m_nextsv;
sList m_hull;
sList m_stock;
/* Methods */
EPA()
{
Initialize();
}
void Initialize()
{
m_status = eStatus::Failed;
m_normal = btVector3(0,0,0);
m_depth = 0;
m_nextsv = 0;
for(U i=0;i<EPA_MAX_FACES;++i)
{
append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
}
}
eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
{
GJK::sSimplex& simplex=*gjk.m_simplex;
if((simplex.rank>1)&&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(;iterations<EPA_MAX_ITERATIONS;++iterations)
{
if(m_nextsv<EPA_MAX_VERTICES)
{
sHorizon horizon;
sSV* w=&m_sv_store[m_nextsv++];
bool valid=true;
best->pass = (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)&&(sqd<mind))
{
minf=f;
mind=sqd;
maxp=f->p;
}
}
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;i<epa.m_result.rank;++i)
{
w0+=shape.Support(epa.m_result.c[i]->d,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

38
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h
View File

@@ -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

316
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp
View File

@@ -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 <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#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);
}
}

103
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h
View File

@@ -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

15
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp
View File

@@ -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;

7
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h
View File

@@ -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,

605
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp
View File

@@ -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 <stdio.h>
#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<numverts;i++)
{
btScalar curLen2 = m_simplexVectorW[i].length2();
if (maxV < curLen2)
maxV = curLen2;
}
return maxV;
}
//return the current simplex
int SpuVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
{
int i;
for (i=0;i<numVertices();i++)
{
yBuf[i] = m_simplexVectorW[i];
pBuf[i] = m_simplexPointsP[i];
qBuf[i] = m_simplexPointsQ[i];
}
return numVertices();
}
bool SpuVoronoiSimplexSolver::inSimplex(const btVector3& w)
{
bool found = false;
int i, numverts = numVertices();
//btScalar maxV = btScalar(0.);
//w is in the current (reduced) simplex
for (i=0;i<numverts;i++)
{
if (m_simplexVectorW[i] == w)
found = true;
}
//check in case lastW is already removed
if (w == m_lastW)
return true;
return found;
}
void SpuVoronoiSimplexSolver::backup_closest(btVector3& v)
{
v = m_cachedV;
}
bool SpuVoronoiSimplexSolver::emptySimplex() const
{
return (numVertices() == 0);
}
void SpuVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
{
updateClosestVectorAndPoints();
p1 = m_cachedP1;
p2 = m_cachedP2;
}
bool SpuVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,SpuSubSimplexClosestResult& result)
{
result.m_usedVertices.reset();
// Check if P in vertex region outside A
btVector3 ab = b - a;
btVector3 ac = c - a;
btVector3 ap = p - a;
btScalar d1 = ab.dot(ap);
btScalar d2 = ac.dot(ap);
if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
{
result.m_closestPointOnSimplex = a;
result.m_usedVertices.usedVertexA = true;
result.setBarycentricCoordinates(1,0,0);
return true;// a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
btVector3 bp = p - b;
btScalar d3 = ab.dot(bp);
btScalar d4 = ac.dot(bp);
if (d3 >= 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;
}

155
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h
View File

@@ -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

13
src/LinearMath/btScalar.h
View File

@@ -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