diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp')
| -rw-r--r-- | thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp | 570 |
1 files changed, 0 insertions, 570 deletions
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp deleted file mode 100644 index ea380bc5f1..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ /dev/null @@ -1,570 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -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 - -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) -{ - - int ve; - btScalar ds, de; - int numVerts = pVtxIn.size(); - if (numVerts < 2) - return; - - btVector3 firstVertex=pVtxIn[pVtxIn.size()-1]; - btVector3 endVertex = pVtxIn[0]; - - ds = planeNormalWS.dot(firstVertex)+planeEqWS; - - for (ve = 0; ve < numVerts; ve++) - { - endVertex=pVtxIn[ve]; - - de = planeNormalWS.dot(endVertex)+planeEqWS; - - if (ds<0) - { - if (de<0) - { - // Start < 0, end < 0, so output endVertex - ppVtxOut.push_back(endVertex); - } - else - { - // Start < 0, end >= 0, so output intersection - ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); - } - } - else - { - 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(endVertex); - } - } - firstVertex = endVertex; - ds = de; - } -} - - -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; - - hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA); - hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB); - - if(Max0<Min1 || Max1<Min0) - return false; - - btScalar d0 = Max0 - Min1; - btAssert(d0>=0.0f); - btScalar d1 = Max1 - Min0; - btAssert(d1>=0.0f); - if (d0<d1) - { - depth = d0; - witnessPointA = witnesPtMaxA; - witnessPointB = witnesPtMinB; - - } else - { - depth = d1; - witnessPointA = witnesPtMinA; - witnessPointB = witnesPtMaxB; - } - - return true; -} - - - -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; - return true; -} - -#ifdef TEST_INTERNAL_OBJECTS - -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); - 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]; -} - -void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr) -{ - const btMatrix3x3& rot = tr.getBasis(); - const btVector3& r0 = rot[0]; - 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(); - - 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) -{ - const btScalar dp = delta_c.dot(axis); - - btVector3 localAxis0; - InverseTransformPoint3x3(localAxis0, axis,trans0); - btVector3 localAxis1; - 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 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) - return false; - return true; -} -#endif //TEST_INTERNAL_OBJECTS - - - - 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 ) -{ - // 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 denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - - if ( denom == 0.0f ) { - tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } - - tB = tA * dirA_dot_dirB - dirB_dot_trans; - - if ( tB < -hlenB ) { - tB = -hlenB; - tA = tB * dirA_dot_dirB + dirA_dot_trans; - - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } else if ( tB > hlenB ) { - tB = hlenB; - tA = tB * dirA_dot_dirB + dirA_dot_trans; - - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } - - // compute the closest points relative to segment centers. - - offsetA = dirA * tA; - offsetB = dirB * tB; - - ptsVector = translation - offsetA + offsetB; -} - - - -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(0,0,0),witnessPointB(0,0,0); - - - 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; -} - -void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) -{ - worldVertsB2.resize(0); - btVertexArray* pVtxIn = &worldVertsB1; - btVertexArray* pVtxOut = &worldVertsB2; - pVtxOut->reserve(pVtxIn->size()); - - int closestFaceA=-1; - { - btScalar dmin = FLT_MAX; - 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) - { - dmin = d; - closestFaceA = face; - } - } - } - 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 - int numVerticesA = polyA.m_indices.size(); - 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 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 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]); - btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3]; - - btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; - btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); -#else - btVector3 planeNormalWS = planeNormalWS1; - btScalar planeEqWS=planeEqWS1; - -#endif - //clip face - - clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); - btSwap(pVtxIn,pVtxOut); - pVtxOut->resize(0); - } - - - -//#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]); - 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 vtx = pVtxIn->at(i); - btScalar depth = planeNormalWS.dot(vtx)+planeEqWS; - if (depth <=minDist) - { -// printf("clamped: depth=%f to minDist=%f\n",depth,minDist); - depth = minDist; - } - - if (depth <=maxDist) - { - btVector3 point = pVtxIn->at(i); -#ifdef ONLY_REPORT_DEEPEST_POINT - curMaxDist = depth; -#else -#if 0 - if (depth<-3) - { - printf("error in btPolyhedralContactClipping depth = %f\n", depth); - printf("likely wrong separatingNormal passed in\n"); - } -#endif - resultOut.addContactPoint(separatingNormal,point,depth); -#endif - } - } - } -#ifdef ONLY_REPORT_DEEPEST_POINT - if (curMaxDist<maxDist) - { - resultOut.addContactPoint(separatingNormal,point,curMaxDist); - } -#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,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 DeltaC2 = c0 - c1; - - - - int closestFaceB=-1; - btScalar dmax = -FLT_MAX; - { - 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; - btScalar d = WorldNormal.dot(separatingNormal); - if (d > dmax) - { - dmax = d; - closestFaceB = face; - } - } - } - 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); - -} |