dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.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 #include "btConvex2dConvex2dAlgorithm.h"
  17
  18 //#include <stdio.h>
  19 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
  20 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
  21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  22 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  23 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
  24
  25 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
  26 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
  27 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  28 #include "BulletCollision/CollisionShapes/btBoxShape.h"
  29 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
  30
  31 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
  32 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
  33 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  34 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
  35
  36 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  37 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  38
  39 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
  40
  41 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
  42 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
  43 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  44
  45 btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
  46 {
  47         m_simplexSolver = simplexSolver;
  48         m_pdSolver = pdSolver;
  49 }
  50
  51 btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
  52 {
  53 }
  54
  55 btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
  56         : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
  57           m_simplexSolver(simplexSolver),
  58           m_pdSolver(pdSolver),
  59           m_ownManifold(false),
  60           m_manifoldPtr(mf),
  61           m_lowLevelOfDetail(false)
  62 {
  63         (void)body0Wrap;
  64         (void)body1Wrap;
  65 }
  66
  67 btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
  68 {
  69         if (m_ownManifold)
  70         {
  71                 if (m_manifoldPtr)
  72                         m_dispatcher->releaseManifold(m_manifoldPtr);
  73         }
  74 }
  75
  76 void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
  77 {
  78         m_lowLevelOfDetail = useLowLevel;
  79 }
  80
  81 extern btScalar gContactBreakingThreshold;
  82
  83 //
  84 // Convex-Convex collision algorithm
  85 //
  86 void btConvex2dConvex2dAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
  87 {
  88         if (!m_manifoldPtr)
  89         {
  90                 //swapped?
  91                 m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
  92                 m_ownManifold = true;
  93         }
  94         resultOut->setPersistentManifold(m_manifoldPtr);
  95
  96         //comment-out next line to test multi-contact generation
  97         //resultOut->getPersistentManifold()->clearManifold();
  98
  99         const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
 100         const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
 101
 102         btVector3 normalOnB;
 103         btVector3 pointOnBWorld;
 104
 105         {
 106                 btGjkPairDetector::ClosestPointInput input;
 107
 108                 btGjkPairDetector gjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver);
 109                 //TODO: if (dispatchInfo.m_useContinuous)
 110                 gjkPairDetector.setMinkowskiA(min0);
 111                 gjkPairDetector.setMinkowskiB(min1);
 112
 113                 {
 114                         input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
 115                         input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
 116                 }
 117
 118                 input.m_transformA = body0Wrap->getWorldTransform();
 119                 input.m_transformB = body1Wrap->getWorldTransform();
 120
 121                 gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
 122
 123                 btVector3 v0, v1;
 124                 btVector3 sepNormalWorldSpace;
 125         }
 126
 127         if (m_ownManifold)
 128         {
 129                 resultOut->refreshContactPoints();
 130         }
 131 }
 132
 133 btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 134 {
 135         (void)resultOut;
 136         (void)dispatchInfo;
 137         ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
 138
 139         ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
 140         ///col0->m_worldTransform,
 141         btScalar resultFraction = btScalar(1.);
 142
 143         btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
 144         btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
 145
 146         if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
 147                 squareMot1 < col1->getCcdSquareMotionThreshold())
 148                 return resultFraction;
 149
 150         //An adhoc way of testing the Continuous Collision Detection algorithms
 151         //One object is approximated as a sphere, to simplify things
 152         //Starting in penetration should report no time of impact
 153         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
 154         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
 155
 156         /// Convex0 against sphere for Convex1
 157         {
 158                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
 159
 160                 btSphereShape sphere1(col1->getCcdSweptSphereRadius());  //todo: allow non-zero sphere sizes, for better approximation
 161                 btConvexCast::CastResult result;
 162                 btVoronoiSimplexSolver voronoiSimplex;
 163                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 164                 ///Simplification, one object is simplified as a sphere
 165                 btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex);
 166                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
 167                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
 168                                                                   col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
 169                 {
 170                         //store result.m_fraction in both bodies
 171
 172                         if (col0->getHitFraction() > result.m_fraction)
 173                                 col0->setHitFraction(result.m_fraction);
 174
 175                         if (col1->getHitFraction() > result.m_fraction)
 176                                 col1->setHitFraction(result.m_fraction);
 177
 178                         if (resultFraction > result.m_fraction)
 179                                 resultFraction = result.m_fraction;
 180                 }
 181         }
 182
 183         /// Sphere (for convex0) against Convex1
 184         {
 185                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
 186
 187                 btSphereShape sphere0(col0->getCcdSweptSphereRadius());  //todo: allow non-zero sphere sizes, for better approximation
 188                 btConvexCast::CastResult result;
 189                 btVoronoiSimplexSolver voronoiSimplex;
 190                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 191                 ///Simplification, one object is simplified as a sphere
 192                 btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex);
 193                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
 194                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
 195                                                                   col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
 196                 {
 197                         //store result.m_fraction in both bodies
 198
 199                         if (col0->getHitFraction() > result.m_fraction)
 200                                 col0->setHitFraction(result.m_fraction);
 201
 202                         if (col1->getHitFraction() > result.m_fraction)
 203                                 col1->setHitFraction(result.m_fraction);
 204
 205                         if (resultFraction > result.m_fraction)
 206                                 resultFraction = result.m_fraction;
 207                 }
 208         }
 209
 210         return resultFraction;
 211 }