Blender  V3.3
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>
32 
38 
43 
46 
48 
53 
55 
57  btVector3& ptsVector,
58  btVector3& offsetA,
59  btVector3& offsetB,
60  btScalar& tA, btScalar& tB,
61  const btVector3& translation,
62  const btVector3& dirA, btScalar hlenA,
63  const btVector3& dirB, btScalar hlenB)
64 {
65  // compute the parameters of the closest points on each line segment
66 
67  btScalar dirA_dot_dirB = btDot(dirA, dirB);
68  btScalar dirA_dot_trans = btDot(dirA, translation);
69  btScalar dirB_dot_trans = btDot(dirB, translation);
70 
71  btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
72 
73  if (denom == 0.0f)
74  {
75  tA = 0.0f;
76  }
77  else
78  {
79  tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom;
80  if (tA < -hlenA)
81  tA = -hlenA;
82  else if (tA > hlenA)
83  tA = hlenA;
84  }
85 
86  tB = tA * dirA_dot_dirB - dirB_dot_trans;
87 
88  if (tB < -hlenB)
89  {
90  tB = -hlenB;
91  tA = tB * dirA_dot_dirB + dirA_dot_trans;
92 
93  if (tA < -hlenA)
94  tA = -hlenA;
95  else if (tA > hlenA)
96  tA = hlenA;
97  }
98  else if (tB > hlenB)
99  {
100  tB = hlenB;
101  tA = tB * dirA_dot_dirB + dirA_dot_trans;
102 
103  if (tA < -hlenA)
104  tA = -hlenA;
105  else if (tA > hlenA)
106  tA = hlenA;
107  }
108 
109  // compute the closest points relative to segment centers.
110 
111  offsetA = dirA * tA;
112  offsetB = dirB * tB;
113 
114  ptsVector = translation - offsetA + offsetB;
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  }
161  else
162  {
163  // compute the contact normal
164  normalOnB = ptsVector * -btRecipSqrt(lenSqr);
165  }
166  pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB;
167 
168  return distance;
169 }
170 
172 
174 {
177  m_pdSolver = pdSolver;
178 }
179 
181 {
182 }
183 
184 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
185  : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
186  m_pdSolver(pdSolver),
187  m_ownManifold(false),
188  m_manifoldPtr(mf),
189  m_lowLevelOfDetail(false),
190 #ifdef USE_SEPDISTANCE_UTIL2
191  m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
192  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
193 #endif
194  m_numPerturbationIterations(numPerturbationIterations),
195  m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
196 {
197  (void)body0Wrap;
198  (void)body1Wrap;
199 }
200 
202 {
203  if (m_ownManifold)
204  {
205  if (m_manifoldPtr)
206  m_dispatcher->releaseManifold(m_manifoldPtr);
207  }
208 }
209 
211 {
212  m_lowLevelOfDetail = useLowLevel;
213 }
214 
216 {
223 
224  btPerturbedContactResult(btManifoldResult* originalResult, const btTransform& transformA, const btTransform& transformB, const btTransform& unPerturbedTransform, bool perturbA, btIDebugDraw* debugDrawer)
225  : m_originalManifoldResult(originalResult),
226  m_transformA(transformA),
227  m_transformB(transformB),
228  m_unPerturbedTransform(unPerturbedTransform),
229  m_perturbA(perturbA),
230  m_debugDrawer(debugDrawer)
231  {
232  }
234  {
235  }
236 
237  virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar orgDepth)
238  {
239  btVector3 endPt, startPt;
240  btScalar newDepth;
241  btVector3 newNormal;
242 
243  if (m_perturbA)
244  {
245  btVector3 endPtOrg = pointInWorld + normalOnBInWorld * orgDepth;
246  endPt = (m_unPerturbedTransform * m_transformA.inverse())(endPtOrg);
247  newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
248  startPt = endPt - normalOnBInWorld * newDepth;
249  }
250  else
251  {
252  endPt = pointInWorld + normalOnBInWorld * orgDepth;
253  startPt = (m_unPerturbedTransform * m_transformB.inverse())(pointInWorld);
254  newDepth = (endPt - startPt).dot(normalOnBInWorld);
255  }
256 
257 //#define DEBUG_CONTACTS 1
258 #ifdef DEBUG_CONTACTS
259  m_debugDrawer->drawLine(startPt, endPt, btVector3(1, 0, 0));
260  m_debugDrawer->drawSphere(startPt, 0.05, btVector3(0, 1, 0));
261  m_debugDrawer->drawSphere(endPt, 0.05, btVector3(0, 0, 1));
262 #endif //DEBUG_CONTACTS
263 
264  m_originalManifoldResult->addContactPoint(normalOnBInWorld, startPt, newDepth);
265  }
266 };
267 
269 
270 //
271 // Convex-Convex collision algorithm
272 //
274 {
275  if (!m_manifoldPtr)
276  {
277  //swapped?
278  m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
279  m_ownManifold = true;
280  }
281  resultOut->setPersistentManifold(m_manifoldPtr);
282 
283  //comment-out next line to test multi-contact generation
284  //resultOut->getPersistentManifold()->clearManifold();
285 
286  const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
287  const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
288 
289  btVector3 normalOnB;
290  btVector3 pointOnBWorld;
291 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
292  if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
293  {
294  //m_manifoldPtr->clearManifold();
295 
296  btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
297  btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
298 
299  btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
300 
301  btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
302  capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(),
303  body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold);
304 
305  if (dist < threshold)
306  {
307  btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON));
308  resultOut->addContactPoint(normalOnB, pointOnBWorld, dist);
309  }
310  resultOut->refreshContactPoints();
311  return;
312  }
313 
314  if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == SPHERE_SHAPE_PROXYTYPE))
315  {
316  //m_manifoldPtr->clearManifold();
317 
318  btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
319  btSphereShape* capsuleB = (btSphereShape*)min1;
320 
321  btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
322 
323  btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
324  0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1,
325  body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold);
326 
327  if (dist < threshold)
328  {
329  btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON));
330  resultOut->addContactPoint(normalOnB, pointOnBWorld, dist);
331  }
332  resultOut->refreshContactPoints();
333  return;
334  }
335 
336  if ((min0->getShapeType() == SPHERE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
337  {
338  //m_manifoldPtr->clearManifold();
339 
340  btSphereShape* capsuleA = (btSphereShape*)min0;
341  btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
342 
343  btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
344 
345  btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, 0., capsuleA->getRadius(),
346  capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(),
347  body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold);
348 
349  if (dist < threshold)
350  {
351  btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON));
352  resultOut->addContactPoint(normalOnB, pointOnBWorld, dist);
353  }
354  resultOut->refreshContactPoints();
355  return;
356  }
357 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
358 
359 #ifdef USE_SEPDISTANCE_UTIL2
360  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
361  {
362  m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform());
363  }
364 
365  if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f)
366 #endif //USE_SEPDISTANCE_UTIL2
367 
368  {
370  btVoronoiSimplexSolver simplexSolver;
371  btGjkPairDetector gjkPairDetector(min0, min1, &simplexSolver, m_pdSolver);
372  //TODO: if (dispatchInfo.m_useContinuous)
373  gjkPairDetector.setMinkowskiA(min0);
374  gjkPairDetector.setMinkowskiB(min1);
375 
376 #ifdef USE_SEPDISTANCE_UTIL2
377  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
378  {
379  input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
380  }
381  else
382 #endif //USE_SEPDISTANCE_UTIL2
383  {
384  //if (dispatchInfo.m_convexMaxDistanceUseCPT)
385  //{
386  // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
387  //} else
388  //{
389  input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold;
390  // }
391 
392  input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
393  }
394 
395  input.m_transformA = body0Wrap->getWorldTransform();
396  input.m_transformB = body1Wrap->getWorldTransform();
397 
398 #ifdef USE_SEPDISTANCE_UTIL2
399  btScalar sepDist = 0.f;
400  if (dispatchInfo.m_useConvexConservativeDistanceUtil)
401  {
402  sepDist = gjkPairDetector.getCachedSeparatingDistance();
403  if (sepDist > SIMD_EPSILON)
404  {
405  sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
406  //now perturbe directions to get multiple contact points
407  }
408  }
409 #endif //USE_SEPDISTANCE_UTIL2
410 
411  if (min0->isPolyhedral() && min1->isPolyhedral())
412  {
413  struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
414  {
415  btVector3 m_normalOnBInWorld;
416  btVector3 m_pointInWorld;
418  bool m_hasContact;
419 
420  btDummyResult()
421  : m_hasContact(false)
422  {
423  }
424 
425  virtual void setShapeIdentifiersA(int partId0, int index0) {}
426  virtual void setShapeIdentifiersB(int partId1, int index1) {}
427  virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
428  {
429  m_hasContact = true;
430  m_normalOnBInWorld = normalOnBInWorld;
431  m_pointInWorld = pointInWorld;
432  m_depth = depth;
433  }
434  };
435 
436  struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
437  {
439  btVector3 m_reportedNormalOnWorld;
440  btScalar m_marginOnA;
441  btScalar m_marginOnB;
442  btScalar m_reportedDistance;
443 
444  bool m_foundResult;
445  btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
446  : m_originalResult(result),
447  m_marginOnA(marginOnA),
448  m_marginOnB(marginOnB),
449  m_foundResult(false)
450  {
451  }
452 
453  virtual void setShapeIdentifiersA(int partId0, int index0) {}
454  virtual void setShapeIdentifiersB(int partId1, int index1) {}
455  virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg)
456  {
457  m_reportedDistance = depthOrg;
458  m_reportedNormalOnWorld = normalOnBInWorld;
459 
460  btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld * m_marginOnB;
461  m_reportedDistance = depthOrg + (m_marginOnA + m_marginOnB);
462  if (m_reportedDistance < 0.f)
463  {
464  m_foundResult = true;
465  }
466  m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance);
467  }
468  };
469 
470  btDummyResult dummy;
471 
473 
474  btScalar min0Margin = min0->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min0->getMargin();
475  btScalar min1Margin = min1->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min1->getMargin();
476 
477  btWithoutMarginResult withoutMargin(resultOut, min0Margin, min1Margin);
478 
479  btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*)min0;
480  btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*)min1;
481  if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
482  {
483  btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
484 
485  btScalar minDist = -1e30f;
486  btVector3 sepNormalWorldSpace;
487  bool foundSepAxis = true;
488 
489  if (dispatchInfo.m_enableSatConvex)
490  {
492  *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
493  body0Wrap->getWorldTransform(),
494  body1Wrap->getWorldTransform(),
495  sepNormalWorldSpace, *resultOut);
496  }
497  else
498  {
499 #ifdef ZERO_MARGIN
500  gjkPairDetector.setIgnoreMargin(true);
501  gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
502 #else
503 
504  gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw);
505  //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
506 #endif //ZERO_MARGIN
507  //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
508  //if (l2>SIMD_EPSILON)
509  {
510  sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld; //gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
511  //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
512  minDist = withoutMargin.m_reportedDistance; //gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
513 
514 #ifdef ZERO_MARGIN
515  foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f;
516 #else
517  foundSepAxis = withoutMargin.m_foundResult && minDist < 0; //-(min0->getMargin()+min1->getMargin());
518 #endif
519  }
520  }
521  if (foundSepAxis)
522  {
523  // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
524 
525  worldVertsB1.resize(0);
526  btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
527  body0Wrap->getWorldTransform(),
528  body1Wrap->getWorldTransform(), minDist - threshold, threshold, worldVertsB1, worldVertsB2,
529  *resultOut);
530  }
531  if (m_ownManifold)
532  {
533  resultOut->refreshContactPoints();
534  }
535  return;
536  }
537  else
538  {
539  //we can also deal with convex versus triangle (without connectivity data)
540  if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE)
541  {
542  btVertexArray worldSpaceVertices;
543  btTriangleShape* tri = (btTriangleShape*)polyhedronB;
544  worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[0]);
545  worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[1]);
546  worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[2]);
547 
548  //tri->initializePolyhedralFeatures();
549 
550  btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
551 
552  btVector3 sepNormalWorldSpace;
553  btScalar minDist = -1e30f;
554  btScalar maxDist = threshold;
555 
556  bool foundSepAxis = false;
557  bool useSatSepNormal = true;
558 
559  if (useSatSepNormal)
560  {
561 #if 0
562  if (0)
563  {
564  //initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle
565  polyhedronB->initializePolyhedralFeatures();
566  } else
567 #endif
568  {
569  btVector3 uniqueEdges[3] = {tri->m_vertices1[1] - tri->m_vertices1[0],
570  tri->m_vertices1[2] - tri->m_vertices1[1],
571  tri->m_vertices1[0] - tri->m_vertices1[2]};
572 
573  uniqueEdges[0].normalize();
574  uniqueEdges[1].normalize();
575  uniqueEdges[2].normalize();
576 
577  btConvexPolyhedron polyhedron;
578  polyhedron.m_vertices.push_back(tri->m_vertices1[2]);
579  polyhedron.m_vertices.push_back(tri->m_vertices1[0]);
580  polyhedron.m_vertices.push_back(tri->m_vertices1[1]);
581 
582  {
583  btFace combinedFaceA;
584  combinedFaceA.m_indices.push_back(0);
585  combinedFaceA.m_indices.push_back(1);
586  combinedFaceA.m_indices.push_back(2);
587  btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]);
588  faceNormal.normalize();
589  btScalar planeEq = 1e30f;
590  for (int v = 0; v < combinedFaceA.m_indices.size(); v++)
591  {
592  btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal);
593  if (planeEq > eq)
594  {
595  planeEq = eq;
596  }
597  }
598  combinedFaceA.m_plane[0] = faceNormal[0];
599  combinedFaceA.m_plane[1] = faceNormal[1];
600  combinedFaceA.m_plane[2] = faceNormal[2];
601  combinedFaceA.m_plane[3] = -planeEq;
602  polyhedron.m_faces.push_back(combinedFaceA);
603  }
604  {
605  btFace combinedFaceB;
606  combinedFaceB.m_indices.push_back(0);
607  combinedFaceB.m_indices.push_back(2);
608  combinedFaceB.m_indices.push_back(1);
609  btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]);
610  faceNormal.normalize();
611  btScalar planeEq = 1e30f;
612  for (int v = 0; v < combinedFaceB.m_indices.size(); v++)
613  {
614  btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal);
615  if (planeEq > eq)
616  {
617  planeEq = eq;
618  }
619  }
620 
621  combinedFaceB.m_plane[0] = faceNormal[0];
622  combinedFaceB.m_plane[1] = faceNormal[1];
623  combinedFaceB.m_plane[2] = faceNormal[2];
624  combinedFaceB.m_plane[3] = -planeEq;
625  polyhedron.m_faces.push_back(combinedFaceB);
626  }
627 
628  polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]);
629  polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]);
630  polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]);
631  polyhedron.initialize2();
632 
633  polyhedronB->setPolyhedralFeatures(polyhedron);
634  }
635 
637  *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
638  body0Wrap->getWorldTransform(),
639  body1Wrap->getWorldTransform(),
640  sepNormalWorldSpace, *resultOut);
641  // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
642  }
643  else
644  {
645 #ifdef ZERO_MARGIN
646  gjkPairDetector.setIgnoreMargin(true);
647  gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
648 #else
649  gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw);
650 #endif //ZERO_MARGIN
651 
652  if (dummy.m_hasContact && dummy.m_depth < 0)
653  {
654  if (foundSepAxis)
655  {
656  if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace) < 0.99)
657  {
658  printf("?\n");
659  }
660  }
661  else
662  {
663  printf("!\n");
664  }
665  sepNormalWorldSpace.setValue(0, 0, 1); // = dummy.m_normalOnBInWorld;
666  //minDist = dummy.m_depth;
667  foundSepAxis = true;
668  }
669 #if 0
670  btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
671  if (l2>SIMD_EPSILON)
672  {
673  sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
674  //minDist = gjkPairDetector.getCachedSeparatingDistance();
675  //maxDist = threshold;
676  minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
677  foundSepAxis = true;
678  }
679 #endif
680  }
681 
682  if (foundSepAxis)
683  {
684  worldVertsB2.resize(0);
685  btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
686  body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2, minDist - threshold, maxDist, *resultOut);
687  }
688 
689  if (m_ownManifold)
690  {
691  resultOut->refreshContactPoints();
692  }
693 
694  return;
695  }
696  }
697  }
698 
699  gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
700 
701  //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
702 
703  //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
704  if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
705  {
706  int i;
707  btVector3 v0, v1;
708  btVector3 sepNormalWorldSpace;
709  btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
710 
711  if (l2 > SIMD_EPSILON)
712  {
713  sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis() * (1.f / l2);
714 
715  btPlaneSpace1(sepNormalWorldSpace, v0, v1);
716 
717  bool perturbeA = true;
718  const btScalar angleLimit = 0.125f * SIMD_PI;
719  btScalar perturbeAngle;
720  btScalar radiusA = min0->getAngularMotionDisc();
721  btScalar radiusB = min1->getAngularMotionDisc();
722  if (radiusA < radiusB)
723  {
724  perturbeAngle = gContactBreakingThreshold / radiusA;
725  perturbeA = true;
726  }
727  else
728  {
729  perturbeAngle = gContactBreakingThreshold / radiusB;
730  perturbeA = false;
731  }
732  if (perturbeAngle > angleLimit)
733  perturbeAngle = angleLimit;
734 
735  btTransform unPerturbedTransform;
736  if (perturbeA)
737  {
738  unPerturbedTransform = input.m_transformA;
739  }
740  else
741  {
742  unPerturbedTransform = input.m_transformB;
743  }
744 
745  for (i = 0; i < m_numPerturbationIterations; i++)
746  {
747  if (v0.length2() > SIMD_EPSILON)
748  {
749  btQuaternion perturbeRot(v0, perturbeAngle);
750  btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations));
751  btQuaternion rotq(sepNormalWorldSpace, iterationAngle);
752 
753  if (perturbeA)
754  {
755  input.m_transformA.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0Wrap->getWorldTransform().getBasis());
756  input.m_transformB = body1Wrap->getWorldTransform();
757 #ifdef DEBUG_CONTACTS
758  dispatchInfo.m_debugDraw->drawTransform(input.m_transformA, 10.0);
759 #endif //DEBUG_CONTACTS
760  }
761  else
762  {
763  input.m_transformA = body0Wrap->getWorldTransform();
764  input.m_transformB.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1Wrap->getWorldTransform().getBasis());
765 #ifdef DEBUG_CONTACTS
766  dispatchInfo.m_debugDraw->drawTransform(input.m_transformB, 10.0);
767 #endif
768  }
769 
770  btPerturbedContactResult perturbedResultOut(resultOut, input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw);
771  gjkPairDetector.getClosestPoints(input, perturbedResultOut, dispatchInfo.m_debugDraw);
772  }
773  }
774  }
775  }
776 
777 #ifdef USE_SEPDISTANCE_UTIL2
778  if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > SIMD_EPSILON))
779  {
780  m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0->getWorldTransform(), body1->getWorldTransform());
781  }
782 #endif //USE_SEPDISTANCE_UTIL2
783  }
784 
785  if (m_ownManifold)
786  {
787  resultOut->refreshContactPoints();
788  }
789 }
790 
791 bool disableCcd = false;
793 {
794  (void)resultOut;
795  (void)dispatchInfo;
797 
800  btScalar resultFraction = btScalar(1.);
801 
802  btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
803  btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
804 
805  if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
806  squareMot1 < col1->getCcdSquareMotionThreshold())
807  return resultFraction;
808 
809  if (disableCcd)
810  return btScalar(1.);
811 
812  //An adhoc way of testing the Continuous Collision Detection algorithms
813  //One object is approximated as a sphere, to simplify things
814  //Starting in penetration should report no time of impact
815  //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
816  //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
817 
819  {
820  btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
821 
822  btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
824  btVoronoiSimplexSolver voronoiSimplex;
825  //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
827  btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex);
828  //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
829  if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
830  col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
831  {
832  //store result.m_fraction in both bodies
833 
834  if (col0->getHitFraction() > result.m_fraction)
835  col0->setHitFraction(result.m_fraction);
836 
837  if (col1->getHitFraction() > result.m_fraction)
838  col1->setHitFraction(result.m_fraction);
839 
840  if (resultFraction > result.m_fraction)
841  resultFraction = result.m_fraction;
842  }
843  }
844 
846  {
847  btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
848 
849  btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
851  btVoronoiSimplexSolver voronoiSimplex;
852  //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
854  btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex);
855  //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
856  if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
857  col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
858  {
859  //store result.m_fraction in both bodies
860 
861  if (col0->getHitFraction() > result.m_fraction)
862  col0->setHitFraction(result.m_fraction);
863 
864  if (col1->getHitFraction() > result.m_fraction)
865  col1->setHitFraction(result.m_fraction);
866 
867  if (resultFraction > result.m_fraction)
868  resultFraction = result.m_fraction;
869  }
870  }
871 
872  return resultFraction;
873 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble v1
ATTR_WARN_UNUSED_RESULT const BMVert * v
@ TRIANGLE_SHAPE_PROXYTYPE
@ SPHERE_SHAPE_PROXYTYPE
@ BOX_SHAPE_PROXYTYPE
@ CAPSULE_SHAPE_PROXYTYPE
btCapsuleShape()
only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
btCollisionObject
btScalar getCcdSquareMotionThreshold() const
SIMD_FORCE_INLINE const btCollisionShape * getCollisionShape() const
virtual btScalar getAngularMotionDisc() const
getAngularMotionDisc returns the maximum radius needed for Conservative Advancement to handle time-of...
static SIMD_FORCE_INLINE 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)
btScalar gContactBreakingThreshold
static SIMD_FORCE_INLINE 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)
btConvexPolyhedron
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btPersistentManifold()
btPolyhedralConvexShape()
#define SIMD_PI
Definition: btScalar.h:526
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btRecipSqrt(x)
Definition: btScalar.h:532
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
#define SIMD_FORCE_INLINE
Definition: btScalar.h:280
#define SIMD_EPSILON
Definition: btScalar.h:543
#define SIMD_2_PI
Definition: btScalar.h:527
#define btAssert(x)
Definition: btScalar.h:295
btVector3 m_depth
btSphereShape(btScalar radius)
Definition: btSphereShape.h:29
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTriangleShape()
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
SIMD_FORCE_INLINE btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
SIMD_FORCE_INLINE btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
btVoronoiSimplexSolver
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE void push_back(const T &_Val)
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
void setLowLevelOfDetail(bool useLowLevel)
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
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
virtual void releaseManifold(btPersistentManifold *manifold)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
GjkConvexCast performs a raycast on a convex object using support mapping.
virtual bool calcTimeOfImpact(const btTransform &fromA, const btTransform &toA, const btTransform &fromB, const btTransform &toB, CastResult &result)
cast a convex against another convex object
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
btScalar getCachedSeparatingDistance() const
void setIgnoreMargin(bool ignoreMargin)
don't use setIgnoreMargin, it's for Bullet's internal use
void setMinkowskiA(const btConvexShape *minkA)
const btVector3 & getCachedSeparatingAxis() const
void setMinkowskiB(const btConvexShape *minkB)
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual void drawSphere(btScalar radius, const btTransform &transform, const btVector3 &color)
Definition: btIDebugDraw.h:92
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
btManifoldResult is a helper class to manage contact results.
const btPersistentManifold * getPersistentManifold() const
void setPersistentManifold(btPersistentManifold *manifoldPtr)
btScalar m_closestPointDistanceThreshold
SIMD_FORCE_INLINE void refreshContactPoints()
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
SyclQueue void void size_t num_bytes void
ccl_gpu_kernel_postfix ccl_global float int int int int float threshold
ccl_global KernelShaderEvalInput * input
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
T distance(const T &a, const T &b)
SIMD_FORCE_INLINE const btCollisionShape * getCollisionShape() const
SIMD_FORCE_INLINE const btTransform & getWorldTransform() const
SIMD_FORCE_INLINE const btCollisionObject * getCollisionObject() const
CreateFunc(btConvexPenetrationDepthSolver *pdSolver)
btConvexPenetrationDepthSolver * m_pdSolver
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)=0
bool m_useConvexConservativeDistanceUtil
Definition: btDispatcher.h:63
btScalar m_convexConservativeDistanceThreshold
Definition: btDispatcher.h:64
class btIDebugDraw * m_debugDraw
Definition: btDispatcher.h:58
btAlignedObjectArray< int > m_indices
btScalar m_plane[4]
btManifoldResult * m_originalManifoldResult
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar orgDepth)
btPerturbedContactResult(btManifoldResult *originalResult, const btTransform &transformA, const btTransform &transformB, const btTransform &unPerturbedTransform, bool perturbA, btIDebugDraw *debugDrawer)
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)
static void clipFaceAgainstHull(const btVector3 &separatingNormal, const btConvexPolyhedron &hullA, const btTransform &transA, btVertexArray &worldVertsB1, btVertexArray &worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result &resultOut)
static bool findSeparatingAxis(const btConvexPolyhedron &hullA, const btConvexPolyhedron &hullB, const btTransform &transA, const btTransform &transB, btVector3 &sep, btDiscreteCollisionDetectorInterface::Result &resultOut)