00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00020
00021
00022 #include "btPolyhedralContactClipping.h"
00023 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
00024
00025 #include <float.h>
00026
00027 int gExpectedNbTests=0;
00028 int gActualNbTests = 0;
00029 bool gUseInternalObject = true;
00030
00031
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
00057 ppVtxOut.push_back(endVertex);
00058 }
00059 else
00060 {
00061
00062 ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
00063 }
00064 }
00065 else
00066 {
00067 if (de<0)
00068 {
00069
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
00128
00129
00130
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
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
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
00245 const btVector3 c0 = transA * hullA.m_localCenter;
00246 const btVector3 c1 = transB * hullB.m_localCenter;
00247 const btVector3 DeltaC2 = c0 - c1;
00248
00249
00250 btScalar dmin = FLT_MAX;
00251 int curPlaneTests=0;
00252
00253 int numFacesA = hullA.m_faces.size();
00254
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
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
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
00370
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
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);
00453 btVector3 worldA1 = transA*a;
00454 btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
00455
00456
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
00470
00471 clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
00472 btSwap(pVtxIn,pVtxOut);
00473 pVtxOut->resize(0);
00474 }
00475
00476
00477
00478
00479
00480 btVector3 point;
00481
00482
00483
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
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
00535
00536
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 }