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

Simplify GJK. Still needs double precision for large differences of feature scales.

Browse Source 
Extract faces directly from btConvexHullComputer (in initializePolyhedralFeatures), instead of reconstructing them, thanks to Josh Klint in #1654
PyBullet: use initializePolyhedralFeatures for convex hulls and boxes (to allow SAT)
PyBullet: expose setPhysicsEngineParameter(enableSAT=0 or 1) to enable Separating Axis Test based collision detection for convex vs convex/box and convex versus concave triangles (in a triangle mesh).
This commit is contained in:
erwincoumans
2018-06-12 16:08:46 -07:00
parent a342af0382
commit 97c6937388
13 changed files with 257 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -513,6 +513,7 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
convexHull->initializePolyhedralFeatures();
compound->addChildShape(identity,convexHull);
}
@@ -694,6 +695,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
btVector3 extents = collision->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
boxShape->initializePolyhedralFeatures();
shape = boxShape;
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
@@ -905,7 +907,7 @@ upAxisMat.setIdentity();
BT_PROFILE("convert btConvexHullShape");
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures();
convexHull->initializePolyhedralFeatures();
convexHull->setMargin(gUrdfDefaultCollisionMargin);
convexHull->recalcLocalAabb();
//convexHull->setLocalScaling(collision->m_geometry.m_meshScale);

10
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -611,8 +611,14 @@ B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle comma
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_enableSAT = enableSAT;
command->m_updateFlags |= SIM_PARAM_ENABLE_SAT;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
{

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -317,6 +317,7 @@ B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryComma
B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode);
B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold);
B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop);
B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT);

7
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6865,6 +6865,8 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
serverCmd.m_simulationParameterResultArgs.m_enableSAT = m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex;
serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm;
serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp;
@@ -6988,6 +6990,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop;
}
if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT)
{
m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
{

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -455,6 +455,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576,
SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152,
SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
SIM_PARAM_ENABLE_SAT = 8388608,
};
enum EnumLoadSoftBodyUpdateFlags

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -797,6 +797,7 @@ struct b3PhysicsSimulationParameters
int m_jointFeedbackMode;
double m_solverResidualThreshold;
double m_contactSlop;
int m_enableSAT;
};

15
examples/pybullet/pybullet.c
View File

@@ -1434,12 +1434,13 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int jointFeedbackMode =-1;
double solverResidualThreshold = -1;
double contactSlop = -1;
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "physicsClientId", NULL};
int enableSAT = -1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &physicsClientId))
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &physicsClientId))
{
return NULL;
}
@@ -1540,6 +1541,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
b3PhysicsParameterSetJointFeedbackMode(command,jointFeedbackMode);
}
if (enableSAT>=0)
{
b3PhysicsParameterSetEnableSAT(command, enableSAT);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}

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

@@ -28,6 +28,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
@@ -442,10 +443,26 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
{
btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld;
btScalar m_depth;
bool m_hasContact;
btDummyResult()
: m_hasContact(false)
{
}
virtual void setShapeIdentifiersA(int partId0,int index0){}
virtual void setShapeIdentifiersB(int partId1,int index1){}
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
m_hasContact = true;
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
m_depth = depth;
}
};
@@ -560,15 +577,18 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
} else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
{
btVertexArray vertices;
btVertexArray worldSpaceVertices;
btTriangleShape* tri = (btTriangleShape*)polyhedronB;
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
//tri->initializePolyhedralFeatures();
@@ -579,17 +599,97 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
btScalar maxDist = threshold;
bool foundSepAxis = false;
if (0)
bool useSatSepNormal = true;
if (useSatSepNormal)
{
polyhedronB->initializePolyhedralFeatures();
if (0)
{
//initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle
polyhedronB->initializePolyhedralFeatures();
} else
{
btVector3 uniqueEdges[3] = {tri->m_vertices1[1]-tri->m_vertices1[0],
tri->m_vertices1[2]-tri->m_vertices1[1],
tri->m_vertices1[0]-tri->m_vertices1[2]};
uniqueEdges[0].normalize();
uniqueEdges[1].normalize();
uniqueEdges[2].normalize();
btConvexPolyhedron polyhedron;
polyhedron.m_vertices.push_back(tri->m_vertices1[2]);
polyhedron.m_vertices.push_back(tri->m_vertices1[0]);
polyhedron.m_vertices.push_back(tri->m_vertices1[1]);
{
btFace combinedFaceA;
combinedFaceA.m_indices.push_back(0);
combinedFaceA.m_indices.push_back(1);
combinedFaceA.m_indices.push_back(2);
btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFaceA.m_indices.size();v++)
{
btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFaceA.m_plane[0] = faceNormal[0];
combinedFaceA.m_plane[1] = faceNormal[1];
combinedFaceA.m_plane[2] = faceNormal[2];
combinedFaceA.m_plane[3] = -planeEq;
polyhedron.m_faces.push_back(combinedFaceA);
}
{
btFace combinedFaceB;
combinedFaceB.m_indices.push_back(0);
combinedFaceB.m_indices.push_back(2);
combinedFaceB.m_indices.push_back(1);
btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFaceB.m_indices.size();v++)
{
btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFaceB.m_plane[0] = faceNormal[0];
combinedFaceB.m_plane[1] = faceNormal[1];
combinedFaceB.m_plane[2] = faceNormal[2];
combinedFaceB.m_plane[3] = -planeEq;
polyhedron.m_faces.push_back(combinedFaceB);
}
polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]);
polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]);
polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]);
polyhedron.initialize2();
polyhedronB->setPolyhedralFeatures(polyhedron);
}
foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace,*resultOut);
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace,*resultOut);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
} else
}
else
{
#ifdef ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
@@ -598,6 +698,24 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
if (dummy.m_hasContact && dummy.m_depth<0)
{
if (foundSepAxis)
{
if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99)
{
printf("?\n");
}
} else
{
printf("!\n");
}
sepNormalWorldSpace.setValue(0,0,1);// = dummy.m_normalOnBInWorld;
//minDist = dummy.m_depth;
foundSepAxis = true;
}
#if 0
btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if (l2>SIMD_EPSILON)
{
@@ -607,6 +725,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
foundSepAxis = true;
}
#endif
}
@@ -614,7 +733,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
{
worldVertsB2.resize(0);
btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
}
@@ -625,6 +744,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
return;
}
}

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

