btConvex2dConvex2dAlgorithm.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "btConvex2dConvex2dAlgorithm.h"
00017 
00018 //#include <stdio.h>
00019 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
00020 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
00021 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00022 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00023 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
00024 
00025 
00026 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
00027 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
00028 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00029 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00030 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
00031 
00032 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
00033 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
00034 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00035 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
00036 
00037 
00038 
00039 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
00040 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00041 
00042 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
00043 
00044 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
00045 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
00046 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00047 
00048 btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                   simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
00049 {
00050         m_numPerturbationIterations = 0;
00051         m_minimumPointsPerturbationThreshold = 3;
00052         m_simplexSolver = simplexSolver;
00053         m_pdSolver = pdSolver;
00054 }
00055 
00056 btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() 
00057 { 
00058 }
00059 
00060 btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
00061 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
00062 m_simplexSolver(simplexSolver),
00063 m_pdSolver(pdSolver),
00064 m_ownManifold (false),
00065 m_manifoldPtr(mf),
00066 m_lowLevelOfDetail(false),
00067  m_numPerturbationIterations(numPerturbationIterations),
00068 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
00069 {
00070         (void)body0Wrap;
00071         (void)body1Wrap;
00072 }
00073 
00074 
00075 
00076 
00077 btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
00078 {
00079         if (m_ownManifold)
00080         {
00081                 if (m_manifoldPtr)
00082                         m_dispatcher->releaseManifold(m_manifoldPtr);
00083         }
00084 }
00085 
00086 void    btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
00087 {
00088         m_lowLevelOfDetail = useLowLevel;
00089 }
00090 
00091 
00092 
00093 extern btScalar gContactBreakingThreshold;
00094 
00095 
00096 //
00097 // Convex-Convex collision algorithm
00098 //
00099 void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00100 {
00101 
00102         if (!m_manifoldPtr)
00103         {
00104                 //swapped?
00105                 m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
00106                 m_ownManifold = true;
00107         }
00108         resultOut->setPersistentManifold(m_manifoldPtr);
00109 
00110         //comment-out next line to test multi-contact generation
00111         //resultOut->getPersistentManifold()->clearManifold();
00112 
00113 
00114         const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
00115         const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
00116 
00117         btVector3  normalOnB;
00118         btVector3  pointOnBWorld;
00119 
00120         {
00121 
00122 
00123                 btGjkPairDetector::ClosestPointInput input;
00124 
00125                 btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
00126                 //TODO: if (dispatchInfo.m_useContinuous)
00127                 gjkPairDetector.setMinkowskiA(min0);
00128                 gjkPairDetector.setMinkowskiB(min1);
00129 
00130                 {
00131                         input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
00132                         input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
00133                 }
00134 
00135                 input.m_stackAlloc = dispatchInfo.m_stackAllocator;
00136                 input.m_transformA = body0Wrap->getWorldTransform();
00137                 input.m_transformB = body1Wrap->getWorldTransform();
00138 
00139                 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
00140 
00141                 btVector3 v0,v1;
00142                 btVector3 sepNormalWorldSpace;
00143 
00144         }
00145 
00146         if (m_ownManifold)
00147         {
00148                 resultOut->refreshContactPoints();
00149         }
00150 
00151 }
00152 
00153 
00154 
00155 
00156 btScalar        btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00157 {
00158         (void)resultOut;
00159         (void)dispatchInfo;
00161 
00164         btScalar resultFraction = btScalar(1.);
00165 
00166 
00167         btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
00168         btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
00169 
00170         if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
00171                 squareMot1 < col1->getCcdSquareMotionThreshold())
00172                 return resultFraction;
00173 
00174 
00175         //An adhoc way of testing the Continuous Collision Detection algorithms
00176         //One object is approximated as a sphere, to simplify things
00177         //Starting in penetration should report no time of impact
00178         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
00179         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
00180 
00181 
00183         {
00184                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
00185 
00186                 btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
00187                 btConvexCast::CastResult result;
00188                 btVoronoiSimplexSolver voronoiSimplex;
00189                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
00191                 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
00192                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
00193                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
00194                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
00195                 {
00196 
00197                         //store result.m_fraction in both bodies
00198 
00199                         if (col0->getHitFraction()> result.m_fraction)
00200                                 col0->setHitFraction( result.m_fraction );
00201 
00202                         if (col1->getHitFraction() > result.m_fraction)
00203                                 col1->setHitFraction( result.m_fraction);
00204 
00205                         if (resultFraction > result.m_fraction)
00206                                 resultFraction = result.m_fraction;
00207 
00208                 }
00209 
00210 
00211 
00212 
00213         }
00214 
00216         {
00217                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
00218 
00219                 btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
00220                 btConvexCast::CastResult result;
00221                 btVoronoiSimplexSolver voronoiSimplex;
00222                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
00224                 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
00225                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
00226                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
00227                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
00228                 {
00229 
00230                         //store result.m_fraction in both bodies
00231 
00232                         if (col0->getHitFraction()      > result.m_fraction)
00233                                 col0->setHitFraction( result.m_fraction);
00234 
00235                         if (col1->getHitFraction() > result.m_fraction)
00236                                 col1->setHitFraction( result.m_fraction);
00237 
00238                         if (resultFraction > result.m_fraction)
00239                                 resultFraction = result.m_fraction;
00240 
00241                 }
00242         }
00243 
00244         return resultFraction;
00245 
00246 }
00247