btPolyhedralContactClipping.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00020 
00021 
00022 #include "btPolyhedralContactClipping.h"
00023 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
00024 
00025 #include <float.h> //for FLT_MAX
00026 
00027 int gExpectedNbTests=0;
00028 int gActualNbTests = 0;
00029 bool gUseInternalObject = true;
00030 
00031 // Clips a face to the back of a plane
00032 void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
00033 {
00034         
00035         int ve;
00036         btScalar ds, de;
00037         int numVerts = pVtxIn.size();
00038         if (numVerts < 2)
00039                 return;
00040 
00041         btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
00042         btVector3 endVertex = pVtxIn[0];
00043         
00044         ds = planeNormalWS.dot(firstVertex)+planeEqWS;
00045 
00046         for (ve = 0; ve < numVerts; ve++)
00047         {
00048                 endVertex=pVtxIn[ve];
00049 
00050                 de = planeNormalWS.dot(endVertex)+planeEqWS;
00051 
00052                 if (ds<0)
00053                 {
00054                         if (de<0)
00055                         {
00056                                 // Start < 0, end < 0, so output endVertex
00057                                 ppVtxOut.push_back(endVertex);
00058                         }
00059                         else
00060                         {
00061                                 // Start < 0, end >= 0, so output intersection
00062                                 ppVtxOut.push_back(     firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
00063                         }
00064                 }
00065                 else
00066                 {
00067                         if (de<0)
00068                         {
00069                                 // Start >= 0, end < 0 so output intersection and end
00070                                 ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
00071                                 ppVtxOut.push_back(endVertex);
00072                         }
00073                 }
00074                 firstVertex = endVertex;
00075                 ds = de;
00076         }
00077 }
00078 
00079 
00080 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)
00081 {
00082         btScalar Min0,Max0;
00083         btScalar Min1,Max1;
00084         btVector3 witnesPtMinA,witnesPtMaxA;
00085         btVector3 witnesPtMinB,witnesPtMaxB;
00086 
00087         hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
00088         hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
00089 
00090         if(Max0<Min1 || Max1<Min0)
00091                 return false;
00092 
00093         btScalar d0 = Max0 - Min1;
00094         btAssert(d0>=0.0f);
00095         btScalar d1 = Max1 - Min0;
00096         btAssert(d1>=0.0f);
00097         if (d0<d1)
00098         {
00099                 depth = d0;
00100                 witnessPointA = witnesPtMaxA;
00101                 witnessPointB = witnesPtMinB;
00102 
00103         } else
00104         {
00105                 depth = d1;
00106                 witnessPointA = witnesPtMinA;
00107                 witnessPointB = witnesPtMaxB;
00108         }
00109         
00110         return true;
00111 }
00112 
00113 
00114 
00115 static int gActualSATPairTests=0;
00116 
00117 inline bool IsAlmostZero(const btVector3& v)
00118 {
00119         if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
00120         return true;
00121 }
00122 
00123 #ifdef TEST_INTERNAL_OBJECTS
00124 
00125 inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
00126 {
00127         // This version is ~11.000 cycles (4%) faster overall in one of the tests.
00128 //      IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
00129 //      IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
00130 //      IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
00131         p[0] = sv[0] < 0.0f ? -extents[0] : extents[0];
00132         p[1] = sv[1] < 0.0f ? -extents[1] : extents[1];
00133         p[2] = sv[2] < 0.0f ? -extents[2] : extents[2];
00134 }
00135 
00136 void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
00137 {
00138         const btMatrix3x3& rot = tr.getBasis();
00139         const btVector3& r0 = rot[0];
00140         const btVector3& r1 = rot[1];
00141         const btVector3& r2 = rot[2];
00142 
00143         const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
00144         const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
00145         const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();
00146 
00147         out.setValue(x, y, z);
00148 }
00149 
00150  bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
00151 {
00152         const btScalar dp = delta_c.dot(axis);
00153 
00154         btVector3 localAxis0;
00155         InverseTransformPoint3x3(localAxis0, axis,trans0);
00156         btVector3 localAxis1;
00157         InverseTransformPoint3x3(localAxis1, axis,trans1);
00158 
00159         btScalar p0[3];
00160         BoxSupport(convex0.m_extents, localAxis0, p0);
00161         btScalar p1[3];
00162         BoxSupport(convex1.m_extents, localAxis1, p1);
00163 
00164         const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z();
00165         const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z();
00166 
00167         const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
00168         const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
00169 
00170         const btScalar MinMaxRadius = MaxRadius + MinRadius;
00171         const btScalar d0 = MinMaxRadius + dp;
00172         const btScalar d1 = MinMaxRadius - dp;
00173 
00174         const btScalar depth = d0<d1 ? d0:d1;
00175         if(depth>dmin)
00176                 return false;
00177         return true;
00178 }
00179 #endif //TEST_INTERNAL_OBJECTS
00180 
00181  
00182  
00183  SIMD_FORCE_INLINE void btSegmentsClosestPoints(
00184         btVector3& ptsVector,
00185         btVector3& offsetA,
00186         btVector3& offsetB,
00187         btScalar& tA, btScalar& tB,
00188         const btVector3& translation,
00189         const btVector3& dirA, btScalar hlenA,
00190         const btVector3& dirB, btScalar hlenB )
00191 {
00192         // compute the parameters of the closest points on each line segment
00193 
00194         btScalar dirA_dot_dirB = btDot(dirA,dirB);
00195         btScalar dirA_dot_trans = btDot(dirA,translation);
00196         btScalar dirB_dot_trans = btDot(dirB,translation);
00197 
00198         btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
00199 
00200         if ( denom == 0.0f ) {
00201                 tA = 0.0f;
00202         } else {
00203                 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
00204                 if ( tA < -hlenA )
00205                         tA = -hlenA;
00206                 else if ( tA > hlenA )
00207                         tA = hlenA;
00208         }
00209 
00210         tB = tA * dirA_dot_dirB - dirB_dot_trans;
00211 
00212         if ( tB < -hlenB ) {
00213                 tB = -hlenB;
00214                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
00215 
00216                 if ( tA < -hlenA )
00217                         tA = -hlenA;
00218                 else if ( tA > hlenA )
00219                         tA = hlenA;
00220         } else if ( tB > hlenB ) {
00221                 tB = hlenB;
00222                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
00223 
00224                 if ( tA < -hlenA )
00225                         tA = -hlenA;
00226                 else if ( tA > hlenA )
00227                         tA = hlenA;
00228         }
00229 
00230         // compute the closest points relative to segment centers.
00231 
00232         offsetA = dirA * tA;
00233         offsetB = dirB * tB;
00234 
00235         ptsVector = translation - offsetA + offsetB;
00236 }
00237 
00238 
00239 
00240 bool btPolyhedralContactClipping::findSeparatingAxis(   const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
00241 {
00242         gActualSATPairTests++;
00243 
00244 //#ifdef TEST_INTERNAL_OBJECTS
00245         const btVector3 c0 = transA * hullA.m_localCenter;
00246         const btVector3 c1 = transB * hullB.m_localCenter;
00247         const btVector3 DeltaC2 = c0 - c1;
00248 //#endif
00249 
00250         btScalar dmin = FLT_MAX;
00251         int curPlaneTests=0;
00252 
00253         int numFacesA = hullA.m_faces.size();
00254         // Test normals from hullA
00255         for(int i=0;i<numFacesA;i++)
00256         {
00257                 const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
00258                 btVector3 faceANormalWS = transA.getBasis() * Normal;
00259                 if (DeltaC2.dot(faceANormalWS)<0)
00260                         faceANormalWS*=-1.f;
00261 
00262                 curPlaneTests++;
00263 #ifdef TEST_INTERNAL_OBJECTS
00264                 gExpectedNbTests++;
00265                 if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
00266                         continue;
00267                 gActualNbTests++;
00268 #endif
00269 
00270                 btScalar d;
00271                 btVector3 wA,wB;
00272                 if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
00273                         return false;
00274 
00275                 if(d<dmin)
00276                 {
00277                         dmin = d;
00278                         sep = faceANormalWS;
00279                 }
00280         }
00281 
00282         int numFacesB = hullB.m_faces.size();
00283         // Test normals from hullB
00284         for(int i=0;i<numFacesB;i++)
00285         {
00286                 const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
00287                 btVector3 WorldNormal = transB.getBasis() * Normal;
00288                 if (DeltaC2.dot(WorldNormal)<0)
00289                         WorldNormal *=-1.f;
00290 
00291                 curPlaneTests++;
00292 #ifdef TEST_INTERNAL_OBJECTS
00293                 gExpectedNbTests++;
00294                 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
00295                         continue;
00296                 gActualNbTests++;
00297 #endif
00298 
00299                 btScalar d;
00300                 btVector3 wA,wB;
00301                 if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
00302                         return false;
00303 
00304                 if(d<dmin)
00305                 {
00306                         dmin = d;
00307                         sep = WorldNormal;
00308                 }
00309         }
00310 
00311         btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
00312         int edgeA=-1;
00313         int edgeB=-1;
00314         btVector3 worldEdgeA;
00315         btVector3 worldEdgeB;
00316         btVector3 witnessPointA,witnessPointB;
00317         
00318 
00319         int curEdgeEdge = 0;
00320         // Test edges
00321         for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
00322         {
00323                 const btVector3 edge0 = hullA.m_uniqueEdges[e0];
00324                 const btVector3 WorldEdge0 = transA.getBasis() * edge0;
00325                 for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
00326                 {
00327                         const btVector3 edge1 = hullB.m_uniqueEdges[e1];
00328                         const btVector3 WorldEdge1 = transB.getBasis() * edge1;
00329 
00330                         btVector3 Cross = WorldEdge0.cross(WorldEdge1);
00331                         curEdgeEdge++;
00332                         if(!IsAlmostZero(Cross))
00333                         {
00334                                 Cross = Cross.normalize();
00335                                 if (DeltaC2.dot(Cross)<0)
00336                                         Cross *= -1.f;
00337 
00338 
00339 #ifdef TEST_INTERNAL_OBJECTS
00340                                 gExpectedNbTests++;
00341                                 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
00342                                         continue;
00343                                 gActualNbTests++;
00344 #endif
00345 
00346                                 btScalar dist;
00347                                 btVector3 wA,wB;
00348                                 if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
00349                                         return false;
00350 
00351                                 if(dist<dmin)
00352                                 {
00353                                         dmin = dist;
00354                                         sep = Cross;
00355                                         edgeA=e0;
00356                                         edgeB=e1;
00357                                         worldEdgeA = WorldEdge0;
00358                                         worldEdgeB = WorldEdge1;
00359                                         witnessPointA=wA;
00360                                         witnessPointB=wB;
00361                                 }
00362                         }
00363                 }
00364 
00365         }
00366 
00367         if (edgeA>=0&&edgeB>=0)
00368         {
00369 //              printf("edge-edge\n");
00370                 //add an edge-edge contact
00371 
00372                 btVector3 ptsVector;
00373                 btVector3 offsetA;
00374                 btVector3 offsetB;
00375                 btScalar tA;
00376                 btScalar tB;
00377 
00378                 btVector3 translation = witnessPointB-witnessPointA;
00379 
00380                 btVector3 dirA = worldEdgeA;
00381                 btVector3 dirB = worldEdgeB;
00382                 
00383                 btScalar hlenB = 1e30f;
00384                 btScalar hlenA = 1e30f;
00385 
00386                 btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
00387                         translation,
00388                         dirA, hlenA,
00389                         dirB,hlenB);
00390 
00391                 btScalar nlSqrt = ptsVector.length2();
00392                 if (nlSqrt>SIMD_EPSILON)
00393                 {
00394                         btScalar nl = btSqrt(nlSqrt);
00395                         ptsVector *= 1.f/nl;
00396                         if (ptsVector.dot(DeltaC2)<0.f)
00397                         {
00398                                 ptsVector*=-1.f;
00399                         }
00400                         btVector3 ptOnB = witnessPointB + offsetB;
00401                         btScalar distance = nl;
00402                         resultOut.addContactPoint(ptsVector, ptOnB,-distance);
00403                 }
00404 
00405         }
00406 
00407 
00408         if((DeltaC2.dot(sep))<0.0f)
00409                 sep = -sep;
00410 
00411         return true;
00412 }
00413 
00414 void    btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
00415 {
00416         btVertexArray worldVertsB2;
00417         btVertexArray* pVtxIn = &worldVertsB1;
00418         btVertexArray* pVtxOut = &worldVertsB2;
00419         pVtxOut->reserve(pVtxIn->size());
00420 
00421         int closestFaceA=-1;
00422         {
00423                 btScalar dmin = FLT_MAX;
00424                 for(int face=0;face<hullA.m_faces.size();face++)
00425                 {
00426                         const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
00427                         const btVector3 faceANormalWS = transA.getBasis() * Normal;
00428                 
00429                         btScalar d = faceANormalWS.dot(separatingNormal);
00430                         if (d < dmin)
00431                         {
00432                                 dmin = d;
00433                                 closestFaceA = face;
00434                         }
00435                 }
00436         }
00437         if (closestFaceA<0)
00438                 return;
00439 
00440         const btFace& polyA = hullA.m_faces[closestFaceA];
00441 
00442                 // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
00443         int numVerticesA = polyA.m_indices.size();
00444         for(int e0=0;e0<numVerticesA;e0++)
00445         {
00446                 const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
00447                 const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
00448                 const btVector3 edge0 = a - b;
00449                 const btVector3 WorldEdge0 = transA.getBasis() * edge0;
00450                 btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
00451 
00452                 btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
00453                 btVector3 worldA1 = transA*a;
00454                 btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
00455                 
00456 //int otherFace=0;
00457 #ifdef BLA1
00458                 int otherFace = polyA.m_connectedFaces[e0];
00459                 btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
00460                 btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
00461 
00462                 btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
00463                 btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
00464 #else 
00465                 btVector3 planeNormalWS = planeNormalWS1;
00466                 btScalar planeEqWS=planeEqWS1;
00467                 
00468 #endif
00469                 //clip face
00470 
00471                 clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
00472                 btSwap(pVtxIn,pVtxOut);
00473                 pVtxOut->resize(0);
00474         }
00475 
00476 
00477 
00478 //#define ONLY_REPORT_DEEPEST_POINT
00479 
00480         btVector3 point;
00481         
00482 
00483         // only keep points that are behind the witness face
00484         {
00485                 btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
00486                 btScalar localPlaneEq = polyA.m_plane[3];
00487                 btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
00488                 btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
00489                 for (int i=0;i<pVtxIn->size();i++)
00490                 {
00491                         btVector3 vtx = pVtxIn->at(i);
00492                         btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
00493                         if (depth <=minDist)
00494                         {
00495 //                              printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
00496                                 depth = minDist;
00497                         }
00498 
00499                         if (depth <=maxDist)
00500                         {
00501                                 btVector3 point = pVtxIn->at(i);
00502 #ifdef ONLY_REPORT_DEEPEST_POINT
00503                                 curMaxDist = depth;
00504 #else
00505 #if 0
00506                                 if (depth<-3)
00507                                 {
00508                                         printf("error in btPolyhedralContactClipping depth = %f\n", depth);
00509                                         printf("likely wrong separatingNormal passed in\n");
00510                                 } 
00511 #endif                          
00512                                 resultOut.addContactPoint(separatingNormal,point,depth);
00513 #endif
00514                         }
00515                 }
00516         }
00517 #ifdef ONLY_REPORT_DEEPEST_POINT
00518         if (curMaxDist<maxDist)
00519         {
00520                 resultOut.addContactPoint(separatingNormal,point,curMaxDist);
00521         }
00522 #endif //ONLY_REPORT_DEEPEST_POINT
00523 
00524 }
00525 
00526 
00527 
00528 
00529 
00530 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)
00531 {
00532 
00533         btVector3 separatingNormal = separatingNormal1.normalized();
00534 //      const btVector3 c0 = transA * hullA.m_localCenter;
00535 //      const btVector3 c1 = transB * hullB.m_localCenter;
00536         //const btVector3 DeltaC2 = c0 - c1;
00537 
00538 
00539 
00540         int closestFaceB=-1;
00541         btScalar dmax = -FLT_MAX;
00542         {
00543                 for(int face=0;face<hullB.m_faces.size();face++)
00544                 {
00545                         const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
00546                         const btVector3 WorldNormal = transB.getBasis() * Normal;
00547                         btScalar d = WorldNormal.dot(separatingNormal);
00548                         if (d > dmax)
00549                         {
00550                                 dmax = d;
00551                                 closestFaceB = face;
00552                         }
00553                 }
00554         }
00555                                 btVertexArray worldVertsB1;
00556                                 {
00557                                         const btFace& polyB = hullB.m_faces[closestFaceB];
00558                                         const int numVertices = polyB.m_indices.size();
00559                                         for(int e0=0;e0<numVertices;e0++)
00560                                         {
00561                                                 const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
00562                                                 worldVertsB1.push_back(transB*b);
00563                                         }
00564                                 }
00565 
00566         
00567         if (closestFaceB>=0)
00568                 clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
00569 
00570 }