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

BU_CollisionPair.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_CollisionPair.h"
#include "NarrowPhaseCollision/BU_VertexPoly.h"
#include "NarrowPhaseCollision/BU_EdgeEdge.h"
#include "BU_Collidable.h"


#include "BU_MotionStateInterface.h"
#include "CollisionShapes/PolyhedralConvexShape.h"
#include <SimdMinMax.h>
#include "SimdTransformUtil.h"



BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance)
: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)),
m_tolerance(tolerance)
{

}

// if there exists a time-of-impact between any feature_pair (edgeA,edgeB),
// (vertexA,faceB) or (vertexB,faceA) in [0..1], report true and smallest time


/*
bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB)

*/

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



      
      SimdVector3 linvelA,angvelA;
      SimdVector3 linvelB,angvelB;

      SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
      SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);


      SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
      SimdQuaternion angularMotionA(0,0,0,1.f);
      SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
      SimdQuaternion angularMotionB(0,0,0,1);
      


      result.m_fraction = 1.f;

      SimdTransform impactTransA;
      SimdTransform impactTransB;

      int index=0;

      SimdScalar toiUnscaled=result.m_fraction;
      const SimdScalar toiUnscaledLimit = result.m_fraction;

      SimdTransform a2w;
      a2w = fromA;
      SimdTransform b2w = fromB;

/* debugging code
      {
            const int numvertsB = m_convexB->GetNumVertices();
            for (int v=0;v<numvertsB;v++)
            {
                  SimdPoint3 pt;
                  m_convexB->GetVertex(v,pt);
                  pt = b2w * pt;
                  char buf[1000];

                  if (pt.y() < 0.)
                  {
                        sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
                        if (debugFile)
                              fwrite(buf,1,strlen(buf),debugFile);
                  } else
                  {
                        sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
                        if (debugFile)
                              fwrite(buf,1,strlen(buf),debugFile);

                  }
            }
      }
*/


      SimdTransform b2wp = b2w;
      
      b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
      b2wp.setRotation( b2w.getRotation() + angularMotionB);

      impactTransB = b2wp;
      
      SimdTransform a2wp;
      a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
      a2wp.setRotation(a2w.getRotation()+angularMotionA);

      impactTransA = a2wp;

      SimdTransform a2winv;
      a2winv = a2w.inverse();

      SimdTransform b2wpinv;
      b2wpinv = b2wp.inverse();

      SimdTransform b2winv;
      b2winv = b2w.inverse();

      SimdTransform a2wpinv;
      a2wpinv = a2wp.inverse();

            //Redon's version with concatenated transforms

      SimdTransform relative;

      relative = b2w * b2wpinv * a2wp * a2winv;

      //relative = a2winv * a2wp  * b2wpinv * b2w;

      SimdQuaternion qrel;
      relative.getBasis().getRotation(qrel);

      SimdVector3 linvel = relative.getOrigin();

      if (linvel.length() < SCREWEPSILON)
      {
            linvel.setValue(0.,0.,0.);
      }
      SimdVector3 angvel;
      angvel[0] = 2.f * SimdAsin (qrel[0]);
      angvel[1] = 2.f * SimdAsin (qrel[1]);
      angvel[2] = 2.f * SimdAsin (qrel[2]);
      
      if (angvel.length() < SCREWEPSILON)
      {
            angvel.setValue(0.f,0.f,0.f);
      }

      //Redon's version with concatenated transforms
      m_screwing = BU_Screwing(linvel,angvel);
      
      SimdTransform w2s;
      m_screwing.LocalMatrix(w2s);

      SimdTransform s2w;
      s2w = w2s.inverse();

      //impactTransA = a2w;
      //impactTransB = b2w;

      bool hit = false;
      
      if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
      {
            //W = 0 , S = 0 , no collision
            //toi = 0;
      /*    
            {
                  const int numvertsB = m_convexB->GetNumVertices();
                  for (int v=0;v<numvertsB;v++)
                  {
                        SimdPoint3 pt;
                        m_convexB->GetVertex(v,pt);
                        pt = impactTransB * pt;
                        char buf[1000];
                        
                        if (pt.y() < 0.)
                        {
                              sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
                              if (debugFile)
                                    fwrite(buf,1,strlen(buf),debugFile);
                        }
                        else
                        {
                              sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
                              if (debugFile)
                                    fwrite(buf,1,strlen(buf),debugFile);
                        }
                  }
            }
      */    
            
            return false;//don't continue moving within epsilon
      }

