== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / BulletCollision / CollisionDispatch / btConvexConvexAlgorithm.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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 ///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
17 ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
18 ///with reproduction case
19 //define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
20
21 #include "btConvexConvexAlgorithm.h"
22
23 //#include <stdio.h>
24 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
25 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
26 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
27 #include "BulletCollision/CollisionShapes/btConvexShape.h"
28 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
29
30
31 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
32 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
33 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
34 #include "BulletCollision/CollisionShapes/btBoxShape.h"
35 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
36
37 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
38 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
39 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
40 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
41
42
43
44 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
45 #include "BulletCollision/CollisionShapes/btSphereShape.h"
46
47 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
48
49 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
50 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
51
52
53
54 ///////////
55
56
57
58 static SIMD_FORCE_INLINE void segmentsClosestPoints(
59         btVector3& ptsVector,
60         btVector3& offsetA,
61         btVector3& offsetB,
62         btScalar& tA, btScalar& tB,
63         const btVector3& translation,
64         const btVector3& dirA, btScalar hlenA,
65         const btVector3& dirB, btScalar hlenB )
66 {
67         // compute the parameters of the closest points on each line segment
68
69         btScalar dirA_dot_dirB = btDot(dirA,dirB);
70         btScalar dirA_dot_trans = btDot(dirA,translation);
71         btScalar dirB_dot_trans = btDot(dirB,translation);
72
73         btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
74
75         if ( denom == 0.0f ) {
76                 tA = 0.0f;
77         } else {
78                 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
79                 if ( tA < -hlenA )
80                         tA = -hlenA;
81                 else if ( tA > hlenA )
82                         tA = hlenA;
83         }
84
85         tB = tA * dirA_dot_dirB - dirB_dot_trans;
86
87         if ( tB < -hlenB ) {
88                 tB = -hlenB;
89                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
90
91                 if ( tA < -hlenA )
92                         tA = -hlenA;
93                 else if ( tA > hlenA )
94                         tA = hlenA;
95         } else if ( tB > hlenB ) {
96                 tB = hlenB;
97                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
98
99                 if ( tA < -hlenA )
100                         tA = -hlenA;
101                 else if ( tA > hlenA )
102                         tA = hlenA;
103         }
104
105         // compute the closest points relative to segment centers.
106
107         offsetA = dirA * tA;
108         offsetB = dirB * tB;
109
110         ptsVector = translation - offsetA + offsetB;
111 }
112
113
114 static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
115         btVector3& normalOnB,
116         btVector3& pointOnB,
117         btScalar capsuleLengthA,
118         btScalar        capsuleRadiusA,
119         btScalar capsuleLengthB,
120         btScalar        capsuleRadiusB,
121         int capsuleAxisA,
122         int capsuleAxisB,
123         const btTransform& transformA,
124         const btTransform& transformB,
125         btScalar distanceThreshold )
126 {
127         btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
128         btVector3 translationA = transformA.getOrigin();
129         btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
130         btVector3 translationB = transformB.getOrigin();
131
132         // translation between centers
133
134         btVector3 translation = translationB - translationA;
135
136         // compute the closest points of the capsule line segments
137
138         btVector3 ptsVector;           // the vector between the closest points
139         
140         btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
141         btScalar tA, tB;              // parameters on line segment
142
143         segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
144                                                    directionA, capsuleLengthA, directionB, capsuleLengthB );
145
146         btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
147
148         if ( distance > distanceThreshold )
149                 return distance;
150
151         btScalar lenSqr = ptsVector.length2();
152         if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
153         {
154                 //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
155                 btVector3 q;
156                 btPlaneSpace1(directionA,normalOnB,q);
157         } else
158         {
159                 // compute the contact normal
160                 normalOnB = ptsVector*-btRecipSqrt(lenSqr);
161         }
162         pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
163
164         return distance;
165 }
166
167
168
169
170
171
172
173 //////////
174
175
176
177
178
179 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
180 {
181         m_numPerturbationIterations = 0;
182         m_minimumPointsPerturbationThreshold = 3;
183         m_simplexSolver = simplexSolver;
184         m_pdSolver = pdSolver;
185 }
186
187 btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
188
189 }
190
191 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
192 : btActivatingCollisionAlgorithm(ci,body0,body1),
193 m_simplexSolver(simplexSolver),
194 m_pdSolver(pdSolver),
195 m_ownManifold (false),
196 m_manifoldPtr(mf),
197 m_lowLevelOfDetail(false),
198 #ifdef USE_SEPDISTANCE_UTIL2
199 m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
200                           (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
201 #endif
202 m_numPerturbationIterations(numPerturbationIterations),
203 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
204 {
205         (void)body0;
206         (void)body1;
207 }
208
209
210
211
212 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
213 {
214         if (m_ownManifold)
215         {
216                 if (m_manifoldPtr)
217                         m_dispatcher->releaseManifold(m_manifoldPtr);
218         }
219 }
220
221 void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
222 {
223         m_lowLevelOfDetail = useLowLevel;
224 }
225
226
227 struct btPerturbedContactResult : public btManifoldResult
228 {
229         btManifoldResult* m_originalManifoldResult;
230         btTransform m_transformA;
231         btTransform m_transformB;
232         btTransform     m_unPerturbedTransform;
233         bool    m_perturbA;
234         btIDebugDraw*   m_debugDrawer;
235
236
237         btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
238                 :m_originalManifoldResult(originalResult),
239                 m_transformA(transformA),
240                 m_transformB(transformB),
241                 m_unPerturbedTransform(unPerturbedTransform),
242                 m_perturbA(perturbA),
243                 m_debugDrawer(debugDrawer)
244         {
245         }
246         virtual ~ btPerturbedContactResult()
247         {
248         }
249
250         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
251         {
252                 btVector3 endPt,startPt;
253                 btScalar newDepth;
254                 btVector3 newNormal;
255
256                 if (m_perturbA)
257                 {
258                         btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
259                         endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
260                         newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
261                         startPt = endPt+normalOnBInWorld*newDepth;
262                 } else
263                 {
264                         endPt = pointInWorld + normalOnBInWorld*orgDepth;
265                         startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
266                         newDepth = (endPt -  startPt).dot(normalOnBInWorld);
267                         
268                 }
269
270 //#define DEBUG_CONTACTS 1
271 #ifdef DEBUG_CONTACTS
272                 m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
273                 m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
274                 m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
275 #endif //DEBUG_CONTACTS
276
277                 
278                 m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
279         }
280
281 };
282
283 extern btScalar gContactBreakingThreshold;
284
285
286 //
287 // Convex-Convex collision algorithm
288 //
289 void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
290 {
291
292         if (!m_manifoldPtr)
293         {
294                 //swapped?
295                 m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
296                 m_ownManifold = true;
297         }
298         resultOut->setPersistentManifold(m_manifoldPtr);
299
300         //comment-out next line to test multi-contact generation
301         //resultOut->getPersistentManifold()->clearManifold();
302         
303
304         btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
305         btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
306
307         btVector3  normalOnB;
308                 btVector3  pointOnBWorld;
309 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
310         if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
311         {
312                 btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
313                 btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
314                 btVector3 localScalingA = capsuleA->getLocalScaling();
315                 btVector3 localScalingB = capsuleB->getLocalScaling();
316                 
317                 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
318
319                 btScalar dist = capsuleCapsuleDistance(normalOnB,       pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
320                         capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
321                         body0->getWorldTransform(),body1->getWorldTransform(),threshold);
322
323                 if (dist<threshold)
324                 {
325                         btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
326                         resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);       
327                 }
328                 resultOut->refreshContactPoints();
329                 return;
330         }
331 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
332
333
334 #ifdef USE_SEPDISTANCE_UTIL2
335         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
336         {
337                 m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
338         }
339
340         if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
341 #endif //USE_SEPDISTANCE_UTIL2
342
343         {
344
345         
346         btGjkPairDetector::ClosestPointInput input;
347
348         btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
349         //TODO: if (dispatchInfo.m_useContinuous)
350         gjkPairDetector.setMinkowskiA(min0);
351         gjkPairDetector.setMinkowskiB(min1);
352
353 #ifdef USE_SEPDISTANCE_UTIL2
354         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
355         {
356                 input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
357         } else
358 #endif //USE_SEPDISTANCE_UTIL2
359         {
360                 input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
361                 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
362         }
363
364         input.m_stackAlloc = dispatchInfo.m_stackAllocator;
365         input.m_transformA = body0->getWorldTransform();
366         input.m_transformB = body1->getWorldTransform();
367
368         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
369
370         
371
372 #ifdef USE_SEPDISTANCE_UTIL2
373         btScalar sepDist = 0.f;
374         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
375         {
376                 sepDist = gjkPairDetector.getCachedSeparatingDistance();
377                 if (sepDist>SIMD_EPSILON)
378                 {
379                         sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
380                         //now perturbe directions to get multiple contact points
381                         
382                 }
383         }
384 #endif //USE_SEPDISTANCE_UTIL2
385
386         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
387         
388         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
389         if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
390         {
391                 
392                 int i;
393                 btVector3 v0,v1;
394                 btVector3 sepNormalWorldSpace;
395         
396                 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
397                 btPlaneSpace1(sepNormalWorldSpace,v0,v1);
398
399
400                 bool perturbeA = true;
401                 const btScalar angleLimit = 0.125f * SIMD_PI;
402                 btScalar perturbeAngle;
403                 btScalar radiusA = min0->getAngularMotionDisc();
404                 btScalar radiusB = min1->getAngularMotionDisc();
405                 if (radiusA < radiusB)
406                 {
407                         perturbeAngle = gContactBreakingThreshold /radiusA;
408                         perturbeA = true;
409                 } else
410                 {
411                         perturbeAngle = gContactBreakingThreshold / radiusB;
412                         perturbeA = false;
413                 }
414                 if ( perturbeAngle > angleLimit ) 
415                                 perturbeAngle = angleLimit;
416
417                 btTransform unPerturbedTransform;
418                 if (perturbeA)
419                 {
420                         unPerturbedTransform = input.m_transformA;
421                 } else
422                 {
423                         unPerturbedTransform = input.m_transformB;
424                 }
425                 
426                 for ( i=0;i<m_numPerturbationIterations;i++)
427                 {
428                         if (v0.length2()>SIMD_EPSILON)
429                         {
430                         btQuaternion perturbeRot(v0,perturbeAngle);
431                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
432                         btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
433                         
434                         
435                         if (perturbeA)
436                         {
437                                 input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
438                                 input.m_transformB = body1->getWorldTransform();
439 #ifdef DEBUG_CONTACTS
440                                 dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
441 #endif //DEBUG_CONTACTS
442                         } else
443                         {
444                                 input.m_transformA = body0->getWorldTransform();
445                                 input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
446 #ifdef DEBUG_CONTACTS
447                                 dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
448 #endif
449                         }
450                         
451                         btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
452                         gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
453                         }
454                         
455                 }
456         }
457
458         
459
460 #ifdef USE_SEPDISTANCE_UTIL2
461         if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
462         {
463                 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
464         }
465 #endif //USE_SEPDISTANCE_UTIL2
466
467
468         }
469
470         if (m_ownManifold)
471         {
472                 resultOut->refreshContactPoints();
473         }
474
475 }
476
477
478
479 bool disableCcd = false;
480 btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
481 {
482         (void)resultOut;
483         (void)dispatchInfo;
484         ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
485     
486         ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
487         ///col0->m_worldTransform,
488         btScalar resultFraction = btScalar(1.);
489
490
491         btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
492         btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
493     
494         if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
495                 squareMot1 < col1->getCcdSquareMotionThreshold())
496                 return resultFraction;
497
498         if (disableCcd)
499                 return btScalar(1.);
500
501
502         //An adhoc way of testing the Continuous Collision Detection algorithms
503         //One object is approximated as a sphere, to simplify things
504         //Starting in penetration should report no time of impact
505         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
506         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
507
508                 
509         /// Convex0 against sphere for Convex1
510         {
511                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
512
513                 btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
514                 btConvexCast::CastResult result;
515                 btVoronoiSimplexSolver voronoiSimplex;
516                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
517                 ///Simplification, one object is simplified as a sphere
518                 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
519                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
520                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
521                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
522                 {
523                 
524                         //store result.m_fraction in both bodies
525                 
526                         if (col0->getHitFraction()> result.m_fraction)
527                                 col0->setHitFraction( result.m_fraction );
528
529                         if (col1->getHitFraction() > result.m_fraction)
530                                 col1->setHitFraction( result.m_fraction);
531
532                         if (resultFraction > result.m_fraction)
533                                 resultFraction = result.m_fraction;
534
535                 }
536                 
537                 
538
539
540         }
541
542         /// Sphere (for convex0) against Convex1
543         {
544                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
545
546                 btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
547                 btConvexCast::CastResult result;
548                 btVoronoiSimplexSolver voronoiSimplex;
549                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
550                 ///Simplification, one object is simplified as a sphere
551                 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
552                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
553                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
554                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
555                 {
556                 
557                         //store result.m_fraction in both bodies
558                 
559                         if (col0->getHitFraction()      > result.m_fraction)
560                                 col0->setHitFraction( result.m_fraction);
561
562                         if (col1->getHitFraction() > result.m_fraction)
563                                 col1->setHitFraction( result.m_fraction);
564
565                         if (resultFraction > result.m_fraction)
566                                 resultFraction = result.m_fraction;
567
568                 }
569         }
570         
571         return resultFraction;
572
573 }
574