bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep) { gActualSATPairTests++; //#ifdef TEST_INTERNAL_OBJECTS const btVector3 c0 = transA * hullA.m_localCenter; const btVector3 c1 = transB * hullB.m_localCenter; const btVector3 DeltaC2 = c0 - c1; //#endif btScalar dmin = FLT_MAX; int curPlaneTests=0; int numFacesA = hullA.m_faces.size(); // Test normals from hullA for(int i=0;i<numFacesA;i++) { const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]); const btVector3 faceANormalWS = transA.getBasis() * Normal; if (DeltaC2.dot(faceANormalWS)<0) continue; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar d; if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d)) return false; if(d<dmin) { dmin = d; sep = faceANormalWS; } } int numFacesB = hullB.m_faces.size(); // Test normals from hullB for(int i=0;i<numFacesB;i++) { const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]); const btVector3 WorldNormal = transB.getBasis() * Normal; if (DeltaC2.dot(WorldNormal)<0) continue; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar d; if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d)) return false; if(d<dmin) { dmin = d; sep = WorldNormal; } } btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend; int curEdgeEdge = 0; // Test edges for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++) { const btVector3 edge0 = hullA.m_uniqueEdges[e0]; const btVector3 WorldEdge0 = transA.getBasis() * edge0; for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++) { const btVector3 edge1 = hullB.m_uniqueEdges[e1]; const btVector3 WorldEdge1 = transB.getBasis() * edge1; btVector3 Cross = WorldEdge0.cross(WorldEdge1); curEdgeEdge++; if(!IsAlmostZero(Cross)) { Cross = Cross.normalize(); if (DeltaC2.dot(Cross)<0) continue; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar dist; if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist)) return false; if(dist<dmin) { dmin = dist; sep = Cross; } } } } const btVector3 deltaC = transB.getOrigin() - transA.getOrigin(); if((deltaC.dot(sep))>0.0f) sep = -sep; return true; }
static bool ReferenceCode(const MyConvex& hull0, const MyConvex& hull1, float& dmin, btVector3& sep) { dmin = FLT_MAX; int i; // Test normals from hull0 for( i=0;i<hull0.mNbPolys;i++) { btVector3 Normal(hull0.mPolys[i].mPlane[0], hull0.mPolys[i].mPlane[1], hull0.mPolys[i].mPlane[2]); // btVector3 WorldNormal = hull0.mTransform * Normal; btVector3 WorldNormal = hull0.mTransform.getBasis() * Normal; float d; if(!TestSepAxis(WorldNormal, hull0, hull1, d)) return false; if(d<dmin) { dmin = d; sep = WorldNormal; } } // Test normals from hull1 for( i=0;i<hull1.mNbPolys;i++) { btVector3 Normal(hull1.mPolys[i].mPlane[0], hull1.mPolys[i].mPlane[1], hull1.mPolys[i].mPlane[2]); // btVector3 WorldNormal = hull1.mTransform * Normal; btVector3 WorldNormal = hull1.mTransform.getBasis() * Normal; float d; if(!TestSepAxis(WorldNormal, hull0, hull1, d)) return false; if(d<dmin) { dmin = d; sep = WorldNormal; } } // Test edges for(int j=0;j<hull0.mNbPolys;j++) { const MyPoly& poly0 = hull0.mPolys[j]; for(int i=0;i<hull1.mNbPolys;i++) { const MyPoly& poly1 = hull1.mPolys[i]; for(int e0=0;e0<poly0.mNbVerts;e0++) { const btVector3& a = hull0.mVerts[poly0.mIndices[e0]]; const btVector3& b = hull0.mVerts[poly0.mIndices[(e0+1)%poly0.mNbVerts]]; btVector3 edge0 = a - b; btVector3 WorldEdge0 = hull0.mTransform.getBasis() * edge0; for(int e1=0;e1<poly1.mNbVerts;e1++) { const btVector3& c = hull1.mVerts[poly1.mIndices[e1]]; const btVector3& d = hull1.mVerts[poly1.mIndices[(e1+1)%poly1.mNbVerts]]; btVector3 edge1 = c - d; btVector3 WorldEdge1 = hull1.mTransform.getBasis() * edge1; btVector3 Cross = WorldEdge0.cross(WorldEdge1); if(!IsAlmostZero(Cross)) { Cross = Cross.normalize(); float d; if(!TestSepAxis(Cross, hull0, hull1, d)) return false; if(d<dmin) { dmin = d; sep = Cross; } } } } } } btVector3 DeltaC = (hull1.mTransform * hull1.mTransform.getOrigin()) - (hull0.mTransform * hull0.mTransform.getOrigin()); if((DeltaC.dot(sep))>0.0f) sep = -sep; return true; }
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) { gActualSATPairTests++; //#ifdef TEST_INTERNAL_OBJECTS const btVector3 c0 = transA * hullA.m_localCenter; const btVector3 c1 = transB * hullB.m_localCenter; const btVector3 DeltaC2 = c0 - c1; //#endif btScalar dmin = FLT_MAX; int curPlaneTests=0; int numFacesA = hullA.m_faces.size(); // Test normals from hullA for(int i=0;i<numFacesA;i++) { const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]); btVector3 faceANormalWS = transA.getBasis() * Normal; if (DeltaC2.dot(faceANormalWS)<0) faceANormalWS*=-1.f; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar d; btVector3 wA, wB; if(!TestSepAxis( hullA, hullB, transA, transB, faceANormalWS, d, wA, wB)) return false; if(d<dmin) { dmin = d; sep = faceANormalWS; } } int numFacesB = hullB.m_faces.size(); // Test normals from hullB for(int i=0;i<numFacesB;i++) { const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]); btVector3 WorldNormal = transB.getBasis() * Normal; if (DeltaC2.dot(WorldNormal)<0) WorldNormal *=-1.f; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar d; btVector3 wA, wB; if(!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d,wA, wB)) return false; if(d<dmin) { dmin = d; sep = WorldNormal; } } btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend; int edgeA=-1; int edgeB=-1; btVector3 worldEdgeA; btVector3 worldEdgeB; btVector3 witnessPointA, witnessPointB; int curEdgeEdge = 0; // Test edges for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++) { const btVector3 edge0 = hullA.m_uniqueEdges[e0]; const btVector3 WorldEdge0 = transA.getBasis() * edge0; for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++) { const btVector3 edge1 = hullB.m_uniqueEdges[e1]; const btVector3 WorldEdge1 = transB.getBasis() * edge1; btVector3 Cross = WorldEdge0.cross(WorldEdge1); curEdgeEdge++; if(!IsAlmostZero(Cross)) { Cross = Cross.normalize(); if (DeltaC2.dot(Cross)<0) Cross *= -1.f; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar dist; btVector3 wA, wB; if(!TestSepAxis( hullA, hullB, transA, transB, Cross, dist, wA, wB)) return false; if(dist<dmin) { dmin = dist; sep = Cross; edgeA=e0; edgeB=e1; worldEdgeA = WorldEdge0; worldEdgeB = WorldEdge1; witnessPointA=wA; witnessPointB=wB; } } } } if (edgeA>=0&&edgeB>=0) { // printf("edge-edge\n"); //add an edge-edge contact btVector3 ptsVector; btVector3 offsetA; btVector3 offsetB; btScalar tA; btScalar tB; btVector3 translation = witnessPointB-witnessPointA; btVector3 dirA = worldEdgeA; btVector3 dirB = worldEdgeB; btScalar hlenB = 1e30f; btScalar hlenA = 1e30f; btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, translation, dirA, hlenA, dirB, hlenB); btScalar nlSqrt = ptsVector.length2(); if (nlSqrt>SIMD_EPSILON) { btScalar nl = btSqrt(nlSqrt); ptsVector *= 1.f/nl; if (ptsVector.dot(DeltaC2)<0.f) { ptsVector*=-1.f; } btVector3 ptOnB = witnessPointB + offsetB; btScalar distance = nl; resultOut.addContactPoint(ptsVector, ptOnB,-distance); } } if((DeltaC2.dot(sep))<0.0f) sep = -sep; return true; }