src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp

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