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

BU_VertexPoly.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 "BU_VertexPoly.h"
#include "BU_Screwing.h"
#include <SimdTransform.h>
#include <SimdPoint3.h>
#include <SimdVector3.h>

#define USE_ALGEBRAIC
#ifdef USE_ALGEBRAIC
      #include "BU_AlgebraicPolynomialSolver.h"
      #define BU_Polynomial BU_AlgebraicPolynomialSolver
#else
      #include "BU_IntervalArithmeticPolynomialSolver.h"
      #define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
#endif

inline bool      TestFuzzyZero(SimdScalar x) { return SimdFabs(x) < 0.0001f; }


BU_VertexPoly::BU_VertexPoly()
{

}
//return true if a collision will occur between [0..1]
//false otherwise. If true, minTime contains the time of impact
bool BU_VertexPoly::GetTimeOfImpact(
            const BU_Screwing& screwAB,
            const SimdPoint3& a,
            const SimdVector4& planeEq,
            SimdScalar &minTime,bool swapAB)
{

      bool hit = false;

      // precondition: s=0 and w= 0 is catched by caller!
      if (TestFuzzyZero(screwAB.GetS()) &&
            TestFuzzyZero(screwAB.GetW()))
      {
            return false;
      }


      //case w<>0 and s<> 0
      const SimdScalar w=screwAB.GetW();
      const SimdScalar s=screwAB.GetS();

      SimdScalar coefs[4];
      const SimdScalar p=planeEq[0];
      const SimdScalar q=planeEq[1];
      const SimdScalar r=planeEq[2];
      const SimdScalar d=planeEq[3];

      const SimdVector3 norm(p,q,r);
      BU_Polynomial polynomialSolver;
      int numroots = 0;

      SimdScalar eps=1e-80f;
      SimdScalar eps2=1e-100f;
      
      if (TestFuzzyZero(screwAB.GetS()) ) 
      {
            //S = 0 , W <> 0
            
            //ax^3+bx^2+cx+d=0
            coefs[0]=0.;
            coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
            coefs[2]=-2*p*a.y()+2*q*a.x();
            coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;

//          numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
            numroots = polynomialSolver.Solve2QuadraticFull(coefs[1],coefs[2],coefs[3]);

      } else
      {
            if (TestFuzzyZero(screwAB.GetW()))
            {
                  // W = 0 , S <> 0
                  //pax+qay+r(az+st)=d
                  
                  SimdScalar dist = (d - a.dot(norm));

                  if (TestFuzzyZero(r))
                  {
                        if (TestFuzzyZero(dist))
                        {
                              // no hit
                        } else
                        {
                              // todo a a' might hit sides of polygon T
                              //printf("unhandled case, w=0,s<>0,r<>0, a a' might hit sides of polygon T \n");
                        }
                        
                  } else
                  {
                        SimdScalar etoi = (dist)/(r*screwAB.GetS());
                        if (swapAB)
                              etoi *= -1;

                        if (etoi >= 0. && etoi <= minTime)
                        {
                              minTime = etoi;
                              hit = true;
                        }
                  }

            } else
            {
                        //ax^3+bx^2+cx+d=0

                        //degenerate coefficients mess things up :(
                        SimdScalar ietsje = (r*s)/SimdTan(w/2.f);
                        if (ietsje*ietsje < 0.01f)
                              ietsje = 0.f;

                        coefs[0]=ietsje;//(r*s)/tan(w/2.);
                        coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
                        coefs[2]=-2.f*p*a.y()+2.f*q*a.x()+ietsje;//((r*s)/(tan(w/2.)));
                        coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;

                        numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
            }
      }


      for (int i=0;i<numroots;i++)
      {
            SimdScalar tau = polynomialSolver.GetRoot(i);

            SimdScalar t = 2.f*SimdAtan(tau)/w;
            //tau = tan (wt/2) so 2*atan (tau)/w
            if (swapAB)
            {
                  t *= -1.;
            }
            if (t>=0 && t<minTime)
            {
                  minTime = t;
                  hit = true;
            }
      }
      
      return hit;
}

Generated by  Doxygen 1.6.0   Back to index