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_pdSolver = pdSolver;
187 }
188 
190 {
191 }
192 
193 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
194 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
195 m_pdSolver(pdSolver),
196 m_ownManifold (false),
197 m_manifoldPtr(mf),
198 m_lowLevelOfDetail(false),
199 #ifdef USE_SEPDISTANCE_UTIL2
200 m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
201  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
202 #endif
203 m_numPerturbationIterations(numPerturbationIterations),
204 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
205 {
206  (void)body0Wrap;
207  (void)body1Wrap;
208 }
209 
210 
211 
212 
214 {
215  if (m_ownManifold)
216  {
217  if (m_manifoldPtr)
219  }
220 }
221 
223 {
224  m_lowLevelOfDetail = useLowLevel;
225 }
226 
227 
229 {
236 
237 
238  btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
239  :m_originalManifoldResult(originalResult),
240  m_transformA(transformA),
241  m_transformB(transformB),
242  m_unPerturbedTransform(unPerturbedTransform),
243  m_perturbA(perturbA),
244  m_debugDrawer(debugDrawer)
245  {
246  }
248  {
249  }
250 
251  virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
252  {
253  btVector3 endPt,startPt;
254  btScalar newDepth;
255  btVector3 newNormal;
256 
257  if (m_perturbA)
258  {
259  btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
260  endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
261  newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
262  startPt = endPt+normalOnBInWorld*newDepth;
263  } else
264  {
265  endPt = pointInWorld + normalOnBInWorld*orgDepth;
266  startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
267  newDepth = (endPt - startPt).dot(normalOnBInWorld);
268 
269  }
270 
271 //#define DEBUG_CONTACTS 1
272 #ifdef DEBUG_CONTACTS
273  m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
274  m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
275  m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
276 #endif //DEBUG_CONTACTS
277 
278 
279  m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
280  }
281 
282 };
283 
285 
286 
287 //
288 // Convex-Convex collision algorithm
289 //
291 {
292 
293  if (!m_manifoldPtr)
294  {
295  //swapped?
297  m_ownManifold = true;
298  }
300 
301  //comment-out next line to test multi-contact generation
302  //resultOut->getPersistentManifold()->clearManifold();
303 
304 
305  const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
306  const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
307 
308  btVector3 normalOnB;
309  btVector3 pointOnBWorld;
310 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
312  {
313  //m_manifoldPtr->clearManifold();
314 
315  btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
316  btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
317 
319 
320  btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
321  capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
322  body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
323 
324  if (dist<threshold)
325  {
326  btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
327  resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
328  }
329  resultOut->refreshContactPoints();
330  return;
331  }
332 
334  {
335  //m_manifoldPtr->clearManifold();
336 
337  btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
338  btSphereShape* capsuleB = (btSphereShape*) min1;
339 
341 
342  btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
343  0.,capsuleB->getRadius(),capsuleA->getUpAxis(),1,
344  body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
345 
346  if (dist<threshold)
347  {
348  btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
349  resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
350  }
351  resultOut->refreshContactPoints();
352  return;
353  }
354 
356  {
357  //m_manifoldPtr->clearManifold();
358 
359  btSphereShape* capsuleA = (btSphereShape*) min0;
360  btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
361 
363 
364  btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,0.,capsuleA->getRadius(),
365  capsuleB->getHalfHeight(),capsuleB->getRadius(),1,capsuleB->getUpAxis(),
366  body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
367 
368  if (dist<threshold)
369  {
370  btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
371  resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
372  }
373  resultOut->refreshContactPoints();
374  return;
375  }
376 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
377 
378 
379 
380 
381 #ifdef USE_SEPDISTANCE_UTIL2
382  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
383  {
384  m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
385  }
386 
387  if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
388 #endif //USE_SEPDISTANCE_UTIL2
389 
390  {
391 
392 
394  btVoronoiSimplexSolver simplexSolver;
395  btGjkPairDetector gjkPairDetector( min0, min1, &simplexSolver, m_pdSolver );
396  //TODO: if (dispatchInfo.m_useContinuous)
397  gjkPairDetector.setMinkowskiA(min0);
398  gjkPairDetector.setMinkowskiB(min1);
399 
400 #ifdef USE_SEPDISTANCE_UTIL2
401  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
402  {
404  } else
405 #endif //USE_SEPDISTANCE_UTIL2
406  {
407  //if (dispatchInfo.m_convexMaxDistanceUseCPT)
408  //{
409  // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
410  //} else
411  //{
413 // }
414 
416  }
417 
418  input.m_transformA = body0Wrap->getWorldTransform();
419  input.m_transformB = body1Wrap->getWorldTransform();
420 
421 
422 
423 
424 
425 #ifdef USE_SEPDISTANCE_UTIL2
426  btScalar sepDist = 0.f;
427  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
428  {
429  sepDist = gjkPairDetector.getCachedSeparatingDistance();
430  if (sepDist>SIMD_EPSILON)
431  {
432  sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
433  //now perturbe directions to get multiple contact points
434 
435  }
436  }
437 #endif //USE_SEPDISTANCE_UTIL2
438 
439  if (min0->isPolyhedral() && min1->isPolyhedral())
440  {
441 
442 
443  struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
444  {
445  virtual void setShapeIdentifiersA(int partId0,int index0){}
446  virtual void setShapeIdentifiersB(int partId1,int index1){}
447  virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
448  {
449  }
450  };
451 
452 
453  struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
454  {
456  btVector3 m_reportedNormalOnWorld;
457  btScalar m_marginOnA;
458  btScalar m_marginOnB;
459  btScalar m_reportedDistance;
460 
461  bool m_foundResult;
462  btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
463  :m_originalResult(result),
464  m_marginOnA(marginOnA),
465  m_marginOnB(marginOnB),
466  m_foundResult(false)
467  {
468  }
469 
470  virtual void setShapeIdentifiersA(int partId0,int index0){}
471  virtual void setShapeIdentifiersB(int partId1,int index1){}
472  virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg)
473  {
474  m_reportedDistance = depthOrg;
475  m_reportedNormalOnWorld = normalOnBInWorld;
476 
477  btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
478  m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
479  if (m_reportedDistance<0.f)
480  {
481  m_foundResult = true;
482  }
483  m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
484  }
485  };
486 
487 
488  btDummyResult dummy;
489 
491 
492  btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
493  btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
494 
495  btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin);
496 
497  btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
498  btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
499  if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
500  {
501 
502 
503 
504 
506 
507  btScalar minDist = -1e30f;
508  btVector3 sepNormalWorldSpace;
509  bool foundSepAxis = true;
510 
511  if (dispatchInfo.m_enableSatConvex)
512  {
514  *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
515  body0Wrap->getWorldTransform(),
516  body1Wrap->getWorldTransform(),
517  sepNormalWorldSpace,*resultOut);
518  } else
519  {
520 #ifdef ZERO_MARGIN
521  gjkPairDetector.setIgnoreMargin(true);
522  gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
523 #else
524 
525 
526  gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
527  //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
528 #endif //ZERO_MARGIN
529  //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
530  //if (l2>SIMD_EPSILON)
531  {
532  sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
533  //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
534  minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
535 
536 #ifdef ZERO_MARGIN
537  foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
538 #else
539  foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
540 #endif
541  }
542  }
543  if (foundSepAxis)
544  {
545 
546 // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
547 
548  worldVertsB1.resize(0);
549  btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
550  body0Wrap->getWorldTransform(),
551  body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2,
552  *resultOut);
553 
554  }
555  if (m_ownManifold)
556  {
557  resultOut->refreshContactPoints();
558  }
559  return;
560 
561  } else
562  {
563  //we can also deal with convex versus triangle (without connectivity data)
564  if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
565  {
566 
567  btVertexArray vertices;
568  btTriangleShape* tri = (btTriangleShape*)polyhedronB;
569  vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
570  vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
571  vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
572 
573  //tri->initializePolyhedralFeatures();
574 
576 
577  btVector3 sepNormalWorldSpace;
578  btScalar minDist =-1e30f;
579  btScalar maxDist = threshold;
580 
581  bool foundSepAxis = false;
582  if (0)
583  {
584  polyhedronB->initializePolyhedralFeatures();
586  *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
587  body0Wrap->getWorldTransform(),
588  body1Wrap->getWorldTransform(),
589  sepNormalWorldSpace,*resultOut);
590  // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
591 
592  } else
593  {
594 #ifdef ZERO_MARGIN
595  gjkPairDetector.setIgnoreMargin(true);
596  gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
597 #else
598  gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
599 #endif//ZERO_MARGIN
600 
601  btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
602  if (l2>SIMD_EPSILON)
603  {
604  sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
605  //minDist = gjkPairDetector.getCachedSeparatingDistance();
606  //maxDist = threshold;
607  minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
608  foundSepAxis = true;
609  }
610  }
611 
612 
613  if (foundSepAxis)
614  {
615  worldVertsB2.resize(0);
616  btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
617  body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
618  }
619 
620 
621  if (m_ownManifold)
622  {
623  resultOut->refreshContactPoints();
624  }
625 
626  return;
627  }
628 
629  }
630 
631 
632  }
633 
634  gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
635 
636  //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
637 
638  //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
640  {
641 
642  int i;
643  btVector3 v0,v1;
644  btVector3 sepNormalWorldSpace;
645  btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
646 
647  if (l2>SIMD_EPSILON)
648  {
649  sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
650 
651  btPlaneSpace1(sepNormalWorldSpace,v0,v1);
652 
653 
654  bool perturbeA = true;
655  const btScalar angleLimit = 0.125f * SIMD_PI;
656  btScalar perturbeAngle;
657  btScalar radiusA = min0->getAngularMotionDisc();
658  btScalar radiusB = min1->getAngularMotionDisc();
659  if (radiusA < radiusB)
660  {
661  perturbeAngle = gContactBreakingThreshold /radiusA;
662  perturbeA = true;
663  } else
664  {
665  perturbeAngle = gContactBreakingThreshold / radiusB;
666  perturbeA = false;
667  }
668  if ( perturbeAngle > angleLimit )
669  perturbeAngle = angleLimit;
670 
671  btTransform unPerturbedTransform;
672  if (perturbeA)
673  {
674  unPerturbedTransform = input.m_transformA;
675  } else
676  {
677  unPerturbedTransform = input.m_transformB;
678  }
679 
680  for ( i=0;i<m_numPerturbationIterations;i++)
681  {
682  if (v0.length2()>SIMD_EPSILON)
683  {
684  btQuaternion perturbeRot(v0,perturbeAngle);
685  btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
686  btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
687 
688 
689  if (perturbeA)
690  {
691  input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
692  input.m_transformB = body1Wrap->getWorldTransform();
693  #ifdef DEBUG_CONTACTS
694  dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
695  #endif //DEBUG_CONTACTS
696  } else
697  {
698  input.m_transformA = body0Wrap->getWorldTransform();
699  input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
700  #ifdef DEBUG_CONTACTS
701  dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
702  #endif
703  }
704 
705  btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
706  gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
707  }
708  }
709  }
710  }
711 
712 
713 
714 #ifdef USE_SEPDISTANCE_UTIL2
715  if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
716  {
717  m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
718  }
719 #endif //USE_SEPDISTANCE_UTIL2
720 
721 
722  }
723 
724  if (m_ownManifold)
725  {
726  resultOut->refreshContactPoints();
727  }
728 
729 }
730 
731 
732 
733 bool disableCcd = false;
735 {
736  (void)resultOut;
737  (void)dispatchInfo;
739 
742  btScalar resultFraction = btScalar(1.);
743 
744 
745  btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
746  btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
747 
748  if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
749  squareMot1 < col1->getCcdSquareMotionThreshold())
750  return resultFraction;
751 
752  if (disableCcd)
753  return btScalar(1.);
754 
755 
756  //An adhoc way of testing the Continuous Collision Detection algorithms
757  //One object is approximated as a sphere, to simplify things
758  //Starting in penetration should report no time of impact
759  //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
760  //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
761 
762 
764  {
765  btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
766 
767  btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
769  btVoronoiSimplexSolver voronoiSimplex;
770  //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
772  btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
773  //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
775  col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
776  {
777 
778  //store result.m_fraction in both bodies
779 
780  if (col0->getHitFraction()> result.m_fraction)
781  col0->setHitFraction( result.m_fraction );
782 
783  if (col1->getHitFraction() > result.m_fraction)
784  col1->setHitFraction( result.m_fraction);
785 
786  if (resultFraction > result.m_fraction)
787  resultFraction = result.m_fraction;
788 
789  }
790 
791 
792 
793 
794  }
795 
797  {
798  btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
799 
800  btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
802  btVoronoiSimplexSolver voronoiSimplex;
803  //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
805  btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
806  //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
808  col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
809  {
810 
811  //store result.m_fraction in both bodies
812 
813  if (col0->getHitFraction() > result.m_fraction)
814  col0->setHitFraction( result.m_fraction);
815 
816  if (col1->getHitFraction() > result.m_fraction)
817  col1->setHitFraction( result.m_fraction);
818 
819  if (resultFraction > result.m_fraction)
820  resultFraction = result.m_fraction;
821 
822  }
823  }
824 
825  return resultFraction;
826 
827 }
828 
virtual void releaseManifold(btPersistentManifold *manifold)=0
#define SIMD_EPSILON
Definition: btScalar.h:521
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:294
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)
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:1283
#define btAssert(x)
Definition: btScalar.h:131
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:81
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
btConvexConvexAlgorithm(btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btConvexPenetrationDepthSolver *pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
cache separating vector to speedup collision detection
const btCollisionShape * getCollisionShape() const
int getUpAxis() const
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:504
btTransform & getWorldTransform()
btScalar getHalfHeight() const
RayResult stores the closest result alternatively, add a callback method to decide about closest/all ...
Definition: btConvexCast.h:36
btScalar m_closestPointDistanceThreshold
#define SIMD_2_PI
Definition: btScalar.h:505
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
btScalar getCcdSquareMotionThreshold() const
btScalar getRadius() const
Definition: btSphereShape.h:50
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:510
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
CreateFunc(btConvexPenetrationDepthSolver *pdSolver)
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:878
int getShapeType() const
virtual btScalar getAngularMotionDisc() const
getAngularMotionDisc returns the maximum 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:901
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:292
const btCollisionShape * getCollisionShape() const
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