#define EDGEEDGE
#ifdef EDGEEDGE

      BU_EdgeEdge edgeEdge;

      //for all edged in A check agains all edges in B
      for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
      {
            SimdPoint3 pA0,pA1;

            m_convexA->GetEdge(ea,pA0,pA1);

            pA0= a2w * pA0;//in world space
            pA0 = w2s * pA0;//in screwing space

            pA1= a2w * pA1;//in world space
            pA1 = w2s * pA1;//in screwing space

            int numedgesB = m_convexB->GetNumEdges();
            for (int eb = 0; eb < numedgesB;eb++)
            {
                  {
                        SimdPoint3 pB0,pB1;
                        m_convexB->GetEdge(eb,pB0,pB1);

                        pB0= b2w * pB0;//in world space
                        pB0 = w2s * pB0;//in screwing space

                        pB1= b2w * pB1;//in world space
                        pB1 = w2s * pB1;//in screwing space


                        SimdScalar lambda,mu;
                        
                        toiUnscaled = 1.;

                        SimdVector3 edgeDirA(pA1-pA0);
                        SimdVector3 edgeDirB(pB1-pB0);

                        if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
                        {
                              //printf("edgeedge potential hit\n");
                              if (toiUnscaled>=0)
                              {
                                    if (toiUnscaled < toiUnscaledLimit)                                     
                                    {
            
                                          //inside check is already done by checking the mu and gamma !

                                          SimdPoint3 vtx  = pA0+lambda * (pA1-pA0);
                                          SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
                                          
                                          SimdPoint3 hitptWorld =   s2w * hitpt;
                                          {

                                                if (toiUnscaled < result.m_fraction)
                                                      result.m_fraction = toiUnscaled;

                                                hit = true;

                                                SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
                                                
                                                hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
                                          

                                                hitNormal.normalize();
                                                
                                                //an approximated normal can be calculated by taking the cross product of both edges
                                                //take care of the sign !
                                                
                                                SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
                                    
                                                SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
                                                if (dist > 0)
                                                      hitNormalWorld *= -1;
                                                
                                                //todo: this is the wrong point, because b2winv is still at begin of motion
                                                // not at time-of-impact location!
                                                //bhitpt = b2winv * hitptWorld;

//                                              m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
                                          }
                              
                                    }
                              }
                        }
                  }

                  index++;
            }
      };
#endif //EDGEEDGE

