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

ContinuousConvexCollision.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 "ContinuousConvexCollision.h"
#include "CollisionShapes/ConvexShape.h"
#include "CollisionShapes/MinkowskiSumShape.h"
#include "NarrowPhaseCollision/SimplexSolverInterface.h"
#include "SimdTransformUtil.h"
#include "CollisionShapes/SphereShape.h"

#include "GjkPairDetector.h"
#include "PointCollector.h"



ContinuousConvexCollision::ContinuousConvexCollision ( ConvexShape*     convexA,ConvexShape*    convexB,SimplexSolverInterface* simplexSolver, ConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_simplexSolver(simplexSolver),
m_penetrationDepthSolver(penetrationDepthSolver),
m_convexA(convexA),m_convexB(convexB)
{
}

/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
/// You don't want your game ever to lock-up.
#define MAX_ITERATIONS 1000

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

      m_simplexSolver->reset();

      /// compute linear and angular velocity for this interval, to interpolate
      SimdVector3 linVelA,angVelA,linVelB,angVelB;
      SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linVelA,angVelA);
      SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linVelB,angVelB);

      SimdScalar boundingRadiusA = m_convexA->GetAngularMotionDisc();
      SimdScalar boundingRadiusB = m_convexB->GetAngularMotionDisc();

      SimdScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;

      float radius = 0.001f;

      SimdScalar lambda = 0.f;
      SimdVector3 v(1,0,0);

      int maxIter = MAX_ITERATIONS;

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

      float lastLambda = lambda;
      float epsilon = 0.001f;

      int numIter = 0;
      //first solution, using GJK


      SimdTransform identityTrans;
      identityTrans.setIdentity();

      SphereShape raySphere(0.0f);
      raySphere.SetMargin(0.f);


//    result.DrawCoordSystem(sphereTr);

      PointCollector    pointCollector1;

      {
            
            GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);        
            GjkPairDetector::ClosestPointInput input;
            input.m_transformA = fromA;
            input.m_transformB = fromB;
            gjk.GetClosestPoints(input,pointCollector1);

            hasResult = pointCollector1.m_hasResult;
            c = pointCollector1.m_pointInWorld;
      }

      if (hasResult)
      {
            SimdScalar dist;
            dist = pointCollector1.m_distance;
            n = pointCollector1.m_normalOnBInWorld;
            
            //not close enough
            while (dist > radius)
            {
                  numIter++;
                  if (numIter > maxIter)
                        return false; //todo: report a failure

                  float dLambda = 0.f;

                  //calculate safe moving fraction from distance / (linear+rotational velocity)
                  
                  //float clippedDist  = GEN_min(angularConservativeRadius,dist);
                  float clippedDist  = dist;
                  
                  float projectedLinearVelocity = (linVelB-linVelA).dot(n);
                  
                  dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);

                  lambda = lambda + dLambda;

                  if (lambda > 1.f)
                        return false;

                  if (lambda < 0.f)
                        return false;

                  //todo: next check with relative epsilon
                  if (lambda <= lastLambda)
                        break;
                  lastLambda = lambda;

                  

                  //interpolate to next lambda
                  SimdTransform interpolatedTransA,interpolatedTransB,relativeTrans;

                  SimdTransformUtil::IntegrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
                  SimdTransformUtil::IntegrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
                  relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);

                  result.DebugDraw( lambda );

                  PointCollector    pointCollector;
                  GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
                  GjkPairDetector::ClosestPointInput input;
                  input.m_transformA = interpolatedTransA;
                  input.m_transformB = interpolatedTransB;
                  gjk.GetClosestPoints(input,pointCollector);
                  if (pointCollector.m_hasResult)
                  {
                        if (pointCollector.m_distance < 0.f)
                        {
                              //degenerate ?!
                              result.m_fraction = lastLambda;
                              result.m_normal = n;
                              return true;
                        }
                        c = pointCollector.m_pointInWorld;        
                        
                        dist = pointCollector.m_distance;
                  } else
                  {
                        //??
                        return false;
                  }

            }

            result.m_fraction = lambda;
            result.m_normal = n;
            return true;
      }

      return false;

/*
//todo:
      //if movement away from normal, discard result
      SimdVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
      if (result.m_fraction < 1.f)
      {
            if (move.dot(result.m_normal) <= 0.f)
            {
            }
      }
*/

}


Generated by  Doxygen 1.6.0   Back to index