diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 3ddcfe080..0af4728bd 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -513,6 +513,7 @@ static btCollisionShape* createConvexHullFromShapes(std::vectorrecalcLocalAabb(); 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); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0dc931a93..63a66f110 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 1776c1da6..b48258ce6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b7ebc132f..3f582d0f3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index dd2434124..248f726d4 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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 diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 4648314c7..25ba5128b 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -797,6 +797,7 @@ struct b3PhysicsSimulationParameters int m_jointFeedbackMode; double m_solverResidualThreshold; double m_contactSlop; + int m_enableSAT; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b2ef1027a..7bb9cdfcf 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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); } diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 960861b52..3587e4767 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -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;vm_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;vm_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; } + } diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 4f45319a8..0fea00df5 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -104,9 +104,9 @@ void btConvexPolyhedron::initialize() btHashMap edges; - btScalar TotalArea = 0.0f; - m_localCenter.setValue(0, 0, 0); + + for(int i=0;im_vertices.resize(numVertices); + for (int p=0;pm_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;vm_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 faceNormals; int numFaces = conv.faces.size(); @@ -311,7 +386,9 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa } - + +#endif //BT_RECONSTRUCT_FACES + m_polyhedron->initialize(); return true; diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 7bf8e01c1..b7ddb6e06 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -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; diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 4d29089a4..4b534ea53 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -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 //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()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); } - }