Back to index

supertuxkart  0.5+dfsg1
btContinuousConvexCollision.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 
00017 #include "btContinuousConvexCollision.h"
00018 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00019 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
00020 #include "LinearMath/btTransformUtil.h"
00021 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00022 
00023 #include "btGjkPairDetector.h"
00024 #include "btPointCollector.h"
00025 
00026 
00027 
00028 btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape*     convexA,const btConvexShape*       convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
00029 :m_simplexSolver(simplexSolver),
00030 m_penetrationDepthSolver(penetrationDepthSolver),
00031 m_convexA(convexA),m_convexB(convexB)
00032 {
00033 }
00034 
00037 #define MAX_ITERATIONS 64
00038 
00039 bool   btContinuousConvexCollision::calcTimeOfImpact(
00040                             const btTransform& fromA,
00041                             const btTransform& toA,
00042                             const btTransform& fromB,
00043                             const btTransform& toB,
00044                             CastResult& result)
00045 {
00046 
00047        m_simplexSolver->reset();
00048 
00050        btVector3 linVelA,angVelA,linVelB,angVelB;
00051        btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
00052        btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
00053 
00054        btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
00055        btScalar boundingRadiusB = m_convexB->getAngularMotionDisc();
00056 
00057        btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
00058 
00059        btScalar radius = btScalar(0.001);
00060 
00061        btScalar lambda = btScalar(0.);
00062        btVector3 v(1,0,0);
00063 
00064        int maxIter = MAX_ITERATIONS;
00065 
00066        btVector3 n;
00067        n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00068        bool hasResult = false;
00069        btVector3 c;
00070 
00071        btScalar lastLambda = lambda;
00072        //btScalar epsilon = btScalar(0.001);
00073 
00074        int numIter = 0;
00075        //first solution, using GJK
00076 
00077 
00078        btTransform identityTrans;
00079        identityTrans.setIdentity();
00080 
00081        btSphereShape raySphere(btScalar(0.0));
00082        raySphere.setMargin(btScalar(0.));
00083 
00084 
00085 //     result.drawCoordSystem(sphereTr);
00086 
00087        btPointCollector     pointCollector1;
00088 
00089        {
00090               
00091               btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);              
00092               btGjkPairDetector::ClosestPointInput input;
00093        
00094               //we don't use margins during CCD
00095        //     gjk.setIgnoreMargin(true);
00096 
00097               input.m_transformA = fromA;
00098               input.m_transformB = fromB;
00099               gjk.getClosestPoints(input,pointCollector1,0);
00100 
00101               hasResult = pointCollector1.m_hasResult;
00102               c = pointCollector1.m_pointInWorld;
00103        }
00104 
00105        if (hasResult)
00106        {
00107               btScalar dist;
00108               dist = pointCollector1.m_distance;
00109               n = pointCollector1.m_normalOnBInWorld;
00110 
00111        
00112 
00113               //not close enough
00114               while (dist > radius)
00115               {
00116                      numIter++;
00117                      if (numIter > maxIter)
00118                      {
00119                             return false; //todo: report a failure
00120                      }
00121                      btScalar dLambda = btScalar(0.);
00122 
00123                             btScalar projectedLinearVelocity = (linVelB-linVelA).dot(n);
00124 
00125                      //calculate safe moving fraction from distance / (linear+rotational velocity)
00126                      
00127                      //btScalar clippedDist  = GEN_min(angularConservativeRadius,dist);
00128                      //btScalar clippedDist  = dist;
00129                      
00130                      
00131                      dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
00132 
00133                      lambda = lambda + dLambda;
00134 
00135                      if (lambda > btScalar(1.))
00136                             return false;
00137 
00138                      if (lambda < btScalar(0.))
00139                             return false;
00140 
00141 
00142                      //todo: next check with relative epsilon
00143                      if (lambda <= lastLambda)
00144                      {
00145                             return false;
00146                             //n.setValue(0,0,0);
00147                             break;
00148                      }
00149                      lastLambda = lambda;
00150 
00151                      
00152 
00153                      //interpolate to next lambda
00154                      btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
00155 
00156                      btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
00157                      btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
00158                      relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
00159 
00160                      result.DebugDraw( lambda );
00161 
00162                      btPointCollector     pointCollector;
00163                      btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
00164                      btGjkPairDetector::ClosestPointInput input;
00165                      input.m_transformA = interpolatedTransA;
00166                      input.m_transformB = interpolatedTransB;
00167                      gjk.getClosestPoints(input,pointCollector,0);
00168                      if (pointCollector.m_hasResult)
00169                      {
00170                             if (pointCollector.m_distance < btScalar(0.))
00171                             {
00172                                    //degenerate ?!
00173                                    result.m_fraction = lastLambda;
00174                                    n = pointCollector.m_normalOnBInWorld;
00175                                    result.m_normal=n;//.setValue(1,1,1);// = n;
00176                                    result.m_hitPoint = pointCollector.m_pointInWorld;
00177                                    return true;
00178                             }
00179                             c = pointCollector.m_pointInWorld;        
00180                             n = pointCollector.m_normalOnBInWorld;
00181                             dist = pointCollector.m_distance;
00182                      } else
00183                      {
00184                             //??
00185                             return false;
00186                      }
00187 
00188               }
00189 
00190               result.m_fraction = lambda;
00191               result.m_normal = n;
00192               result.m_hitPoint = c;
00193               return true;
00194        }
00195 
00196        return false;
00197 
00198 /*
00199 //todo:
00200        //if movement away from normal, discard result
00201        btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
00202        if (result.m_fraction < btScalar(1.))
00203        {
00204               if (move.dot(result.m_normal) <= btScalar(0.))
00205               {
00206               }
00207        }
00208 */
00209 
00210 }
00211