Back to index

supertuxkart  0.5+dfsg1
btConvexConvexAlgorithm.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 "btConvexConvexAlgorithm.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/NarrowPhaseCollision/btGjkPairDetector.h"
00024 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
00025 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00026 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00027 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
00028 
00029 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
00030 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
00031 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00032 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
00033 
00034 
00035 
00036 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
00037 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00038 
00039 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
00040 
00041 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
00042 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
00043 
00044 
00045 
00046 
00047 
00048 
00049 
00050 
00051 
00052 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                  simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
00053 {
00054        m_simplexSolver = simplexSolver;
00055        m_pdSolver = pdSolver;
00056 }
00057 
00058 btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
00059 { 
00060 }
00061 
00062 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
00063 : btCollisionAlgorithm(ci),
00064 m_gjkPairDetector(0,0,simplexSolver,pdSolver),
00065 m_ownManifold (false),
00066 m_manifoldPtr(mf),
00067 m_lowLevelOfDetail(false)
00068 {
00069        (void)body0;
00070        (void)body1;
00071 
00072 
00073 }
00074 
00075 
00076 
00077 
00078 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
00079 {
00080        if (m_ownManifold)
00081        {
00082               if (m_manifoldPtr)
00083                      m_dispatcher->releaseManifold(m_manifoldPtr);
00084        }
00085 }
00086 
00087 void   btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
00088 {
00089        m_lowLevelOfDetail = useLowLevel;
00090 }
00091 
00092 
00093 
00094 
00095 
00096 //
00097 // Convex-Convex collision algorithm
00098 //
00099 void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00100 {
00101 
00102        if (!m_manifoldPtr)
00103        {
00104               //swapped?
00105               m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
00106               m_ownManifold = true;
00107        }
00108        resultOut->setPersistentManifold(m_manifoldPtr);
00109 
00110 #ifdef USE_BT_GJKEPA
00111        btConvexShape*                            shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
00112        btConvexShape*                            shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
00113        const btScalar                            radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
00114        btGjkEpaSolver::sResults    results;
00115        if(btGjkEpaSolver::Collide( shape0,body0->getWorldTransform(),
00116                                                         shape1,body1->getWorldTransform(),
00117                                                         radialmargin,results))
00118               {
00119               dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
00120               resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
00121               }
00122 #else
00123 
00124        btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
00125        btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
00126        
00127        btGjkPairDetector::ClosestPointInput input;
00128 
00129        //TODO: if (dispatchInfo.m_useContinuous)
00130        m_gjkPairDetector.setMinkowskiA(min0);
00131        m_gjkPairDetector.setMinkowskiB(min1);
00132        input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
00133        input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
00134        input.m_stackAlloc = dispatchInfo.m_stackAllocator;
00135 
00136 //     input.m_maximumDistanceSquared = btScalar(1e30);
00137        
00138        input.m_transformA = body0->getWorldTransform();
00139        input.m_transformB = body1->getWorldTransform();
00140        
00141        m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
00142 #endif
00143 
00144        if (m_ownManifold)
00145        {
00146               resultOut->refreshContactPoints();
00147        }
00148 
00149 }
00150 
00151 
00152 
00153 bool disableCcd = false;
00154 btScalar      btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00155 {
00156        (void)resultOut;
00157        (void)dispatchInfo;
00159     
00162        btScalar resultFraction = btScalar(1.);
00163 
00164 
00165        btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
00166        btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
00167     
00168        if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
00169               squareMot1 < col1->getCcdSquareMotionThreshold())
00170               return resultFraction;
00171 
00172        if (disableCcd)
00173               return btScalar(1.);
00174 
00175 
00176        //An adhoc way of testing the Continuous Collision Detection algorithms
00177        //One object is approximated as a sphere, to simplify things
00178        //Starting in penetration should report no time of impact
00179        //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
00180        //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
00181 
00182               
00184        {
00185               btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
00186 
00187               btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
00188               btConvexCast::CastResult result;
00189               btVoronoiSimplexSolver voronoiSimplex;
00190               //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
00192               btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
00193               //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
00194               if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
00195                      col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
00196               {
00197               
00198                      //store result.m_fraction in both bodies
00199               
00200                      if (col0->getHitFraction()> result.m_fraction)
00201                             col0->setHitFraction( result.m_fraction );
00202 
00203                      if (col1->getHitFraction() > result.m_fraction)
00204                             col1->setHitFraction( result.m_fraction);
00205 
00206                      if (resultFraction > result.m_fraction)
00207                             resultFraction = result.m_fraction;
00208 
00209               }
00210               
00211               
00212 
00213 
00214        }
00215 
00217        {
00218               btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
00219 
00220               btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
00221               btConvexCast::CastResult result;
00222               btVoronoiSimplexSolver voronoiSimplex;
00223               //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
00225               btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
00226               //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
00227               if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
00228                      col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
00229               {
00230               
00231                      //store result.m_fraction in both bodies
00232               
00233                      if (col0->getHitFraction()  > result.m_fraction)
00234                             col0->setHitFraction( result.m_fraction);
00235 
00236                      if (col1->getHitFraction() > result.m_fraction)
00237                             col1->setHitFraction( result.m_fraction);
00238 
00239                      if (resultFraction > result.m_fraction)
00240                             resultFraction = result.m_fraction;
00241 
00242               }
00243        }
00244        
00245        return resultFraction;
00246 
00247 }
00248