Bullet Collision Detection & Physics Library
btConvexConvexAlgorithm.cpp
Go to the documentation of this file.
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 
19 //define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
20 //#define ZERO_MARGIN
21 
23 
24 //#include <stdio.h>
31 
32 
33 
39 
44 
45 
46 
49 
51 
56 
58 
59 
60 
62  btVector3& ptsVector,
63  btVector3& offsetA,
64  btVector3& offsetB,
65  btScalar& tA, btScalar& tB,
66  const btVector3& translation,
67  const btVector3& dirA, btScalar hlenA,
68  const btVector3& dirB, btScalar hlenB )
69 {
70  // compute the parameters of the closest points on each line segment
71 
72  btScalar dirA_dot_dirB = btDot(dirA,dirB);
73  btScalar dirA_dot_trans = btDot(dirA,translation);
74  btScalar dirB_dot_trans = btDot(dirB,translation);
75 
76  btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
77 
78  if ( denom == 0.0f ) {
79  tA = 0.0f;
80  } else {
81  tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
82  if ( tA < -hlenA )
83  tA = -hlenA;
84  else if ( tA > hlenA )
85  tA = hlenA;
86  }
87 
88  tB = tA * dirA_dot_dirB - dirB_dot_trans;
89 
90  if ( tB < -hlenB ) {
91  tB = -hlenB;
92  tA = tB * dirA_dot_dirB + dirA_dot_trans;
93 
94  if ( tA < -hlenA )
95  tA = -hlenA;
96  else if ( tA > hlenA )
97  tA = hlenA;
98  } else if ( tB > hlenB ) {
99  tB = hlenB;
100  tA = tB * dirA_dot_dirB + dirA_dot_trans;
101 
102  if ( tA < -hlenA )
103  tA = -hlenA;
104  else if ( tA > hlenA )
105  tA = hlenA;
106  }
107 
108  // compute the closest points relative to segment centers.
109 
110  offsetA = dirA * tA;
111  offsetB = dirB * tB;
112 
113  ptsVector = translation - offsetA + offsetB;
114 }
115 
116 
118  btVector3& normalOnB,
119  btVector3& pointOnB,
120  btScalar capsuleLengthA,
121  btScalar capsuleRadiusA,
122  btScalar capsuleLengthB,
123  btScalar capsuleRadiusB,
124  int capsuleAxisA,
125  int capsuleAxisB,
126  const btTransform& transformA,
127  const btTransform& transformB,
128  btScalar distanceThreshold )
129 {
130  btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
131  btVector3 translationA = transformA.getOrigin();
132  btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
133  btVector3 translationB = transformB.getOrigin();
134 
135  // translation between centers
136 
137  btVector3 translation = translationB - translationA;
138 
139  // compute the closest points of the capsule line segments
140 
141  btVector3 ptsVector; // the vector between the closest points
142 
143  btVector3 offsetA, offsetB; // offsets from segment centers to their closest points
144  btScalar tA, tB; // parameters on line segment
145 
146  segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
147  directionA, capsuleLengthA, directionB, capsuleLengthB );
148 
149  btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
150 
151  if ( distance > distanceThreshold )
152  return distance;
153 
154  btScalar lenSqr = ptsVector.length2();
155  if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
156  {
157  //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
158  btVector3 q;
159  btPlaneSpace1(directionA,normalOnB,q);
160  } else
161  {
162  // compute the contact normal
163  normalOnB = ptsVector*-btRecipSqrt(lenSqr);
164  }
165  pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
166 
167  return distance;
168 }
169 
170 
171 
172 
173 
174 
175 
177 
178 
179 
180 
181 
183 {
186  m_simplexSolver = simplexSolver;
187  m_pdSolver = pdSolver;
188 }
189 
191 {
192 }
193 
194 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
195 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
196 m_simplexSolver(simplexSolver),
197 m_pdSolver(pdSolver),
198 m_ownManifold (false),
199 m_manifoldPtr(mf),
200 m_lowLevelOfDetail(false),
201 #ifdef USE_SEPDISTANCE_UTIL2
202 m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
203  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
204 #endif
205 m_numPerturbationIterations(numPerturbationIterations),
206 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
207 {
208  (void)body0Wrap;
209  (void)body1Wrap;
210 }
211 
212 
213 
214 
216 {
217  if (m_ownManifold)
218  {
219  if (m_manifoldPtr)
221  }
222 }
223 
225 {
226  m_lowLevelOfDetail = useLowLevel;
227 }
228 
229 
231 {
238 
239 
240  btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
241  :m_originalManifoldResult(originalResult),
242  m_transformA(transformA),
243  m_transformB(transformB),
244  m_unPerturbedTransform(unPerturbedTransform),
245  m_perturbA(perturbA),
246  m_debugDrawer(debugDrawer)
247  {
248  }
250  {
251  }
252 
253  virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
254  {
255  btVector3 endPt,startPt;
256  btScalar newDepth;
257  btVector3 newNormal;
258 
259  if (m_perturbA)
260  {
261  btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
262  endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
263  newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
264  startPt = endPt+normalOnBInWorld*newDepth;
265  } else
266  {
267  endPt = pointInWorld + normalOnBInWorld*orgDepth;
268  startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
269  newDepth = (endPt - startPt).dot(normalOnBInWorld);
270 
271  }
272 
273 //#define DEBUG_CONTACTS 1
274 #ifdef DEBUG_CONTACTS
275  m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
276  m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
277  m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
278 #endif //DEBUG_CONTACTS
279 
280 
281  m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
282  }
283 
284 };
285 
287 
288 
289 //
290 // Convex-Convex collision algorithm
291 //
293 {
294 
295  if (!m_manifoldPtr)
296  {
297  //swapped?
299  m_ownManifold = true;
300  }
302 
303  //comment-out next line to test multi-contact generation
304  //resultOut->getPersistentManifold()->clearManifold();
305 
306 
307  const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
308  const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
309 
310  btVector3 normalOnB;
311  btVector3 pointOnBWorld;
312 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
314  {
315  btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
316  btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
317  // btVector3 localScalingA = capsuleA->getLocalScaling();
318  // btVector3 localScalingB = capsuleB->getLocalScaling();
319 
321 
322  btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
323  capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
324  body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
325 
326  if (dist<threshold)
327  {
328  btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
329  resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
330  }
331  resultOut->refreshContactPoints();
332  return;
333  }
334 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
335 
336 
337 
338 
339 #ifdef USE_SEPDISTANCE_UTIL2
340  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
341  {
342  m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
343  }
344 
345  if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
346 #endif //USE_SEPDISTANCE_UTIL2
347 
348  {
349 
350 
352 
353  btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
354  //TODO: if (dispatchInfo.m_useContinuous)
355  gjkPairDetector.setMinkowskiA(min0);
356  gjkPairDetector.setMinkowskiB(min1);
357 
358 #ifdef USE_SEPDISTANCE_UTIL2
359  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
360  {
362  } else
363 #endif //USE_SEPDISTANCE_UTIL2
364  {
365  //if (dispatchInfo.m_convexMaxDistanceUseCPT)
366  //{
367  // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
368  //} else
369  //{
371 // }
372 
374  }
375 
376  input.m_transformA = body0Wrap->getWorldTransform();
377  input.m_transformB = body1Wrap->getWorldTransform();
378 
379 
380 
381 
382 
383 #ifdef USE_SEPDISTANCE_UTIL2
384  btScalar sepDist = 0.f;
385  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
386  {
387  sepDist = gjkPairDetector.getCachedSeparatingDistance();
388  if (sepDist>SIMD_EPSILON)
389  {
390  sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
391  //now perturbe directions to get multiple contact points
392 
393  }
394  }
395 #endif //USE_SEPDISTANCE_UTIL2
396 
397  if (min0->isPolyhedral() && min1->isPolyhedral())
398  {
399 
400 
401  struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
402  {
403  virtual void setShapeIdentifiersA(int partId0,int index0){}
404  virtual void setShapeIdentifiersB(int partId1,int index1){}
405  virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
406  {
407  }
408  };
409 
410 
411  struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
412  {
414  btVector3 m_reportedNormalOnWorld;
415  btScalar m_marginOnA;
416  btScalar m_marginOnB;
417  btScalar m_reportedDistance;
418 
419  bool m_foundResult;
420  btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
421  :m_originalResult(result),
422  m_marginOnA(marginOnA),
423  m_marginOnB(marginOnB),
424  m_foundResult(false)
425  {
426  }
427 
428  virtual void setShapeIdentifiersA(int partId0,int index0){}
429  virtual void setShapeIdentifiersB(int partId1,int index1){}
430  virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg)
431  {
432  m_reportedDistance = depthOrg;
433  m_reportedNormalOnWorld = normalOnBInWorld;
434 
435  btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
436  m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
437  if (m_reportedDistance<0.f)
438  {
439  m_foundResult = true;
440  }
441  m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
442  }
443  };
444 
445 
446  btDummyResult dummy;
447 
449 
450  btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
451  btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
452 
453  btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin);
454 
455  btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
456  btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
457  if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
458  {
459 
460 
461 
462 
464 
465  btScalar minDist = -1e30f;
466  btVector3 sepNormalWorldSpace;
467  bool foundSepAxis = true;
468 
469  if (dispatchInfo.m_enableSatConvex)
470  {
472  *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
473  body0Wrap->getWorldTransform(),
474  body1Wrap->getWorldTransform(),
475  sepNormalWorldSpace,*resultOut);
476  } else
477  {
478 #ifdef ZERO_MARGIN
479  gjkPairDetector.setIgnoreMargin(true);
480  gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
481 #else
482 
483 
484  gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
485  //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
486 #endif //ZERO_MARGIN
487  //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
488  //if (l2>SIMD_EPSILON)
489  {
490  sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
491  //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
492  minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
493 
494 #ifdef ZERO_MARGIN
495  foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
496 #else
497  foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
498 #endif
499  }
500  }
501  if (foundSepAxis)
502  {
503 
504 // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
505 
506  worldVertsB1.resize(0);
507  btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
508  body0Wrap->getWorldTransform(),
509  body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2,
510  *resultOut);
511 
512  }
513  if (m_ownManifold)
514  {
515  resultOut->refreshContactPoints();
516  }
517  return;
518 
519  } else
520  {
521  //we can also deal with convex versus triangle (without connectivity data)
522  if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
523  {
524 
525  btVertexArray vertices;
526  btTriangleShape* tri = (btTriangleShape*)polyhedronB;
527  vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
528  vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
529  vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
530 
531  //tri->initializePolyhedralFeatures();
532 
534 
535  btVector3 sepNormalWorldSpace;
536  btScalar minDist =-1e30f;
537  btScalar maxDist = threshold;
538 
539  bool foundSepAxis = false;
540  if (0)
541  {
542  polyhedronB->initializePolyhedralFeatures();
544  *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
545  body0Wrap->getWorldTransform(),
546  body1Wrap->getWorldTransform(),
547  sepNormalWorldSpace,*resultOut);
548  // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
549 
550  } else
551  {
552 #ifdef ZERO_MARGIN
553  gjkPairDetector.setIgnoreMargin(true);
554  gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
555 #else
556  gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
557 #endif//ZERO_MARGIN
558 
559  btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
560  if (l2>SIMD_EPSILON)
561  {
562  sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
563  //minDist = gjkPairDetector.getCachedSeparatingDistance();
564  //maxDist = threshold;
565  minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
566  foundSepAxis = true;
567  }
568  }
569 
570 
571  if (foundSepAxis)
572  {
573  worldVertsB2.resize(0);
574  btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
575  body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
576  }
577 
578 
579  if (m_ownManifold)
580  {
581  resultOut->refreshContactPoints();
582  }
583 
584  return;
585  }
586 
587  }
588 
589 
590  }
591 
592  gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
593 
594  //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
595 
596  //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
598  {
599 
600  int i;
601  btVector3 v0,v1;
602  btVector3 sepNormalWorldSpace;
603  btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
604 
605  if (l2>SIMD_EPSILON)
606  {
607  sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
608 
609  btPlaneSpace1(sepNormalWorldSpace,v0,v1);
610 
611 
612  bool perturbeA = true;
613  const btScalar angleLimit = 0.125f * SIMD_PI;
614  btScalar perturbeAngle;
615  btScalar radiusA = min0->getAngularMotionDisc();
616  btScalar radiusB = min1->getAngularMotionDisc();
617  if (radiusA < radiusB)
618  {
619  perturbeAngle = gContactBreakingThreshold /radiusA;
620  perturbeA = true;
621  } else
622  {
623  perturbeAngle = gContactBreakingThreshold / radiusB;
624  perturbeA = false;
625  }
626  if ( perturbeAngle > angleLimit )
627  perturbeAngle = angleLimit;
628 
629  btTransform unPerturbedTransform;
630  if (perturbeA)
631  {
632  unPerturbedTransform = input.m_transformA;
633  } else
634  {
635  unPerturbedTransform = input.m_transformB;
636  }
637 
638  for ( i=0;i<m_numPerturbationIterations;i++)
639  {
640  if (v0.length2()>SIMD_EPSILON)
641  {
642  btQuaternion perturbeRot(v0,perturbeAngle);
643  btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
644  btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
645 
646 
647  if (perturbeA)
648  {
649  input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
650  input.m_transformB = body1Wrap->getWorldTransform();
651  #ifdef DEBUG_CONTACTS
652  dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
653  #endif //DEBUG_CONTACTS
654  } else
655  {
656  input.m_transformA = body0Wrap->getWorldTransform();
657  input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
658  #ifdef DEBUG_CONTACTS
659  dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
660  #endif
661  }
662 
663  btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
664  gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
665  }
666  }
667  }
668  }
669 
670 
671 
672 #ifdef USE_SEPDISTANCE_UTIL2
673  if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
674  {
675  m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
676  }
677 #endif //USE_SEPDISTANCE_UTIL2
678 
679 
680  }
681 
682  if (m_ownManifold)
683  {
684  resultOut->refreshContactPoints();
685  }
686 
687 }
688 
689 
690 
691 bool disableCcd = false;
693 {
694  (void)resultOut;
695  (void)dispatchInfo;
697 
700  btScalar resultFraction = btScalar(1.);
701 
702 
703  btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
704  btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
705 
706  if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
707  squareMot1 < col1->getCcdSquareMotionThreshold())
708  return resultFraction;
709 
710  if (disableCcd)
711  return btScalar(1.);
712 
713 
714  //An adhoc way of testing the Continuous Collision Detection algorithms
715  //One object is approximated as a sphere, to simplify things
716  //Starting in penetration should report no time of impact
717  //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
718  //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
719 
720 
722  {
723  btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
724 
725  btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
727  btVoronoiSimplexSolver voronoiSimplex;
728  //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
730  btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
731  //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
733  col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
734  {
735 
736  //store result.m_fraction in both bodies
737 
738  if (col0->getHitFraction()> result.m_fraction)
739  col0->setHitFraction( result.m_fraction );
740 
741  if (col1->getHitFraction() > result.m_fraction)
742  col1->setHitFraction( result.m_fraction);
743 
744  if (resultFraction > result.m_fraction)
745  resultFraction = result.m_fraction;
746 
747  }
748 
749 
750 
751 
752  }
753 
755  {
756  btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
757 
758  btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
760  btVoronoiSimplexSolver voronoiSimplex;
761  //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
763  btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
764  //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
766  col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
767  {
768 
769  //store result.m_fraction in both bodies
770 
771  if (col0->getHitFraction() > result.m_fraction)
772  col0->setHitFraction( result.m_fraction);
773 
774  if (col1->getHitFraction() > result.m_fraction)
775  col1->setHitFraction( result.m_fraction);
776 
777  if (resultFraction > result.m_fraction)
778  resultFraction = result.m_fraction;
779 
780  }
781  }
782 
783  return resultFraction;
784 
785 }
786 
virtual void releaseManifold(btPersistentManifold *manifold)=0
#define SIMD_EPSILON
Definition: btScalar.h:495
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
void push_back(const T &_Val)
btScalar getContactBreakingThreshold() const
#define BT_LARGE_FLOAT
Definition: btScalar.h:281
btScalar getCachedSeparatingDistance() const
btScalar getRadius() const
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
CreateFunc(btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *pdSolver)
static void clipFaceAgainstHull(const btVector3 &separatingNormal, const btConvexPolyhedron &hullA, const btTransform &transA, btVertexArray &worldVertsB1, btVertexArray &worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result &resultOut)
void setMinkowskiB(const btConvexShape *minkB)
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btScalar m_convexConservativeDistanceThreshold
Definition: btDispatcher.h:64
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:159
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
void setPersistentManifold(btPersistentManifold *manifoldPtr)
The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned ...
static void segmentsClosestPoints(btVector3 &ptsVector, btVector3 &offsetA, btVector3 &offsetB, btScalar &tA, btScalar &tB, const btVector3 &translation, const btVector3 &dirA, btScalar hlenA, const btVector3 &dirB, btScalar hlenB)
Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ra...
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1282
#define btAssert(x)
Definition: btScalar.h:114
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
void setHitFraction(btScalar hitFraction)
void setMinkowskiA(const btConvexShape *minkA)
static btScalar capsuleCapsuleDistance(btVector3 &normalOnB, btVector3 &pointOnB, btScalar capsuleLengthA, btScalar capsuleRadiusA, btScalar capsuleLengthB, btScalar capsuleRadiusB, int capsuleAxisA, int capsuleAxisB, const btTransform &transformA, const btTransform &transformB, btScalar distanceThreshold)
#define SIMD_FORCE_INLINE
Definition: btScalar.h:64
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
Definition: btSphereShape.h:22
btPersistentManifold * m_manifoldPtr
bool m_useConvexConservativeDistanceUtil
Definition: btDispatcher.h:63
btManifoldResult is a helper class to manage contact results.
class btIDebugDraw * m_debugDraw
Definition: btDispatcher.h:58
const btCollisionShape * getCollisionShape() const
btSimplexSolverInterface * m_simplexSolver
int getUpAxis() const
btSimplexSolverInterface * m_simplexSolver
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar orgDepth)
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
GjkConvexCast performs a raycast on a convex object using support mapping.
void setLowLevelOfDetail(bool useLowLevel)
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
const btVector3 & getCachedSeparatingAxis() const
virtual bool calcTimeOfImpact(const btTransform &fromA, const btTransform &toA, const btTransform &fromB, const btTransform &toB, CastResult &result)
cast a convex against another convex object
#define SIMD_PI
Definition: btScalar.h:477
btTransform & getWorldTransform()
btScalar getHalfHeight() const
RayResult stores the closest result alternatively, add a callback method to decide about closest/all ...
Definition: btConvexCast.h:36
#define SIMD_2_PI
Definition: btScalar.h:478
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
#define btSimplexSolverInterface
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
btScalar getCcdSquareMotionThreshold() const
const btCollisionObject * getCollisionObject() const
virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0)
optional method mainly used to generate multiple contact points by clipping polyhedral features (face...
btScalar gContactBreakingThreshold
btCollisionObject can be used to manage collision detection objects.
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
#define btRecipSqrt(x)
Definition: btScalar.h:484
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
const btTransform & getInterpolationWorldTransform() const
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
virtual btScalar getMargin() const =0
btPerturbedContactResult(btManifoldResult *originalResult, const btTransform &transformA, const btTransform &transformB, const btTransform &unPerturbedTransform, bool perturbA, btIDebugDraw *debugDrawer)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
bool isPolyhedral() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)=0
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btScalar getHitFraction() const
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
void resize(int newsize, const T &fillData=T())
const btTransform & getWorldTransform() const
static bool findSeparatingAxis(const btConvexPolyhedron &hullA, const btConvexPolyhedron &hullB, const btTransform &transA, const btTransform &transB, btVector3 &sep, btDiscreteCollisionDetectorInterface::Result &resultOut)
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
static void clipHullAgainstHull(const btVector3 &separatingNormal1, const btConvexPolyhedron &hullA, const btConvexPolyhedron &hullB, const btTransform &transA, const btTransform &transB, const btScalar minDist, btScalar maxDist, btVertexArray &worldVertsB1, btVertexArray &worldVertsB2, btDiscreteCollisionDetectorInterface::Result &resultOut)
btConvexPenetrationDepthSolver * m_pdSolver
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:848
int getShapeType() const
virtual btScalar getAngularMotionDisc() const
getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of...
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:899
btManifoldResult * m_originalManifoldResult
btScalar getCcdSweptSphereRadius() const
Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
const btPersistentManifold * getPersistentManifold() const
virtual void drawSphere(btScalar radius, const btTransform &transform, const btVector3 &color)
Definition: btIDebugDraw.h:93
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
btVector3 m_vertices1[3]
const btConvexPolyhedron * getConvexPolyhedron() const
btConvexPenetrationDepthSolver * m_pdSolver
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
const btCollisionShape * getCollisionShape() const
btConvexConvexAlgorithm(btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
cache separating vector to speedup collision detection
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
void setIgnoreMargin(bool ignoreMargin)
don&#39;t use setIgnoreMargin, it&#39;s for Bullet&#39;s internal use