Back to index

supertuxkart  0.5+dfsg1
btGjkConvexCast.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 
00018 #include "btGjkConvexCast.h"
00019 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00020 #include "btGjkPairDetector.h"
00021 #include "btPointCollector.h"
00022 #include "LinearMath/btTransformUtil.h"
00023 
00024 #ifdef BT_USE_DOUBLE_PRECISION
00025 #define MAX_ITERATIONS 64
00026 #else
00027 #define MAX_ITERATIONS 32
00028 #endif
00029 
00030 btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
00031 :m_simplexSolver(simplexSolver),
00032 m_convexA(convexA),
00033 m_convexB(convexB)
00034 {
00035 }
00036 
00037 bool   btGjkConvexCast::calcTimeOfImpact(
00038                                    const btTransform& fromA,
00039                                    const btTransform& toA,
00040                                    const btTransform& fromB,
00041                                    const btTransform& toB,
00042                                    CastResult& result)
00043 {
00044 
00045 
00046        m_simplexSolver->reset();
00047 
00049        //assume no rotation/angular velocity, assert here?
00050        btVector3 linVelA,linVelB;
00051        linVelA = toA.getOrigin()-fromA.getOrigin();
00052        linVelB = toB.getOrigin()-fromB.getOrigin();
00053 
00054        btScalar radius = btScalar(0.001);
00055        btScalar lambda = btScalar(0.);
00056        btVector3 v(1,0,0);
00057 
00058        int maxIter = MAX_ITERATIONS;
00059 
00060        btVector3 n;
00061        n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00062        bool hasResult = false;
00063        btVector3 c;
00064        btVector3 r = (linVelA-linVelB);
00065 
00066        btScalar lastLambda = lambda;
00067        //btScalar epsilon = btScalar(0.001);
00068 
00069        int numIter = 0;
00070        //first solution, using GJK
00071 
00072 
00073        btTransform identityTrans;
00074        identityTrans.setIdentity();
00075 
00076 
00077 //     result.drawCoordSystem(sphereTr);
00078 
00079        btPointCollector     pointCollector;
00080 
00081               
00082        btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);         
00083        btGjkPairDetector::ClosestPointInput input;
00084 
00085        //we don't use margins during CCD
00086        //     gjk.setIgnoreMargin(true);
00087 
00088        input.m_transformA = fromA;
00089        input.m_transformB = fromB;
00090        gjk.getClosestPoints(input,pointCollector,0);
00091 
00092        hasResult = pointCollector.m_hasResult;
00093        c = pointCollector.m_pointInWorld;
00094 
00095        if (hasResult)
00096        {
00097               btScalar dist;
00098               dist = pointCollector.m_distance;
00099               n = pointCollector.m_normalOnBInWorld;
00100 
00101        
00102 
00103               //not close enough
00104               while (dist > radius)
00105               {
00106                      numIter++;
00107                      if (numIter > maxIter)
00108                      {
00109                             return false; //todo: report a failure
00110                      }
00111                      btScalar dLambda = btScalar(0.);
00112 
00113                      btScalar projectedLinearVelocity = r.dot(n);
00114                      
00115                      dLambda = dist / (projectedLinearVelocity);
00116 
00117                      lambda = lambda - dLambda;
00118 
00119                      if (lambda > btScalar(1.))
00120                             return false;
00121 
00122                      if (lambda < btScalar(0.))
00123                             return false;
00124 
00125                      //todo: next check with relative epsilon
00126                      if (lambda <= lastLambda)
00127                      {
00128                             return false;
00129                             //n.setValue(0,0,0);
00130                             break;
00131                      }
00132                      lastLambda = lambda;
00133 
00134                      //interpolate to next lambda
00135                      result.DebugDraw( lambda );
00136                      input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
00137                      input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
00138                      
00139                      gjk.getClosestPoints(input,pointCollector,0);
00140                      if (pointCollector.m_hasResult)
00141                      {
00142                             if (pointCollector.m_distance < btScalar(0.))
00143                             {
00144                                    result.m_fraction = lastLambda;
00145                                    n = pointCollector.m_normalOnBInWorld;
00146                                    result.m_normal=n;
00147                                    result.m_hitPoint = pointCollector.m_pointInWorld;
00148                                    return true;
00149                             }
00150                             c = pointCollector.m_pointInWorld;        
00151                             n = pointCollector.m_normalOnBInWorld;
00152                             dist = pointCollector.m_distance;
00153                      } else
00154                      {
00155                             //??
00156                             return false;
00157                      }
00158 
00159               }
00160 
00161               result.m_fraction = lambda;
00162               result.m_normal = n;
00163               result.m_hitPoint = c;
00164               return true;
00165        }
00166 
00167        return false;
00168 
00169 
00170 }
00171