From 642a6821a5dd64cc863c4893426f67ce57845c9b Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Tue, 30 Aug 2011 03:28:32 +0000 Subject: [PATCH] remove speculative contacts, they break restitution and cause other artifacts use btConvexPolyhedron for debug rendering, if available fixes in btConvexConvexAlgorithm for polyhedral contact clipping, if GJK separating normal is zero don't shift vertices in btPolyhedralConvexShape --- Demos/OpenGL/GL_ShapeDrawer.cpp | 60 ++++---- .../btConvexConvexAlgorithm.cpp | 135 ++++++++++-------- .../CollisionDispatch/btManifoldResult.cpp | 4 +- .../btPolyhedralConvexShape.cpp | 12 +- .../btPersistentManifold.h | 8 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 111 -------------- .../Dynamics/btDiscreteDynamicsWorld.h | 2 - src/BulletSoftBody/btSoftBodyHelpers.cpp | 2 +- src/LinearMath/btQuickprof.cpp | 3 +- src/LinearMath/btQuickprof.h | 9 +- 10 files changed, 127 insertions(+), 219 deletions(-) diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp index 850f70b6d..bfcb5f735 100644 --- a/Demos/OpenGL/GL_ShapeDrawer.cpp +++ b/Demos/OpenGL/GL_ShapeDrawer.cpp @@ -35,7 +35,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btUniformScalingShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" - +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" /// #include "BulletCollision/CollisionShapes/btShapeHull.h" @@ -729,43 +729,41 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons default: { - - if (shape->isConvex()) { - ShapeCache* sc=cache((btConvexShape*)shape); -#if 0 - btConvexShape* convexShape = (btConvexShape*)shape; - if (!shape->getUserPointer()) + const btConvexPolyhedron* poly = shape->isPolyhedral() ? ((btPolyhedralConvexShape*) shape)->getConvexPolyhedron() : 0; + if (poly) { - //create a hull approximation - void* mem = btAlignedAlloc(sizeof(btShapeHull),16); - btShapeHull* hull = new(mem) btShapeHull(convexShape); - - ///cleanup memory - m_shapeHulls.push_back(hull); - - btScalar margin = shape->getMargin(); - hull->buildHull(margin); - convexShape->setUserPointer(hull); - - - // printf("numTriangles = %d\n", hull->numTriangles ()); - // printf("numIndices = %d\n", hull->numIndices ()); - // printf("numVertices = %d\n", hull->numVertices ()); - - - } -#endif - - - - //if (shape->getUserPointer()) + int i; + glBegin (GL_TRIANGLES); + for (i=0;im_faces.size();i++) + { + btVector3 centroid(0,0,0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts>2) + { + btVector3 v1 = poly->m_vertices[poly->m_faces[i].m_indices[0]]; + for (int v=0;vm_faces[i].m_indices.size()-2;v++) + { + + btVector3 v2 = poly->m_vertices[poly->m_faces[i].m_indices[v+1]]; + btVector3 v3 = poly->m_vertices[poly->m_faces[i].m_indices[v+2]]; + btVector3 normal = (v3-v1).cross(v2-v1); + normal.normalize (); + glNormal3f(normal.getX(),normal.getY(),normal.getZ()); + glVertex3f (v1.x(), v1.y(), v1.z()); + glVertex3f (v2.x(), v2.y(), v2.z()); + glVertex3f (v3.x(), v3.y(), v3.z()); + } + } + } + glEnd (); + } else { + ShapeCache* sc=cache((btConvexShape*)shape); //glutSolidCube(1.0); btShapeHull* hull = &sc->m_shapehull/*(btShapeHull*)shape->getUserPointer()*/; - if (hull->numTriangles () > 0) { int index = 0; diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index d575c427b..dd1f3e249 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -441,15 +441,19 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl //gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif //ZERO_MARGIN - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); - minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); #ifdef ZERO_MARGIN - foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; + foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; #else - foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin()); + foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin()); #endif + } } if (foundSepAxis) { @@ -486,7 +490,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl btScalar minDist =-1e30f; btScalar maxDist = threshold; - bool foundSepAxis = true; + bool foundSepAxis = false; if (0) { polyhedronB->initializePolyhedralFeatures(); @@ -506,10 +510,15 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - //minDist = gjkPairDetector.getCachedSeparatingDistance(); - //maxDist = threshold; - minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = gjkPairDetector.getCachedSeparatingDistance(); + //maxDist = threshold; + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + foundSepAxis = true; + } } @@ -544,66 +553,70 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl int i; btVector3 v0,v1; btVector3 sepNormalWorldSpace; + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - btPlaneSpace1(sepNormalWorldSpace,v0,v1); - - - bool perturbeA = true; - const btScalar angleLimit = 0.125f * SIMD_PI; - btScalar perturbeAngle; - btScalar radiusA = min0->getAngularMotionDisc(); - btScalar radiusB = min1->getAngularMotionDisc(); - if (radiusA < radiusB) + if (l2>SIMD_EPSILON) { - perturbeAngle = gContactBreakingThreshold /radiusA; - perturbeA = true; - } else - { - perturbeAngle = gContactBreakingThreshold / radiusB; - perturbeA = false; - } - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; - - btTransform unPerturbedTransform; - if (perturbeA) - { - unPerturbedTransform = input.m_transformA; - } else - { - unPerturbedTransform = input.m_transformB; - } - - for ( i=0;iSIMD_EPSILON) - { - btQuaternion perturbeRot(v0,perturbeAngle); - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(sepNormalWorldSpace,iterationAngle); + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - - if (perturbeA) + btPlaneSpace1(sepNormalWorldSpace,v0,v1); + + + bool perturbeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) { - input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); - input.m_transformB = body1->getWorldTransform(); -#ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); -#endif //DEBUG_CONTACTS + perturbeAngle = gContactBreakingThreshold /radiusA; + perturbeA = true; } else { - input.m_transformA = body0->getWorldTransform(); - input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); -#ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); -#endif + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; + if (perturbeA) + { + unPerturbedTransform = input.m_transformA; + } else + { + unPerturbedTransform = input.m_transformB; } - btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); - gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + for ( i=0;iSIMD_EPSILON) + { + btQuaternion perturbeRot(v0,perturbeAngle); + btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); + btQuaternion rotq(sepNormalWorldSpace,iterationAngle); + + + if (perturbeA) + { + input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); + input.m_transformB = body1->getWorldTransform(); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); + #endif //DEBUG_CONTACTS + } else + { + input.m_transformA = body0->getWorldTransform(); + input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); + #endif + } + + btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + } } - } } diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index fd684c056..bf24246ea 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -64,8 +64,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b btAssert(m_manifoldPtr); //order in manifold needs to match -// if (depth > m_manifoldPtr->getContactBreakingThreshold()) - if (depth > m_manifoldPtr->getContactProcessingThreshold()) + if (depth > m_manifoldPtr->getContactBreakingThreshold()) +// if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0; diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index a9a887d3e..df1fbe927 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -53,7 +53,7 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() getVertex(i,newVertex); } - +#if 0 btAlignedObjectArray planeEquations; btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); @@ -68,11 +68,17 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() btAlignedObjectArray tmpVertices; btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); - - btConvexHullComputer conv; conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); +#else + btConvexHullComputer conv; + conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); + +#endif + + + btAlignedObjectArray faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 83d17f2a6..d877f0994 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -197,14 +197,10 @@ public: #endif } + bool validContactDistance(const btManifoldPoint& pt) const { - if (pt.m_lifeTime >1) - { - return pt.m_distance1 <= getContactBreakingThreshold(); - } - return pt.m_distance1 <= getContactProcessingThreshold(); - + return pt.m_distance1 <= getContactBreakingThreshold(); } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin void refreshContactPoints( const btTransform& trA,const btTransform& trB); diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 1eb3e4105..9b39e6802 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -327,9 +327,6 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///perform collision detection performDiscreteCollisionDetection(); - if (getDispatchInfo().m_useContinuous) - addSpeculativeContacts(timeStep); - calculateSimulationIslands(); @@ -951,114 +948,6 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) } } -void btDiscreteDynamicsWorld::addSpeculativeContacts(btScalar timeStep) -{ - BT_PROFILE("addSpeculativeContacts"); - btTransform predictedTrans; - for ( int i=0;isetHitFraction(1.f); - - if (body->isActive() && (!body->isStaticOrKinematicObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) - { - BT_PROFILE("search speculative contacts"); - if (body->getCollisionShape()->isConvex()) - { - gNumClampedCcdMotions++; - - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); - //btConvexShape* convexShape = static_cast(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(body->getCollisionShape()); - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - btTransform modifiedPredictedTrans; - modifiedPredictedTrans = predictedTrans; - modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); - if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) - { - btBroadphaseProxy* proxy0 = body->getBroadphaseHandle(); - btBroadphaseProxy* proxy1 = sweepResults.m_hitCollisionObject->getBroadphaseHandle(); - btBroadphasePair* pair = sweepResults.m_pairCache->findPair(proxy0,proxy1); - if (pair) - { - if (pair->m_algorithm) - { - btManifoldArray contacts; - pair->m_algorithm->getAllContactManifolds(contacts); - if (contacts.size()) - { - btManifoldResult result(body,sweepResults.m_hitCollisionObject); - result.setPersistentManifold(contacts[0]); - - btVector3 vec = (modifiedPredictedTrans.getOrigin()-body->getWorldTransform().getOrigin()); - vec*=sweepResults.m_closestHitFraction; - - btScalar lenSqr = vec.length2(); - btScalar depth = 0.f; - btVector3 pointWorld = sweepResults.m_hitPointWorld; - if (lenSqr>SIMD_EPSILON) - { - depth = btSqrt(lenSqr); - pointWorld -= vec; - vec /= depth; - } - - if (contacts[0]->getBody0()==body) - { - result.addContactPoint(sweepResults.m_hitNormalWorld,pointWorld,depth); -#if 0 - debugContacts.push_back(sweepResults.m_hitPointWorld);//sweepResults.m_hitPointWorld); - debugNormals.push_back(sweepResults.m_hitNormalWorld); -#endif - } else - { - //swapped - result.addContactPoint(-sweepResults.m_hitNormalWorld,pointWorld,depth); - //sweepResults.m_hitPointWorld,depth); - -#if 0 - if (1)//firstHit==1) - { - firstHit=0; - debugNormals.push_back(sweepResults.m_hitNormalWorld); - debugContacts.push_back(pointWorld);//sweepResults.m_hitPointWorld); - debugNormals.push_back(sweepResults.m_hitNormalWorld); - debugContacts.push_back(sweepResults.m_hitPointWorld); - } - firstHit--; -#endif - } - } - - } else - { - //no algorithm, use dispatcher to create one - - } - - - } else - { - //add an overlapping pair - //printf("pair missing\n"); - - } - } - } - } - - } - } -} diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 3ba39a1c4..df47c2904 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -62,8 +62,6 @@ protected: virtual void integrateTransforms(btScalar timeStep); - virtual void addSpeculativeContacts(btScalar timeStep); - virtual void calculateSimulationIslands(); virtual void solveConstraints(btContactSolverInfo& solverInfo); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 96a50a3f0..0fb3560e9 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -329,7 +329,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, { const btScalar scl=(btScalar)0.8; const btScalar alp=(btScalar)1; - const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7); + const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7); for(int i=0;im_tetras.size();++i) { const btSoftBody::Tetra& t=psb->m_tetras[i]; diff --git a/src/LinearMath/btQuickprof.cpp b/src/LinearMath/btQuickprof.cpp index 14534068a..544aee89d 100644 --- a/src/LinearMath/btQuickprof.cpp +++ b/src/LinearMath/btQuickprof.cpp @@ -276,7 +276,8 @@ CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) : RecursionCounter( 0 ), Parent( parent ), Child( NULL ), - Sibling( NULL ) + Sibling( NULL ), + m_userPtr(0) { Reset(); } diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h index 0da13ab3c..93f3f4a60 100644 --- a/src/LinearMath/btQuickprof.h +++ b/src/LinearMath/btQuickprof.h @@ -82,7 +82,8 @@ public: const char * Get_Name( void ) { return Name; } int Get_Total_Calls( void ) { return TotalCalls; } float Get_Total_Time( void ) { return TotalTime; } - + void* GetUserPointer() const {return m_userPtr;} + void SetUserPointer(void* ptr) { m_userPtr = ptr;} protected: const char * Name; @@ -94,6 +95,7 @@ protected: CProfileNode * Parent; CProfileNode * Child; CProfileNode * Sibling; + void* m_userPtr; }; ///An iterator to navigate through the tree @@ -115,15 +117,20 @@ public: int Get_Current_Total_Calls( void ) { return CurrentChild->Get_Total_Calls(); } float Get_Current_Total_Time( void ) { return CurrentChild->Get_Total_Time(); } + void* Get_Current_UserPointer( void ) { return CurrentChild->GetUserPointer(); } + void Set_Current_UserPointer(void* ptr) {CurrentChild->SetUserPointer(ptr);} // Access the current parent const char * Get_Current_Parent_Name( void ) { return CurrentParent->Get_Name(); } int Get_Current_Parent_Total_Calls( void ) { return CurrentParent->Get_Total_Calls(); } float Get_Current_Parent_Total_Time( void ) { return CurrentParent->Get_Total_Time(); } + + protected: CProfileNode * CurrentParent; CProfileNode * CurrentChild; + CProfileIterator( CProfileNode * start ); friend class CProfileManager;