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

enable convex separation util, potentially improves performance. set threshold to zero (docs follow)

Browse Source 
fix scaling issue with btConvexHullShape
use virtual getSupportingVertex on non-SPU platform
This commit is contained in:
erwin.coumans
2008-11-06 23:38:18 +00:00
parent c1fc609d74
commit a4c205afc0
13 changed files with 847 additions and 979 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletCollision/BroadphaseCollision/btDispatcher.h
View File

@@ -47,7 +47,7 @@ struct btDispatcherInfo
m_useEpa(true),
m_allowedCcdPenetration(btScalar(0.04)),
m_useConvexConservativeDistanceUtil(true),
m_convexConservativeDistanceThreshold(0.01f),
m_convexConservativeDistanceThreshold(0.0f),
m_stackAllocator(0)
{

23
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -129,22 +129,31 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.setMinkowskiA(min0);
gjkPairDetector.setMinkowskiB(min1);
input.m_maximumDistanceSquared = 1e30f;//min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
//input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
#ifdef USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = 1e30f;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
}
input.m_stackAlloc = dispatchInfo.m_stackAllocator;
// input.m_maximumDistanceSquared = btScalar(1e30);
input.m_transformA = body0->getWorldTransform();
input.m_transformB = body1->getWorldTransform();
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
#ifdef USE_SEPDISTANCE_UTIL2
m_sepDistance.initSeparatingDistance(m_gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2

2
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
View File

@@ -27,7 +27,7 @@ subject to the following restrictions:
class btConvexPenetrationDepthSolver;
//#define USE_SEPDISTANCE_UTIL2 1
#define USE_SEPDISTANCE_UTIL2 1
///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
class btConvexConvexAlgorithm : public btCollisionAlgorithm

28
src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
View File

@@ -22,14 +22,14 @@ subject to the following restrictions:
btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexShape ()
{
m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
m_points.resize(numPoints);
m_unscaledPoints.resize(numPoints);
unsigned char* pointsBaseAddress = (unsigned char*)points;
for (int i=0;i<numPoints;i++)
{
btVector3* point = (btVector3*)(pointsBaseAddress + i*stride);
m_points[i] = point[0];
m_unscaledPoints[i] = point[0];
}
recalcLocalAabb();
@@ -46,7 +46,7 @@ void btConvexHullShape::setLocalScaling(const btVector3& scaling)
void btConvexHullShape::addPoint(const btVector3& point)
{
m_points.push_back(point);
m_unscaledPoints.push_back(point);
recalcLocalAabb();
}
@@ -68,9 +68,9 @@ btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVecto
}
for (int i=0;i<m_points.size();i++)
for (int i=0;i<m_unscaledPoints.size();i++)
{
btVector3 vtx = m_points[i] * m_localScaling;
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
newDot = vec.dot(vtx);
if (newDot > maxDot)
@@ -92,9 +92,9 @@ void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const
supportVerticesOut[i][3] = btScalar(-1e30);
}
}
for (int i=0;i<m_points.size();i++)
for (int i=0;i<m_unscaledPoints.size();i++)
{
btVector3 vtx = m_points[i] * m_localScaling;
btVector3 vtx = getScaledPoint(i);
for (int j=0;j<numVectors;j++)
{
@@ -145,26 +145,26 @@ btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const
//Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
int btConvexHullShape::getNumVertices() const
{
return m_points.size();
return m_unscaledPoints.size();
}
int btConvexHullShape::getNumEdges() const
{
return m_points.size();
return m_unscaledPoints.size();
}
void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const
{
int index0 = i%m_points.size();
int index1 = (i+1)%m_points.size();
pa = m_points[index0]*m_localScaling;
pb = m_points[index1]*m_localScaling;
int index0 = i%m_unscaledPoints.size();
int index1 = (i+1)%m_unscaledPoints.size();
pa = getScaledPoint(index0);
pb = getScaledPoint(index1);
}
void btConvexHullShape::getVertex(int i,btVector3& vtx) const
{
vtx = m_points[i]*m_localScaling;
vtx = getScaledPoint(i);
}
int btConvexHullShape::getNumPlanes() const

19
src/BulletCollision/CollisionShapes/btConvexHullShape.h
View File

@@ -24,7 +24,7 @@ subject to the following restrictions:
///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex.
ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape
{
btAlignedObjectArray<btVector3> m_points;
btAlignedObjectArray<btVector3> m_unscaledPoints;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -37,19 +37,24 @@ public:
void addPoint(const btVector3& point);
btVector3* getPoints()
btVector3* getUnscaledPoints()
{
return &m_points[0];
return &m_unscaledPoints[0];
}
const btVector3* getPoints() const
const btVector3* getUnscaledPoints() const
{
return &m_points[0];
return &m_unscaledPoints[0];
}
int getNumPoints() const
SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const
{
return m_points.size();
return m_unscaledPoints[i] * m_localScaling;
}
SIMD_FORCE_INLINE int getNumPoints() const
{
return m_unscaledPoints.size();
}
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;

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

@@ -17,8 +17,9 @@ subject to the following restrictions:
#include "btConvexInternalShape.h"
btConvexInternalShape::btConvexInternalShape()
: btConvexShape (), m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
m_collisionMargin(CONVEX_DISTANCE_MARGIN)
{
}
@@ -51,6 +52,7 @@ void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAa
}
btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const
{
#ifndef __SPU__
@@ -70,6 +72,7 @@ btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)c
return supVertex;
#else
btAssert(0);
return btVector3(0,0,0);
#endif //__SPU__

7
src/BulletCollision/CollisionShapes/btConvexInternalShape.h
View File

@@ -30,14 +30,7 @@ public:
}
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
#ifndef __SPU__
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0;
//notice that the vectors should be unit length
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
#endif //#ifndef __SPU__
const btVector3& getImplicitShapeDimensions() const
{

6
src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
View File

@@ -43,7 +43,7 @@ btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const b
for (int i=0;i<m_numPoints;i++)
{
btVector3 vtx = m_points[i] * m_localScaling;
btVector3 vtx = getScaledPoint(i);
newDot = vec.dot(vtx);
if (newDot > maxDot)
@@ -67,7 +67,7 @@ void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(
}
for (int i=0;i<m_numPoints;i++)
{
btVector3 vtx = m_points[i] * m_localScaling;
btVector3 vtx = getScaledPoint(i);
for (int j=0;j<numVectors;j++)
{
@@ -133,7 +133,7 @@ void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const
void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const
{
vtx = m_points[i]*m_localScaling;
vtx = m_unscaledPoints[i]*m_localScaling;
}
int btConvexPointCloudShape::getNumPlanes() const

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

@@ -23,15 +23,17 @@ subject to the following restrictions:
///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape
{
btVector3* m_points;
btVector3* m_unscaledPoints;
int m_numPoints;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btConvexPointCloudShape(btVector3* points,int numPoints, bool computeAabb = true)
btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true)
{
m_localScaling = localScaling;
m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
m_points = points;
m_unscaledPoints = points;
m_numPoints = numPoints;
if (computeAabb)
@@ -40,28 +42,33 @@ public:
void setPoints (btVector3* points, int numPoints, bool computeAabb = true)
{
m_points = points;
m_unscaledPoints = points;
m_numPoints = numPoints;
if (computeAabb)
recalcLocalAabb();
}
btVector3* getPoints()
SIMD_FORCE_INLINE btVector3* getUnscaledPoints()
{
return m_points;
return m_unscaledPoints;
}
const btVector3* getPoints() const
SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const
{
return m_points;
return m_unscaledPoints;
}
int getNumPoints() const
SIMD_FORCE_INLINE int getNumPoints() const
{
return m_numPoints;
}
SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const
{
return m_unscaledPoints[index] * m_localScaling;
}
#ifndef __SPU__
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;

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

@@ -14,7 +14,6 @@ subject to the following restrictions:
*/
#include "btConvexShape.h"
#include "btConvexInternalShape.h"
#include "btTriangleShape.h"
#include "btSphereShape.h"
#include "btCylinderShape.h"
@@ -22,7 +21,18 @@ subject to the following restrictions:
#include "btConvexHullShape.h"
#include "btConvexPointCloudShape.h"
static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints)
btConvexShape::btConvexShape ()
{
}
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(-1e30);
@@ -41,7 +51,7 @@ static btVector3 convexHullSupport (const btVector3& localDir, const btVector3*
for (int i=0;i<numPoints;i++)
{
btVector3 vtx = points[i];// * m_localScaling;
btVector3 vtx = points[i] * localScaling;
newDot = vec.dot(vtx);
if (newDot > maxDot)
@@ -64,7 +74,7 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV
break;
case BOX_SHAPE_PROXYTYPE:
{
btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
btBoxShape* convexShape = (btBoxShape*)this;
const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
@@ -203,16 +213,16 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV
case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
{
btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
btVector3* points = convexPointCloudShape->getPoints ();
btVector3* points = convexPointCloudShape->getUnscaledPoints ();
int numPoints = convexPointCloudShape->getNumPoints ();
return convexHullSupport (localDir, points, numPoints);
return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
}
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
btVector3* points = convexHullShape->getPoints ();
btVector3* points = convexHullShape->getUnscaledPoints();
int numPoints = convexHullShape->getNumPoints ();
return convexHullSupport (localDir, points, numPoints);
return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
}
break;
default:
@@ -237,174 +247,8 @@ btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& local
localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
}
localDirNorm.normalize ();
switch (m_shapeType)
{
case SPHERE_SHAPE_PROXYTYPE:
{
return btVector3(0,0,0) + getMarginNonVirtual() * localDirNorm;
}
break;
case BOX_SHAPE_PROXYTYPE:
{
btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
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()) + getMarginNonVirtual() * localDirNorm;
}
break;
case TRIANGLE_SHAPE_PROXYTYPE:
{
btTriangleShape* triangleShape = (btTriangleShape*)this;
btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
btVector3* vertices = &triangleShape->m_vertices1[0];
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()) + getMarginNonVirtual() * localDirNorm;
}
break;
case CYLINDER_SHAPE_PROXYTYPE:
{
btCylinderShape* cylShape = (btCylinderShape*)this;
//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);
break;
};
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()) + getMarginNonVirtual() * localDirNorm;
} 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()) + getMarginNonVirtual() * localDirNorm;
}
}
break;
case CAPSULE_SHAPE_PROXYTYPE:
{
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
btScalar halfHeight = capsuleShape->getHalfHeight();
int capsuleUpAxis = capsuleShape->getUpAxis();
btScalar radius = capsuleShape->getRadius();
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-1e30));
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()) + getMarginNonVirtual() * localDirNorm;
}
break;
case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
{
btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
btVector3* points = convexPointCloudShape->getPoints ();
int numPoints = convexPointCloudShape->getNumPoints ();
return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm;
}
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
btVector3* points = convexHullShape->getPoints ();
int numPoints = convexHullShape->getNumPoints ();
return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm;
}
break;
default:
#ifndef __SPU__
return this->localGetSupportingVertex (localDir);
#else
btAssert (0);
#endif
break;
}
// should never reach here
btAssert (0);
return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
}
/* TODO: This should be bumped up to btCollisionShape () */
@@ -420,7 +264,7 @@ btScalar btConvexShape::getMarginNonVirtual () const
break;
case BOX_SHAPE_PROXYTYPE:
{
btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
btBoxShape* convexShape = (btBoxShape*)this;
return convexShape->getMarginNV ();
}
break;
@@ -483,7 +327,7 @@ void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin,
/* fall through */
case BOX_SHAPE_PROXYTYPE:
{
btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
btBoxShape* convexShape = (btBoxShape*)this;
float margin=convexShape->getMarginNonVirtual();
btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
halfExtents += btVector3(margin,margin,margin);

28
src/BulletCollision/CollisionShapes/btConvexShape.h
View File

@@ -36,29 +36,26 @@ public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btConvexShape ()
{
}
btConvexShape ();
virtual ~btConvexShape()
{
virtual ~btConvexShape();
}
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const = 0;
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const =0;
#ifndef __SPU__
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0;
//notice that the vectors should be unit length
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
#endif //#ifndef __SPU__
////////
#ifndef __SPU__
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0;
#endif //#ifndef __SPU__
btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const;
btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const;
btScalar getMarginNonVirtual () const;
void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
//notice that the vectors should be unit length
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
@@ -75,6 +72,9 @@ public:
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0;
};

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

