diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 83d48ad9e..77e784ec2 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -42,7 +42,7 @@ struct btDispatcherInfo m_timeOfImpact(btScalar(1.)), m_useContinuous(false), m_debugDraw(0), - m_enableSatConvex(false), + m_enableSatConvex(true), m_enableSPU(true), m_useEpa(true), m_allowedCcdPenetration(btScalar(0.04)), diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 4bfa428e2..206ff4028 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -33,6 +33,7 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" #include "LinearMath/btSerializer.h" +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION @@ -1339,14 +1340,44 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; int i; - for (i=0;igetNumEdges();i++) + if (polyshape->getConvexPolyhedron()) { - btVector3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); + const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); + for (i=0;im_faces.size();i++) + { + btVector3 centroid(0,0,0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts) + { + int lastV = poly->m_faces[i].m_indices[numVerts-1]; + for (int v=0;vm_faces[i].m_indices.size();v++) + { + int curVert = poly->m_faces[i].m_indices[v]; + centroid+=poly->m_vertices[curVert]; + getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color); + lastV = curVert; + } + } + centroid*= 1./btScalar(numVerts); + btVector3 normalColor(1,1,0); + btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]); + getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor); + + + } + + + } else + { + for (i=0;igetNumEdges();i++) + { + btVector3 a,b; + polyshape->getEdge(i,a,b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa,wb,color); + } } diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 53a0d290f..81ffa2a19 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -332,6 +332,9 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + + + #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { @@ -390,7 +393,6 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } #endif //USE_SEPDISTANCE_UTIL2 - if (min0->isPolyhedral() && min1->isPolyhedral()) { btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; @@ -399,38 +401,40 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl { btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btScalar minDist = 0.f; btVector3 sepNormalWorldSpace; -//#define USE_SAT_TEST -#ifdef USE_SAT_TEST - bool foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0->getWorldTransform(), - body1->getWorldTransform(), - sepNormalWorldSpace); -#else bool foundSepAxis = true; - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); -#endif //USE_SAT_TEST + + if (dispatchInfo.m_enableSatConvex) + { + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0->getWorldTransform(), + body1->getWorldTransform(), + sepNormalWorldSpace); + } else + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); + minDist = gjkPairDetector.getCachedSeparatingDistance(); + } if (foundSepAxis) { - btScalar minDist = gjkPairDetector.getCachedSeparatingDistance(); +// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0->getWorldTransform(), body1->getWorldTransform(), minDist-threshold, threshold, *resultOut); - - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } - return; + } - + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + return; } else { //we can also deal with convex versus triangle (without connectivity data) - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) { @@ -446,13 +450,17 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl btScalar minDist = gjkPairDetector.getCachedSeparatingDistance(); btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), body0->getWorldTransform(), vertices, minDist-threshold, threshold, *resultOut); - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } - return; + + + } + if (m_ownManifold) + { + resultOut->refreshContactPoints(); } } + + return; + } //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 6674a88ae..e84767eae 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -50,6 +50,8 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() btConvexHullComputer conv; conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); + + btAlignedObjectArray faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); @@ -89,25 +91,9 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() btVector3 wb = convexUtil->vertices[targ]; btVector3 newEdge = wb-wa; - if (!newEdge.fuzzyZero()) - { - newEdge.normalize(); - if (!numEdges) - { - edges[numEdges++] = newEdge; - } else - { - btVector3 cr = (edges[0].cross(newEdge)); - btScalar cr2 = cr.length2(); - if (cr2 > maxCross2) - { - chosenEdge = m_polyhedron->m_faces[i].m_indices.size(); - numEdges=1;//replace current edge - edges[numEdges++] = newEdge; - maxCross2=cr2; - } - } - } + newEdge.normalize(); + if (numEdges<2) + edges[numEdges++] = newEdge; edge = edge->getNextEdgeOfFace(); } while (edge!=firstEdge); @@ -143,6 +129,34 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() } + if (m_polyhedron->m_faces.size() && conv.vertices.size()) + { + + for (int f=0;fm_faces.size();f++) + { + + btVector3 planeNormal(m_polyhedron->m_faces[f].m_plane[0],m_polyhedron->m_faces[f].m_plane[1],m_polyhedron->m_faces[f].m_plane[2]); + btScalar planeEq = m_polyhedron->m_faces[f].m_plane[3]; + + btVector3 supVec = localGetSupportingVertex(-planeNormal); + + if (supVec.dot(planeNormal)m_faces[f].m_plane[0] *= -1; + m_polyhedron->m_faces[f].m_plane[1] *= -1; + m_polyhedron->m_faces[f].m_plane[2] *= -1; + m_polyhedron->m_faces[f].m_plane[3] *= -1; + int numVerts = m_polyhedron->m_faces[f].m_indices.size(); + for (int v=0;vm_faces[f].m_indices[v],m_polyhedron->m_faces[f].m_indices[numVerts-1-v]); + } + } + } + } + + + m_polyhedron->initialize(); return true; diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index c1c009daa..31fe7da87 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -208,6 +208,11 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& } } + + const btVector3 deltaC = transB.getOrigin() - transA.getOrigin(); + if((deltaC.dot(sep))>0.0f) + sep = -sep; + return true; } @@ -244,10 +249,12 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin int numVerticesA = polyA.m_indices.size(); for(int e0=0;e0