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