@@ -37,7 +37,7 @@ GJK-EPA collision solver by Nathanael Presson, 2008
namespace gjkepa2_impl
{
// Config
// Config
/* GJK */
#define GJK_MAX_ITERATIONS 128
@@ -58,12 +58,12 @@ namespace gjkepa2_impl
#define EPA_INSIDE_EPS ((btScalar)0.01)
// Shorthands
typedef unsigned int U;
typedef unsigned char U1;
// Shorthands
typedef unsigned int U;
typedef unsigned char U1;
// MinkowskiDiff
struct MinkowskiDiff
// MinkowskiDiff
struct MinkowskiDiff
{
const btConvexShape* m_shapes[2];
btMatrix3x3 m_toshape1;
@@ -97,44 +97,44 @@ struct MinkowskiDiff
}
};
typedef MinkowskiDiff tShape;
typedef MinkowskiDiff tShape;
// GJK
struct GJK
{
/* Types */
struct sSV
// GJK
struct GJK
{
/* Types */
struct sSV
{
btVector3 d,w;
};
struct sSimplex
struct sSimplex
{
sSV* c[4];
btScalar p[4];
U rank;
};
struct eStatus { enum _ {
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 */
/* 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()
void Initialize()
{
m_ray = btVector3(0,0,0);
m_nfree = 0;
@@ -142,7 +142,7 @@ void Initialize()
m_current = 0;
m_distance = 0;
}
eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
{
U iterations=0;
btScalar sqdist=0;
@@ -262,7 +262,7 @@ eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
}
return(m_status);
}
bool EncloseOrigin()
bool EncloseOrigin()
{
switch(m_simplex->rank)
{
@@ -327,29 +327,29 @@ bool EncloseOrigin()
}
return(false);
}
/* Internals */
void getsupport(const btVector3& d,sSV& sv) const
/* 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)
void removevertice(sSimplex& simplex)
{
m_free[m_nfree++]=simplex.c[--simplex.rank];
}
void appendvertice(sSimplex& simplex,const btVector3& v)
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)
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,
static btScalar projectorigin( const btVector3& a,
const btVector3& b,
btScalar* w,U& m)
{
@@ -364,7 +364,7 @@ static btScalar projectorigin( const btVector3& a,
}
return(-1);
}
static btScalar projectorigin( const btVector3& a,
static btScalar projectorigin( const btVector3& a,
const btVector3& b,
const btVector3& c,
btScalar* w,U& m)
@@ -410,7 +410,7 @@ static btScalar projectorigin( const btVector3& a,
}
return(-1);
}
static btScalar projectorigin( const btVector3& a,
static btScalar projectorigin( const btVector3& a,
const btVector3& b,
const btVector3& c,
const btVector3& d,
@@ -459,14 +459,14 @@ static btScalar projectorigin( const btVector3& a,
}
return(-1);
}
};
};
// EPA
struct EPA
{
/* Types */
typedef GJK::sSV sSV;
struct sFace
// EPA
struct EPA
{
/* Types */
typedef GJK::sSV sSV;
struct sFace
{
btVector3 n;
btScalar d;
@@ -477,20 +477,20 @@ struct sFace
U1 e[3];
U1 pass;
};
struct sList
struct sList
{
sFace* root;
U count;
sList() : root(0),count(0) {}
};
struct sHorizon
struct sHorizon
{
sFace* cf;
sFace* ff;
U nf;
sHorizon() : cf(0),ff(0),nf(0) {}
};
struct eStatus { enum _ {
struct eStatus { enum _ {
Valid,
Touching,
Degenerated,
@@ -501,17 +501,17 @@ struct eStatus { enum _ {
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 */
/* 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();
@@ -523,7 +523,7 @@ sList m_stock;
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)
static inline void append(sList& list,sFace* face)
{
face->l[0] = 0;
face->l[1] = list.root;
@@ -531,7 +531,7 @@ static inline void append(sList& list,sFace* face)
list.root = face;
++list.count;
}
static inline void remove(sList& list,sFace* face)
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];
@@ -540,7 +540,7 @@ static inline void remove(sList& list,sFace* face)
}
void Initialize()
void Initialize()
{
m_status = eStatus::Failed;
m_normal = btVector3(0,0,0);
@@ -551,7 +551,7 @@ void Initialize()
append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
}
}
eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
{
GJK::sSimplex& simplex=*gjk.m_simplex;
if((simplex.rank>1)&&gjk.EncloseOrigin())
@@ -655,7 +655,7 @@ eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
m_result.p[0]=1;
return(m_status);
}
sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
{
if(m_stock.root)
{
@@ -691,7 +691,7 @@ sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
return(0);
}
sFace* findbest()
sFace* findbest()
{
sFace* minf=m_hull.root;
btScalar mind=minf->d*minf->d;
@@ -708,7 +708,7 @@ sFace* findbest()
}
return(minf);
}
bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
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};
@@ -743,26 +743,26 @@ bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
return(false);
}
};
};
//
static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
//
static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1,
btGjkEpaSolver2::sResults& results,
tShape& shape,
bool withmargins)
{
/* Results */
results.witnesses[0] =
results.witnesses[1] = btVector3(0,0,0);
results.status = btGjkEpaSolver2::sResults::Separated;
/* Shape */
shape.m_shapes[0] = shape0;
shape.m_shapes[1] = shape1;
shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
shape.EnableMargin(withmargins);
}
{
/* Results */
results.witnesses[0] =
results.witnesses[1] = btVector3(0,0,0);
results.status = btGjkEpaSolver2::sResults::Separated;
/* Shape */
shape.m_shapes[0] = shape0;
shape.m_shapes[1] = shape1;
shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
shape.EnableMargin(withmargins);
}
}
@@ -775,7 +775,7 @@ using namespace gjkepa2_impl;
//
int btGjkEpaSolver2::StackSizeRequirement()
{
return(sizeof(GJK)+sizeof(EPA));
return(sizeof(GJK)+sizeof(EPA));
}
//
@@ -786,11 +786,11 @@ bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
const btVector3& guess,
sResults& results)
{
tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
if(gjk_status==GJK::eStatus::Valid)
tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
if(gjk_status==GJK::eStatus::Valid)
{
btVector3 w0=btVector3(0,0,0);
btVector3 w1=btVector3(0,0,0);
@@ -825,11 +825,11 @@ bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
sResults& results,
bool usemargins)
{
tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
switch(gjk_status)
tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
switch(gjk_status)
{
case GJK::eStatus::Inside:
{
@@ -855,7 +855,7 @@ switch(gjk_status)
results.status=sResults::GJK_Failed;
break;
}
return(false);
return(false);
}
//
@@ -865,13 +865,13 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
const btTransform& wtrs0,
sResults& results)
{
tShape shape;
btSphereShape shape1(margin);
btTransform wtrs1(btQuaternion(0,0,0,1),position);
Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
if(gjk_status==GJK::eStatus::Valid)
tShape shape;
btSphereShape shape1(margin);
btTransform wtrs1(btQuaternion(0,0,0,1),position);
Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
if(gjk_status==GJK::eStatus::Valid)
{
btVector3 w0=btVector3(0,0,0);
btVector3 w1=btVector3(0,0,0);
@@ -907,7 +907,7 @@ if(gjk_status==GJK::eStatus::Valid)
}
}
}
return(SIMD_INFINITY);
return(SIMD_INFINITY);
}
//
@@ -918,7 +918,7 @@ bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
const btVector3& guess,
sResults& results)
{
if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
else
return(true);

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

@@ -64,17 +64,21 @@ 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->getMargin();
btScalar marginBv = m_minkowskiB->getMargin();
btScalar marginAv = m_minkowskiA->getMarginNonVirtual();
btScalar marginBv = m_minkowskiB->getMarginNonVirtual();
btAssert(marginA == marginAv);
btAssert(marginB == marginBv);
#endif //TEST_NON_VIRTUAL
#endif
gNumGjkChecks++;
@@ -119,16 +123,19 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
#ifdef __SPU__
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
#else
btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
#ifdef TEST_NON_VIRTUAL
btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
#endif
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
#ifdef TEST_NON_VIRTUAL
btAssert((pInAv-pInA).length() < 0.0001);
btAssert((qInBv-qInB).length() < 0.0001);
#endif //
#endif //__SPU__
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);