diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp | 358 |
1 files changed, 168 insertions, 190 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index d5f4a964bf3..9d1836037d5 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -13,45 +13,42 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans ///Separating axis rest based on work from Pierre Terdiman, see ///And contact clipping based on work from Simon Hobbs - #include "btPolyhedralContactClipping.h" #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" -#include <float.h> //for FLT_MAX +#include <float.h> //for FLT_MAX -int gExpectedNbTests=0; +int gExpectedNbTests = 0; int gActualNbTests = 0; bool gUseInternalObject = true; // Clips a face to the back of a plane -void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS) +void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS) { - int ve; btScalar ds, de; int numVerts = pVtxIn.size(); if (numVerts < 2) return; - btVector3 firstVertex=pVtxIn[pVtxIn.size()-1]; + btVector3 firstVertex = pVtxIn[pVtxIn.size() - 1]; btVector3 endVertex = pVtxIn[0]; - - ds = planeNormalWS.dot(firstVertex)+planeEqWS; + + ds = planeNormalWS.dot(firstVertex) + planeEqWS; for (ve = 0; ve < numVerts; ve++) { - endVertex=pVtxIn[ve]; + endVertex = pVtxIn[ve]; - de = planeNormalWS.dot(endVertex)+planeEqWS; + de = planeNormalWS.dot(endVertex) + planeEqWS; - if (ds<0) + if (ds < 0) { - if (de<0) + if (de < 0) { // Start < 0, end < 0, so output endVertex ppVtxOut.push_back(endVertex); @@ -59,15 +56,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex else { // Start < 0, end >= 0, so output intersection - ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de)))); } } else { - if (de<0) + if (de < 0) { // Start >= 0, end < 0 so output intersection and end - ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de)))); ppVtxOut.push_back(endVertex); } } @@ -76,47 +73,44 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex } } - -static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) +static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) { - btScalar Min0,Max0; - btScalar Min1,Max1; - btVector3 witnesPtMinA,witnesPtMaxA; - btVector3 witnesPtMinB,witnesPtMaxB; + btScalar Min0, Max0; + btScalar Min1, Max1; + btVector3 witnesPtMinA, witnesPtMaxA; + btVector3 witnesPtMinB, witnesPtMaxB; - hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA); - hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB); + hullA.project(transA, sep_axis, Min0, Max0, witnesPtMinA, witnesPtMaxA); + hullB.project(transB, sep_axis, Min1, Max1, witnesPtMinB, witnesPtMaxB); - if(Max0<Min1 || Max1<Min0) + if (Max0 < Min1 || Max1 < Min0) return false; btScalar d0 = Max0 - Min1; - btAssert(d0>=0.0f); + btAssert(d0 >= 0.0f); btScalar d1 = Max1 - Min0; - btAssert(d1>=0.0f); - if (d0<d1) + btAssert(d1 >= 0.0f); + if (d0 < d1) { depth = d0; witnessPointA = witnesPtMaxA; witnessPointB = witnesPtMinB; - - } else + } + else { depth = d1; witnessPointA = witnesPtMinA; witnessPointB = witnesPtMaxB; } - + return true; } - - -static int gActualSATPairTests=0; +static int gActualSATPairTests = 0; inline bool IsAlmostZero(const btVector3& v) { - if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false; + if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; } @@ -125,9 +119,9 @@ inline bool IsAlmostZero(const btVector3& v) inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3]) { // This version is ~11.000 cycles (4%) faster overall in one of the tests. -// IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); -// IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); -// IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); + // IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); + // IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); + // IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); p[0] = sv[0] < 0.0f ? -extents[0] : extents[0]; p[1] = sv[1] < 0.0f ? -extents[1] : extents[1]; p[2] = sv[2] < 0.0f ? -extents[2] : extents[2]; @@ -140,90 +134,94 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans const btVector3& r1 = rot[1]; const btVector3& r2 = rot[2]; - const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z(); - const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z(); - const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z(); + const btScalar x = r0.x() * in.x() + r1.x() * in.y() + r2.x() * in.z(); + const btScalar y = r0.y() * in.x() + r1.y() * in.y() + r2.y() * in.z(); + const btScalar z = r0.z() * in.x() + r1.z() * in.y() + r2.z() * in.z(); out.setValue(x, y, z); } - bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) +bool TestInternalObjects(const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) { const btScalar dp = delta_c.dot(axis); btVector3 localAxis0; - InverseTransformPoint3x3(localAxis0, axis,trans0); + InverseTransformPoint3x3(localAxis0, axis, trans0); btVector3 localAxis1; - InverseTransformPoint3x3(localAxis1, axis,trans1); + InverseTransformPoint3x3(localAxis1, axis, trans1); btScalar p0[3]; BoxSupport(convex0.m_extents, localAxis0, p0); btScalar p1[3]; BoxSupport(convex1.m_extents, localAxis1, p1); - const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z(); - const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z(); + const btScalar Radius0 = p0[0] * localAxis0.x() + p0[1] * localAxis0.y() + p0[2] * localAxis0.z(); + const btScalar Radius1 = p1[0] * localAxis1.x() + p1[1] * localAxis1.y() + p1[2] * localAxis1.z(); - const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius; - const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius; + const btScalar MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius; + const btScalar MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius; const btScalar MinMaxRadius = MaxRadius + MinRadius; const btScalar d0 = MinMaxRadius + dp; const btScalar d1 = MinMaxRadius - dp; - const btScalar depth = d0<d1 ? d0:d1; - if(depth>dmin) + const btScalar depth = d0 < d1 ? d0 : d1; + if (depth > dmin) return false; return true; } -#endif //TEST_INTERNAL_OBJECTS +#endif //TEST_INTERNAL_OBJECTS - - - SIMD_FORCE_INLINE void btSegmentsClosestPoints( +SIMD_FORCE_INLINE void btSegmentsClosestPoints( btVector3& ptsVector, btVector3& offsetA, btVector3& offsetB, btScalar& tA, btScalar& tB, const btVector3& translation, const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) + const btVector3& dirB, btScalar hlenB) { // compute the parameters of the closest points on each line segment - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); + btScalar dirA_dot_dirB = btDot(dirA, dirB); + btScalar dirA_dot_trans = btDot(dirA, translation); + btScalar dirB_dot_trans = btDot(dirB, translation); btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - if ( denom == 0.0f ) { + if (denom == 0.0f) + { tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) + } + else + { + tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom; + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } tB = tA * dirA_dot_dirB - dirB_dot_trans; - if ( tB < -hlenB ) { + if (tB < -hlenB) + { tB = -hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; - } else if ( tB > hlenB ) { + } + else if (tB > hlenB) + { tB = hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } @@ -235,44 +233,42 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans ptsVector = translation - offsetA + offsetB; } - - -bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) +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 + //#ifdef TEST_INTERNAL_OBJECTS const btVector3 c0 = transA * hullA.m_localCenter; const btVector3 c1 = transB * hullB.m_localCenter; const btVector3 DeltaC2 = c0 - c1; -//#endif + //#endif btScalar dmin = FLT_MAX; - int curPlaneTests=0; + int curPlaneTests = 0; int numFacesA = hullA.m_faces.size(); // Test normals from hullA - for(int i=0;i<numFacesA;i++) + 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; + if (DeltaC2.dot(faceANormalWS) < 0) + faceANormalWS *= -1.f; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + 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)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, faceANormalWS, d, wA, wB)) return false; - if(d<dmin) + if (d < dmin) { dmin = d; sep = faceANormalWS; @@ -281,92 +277,89 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& int numFacesB = hullB.m_faces.size(); // Test normals from hullB - for(int i=0;i<numFacesB;i++) + 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; + if (DeltaC2.dot(WorldNormal) < 0) + WorldNormal *= -1.f; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin)) + 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)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d, wA, wB)) return false; - if(d<dmin) + if (d < dmin) { dmin = d; sep = WorldNormal; } } - btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend; - int edgeA=-1; - int edgeB=-1; + btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend; + int edgeA = -1; + int edgeB = -1; btVector3 worldEdgeA; btVector3 worldEdgeB; - btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0); - + btVector3 witnessPointA(0, 0, 0), witnessPointB(0, 0, 0); int curEdgeEdge = 0; // Test edges - for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++) + 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++) + 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)) + if (!IsAlmostZero(Cross)) { Cross = Cross.normalize(); - if (DeltaC2.dot(Cross)<0) + if (DeltaC2.dot(Cross) < 0) Cross *= -1.f; - #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin)) + 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)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, Cross, dist, wA, wB)) return false; - if(dist<dmin) + if (dist < dmin) { dmin = dist; sep = Cross; - edgeA=e0; - edgeB=e1; + edgeA = e0; + edgeB = e1; worldEdgeA = WorldEdge0; worldEdgeB = WorldEdge1; - witnessPointA=wA; - witnessPointB=wB; + witnessPointA = wA; + witnessPointB = wB; } } } - } - if (edgeA>=0&&edgeB>=0) + if (edgeA >= 0 && edgeB >= 0) { -// printf("edge-edge\n"); + // printf("edge-edge\n"); //add an edge-edge contact btVector3 ptsVector; @@ -375,57 +368,55 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& btScalar tA; btScalar tB; - btVector3 translation = witnessPointB-witnessPointA; + 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); + btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, + translation, + dirA, hlenA, + dirB, hlenB); btScalar nlSqrt = ptsVector.length2(); - if (nlSqrt>SIMD_EPSILON) + if (nlSqrt > SIMD_EPSILON) { btScalar nl = btSqrt(nlSqrt); - ptsVector *= 1.f/nl; - if (ptsVector.dot(DeltaC2)<0.f) + ptsVector *= 1.f / nl; + if (ptsVector.dot(DeltaC2) < 0.f) { - ptsVector*=-1.f; + ptsVector *= -1.f; } btVector3 ptOnB = witnessPointB + offsetB; btScalar distance = nl; - resultOut.addContactPoint(ptsVector, ptOnB,-distance); + resultOut.addContactPoint(ptsVector, ptOnB, -distance); } - } - - if((DeltaC2.dot(sep))<0.0f) + if ((DeltaC2.dot(sep)) < 0.0f) sep = -sep; return true; } -void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut) { - btVertexArray worldVertsB2; + worldVertsB2.resize(0); btVertexArray* pVtxIn = &worldVertsB1; btVertexArray* pVtxOut = &worldVertsB2; pVtxOut->reserve(pVtxIn->size()); - int closestFaceA=-1; + int closestFaceA = -1; { btScalar dmin = FLT_MAX; - for(int face=0;face<hullA.m_faces.size();face++) + for (int face = 0; face < hullA.m_faces.size(); face++) { const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]); const btVector3 faceANormalWS = transA.getBasis() * Normal; - + btScalar d = faceANormalWS.dot(separatingNormal); if (d < dmin) { @@ -434,69 +425,66 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin } } } - if (closestFaceA<0) + if (closestFaceA < 0) return; const btFace& polyA = hullA.m_faces[closestFaceA]; - // 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 numVerticesA = polyA.m_indices.size(); - for(int e0=0;e0<numVerticesA;e0++) + for (int e0 = 0; e0 < numVerticesA; e0++) { const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]]; - const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]]; + const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]]; const btVector3 edge0 = a - b; const btVector3 WorldEdge0 = transA.getBasis() * edge0; - btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + btVector3 worldPlaneAnormal1 = transA.getBasis() * btVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); - btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0); - btVector3 worldA1 = transA*a; + btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1); //.cross(WorldEdge0); + btVector3 worldA1 = transA * a; btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1); - + //int otherFace=0; #ifdef BLA1 int otherFace = polyA.m_connectedFaces[e0]; - btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]); + btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]); btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3]; - btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; - btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); -#else + btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal; + btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin()); +#else btVector3 planeNormalWS = planeNormalWS1; - btScalar planeEqWS=planeEqWS1; - + btScalar planeEqWS = planeEqWS1; + #endif //clip face - clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); - btSwap(pVtxIn,pVtxOut); + clipFace(*pVtxIn, *pVtxOut, planeNormalWS, planeEqWS); + btSwap(pVtxIn, pVtxOut); pVtxOut->resize(0); } - - -//#define ONLY_REPORT_DEEPEST_POINT + //#define ONLY_REPORT_DEEPEST_POINT btVector3 point; - // only keep points that are behind the witness face { - btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + btVector3 localPlaneNormal(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); btScalar localPlaneEq = polyA.m_plane[3]; - btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; - btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); - for (int i=0;i<pVtxIn->size();i++) + btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal; + btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin()); + for (int i = 0; i < pVtxIn->size(); i++) { btVector3 vtx = pVtxIn->at(i); - btScalar depth = planeNormalWS.dot(vtx)+planeEqWS; - if (depth <=minDist) + btScalar depth = planeNormalWS.dot(vtx) + planeEqWS; + if (depth <= minDist) { -// printf("clamped: depth=%f to minDist=%f\n",depth,minDist); + // printf("clamped: depth=%f to minDist=%f\n",depth,minDist); depth = minDist; } - if (depth <=maxDist) + if (depth <= maxDist) { btVector3 point = pVtxIn->at(i); #ifdef ONLY_REPORT_DEEPEST_POINT @@ -507,40 +495,32 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin { printf("error in btPolyhedralContactClipping depth = %f\n", depth); printf("likely wrong separatingNormal passed in\n"); - } -#endif - resultOut.addContactPoint(separatingNormal,point,depth); + } +#endif + resultOut.addContactPoint(separatingNormal, point, depth); #endif } } } #ifdef ONLY_REPORT_DEEPEST_POINT - if (curMaxDist<maxDist) + if (curMaxDist < maxDist) { - resultOut.addContactPoint(separatingNormal,point,curMaxDist); + resultOut.addContactPoint(separatingNormal, point, curMaxDist); } -#endif //ONLY_REPORT_DEEPEST_POINT - +#endif //ONLY_REPORT_DEEPEST_POINT } - - - - -void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut) { - btVector3 separatingNormal = separatingNormal1.normalized(); -// const btVector3 c0 = transA * hullA.m_localCenter; -// const btVector3 c1 = transB * hullB.m_localCenter; + // const btVector3 c0 = transA * hullA.m_localCenter; + // const btVector3 c1 = transB * hullB.m_localCenter; //const btVector3 DeltaC2 = c0 - c1; - - - int closestFaceB=-1; + int closestFaceB = -1; btScalar dmax = -FLT_MAX; { - for(int face=0;face<hullB.m_faces.size();face++) + for (int face = 0; face < hullB.m_faces.size(); face++) { const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]); const btVector3 WorldNormal = transB.getBasis() * Normal; @@ -552,19 +532,17 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin } } } - btVertexArray worldVertsB1; - { - const btFace& polyB = hullB.m_faces[closestFaceB]; - const int numVertices = polyB.m_indices.size(); - for(int e0=0;e0<numVertices;e0++) - { - const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]]; - worldVertsB1.push_back(transB*b); - } - } - - - if (closestFaceB>=0) - clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut); + worldVertsB1.resize(0); + { + const btFace& polyB = hullB.m_faces[closestFaceB]; + const int numVertices = polyB.m_indices.size(); + for (int e0 = 0; e0 < numVertices; e0++) + { + const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]]; + worldVertsB1.push_back(transB * b); + } + } + if (closestFaceB >= 0) + clipFaceAgainstHull(separatingNormal, hullA, transA, worldVertsB1, worldVertsB2, minDist, maxDist, resultOut); } |