@@ -104,9 +104,9 @@ void btConvexPolyhedron::initialize()
btHashMap<btInternalVertexPair,btInternalEdge> edges;
btScalar TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@@ -172,6 +172,13 @@ void btConvexPolyhedron::initialize()
}
#endif//USE_CONNECTED_FACES
initialize2();
}
void btConvexPolyhedron::initialize2()
{
m_localCenter.setValue(0, 0, 0);
btScalar TotalArea = 0.0f;
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@@ -274,7 +281,6 @@ void btConvexPolyhedron::initialize()
}
#endif
}
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
minProj = FLT_MAX;

1
src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
View File

@@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
btVector3 mE;
void initialize();
void initialize2();
bool testContainment() const;
void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;

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

@@ -39,6 +39,17 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
}
}
void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron)
{
if (m_polyhedron)
{
*m_polyhedron = polyhedron;
} else
{
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron(polyhedron);
}
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
{
@@ -87,7 +98,71 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
}
#ifndef BT_RECONSTRUCT_FACES
int numVertices = conv.vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = conv.vertices[p];
}
int v0, v1;
for (int j = 0; j < conv.faces.size(); j++)
{
btVector3 edges[3];
int numEdges = 0;
btFace combinedFace;
const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
v0 = edge->getSourceVertex();
int prevVertex=v0;
combinedFace.m_indices.push_back(v0);
v1 = edge->getTargetVertex();
while (v1 != v0)
{
btVector3 wa = conv.vertices[prevVertex];
btVector3 wb = conv.vertices[v1];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
//face->addIndex(v1);
combinedFace.m_indices.push_back(v1);
edge = edge->getNextEdgeOfFace();
prevVertex = v1;
int v01 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
}
btAssert(combinedFace.m_indices.size() > 2);
btVector3 faceNormal = edges[0].cross(edges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFace.m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFace.m_plane[0] = faceNormal.getX();
combinedFace.m_plane[1] = faceNormal.getY();
combinedFace.m_plane[2] = faceNormal.getZ();
combinedFace.m_plane[3] = -planeEq;
m_polyhedron->m_faces.push_back(combinedFace);
}
#else//BT_RECONSTRUCT_FACES
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
@@ -311,7 +386,9 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
}
#endif //BT_RECONSTRUCT_FACES
m_polyhedron->initialize();
return true;

3
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
View File

@@ -43,6 +43,9 @@ public:
///experimental/work-in-progress
virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0);
virtual void setPolyhedralFeatures(btConvexPolyhedron& polyhedron);
const btConvexPolyhedron* getConvexPolyhedron() const
{
return m_polyhedron;

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

@@ -19,7 +19,6 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
#if defined(DEBUG) || defined (_DEBUG)
//#define TEST_NON_VIRTUAL 1
#include <stdio.h> //for debug printf
@@ -29,14 +28,6 @@ subject to the following restrictions:
#endif //__SPU__
#endif
//must be above the machine epsilon
#ifdef BT_USE_DOUBLE_PRECISION
#define REL_ERROR2 btScalar(1.0e-12)
btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
#else
#define REL_ERROR2 btScalar(1.0e-6)
btScalar gGjkEpaPenetrationTolerance = 0.001;
#endif
//temp globals, to improve GJK/EPA/penetration calculations
int gNumDeepPenetrationChecks = 0;
@@ -136,177 +127,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
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->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
if (check2d)
{
pWorld[2] = 0.f;
qWorld[2] = 0.f;
}
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))
{
m_degenerateSimplex = 10;
checkSimplex=true;
//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;
} else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver->addVertex(w, pWorld, qWorld);
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
if (!m_simplexSolver->closest(newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if(newCachedSeparatingAxis.length2()<REL_ERROR2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
btScalar previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.length2();
#if 0
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//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;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
#if defined(DEBUG) || defined (_DEBUG)
printf("btGjkPairDetector 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_minkowskiA->getShapeType(),
m_minkowskiB->getShapeType());
#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);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
m_simplexSolver->compute_points(pointOnA, pointOnB);
normalInB = m_cachedSeparatingAxis;
btScalar lenSqr =m_cachedSeparatingAxis.length2();
//valid normal
if (lenSqr < REL_ERROR2)
{
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) < gGjkEpaPenetrationTolerance));
bool catchDegeneratePenetrationCase = 1;
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
@@ -451,14 +272,9 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
}
}
output.addContactPoint(
normalInB,
pointOnB+positionOffset,
distance);
output.addContactPoint( normalInB, pointOnB+positionOffset, distance);
}
}