Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletCollision / NarrowPhaseCollision / btPolyhedralContactClipping.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org
4
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:
10
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.
14 */
15
16
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
20
21
22 #include "btPolyhedralContactClipping.h"
23 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
24
25 #include <float.h> //for FLT_MAX
26
27 int gExpectedNbTests=0;
28 int gActualNbTests = 0;
29 bool gUseInternalObject = true;
30
31 // Clips a face to the back of a plane
32 void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
33 {
34         
35         int ve;
36         btScalar ds, de;
37         int numVerts = pVtxIn.size();
38         if (numVerts < 2)
39                 return;
40
41         btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
42         btVector3 endVertex = pVtxIn[0];
43         
44         ds = planeNormalWS.dot(firstVertex)+planeEqWS;
45
46         for (ve = 0; ve < numVerts; ve++)
47         {
48                 endVertex=pVtxIn[ve];
49
50                 de = planeNormalWS.dot(endVertex)+planeEqWS;
51
52                 if (ds<0)
53                 {
54                         if (de<0)
55                         {
56                                 // Start < 0, end < 0, so output endVertex
57                                 ppVtxOut.push_back(endVertex);
58                         }
59                         else
60                         {
61                                 // Start < 0, end >= 0, so output intersection
62                                 ppVtxOut.push_back(     firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
63                         }
64                 }
65                 else
66                 {
67                         if (de<0)
68                         {
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);
72                         }
73                 }
74                 firstVertex = endVertex;
75                 ds = de;
76         }
77 }
78
79
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)
81 {
82         btScalar Min0,Max0;
83         btScalar Min1,Max1;
84         btVector3 witnesPtMinA,witnesPtMaxA;
85         btVector3 witnesPtMinB,witnesPtMaxB;
86
87         hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
88         hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
89
90         if(Max0<Min1 || Max1<Min0)
91                 return false;
92
93         btScalar d0 = Max0 - Min1;
94         btAssert(d0>=0.0f);
95         btScalar d1 = Max1 - Min0;
96         btAssert(d1>=0.0f);
97         if (d0<d1)
98         {
99                 depth = d0;
100                 witnessPointA = witnesPtMaxA;
101                 witnessPointB = witnesPtMinB;
102
103         } else
104         {
105                 depth = d1;
106                 witnessPointA = witnesPtMinA;
107                 witnessPointB = witnesPtMaxB;
108         }
109         
110         return true;
111 }
112
113
114
115 static int gActualSATPairTests=0;
116
117 inline bool IsAlmostZero(const btVector3& v)
118 {
119         if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6)      return false;
120         return true;
121 }
122
123 #ifdef TEST_INTERNAL_OBJECTS
124
125 inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
126 {
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];
134 }
135
136 void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
137 {
138         const btMatrix3x3& rot = tr.getBasis();
139         const btVector3& r0 = rot[0];
140         const btVector3& r1 = rot[1];
141         const btVector3& r2 = rot[2];
142
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();
146
147         out.setValue(x, y, z);
148 }
149
150  bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
151 {
152         const btScalar dp = delta_c.dot(axis);
153
154         btVector3 localAxis0;
155         InverseTransformPoint3x3(localAxis0, axis,trans0);
156         btVector3 localAxis1;
157         InverseTransformPoint3x3(localAxis1, axis,trans1);
158
159         btScalar p0[3];
160         BoxSupport(convex0.m_extents, localAxis0, p0);
161         btScalar p1[3];
162         BoxSupport(convex1.m_extents, localAxis1, p1);
163
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();
166
167         const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
168         const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
169
170         const btScalar MinMaxRadius = MaxRadius + MinRadius;
171         const btScalar d0 = MinMaxRadius + dp;
172         const btScalar d1 = MinMaxRadius - dp;
173
174         const btScalar depth = d0<d1 ? d0:d1;
175         if(depth>dmin)
176                 return false;
177         return true;
178 }
179 #endif //TEST_INTERNAL_OBJECTS
180
181  
182  
183  SIMD_FORCE_INLINE void btSegmentsClosestPoints(
184         btVector3& ptsVector,
185         btVector3& offsetA,
186         btVector3& offsetB,
187         btScalar& tA, btScalar& tB,
188         const btVector3& translation,
189         const btVector3& dirA, btScalar hlenA,
190         const btVector3& dirB, btScalar hlenB )
191 {
192         // compute the parameters of the closest points on each line segment
193
194         btScalar dirA_dot_dirB = btDot(dirA,dirB);
195         btScalar dirA_dot_trans = btDot(dirA,translation);
196         btScalar dirB_dot_trans = btDot(dirB,translation);
197
198         btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
199
200         if ( denom == 0.0f ) {
201                 tA = 0.0f;
202         } else {
203                 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
204                 if ( tA < -hlenA )
205                         tA = -hlenA;
206                 else if ( tA > hlenA )
207                         tA = hlenA;
208         }
209
210         tB = tA * dirA_dot_dirB - dirB_dot_trans;
211
212         if ( tB < -hlenB ) {
213                 tB = -hlenB;
214                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
215
216                 if ( tA < -hlenA )
217                         tA = -hlenA;
218                 else if ( tA > hlenA )
219                         tA = hlenA;
220         } else if ( tB > hlenB ) {
221                 tB = hlenB;
222                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
223
224                 if ( tA < -hlenA )
225                         tA = -hlenA;
226                 else if ( tA > hlenA )
227                         tA = hlenA;
228         }
229
230         // compute the closest points relative to segment centers.
231
232         offsetA = dirA * tA;
233         offsetB = dirB * tB;
234
235         ptsVector = translation - offsetA + offsetB;
236 }
237
238
239
240 bool btPolyhedralContactClipping::findSeparatingAxis(   const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
241 {
242         gActualSATPairTests++;
243
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;
248 //#endif
249
250         btScalar dmin = FLT_MAX;
251         int curPlaneTests=0;
252
253         int numFacesA = hullA.m_faces.size();
254         // Test normals from hullA
255         for(int i=0;i<numFacesA;i++)
256         {
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)
260                         faceANormalWS*=-1.f;
261
262                 curPlaneTests++;
263 #ifdef TEST_INTERNAL_OBJECTS
264                 gExpectedNbTests++;
265                 if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
266                         continue;
267                 gActualNbTests++;
268 #endif
269
270                 btScalar d;
271                 btVector3 wA,wB;
272                 if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
273                         return false;
274
275                 if(d<dmin)
276                 {
277                         dmin = d;
278                         sep = faceANormalWS;
279                 }
280         }
281
282         int numFacesB = hullB.m_faces.size();
283         // Test normals from hullB
284         for(int i=0;i<numFacesB;i++)
285         {
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)
289                         WorldNormal *=-1.f;
290
291                 curPlaneTests++;
292 #ifdef TEST_INTERNAL_OBJECTS
293                 gExpectedNbTests++;
294                 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
295                         continue;
296                 gActualNbTests++;
297 #endif
298
299                 btScalar d;
300                 btVector3 wA,wB;
301                 if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
302                         return false;
303
304                 if(d<dmin)
305                 {
306                         dmin = d;
307                         sep = WorldNormal;
308                 }
309         }
310
311         btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
312         int edgeA=-1;
313         int edgeB=-1;
314         btVector3 worldEdgeA;
315         btVector3 worldEdgeB;
316         btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
317         
318
319         int curEdgeEdge = 0;
320         // Test edges
321         for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
322         {
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++)
326                 {
327                         const btVector3 edge1 = hullB.m_uniqueEdges[e1];
328                         const btVector3 WorldEdge1 = transB.getBasis() * edge1;
329
330                         btVector3 Cross = WorldEdge0.cross(WorldEdge1);
331                         curEdgeEdge++;
332                         if(!IsAlmostZero(Cross))
333                         {
334                                 Cross = Cross.normalize();
335                                 if (DeltaC2.dot(Cross)<0)
336                                         Cross *= -1.f;
337
338
339 #ifdef TEST_INTERNAL_OBJECTS
340                                 gExpectedNbTests++;
341                                 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
342                                         continue;
343                                 gActualNbTests++;
344 #endif
345
346                                 btScalar dist;
347                                 btVector3 wA,wB;
348                                 if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
349                                         return false;
350
351                                 if(dist<dmin)
352                                 {
353                                         dmin = dist;
354                                         sep = Cross;
355                                         edgeA=e0;
356                                         edgeB=e1;
357                                         worldEdgeA = WorldEdge0;
358                                         worldEdgeB = WorldEdge1;
359                                         witnessPointA=wA;
360                                         witnessPointB=wB;
361                                 }
362                         }
363                 }
364
365         }
366
367         if (edgeA>=0&&edgeB>=0)
368         {
369 //              printf("edge-edge\n");
370                 //add an edge-edge contact
371
372                 btVector3 ptsVector;
373                 btVector3 offsetA;
374                 btVector3 offsetB;
375                 btScalar tA;
376                 btScalar tB;
377
378                 btVector3 translation = witnessPointB-witnessPointA;
379
380                 btVector3 dirA = worldEdgeA;
381                 btVector3 dirB = worldEdgeB;
382                 
383                 btScalar hlenB = 1e30f;
384                 btScalar hlenA = 1e30f;
385
386                 btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
387                         translation,
388                         dirA, hlenA,
389                         dirB,hlenB);
390
391                 btScalar nlSqrt = ptsVector.length2();
392                 if (nlSqrt>SIMD_EPSILON)
393                 {
394                         btScalar nl = btSqrt(nlSqrt);
395                         ptsVector *= 1.f/nl;
396                         if (ptsVector.dot(DeltaC2)<0.f)
397                         {
398                                 ptsVector*=-1.f;
399                         }
400                         btVector3 ptOnB = witnessPointB + offsetB;
401                         btScalar distance = nl;
402                         resultOut.addContactPoint(ptsVector, ptOnB,-distance);
403                 }
404
405         }
406
407
408         if((DeltaC2.dot(sep))<0.0f)
409                 sep = -sep;
410
411         return true;
412 }
413
414 void    btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
415 {
416         btVertexArray worldVertsB2;
417         btVertexArray* pVtxIn = &worldVertsB1;
418         btVertexArray* pVtxOut = &worldVertsB2;
419         pVtxOut->reserve(pVtxIn->size());
420
421         int closestFaceA=-1;
422         {
423                 btScalar dmin = FLT_MAX;
424                 for(int face=0;face<hullA.m_faces.size();face++)
425                 {
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;
428                 
429                         btScalar d = faceANormalWS.dot(separatingNormal);
430                         if (d < dmin)
431                         {
432                                 dmin = d;
433                                 closestFaceA = face;
434                         }
435                 }
436         }
437         if (closestFaceA<0)
438                 return;
439
440         const btFace& polyA = hullA.m_faces[closestFaceA];
441
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++)
445         {
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]);
451
452                 btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
453                 btVector3 worldA1 = transA*a;
454                 btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
455                 
456 //int otherFace=0;
457 #ifdef BLA1
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];
461
462                 btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
463                 btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
464 #else 
465                 btVector3 planeNormalWS = planeNormalWS1;
466                 btScalar planeEqWS=planeEqWS1;
467                 
468 #endif
469                 //clip face
470
471                 clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
472                 btSwap(pVtxIn,pVtxOut);
473                 pVtxOut->resize(0);
474         }
475
476
477
478 //#define ONLY_REPORT_DEEPEST_POINT
479
480         btVector3 point;
481         
482
483         // only keep points that are behind the witness face
484         {
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++)
490                 {
491                         btVector3 vtx = pVtxIn->at(i);
492                         btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
493                         if (depth <=minDist)
494                         {
495 //                              printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
496                                 depth = minDist;
497                         }
498
499                         if (depth <=maxDist)
500                         {
501                                 btVector3 point = pVtxIn->at(i);
502 #ifdef ONLY_REPORT_DEEPEST_POINT
503                                 curMaxDist = depth;
504 #else
505 #if 0
506                                 if (depth<-3)
507                                 {
508                                         printf("error in btPolyhedralContactClipping depth = %f\n", depth);
509                                         printf("likely wrong separatingNormal passed in\n");
510                                 } 
511 #endif                          
512                                 resultOut.addContactPoint(separatingNormal,point,depth);
513 #endif
514                         }
515                 }
516         }
517 #ifdef ONLY_REPORT_DEEPEST_POINT
518         if (curMaxDist<maxDist)
519         {
520                 resultOut.addContactPoint(separatingNormal,point,curMaxDist);
521         }
522 #endif //ONLY_REPORT_DEEPEST_POINT
523
524 }
525
526
527
528
529
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)
531 {
532
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;
537
538
539
540         int closestFaceB=-1;
541         btScalar dmax = -FLT_MAX;
542         {
543                 for(int face=0;face<hullB.m_faces.size();face++)
544                 {
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);
548                         if (d > dmax)
549                         {
550                                 dmax = d;
551                                 closestFaceB = face;
552                         }
553                 }
554         }
555                                 btVertexArray worldVertsB1;
556                                 {
557                                         const btFace& polyB = hullB.m_faces[closestFaceB];
558                                         const int numVertices = polyB.m_indices.size();
559                                         for(int e0=0;e0<numVertices;e0++)
560                                         {
561                                                 const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
562                                                 worldVertsB1.push_back(transB*b);
563                                         }
564                                 }
565
566         
567         if (closestFaceB>=0)
568                 clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
569
570 }