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 (lambda > btScalar(1.0))
00092                 {
00093                         return false;
00094                 }
00095 
00096                 if ( VdotW > btScalar(0.))
00097                 {
00098                         VdotR = v.dot(r);
00099 
00100                         if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
00101                                 return false;
00102                         else
00103                         {
00104                                 lambda = lambda - VdotW / VdotR;
00105                                 //interpolate to next lambda
00106                                 //      x = s + lambda * r;
00107                                 interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
00108                                 interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
00109                                 //m_simplexSolver->reset();
00110                                 //check next line
00111                                  w = supVertexA-supVertexB;
00112                                 lastLambda = lambda;
00113                                 n = v;
00114                                 hasResult = true;
00115                         }
00116                 } 
00118                 if (!m_simplexSolver->inSimplex(w))
00119                         m_simplexSolver->addVertex( w, supVertexA , supVertexB);
00120 
00121                 if (m_simplexSolver->closest(v))
00122                 {
00123                         dist2 = v.length2();
00124                         hasResult = true;
00125                         //todo: check this normal for validity
00126                         //n=v;
00127                         //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
00128                         //printf("DIST2=%f\n",dist2);
00129                         //printf("numverts = %i\n",m_simplexSolver->numVertices());
00130                 } else
00131                 {
00132                         dist2 = btScalar(0.);
00133                 } 
00134         }
00135 
00136         //int numiter = MAX_ITERATIONS - maxIter;
00137 //      printf("number of iterations: %d", numiter);
00138         
00139         //don't report a time of impact when moving 'away' from the hitnormal
00140         
00141 
00142         result.m_fraction = lambda;
00143         if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
00144                 result.m_normal = n.normalized();
00145         else
00146                 result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00147 
00148         //don't report time of impact for motion away from the contact normal (or causes minor penetration)
00149         if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
00150                 return false;
00151 
00152         btVector3 hitA,hitB;
00153         m_simplexSolver->compute_points(hitA,hitB);
00154         result.m_hitPoint=hitB;
00155         return true;
00156 }
00157 
00158 
00159 
00160