Logo Search packages:      
Sourcecode: blender version File versions  Download package

SubSimplexConvexCast.cpp

/*
 * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
 *
 * Permission to use, copy, modify, distribute and sell this software
 * and its documentation for any purpose is hereby granted without fee,
 * provided that the above copyright notice appear in all copies.
 * Erwin Coumans makes no representations about the suitability 
 * of this software for any purpose.  
 * It is provided "as is" without express or implied warranty.
*/

#include "SubSimplexConvexCast.h"
#include "CollisionShapes/ConvexShape.h"
#include "CollisionShapes/MinkowskiSumShape.h"
#include "NarrowPhaseCollision/SimplexSolverInterface.h"


SubsimplexConvexCast::SubsimplexConvexCast (ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver)
:m_simplexSolver(simplexSolver),
m_convexA(convexA),m_convexB(convexB)
{
}


#define MAX_ITERATIONS 1000

00027 bool  SubsimplexConvexCast::calcTimeOfImpact(
            const SimdTransform& fromA,
            const SimdTransform& toA,
            const SimdTransform& fromB,
            const SimdTransform& toB,
            CastResult& result)
{

            MinkowskiSumShape combi(m_convexA,m_convexB);
      MinkowskiSumShape* convex = &combi;

      SimdTransform     rayFromLocalA;
      SimdTransform     rayToLocalA;

      rayFromLocalA = fromA.inverse()* fromB;
      rayToLocalA = toA.inverse()* toB;


      m_simplexSolver->reset();

      convex->SetTransformB(SimdTransform(rayFromLocalA.getBasis()));

      float radius = 0.01f;

      SimdScalar lambda = 0.f;
      SimdVector3 s = rayFromLocalA.getOrigin();
      SimdVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
      SimdVector3 x = s;
      SimdVector3 v;
      SimdVector3 arbitraryPoint = convex->LocalGetSupportingVertex(r);
      
      v = x - arbitraryPoint;

      int maxIter = MAX_ITERATIONS;

      SimdVector3 n;
      n.setValue(0.f,0.f,0.f);
      bool hasResult = false;
      SimdVector3 c;

      float lastLambda = lambda;


      float dist2 = v.length2();
      float epsilon = 0.0001f;

      SimdVector3 w,p;
      float VdotR;
      
      while ( (dist2 > epsilon) && maxIter--)
      {
            p = convex->LocalGetSupportingVertex( v);
             w = x - p;

            float VdotW = v.dot(w);

            if ( VdotW > 0.f)
            {
                  VdotR = v.dot(r);

                  if (VdotR >= 0.f)
                        return false;
                  else
                  {
                        lambda = lambda - VdotW / VdotR;
                        x = s + lambda * r;
                        m_simplexSolver->reset();
                        //check next line
                        w = x-p;
                        lastLambda = lambda;
                        n = v;
                        hasResult = true;
                  }
            } 
            m_simplexSolver->addVertex( w, x , p);
            if (m_simplexSolver->closest(v))
            {
                  dist2 = v.length2();
                  hasResult = true;
                  //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
                  //printf("DIST2=%f\n",dist2);
                  //printf("numverts = %i\n",m_simplexSolver->numVertices());
            } else
            {
                  dist2 = 0.f;
            } 
      }

      int numiter = MAX_ITERATIONS - maxIter;
//    printf("number of iterations: %d", numiter);
      result.m_fraction = lambda;
      result.m_normal = n;

      return true;
}




Generated by  Doxygen 1.6.0   Back to index