#define VERTEXFACE
#ifdef VERTEXFACE

      // for all vertices in A, for each face in B,do vertex-face
      {
            const int numvertsA = m_convexA->GetNumVertices();
            for (int v=0;v<numvertsA;v++)
            //int v=3;

            {
                  SimdPoint3 vtx;
                  m_convexA->GetVertex(v,vtx);

                  vtx = a2w * vtx;//in world space
                  vtx = w2s * vtx;//in screwing space

                  const int numplanesB = m_convexB->GetNumPlanes();

                  for (int p = 0 ; p < numplanesB; p++)
                  //int p=2;
                  {

                        {
                        
                              SimdVector3 planeNorm;
                              SimdPoint3 planeSupport;

                              m_convexB->GetPlane(planeNorm,planeSupport,p);


                              planeSupport = b2w * planeSupport;//transform to world space
                              SimdVector3 planeNormWorld =  b2w.getBasis() * planeNorm;
                        
                              planeSupport =  w2s * planeSupport  ; //transform to screwing space
                              planeNorm =  w2s.getBasis() * planeNormWorld;

                              planeNorm.normalize();

                              SimdScalar d = planeSupport.dot(planeNorm);
                              
                              SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
                        
                              BU_VertexPoly vtxApolyB;

                              toiUnscaled = 1.;

                              if ((p==2) && (v==6))
                              {
//                                  printf("%f toiUnscaled\n",toiUnscaled);

                              }
                              if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
                              {
                              


                                    
                                    if (toiUnscaled >= 0. )
                                    {
                                          //not only collect the first point, get every contactpoint, later we have to check the
                                          //manifold properly!

                                          if (toiUnscaled <= toiUnscaledLimit)
                                          {
      //                                        printf("toiUnscaled %f\n",toiUnscaled );

                                                SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
                                                SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);

                                                SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
                                                SimdPoint3 hitptWorld = s2w * hitpt;


                                                hitpt = b2winv * hitptWorld;
                                                //vertex has to be 'within' the facet's boundary
                                                if (m_convexB->IsInside(hitpt,m_tolerance))
                                                {
//                                                    m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
                                                      
                                                      if (toiUnscaled < result.m_fraction)
                                                            result.m_fraction= toiUnscaled;
                                                      hit = true;

                                                }
                                          }
                                    }
                              }
                              
                        }

                        index++;
                  }
            }
      }

      //
      // for all vertices in B, for each face in A,do vertex-face
      //copy and pasted from all verts A -> all planes B so potential typos!
      //todo: make this into one method with a kind of 'swapped' logic
      //
      {
            const int numvertsB = m_convexB->GetNumVertices();
            for (int v=0;v<numvertsB;v++)
            //int v=0;

            {
                  SimdPoint3 vtx;
                  m_convexB->GetVertex(v,vtx);

                  vtx = b2w * vtx;//in world space
/*
                  
                  char buf[1000];

                  if (vtx.y() < 0.)
                  {
                        sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
                        if (debugFile)
                              fwrite(buf,1,strlen(buf),debugFile);
                  }
                  sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
                  if (debugFile)
                        fwrite(buf,1,strlen(buf),debugFile);

*/                
                  vtx = w2s * vtx;//in screwing space

                  const int numplanesA = m_convexA->GetNumPlanes();

                  for (int p = 0 ; p < numplanesA; p++)
                  //int p=2;
                  {

                        {
                              SimdVector3 planeNorm;
                              SimdPoint3 planeSupport;

                              m_convexA->GetPlane(planeNorm,planeSupport,p);


                              planeSupport = a2w * planeSupport;//transform to world space
                              SimdVector3 planeNormWorld =  a2w.getBasis() * planeNorm;
                        
                              planeSupport =  w2s * planeSupport  ; //transform to screwing space
                              planeNorm =  w2s.getBasis() * planeNormWorld;

                              planeNorm.normalize();

                              SimdScalar d = planeSupport.dot(planeNorm);
                              
                              SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
                        
                              BU_VertexPoly vtxBpolyA;

                              toiUnscaled = 1.;

                              if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
                              {
                                    if (toiUnscaled>=0.)
                                    {
                                          if (toiUnscaled < toiUnscaledLimit)
                                          {
                                                SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
                                                SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
                                                SimdScalar len =  hitNormal.length()-1;

                                                //assert( SimdFuzzyZero(len) );

                                                
                                                SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
                                                SimdPoint3 hitptWorld = s2w * hitpt;
                                                hitpt = a2winv * hitptWorld;
                                          
                                          
                                                //vertex has to be 'within' the facet's boundary
                                                if (m_convexA->IsInside(hitpt,m_tolerance))
                                                {
                                                      
//                                                    m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
                                                      if (toiUnscaled <result.m_fraction)
                                                            result.m_fraction = toiUnscaled;
                                                      hit = true;
                                                }
                                          }
                                    
                                    }
                              
                              }
                              }

                  }
            
                  index++;
            }
      }
      

#endif// VERTEXFACE

      //the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
      //in addition there are contact points from previous frames
      //we have to cleanup the manifold, using an additional epsilon/tolerance
      //as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
      //else throw it away
      

      if (hit)
      {

            //try to avoid numerical drift on close contact
            
            if (result.m_fraction < 0.00001)
            {
//                printf("toiUnscaledMin< 0.00001\n");
                  impactTransA = a2w;
                  impactTransB = b2w;

            } else
            {

                  //SimdScalar vel = linearMotionB.length();
                  
                  //todo: check this margin
                  result.m_fraction *= 0.99f;

                  //move B to new position
                  impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
                  SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
                  ornB.normalize();
                  impactTransB.setRotation(ornB);

                  //now transform A
                  SimdTransform a2s,a2b;
                  a2s.mult( w2s , a2w);
                  a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
                  a2s.multInverseLeft(w2s,a2s);
                  a2b.multInverseLeft(b2w, a2s);

                  //transform by motion B
                  impactTransA.mult(impactTransB, a2b);
                  //normalize rotation
                  SimdQuaternion orn;
                  impactTransA.getBasis().getRotation(orn);
                  orn.normalize();
                  impactTransA.setBasis(SimdMatrix3x3(orn));
            }
      }

/*
      {
            const int numvertsB = m_convexB->GetNumVertices();
            for (int v=0;v<numvertsB;v++)
            {
                  SimdPoint3 pt;
                  m_convexB->GetVertex(v,pt);
                  pt = impactTransB * pt;
                  char buf[1000];

                  if (pt.y() < 0.)
                  {
                        sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
                        if (debugFile)
                              fwrite(buf,1,strlen(buf),debugFile);
                  }
                  else
                  {
                        sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
                        if (debugFile)
                              fwrite(buf,1,strlen(buf),debugFile);
                  }
            }
      }
*/
      return hit;
}



Generated by  Doxygen 1.6.0   Back to index