2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
17 ///This file was written by Erwin Coumans
18 ///Separating axis rest based on work from Pierre Terdiman, see
19 ///And contact clipping based on work from Simon Hobbs
22 #include "btPolyhedralContactClipping.h"
23 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
25 #include <float.h> //for FLT_MAX
27 int gExpectedNbTests=0;
28 int gActualNbTests = 0;
29 bool gUseInternalObject = true;
31 // Clips a face to the back of a plane
32 void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
37 int numVerts = pVtxIn.size();
41 btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
42 btVector3 endVertex = pVtxIn[0];
44 ds = planeNormalWS.dot(firstVertex)+planeEqWS;
46 for (ve = 0; ve < numVerts; ve++)
50 de = planeNormalWS.dot(endVertex)+planeEqWS;
56 // Start < 0, end < 0, so output endVertex
57 ppVtxOut.push_back(endVertex);
61 // Start < 0, end >= 0, so output intersection
62 ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
69 // Start >= 0, end < 0 so output intersection and end
70 ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
71 ppVtxOut.push_back(endVertex);
74 firstVertex = endVertex;
80 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)
84 btVector3 witnesPtMinA,witnesPtMaxA;
85 btVector3 witnesPtMinB,witnesPtMaxB;
87 hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
88 hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
90 if(Max0<Min1 || Max1<Min0)
93 btScalar d0 = Max0 - Min1;
95 btScalar d1 = Max1 - Min0;
100 witnessPointA = witnesPtMaxA;
101 witnessPointB = witnesPtMinB;
106 witnessPointA = witnesPtMinA;
107 witnessPointB = witnesPtMaxB;
115 static int gActualSATPairTests=0;
117 inline bool IsAlmostZero(const btVector3& v)
119 if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
123 #ifdef TEST_INTERNAL_OBJECTS
125 inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
127 // This version is ~11.000 cycles (4%) faster overall in one of the tests.
128 // IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
129 // IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
130 // IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
131 p[0] = sv[0] < 0.0f ? -extents[0] : extents[0];
132 p[1] = sv[1] < 0.0f ? -extents[1] : extents[1];
133 p[2] = sv[2] < 0.0f ? -extents[2] : extents[2];
136 void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
138 const btMatrix3x3& rot = tr.getBasis();
139 const btVector3& r0 = rot[0];
140 const btVector3& r1 = rot[1];
141 const btVector3& r2 = rot[2];
143 const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
144 const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
145 const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();
147 out.setValue(x, y, z);
150 bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
152 const btScalar dp = delta_c.dot(axis);
154 btVector3 localAxis0;
155 InverseTransformPoint3x3(localAxis0, axis,trans0);
156 btVector3 localAxis1;
157 InverseTransformPoint3x3(localAxis1, axis,trans1);
160 BoxSupport(convex0.m_extents, localAxis0, p0);
162 BoxSupport(convex1.m_extents, localAxis1, p1);
164 const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z();
165 const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z();
167 const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
168 const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
170 const btScalar MinMaxRadius = MaxRadius + MinRadius;
171 const btScalar d0 = MinMaxRadius + dp;
172 const btScalar d1 = MinMaxRadius - dp;
174 const btScalar depth = d0<d1 ? d0:d1;
179 #endif //TEST_INTERNAL_OBJECTS
183 SIMD_FORCE_INLINE void btSegmentsClosestPoints(
\r
184 btVector3& ptsVector,
\r
185 btVector3& offsetA,
\r
186 btVector3& offsetB,
\r
187 btScalar& tA, btScalar& tB,
\r
188 const btVector3& translation,
\r
189 const btVector3& dirA, btScalar hlenA,
\r
190 const btVector3& dirB, btScalar hlenB )
\r
192 // compute the parameters of the closest points on each line segment
\r
194 btScalar dirA_dot_dirB = btDot(dirA,dirB);
\r
195 btScalar dirA_dot_trans = btDot(dirA,translation);
\r
196 btScalar dirB_dot_trans = btDot(dirB,translation);
\r
198 btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
\r
200 if ( denom == 0.0f ) {
\r
203 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
\r
206 else if ( tA > hlenA )
\r
210 tB = tA * dirA_dot_dirB - dirB_dot_trans;
\r
212 if ( tB < -hlenB ) {
\r
214 tA = tB * dirA_dot_dirB + dirA_dot_trans;
\r
218 else if ( tA > hlenA )
\r
220 } else if ( tB > hlenB ) {
\r
222 tA = tB * dirA_dot_dirB + dirA_dot_trans;
\r
226 else if ( tA > hlenA )
\r
230 // compute the closest points relative to segment centers.
\r
232 offsetA = dirA * tA;
\r
233 offsetB = dirB * tB;
\r
235 ptsVector = translation - offsetA + offsetB;
\r
240 bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
242 gActualSATPairTests++;
244 //#ifdef TEST_INTERNAL_OBJECTS
245 const btVector3 c0 = transA * hullA.m_localCenter;
246 const btVector3 c1 = transB * hullB.m_localCenter;
247 const btVector3 DeltaC2 = c0 - c1;
250 btScalar dmin = FLT_MAX;
253 int numFacesA = hullA.m_faces.size();
254 // Test normals from hullA
255 for(int i=0;i<numFacesA;i++)
257 const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
258 btVector3 faceANormalWS = transA.getBasis() * Normal;
259 if (DeltaC2.dot(faceANormalWS)<0)
263 #ifdef TEST_INTERNAL_OBJECTS
265 if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
272 if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
282 int numFacesB = hullB.m_faces.size();
283 // Test normals from hullB
284 for(int i=0;i<numFacesB;i++)
286 const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
287 btVector3 WorldNormal = transB.getBasis() * Normal;
288 if (DeltaC2.dot(WorldNormal)<0)
292 #ifdef TEST_INTERNAL_OBJECTS
294 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
301 if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
311 btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
314 btVector3 worldEdgeA;
315 btVector3 worldEdgeB;
316 btVector3 witnessPointA,witnessPointB;
321 for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
323 const btVector3 edge0 = hullA.m_uniqueEdges[e0];
324 const btVector3 WorldEdge0 = transA.getBasis() * edge0;
325 for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
327 const btVector3 edge1 = hullB.m_uniqueEdges[e1];
328 const btVector3 WorldEdge1 = transB.getBasis() * edge1;
330 btVector3 Cross = WorldEdge0.cross(WorldEdge1);
332 if(!IsAlmostZero(Cross))
334 Cross = Cross.normalize();
335 if (DeltaC2.dot(Cross)<0)
339 #ifdef TEST_INTERNAL_OBJECTS
341 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
348 if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
357 worldEdgeA = WorldEdge0;
358 worldEdgeB = WorldEdge1;
367 if (edgeA>=0&&edgeB>=0)
369 // printf("edge-edge\n");
370 //add an edge-edge contact
372 btVector3 ptsVector;
\r
378 btVector3 translation = witnessPointB-witnessPointA;
\r
380 btVector3 dirA = worldEdgeA;
\r
381 btVector3 dirB = worldEdgeB;
\r
383 btScalar hlenB = 1e30f;
\r
384 btScalar hlenA = 1e30f;
\r
386 btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
\r
391 btScalar nlSqrt = ptsVector.length2();
392 if (nlSqrt>SIMD_EPSILON)
394 btScalar nl = btSqrt(nlSqrt);
396 if (ptsVector.dot(DeltaC2)<0.f)
400 btVector3 ptOnB = witnessPointB + offsetB;
401 btScalar distance = nl;
402 resultOut.addContactPoint(ptsVector, ptOnB,-distance);
408 if((DeltaC2.dot(sep))<0.0f)
414 void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
416 btVertexArray worldVertsB2;
417 btVertexArray* pVtxIn = &worldVertsB1;
418 btVertexArray* pVtxOut = &worldVertsB2;
419 pVtxOut->reserve(pVtxIn->size());
423 btScalar dmin = FLT_MAX;
424 for(int face=0;face<hullA.m_faces.size();face++)
426 const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
427 const btVector3 faceANormalWS = transA.getBasis() * Normal;
429 btScalar d = faceANormalWS.dot(separatingNormal);
440 const btFace& polyA = hullA.m_faces[closestFaceA];
442 // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
443 int numVerticesA = polyA.m_indices.size();
444 for(int e0=0;e0<numVerticesA;e0++)
446 const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
447 const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
448 const btVector3 edge0 = a - b;
449 const btVector3 WorldEdge0 = transA.getBasis() * edge0;
450 btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
452 btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
453 btVector3 worldA1 = transA*a;
454 btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
458 int otherFace = polyA.m_connectedFaces[e0];
459 btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
460 btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
462 btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
463 btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
465 btVector3 planeNormalWS = planeNormalWS1;
466 btScalar planeEqWS=planeEqWS1;
471 clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
472 btSwap(pVtxIn,pVtxOut);
478 //#define ONLY_REPORT_DEEPEST_POINT
483 // only keep points that are behind the witness face
485 btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
486 btScalar localPlaneEq = polyA.m_plane[3];
487 btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
488 btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
489 for (int i=0;i<pVtxIn->size();i++)
491 btVector3 vtx = pVtxIn->at(i);
492 btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
495 // printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
501 btVector3 point = pVtxIn->at(i);
502 #ifdef ONLY_REPORT_DEEPEST_POINT
508 printf("error in btPolyhedralContactClipping depth = %f\n", depth);
509 printf("likely wrong separatingNormal passed in\n");
512 resultOut.addContactPoint(separatingNormal,point,depth);
517 #ifdef ONLY_REPORT_DEEPEST_POINT
518 if (curMaxDist<maxDist)
520 resultOut.addContactPoint(separatingNormal,point,curMaxDist);
522 #endif //ONLY_REPORT_DEEPEST_POINT
530 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)
533 btVector3 separatingNormal = separatingNormal1.normalized();
534 // const btVector3 c0 = transA * hullA.m_localCenter;
535 // const btVector3 c1 = transB * hullB.m_localCenter;
536 //const btVector3 DeltaC2 = c0 - c1;
541 btScalar dmax = -FLT_MAX;
543 for(int face=0;face<hullB.m_faces.size();face++)
545 const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
546 const btVector3 WorldNormal = transB.getBasis() * Normal;
547 btScalar d = WorldNormal.dot(separatingNormal);
555 btVertexArray worldVertsB1;
557 const btFace& polyB = hullB.m_faces[closestFaceB];
558 const int numVertices = polyB.m_indices.size();
559 for(int e0=0;e0<numVertices;e0++)
561 const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
562 worldVertsB1.push_back(transB*b);
568 clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);