dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 ///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
  17 ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
  18 ///with reproduction case
  19 //#define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
  20 //#define ZERO_MARGIN
  21
  22 #include "btConvexConvexAlgorithm.h"
  23
  24 //#include <stdio.h>
  25 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
  26 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
  27 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  28 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  29 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
  30 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
  31 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
  32
  33 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
  34 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
  35 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  36 #include "BulletCollision/CollisionShapes/btBoxShape.h"
  37 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
  38
  39 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
  40 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
  41 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  42 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
  43
  44 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  45 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  46
  47 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
  48
  49 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
  50 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
  51 #include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
  52 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  53
  54 ///////////
  55
  56 static SIMD_FORCE_INLINE void segmentsClosestPoints(
  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
 117 static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
 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
 171 //////////
 172
 173 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver)
 174 {
 175         m_numPerturbationIterations = 0;
 176         m_minimumPointsPerturbationThreshold = 3;
 177         m_pdSolver = pdSolver;
 178 }
 179
 180 btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
 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
 201 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
 202 {
 203         if (m_ownManifold)
 204         {
 205                 if (m_manifoldPtr)
 206                         m_dispatcher->releaseManifold(m_manifoldPtr);
 207         }
 208 }
 209
 210 void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
 211 {
 212         m_lowLevelOfDetail = useLowLevel;
 213 }
 214
 215 struct btPerturbedContactResult : public btManifoldResult
 216 {
 217         btManifoldResult* m_originalManifoldResult;
 218         btTransform m_transformA;
 219         btTransform m_transformB;
 220         btTransform m_unPerturbedTransform;
 221         bool m_perturbA;
 222         btIDebugDraw* m_debugDrawer;
 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         }
 233         virtual ~btPerturbedContactResult()
 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
 268 extern btScalar gContactBreakingThreshold;
 269
 270 //
 271 // Convex-Convex collision algorithm
 272 //
 273 void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 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         {
 369                 btGjkPairDetector::ClosestPointInput input;
 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;
 417                                 btScalar m_depth;
 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                         {
 438                                 btDiscreteCollisionDetectorInterface::Result* m_originalResult;
 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
 472                         ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
 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                                 {
 491                                         foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
 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
 636                                                 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
 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;
 792 btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 793 {
 794         (void)resultOut;
 795         (void)dispatchInfo;
 796         ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
 797
 798         ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
 799         ///col0->m_worldTransform,
 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
 818         /// Convex0 against sphere for Convex1
 819         {
 820                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
 821
 822                 btSphereShape sphere1(col1->getCcdSweptSphereRadius());  //todo: allow non-zero sphere sizes, for better approximation
 823                 btConvexCast::CastResult result;
 824                 btVoronoiSimplexSolver voronoiSimplex;
 825                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 826                 ///Simplification, one object is simplified as a sphere
 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
 845         /// Sphere (for convex0) against Convex1
 846         {
 847                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
 848
 849                 btSphereShape sphere0(col0->getCcdSweptSphereRadius());  //todo: allow non-zero sphere sizes, for better approximation
 850                 btConvexCast::CastResult result;
 851                 btVoronoiSimplexSolver voronoiSimplex;
 852                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 853                 ///Simplification, one object is simplified as a sphere
 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 }