Back to index

supertuxkart  0.5+dfsg1
btSubSimplexConvexCast.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 "btSubSimplexConvexCast.h"
00018 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00019 
00020 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
00022 #include "btPointCollector.h"
00023 #include "LinearMath/btTransformUtil.h"
00024 
00025 btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
00026 :m_simplexSolver(simplexSolver),
00027 m_convexA(convexA),m_convexB(convexB)
00028 {
00029 }
00030 
00033 #ifdef BT_USE_DOUBLE_PRECISION
00034 #define MAX_ITERATIONS 64
00035 #else
00036 #define MAX_ITERATIONS 32
00037 #endif
00038 bool   btSubsimplexConvexCast::calcTimeOfImpact(
00039               const btTransform& fromA,
00040               const btTransform& toA,
00041               const btTransform& fromB,
00042               const btTransform& toB,
00043               CastResult& result)
00044 {
00045 
00046        m_simplexSolver->reset();
00047 
00048        btVector3 linVelA,linVelB;
00049        linVelA = toA.getOrigin()-fromA.getOrigin();
00050        linVelB = toB.getOrigin()-fromB.getOrigin();
00051 
00052        btScalar lambda = btScalar(0.);
00053 
00054        btTransform interpolatedTransA = fromA;
00055        btTransform interpolatedTransB = fromB;
00056 
00058        btVector3 r = (linVelA-linVelB);
00059        btVector3 v;
00060        
00061        btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
00062        btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
00063        v = supVertexA-supVertexB;
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 
00073 
00074        btScalar dist2 = v.length2();
00075 #ifdef BT_USE_DOUBLE_PRECISION
00076        btScalar epsilon = btScalar(0.0001);
00077 #else
00078        btScalar epsilon = btScalar(0.0001);
00079 #endif //BT_USE_DOUBLE_PRECISION
00080        btVector3     w,p;
00081        btScalar VdotR;
00082        
00083        while ( (dist2 > epsilon) && maxIter--)
00084        {
00085               supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
00086               supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
00087               w = supVertexA-supVertexB;
00088 
00089               btScalar VdotW = v.dot(w);
00090 
00091               if ( VdotW > btScalar(0.))
00092               {
00093                      VdotR = v.dot(r);
00094 
00095                      if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
00096                             return false;
00097                      else
00098                      {
00099                             lambda = lambda - VdotW / VdotR;
00100                             //interpolate to next lambda
00101                             //     x = s + lambda * r;
00102                             interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
00103                             interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
00104                             //m_simplexSolver->reset();
00105                             //check next line
00106                              w = supVertexA-supVertexB;
00107                             lastLambda = lambda;
00108                             n = v;
00109                             hasResult = true;
00110                      }
00111               } 
00112               m_simplexSolver->addVertex( w, supVertexA , supVertexB);
00113               if (m_simplexSolver->closest(v))
00114               {
00115                      dist2 = v.length2();
00116                      hasResult = true;
00117                      //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
00118                      //printf("DIST2=%f\n",dist2);
00119                      //printf("numverts = %i\n",m_simplexSolver->numVertices());
00120               } else
00121               {
00122                      dist2 = btScalar(0.);
00123               } 
00124        }
00125 
00126        //int numiter = MAX_ITERATIONS - maxIter;
00127 //     printf("number of iterations: %d", numiter);
00128        result.m_fraction = lambda;
00129        result.m_normal = n.normalized();
00130        btVector3 hitA,hitB;
00131        m_simplexSolver->compute_points(hitA,hitB);
00132        result.m_hitPoint=hitB;
00133        return true;
00134 }
00135 
00136 
00137