diff --git a/Demos/OpenGL/GLDebugDrawer.cpp b/Demos/OpenGL/GLDebugDrawer.cpp index 7933ade9f..79758df6c 100644 --- a/Demos/OpenGL/GLDebugDrawer.cpp +++ b/Demos/OpenGL/GLDebugDrawer.cpp @@ -114,20 +114,19 @@ void GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& { { - btVector3 to=pointOnB+normalOnB*distance; + btVector3 to=pointOnB+normalOnB*1;//distance; const btVector3&from = pointOnB; glColor4f(color.getX(), color.getY(), color.getZ(),1.f); //glColor4f(0,0,0,1.f); - glBegin(GL_LINES); glVertex3d(from.getX(), from.getY(), from.getZ()); glVertex3d(to.getX(), to.getY(), to.getZ()); glEnd(); - glRasterPos3f(from.x(), from.y(), from.z()); - char buf[12]; - sprintf(buf," %d",lifeTime); +// glRasterPos3f(from.x(), from.y(), from.z()); +// char buf[12]; +// sprintf(buf," %d",lifeTime); //BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 268ec4b6c..d2b2c2214 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -91,7 +91,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i btCollisionObject* ob = static_cast(m_triBody); - +#if 0 ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) { @@ -100,17 +100,8 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); - - //btVector3 center = triangle[0] + triangle[1]+triangle[2]; - //center *= btScalar(0.333333); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color); - } - - - //btCollisionObject* colObj = static_cast(m_convexProxy->m_clientObject); +#endif if (m_convexBody->getCollisionShape()->isConvex()) { @@ -119,7 +110,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( &tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); if (m_resultOut->getBody0Internal() == m_triBody) diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 302bfe0a2..53a0d290f 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -26,6 +26,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" + #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" @@ -395,18 +397,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) { - - btScalar maxDist = 0.f; + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); - if (dispatchInfo.m_convexMaxDistanceUseCPT) - { - maxDist = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); - } else - { - maxDist = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - } - - maxDist =0.f; btVector3 sepNormalWorldSpace; //#define USE_SAT_TEST #ifdef USE_SAT_TEST @@ -421,10 +413,12 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl #endif //USE_SAT_TEST if (foundSepAxis) { - btPolyhedralContactClipping::clipFaceContacts(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0->getWorldTransform(), - body1->getWorldTransform(), maxDist, *resultOut); + btScalar minDist = gjkPairDetector.getCachedSeparatingDistance(); + btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0->getWorldTransform(), + body1->getWorldTransform(), minDist-threshold, threshold, *resultOut); + if (m_ownManifold) { resultOut->refreshContactPoints(); @@ -433,6 +427,31 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } + } else + { + //we can also deal with convex versus triangle (without connectivity data) + + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) + { + + btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); + + btVertexArray vertices; + btTriangleShape* tri = (btTriangleShape*)polyhedronB; + vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[0]); + vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[1]); + vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[2]); + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btScalar minDist = gjkPairDetector.getCachedSeparatingDistance(); + btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), + body0->getWorldTransform(), vertices, minDist-threshold, threshold, *resultOut); + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + return; + } } } diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index ea3d185a5..c1c009daa 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -208,29 +208,17 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& } } + return true; } -void btPolyhedralContactClipping::clipFaceContacts(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut) + +void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) { + btVertexArray worldVertsB2; + btVertexArray* pVtxIn = &worldVertsB1; + btVertexArray* pVtxOut = &worldVertsB2; + pVtxOut->reserve(pVtxIn->size()); - btScalar curMaxDist=maxDist; - int closestFaceA=-1, closestFaceB=-1; - - { - btScalar dmax = -FLT_MAX; - for(int face=0;face dmax) - { - dmax = d; - closestFaceB = face; - } - } - } - + int closestFaceA=-1; { btScalar dmin = FLT_MAX; for(int face=0;facepush_back(transB*b); - } - } - pVtxOut->reserve(pVtxIn->size()); - // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face int numContacts = pVtxIn->size(); int numVerticesA = polyA.m_indices.size(); for(int e0=0;e0at(i))+planeEqWS; - if (depth <=0) + if (depth <=maxDist && depth >=minDist) { btVector3 point = pVtxIn->at(i); #ifdef ONLY_REPORT_DEEPEST_POINT @@ -334,4 +303,51 @@ void btPolyhedralContactClipping::clipFaceContacts(const btVector3& separatingNo } #endif //ONLY_REPORT_DEEPEST_POINT +} + +void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +{ + + btScalar curMaxDist=maxDist; + int closestFaceB=-1; + + { + btScalar dmax = -FLT_MAX; + for(int face=0;face dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + + + if (closestFaceB<0) + { + return; + } + + + + // setup initial clip face (minimizing face from hull B) + btVertexArray worldVertsB1; + { + const btFace& polyB = hullB.m_faces[closestFaceB]; + const int numVertices = polyB.m_indices.size(); + for(int e0=0;e0 btVertexArray; // Clips a face to the back of a plane struct btPolyhedralContactClipping { - static void clipFaceContacts(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); - - static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS); + static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); + static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep); + + ///the clipFace method is used internally + static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS); + }; #endif // __POLYHEDRAL_CONTACT_CLIPPING_H