Logo Search packages:      
Sourcecode: blender version File versions

btVoronoiSimplexSolver.cpp

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
      
      Elsevier CDROM license agreements grants nonexclusive license to use the software
      for any purpose, commercial or non-commercial as long as the following credit is included
      identifying the original source of the software:

      Parts of the source are "from the book Real-Time Collision Detection by
      Christer Ericson, published by Morgan Kaufmann Publishers,
      (c) 2005 Elsevier Inc."
            
*/


#include "btVoronoiSimplexSolver.h"
#include <assert.h>
//#include <stdio.h>

#define VERTA  0
#define VERTB  1
#define VERTC  2
#define VERTD  3

#define CATCH_DEGENERATE_TETRAHEDRON 1
void  btVoronoiSimplexSolver::removeVertex(int index)
{
      
      assert(m_numVertices>0);
      m_numVertices--;
      m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
      m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
      m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
}

void  btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
{
      if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
            removeVertex(3);

      if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
            removeVertex(2);

      if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
            removeVertex(1);
      
      if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
            removeVertex(0);

}





//clear the simplex, remove all the vertices
void btVoronoiSimplexSolver::reset()
{
      m_cachedValidClosest = false;
      m_numVertices = 0;
      m_needsUpdate = true;
      m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
      m_cachedBC.reset();
}



      //add a vertex
void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
{
      m_lastW = w;
      m_needsUpdate = true;

      m_simplexVectorW[m_numVertices] = w;
      m_simplexPointsP[m_numVertices] = p;
      m_simplexPointsQ[m_numVertices] = q;

      m_numVertices++;
}

bool  btVoronoiSimplexSolver::updateClosestVectorAndPoints()
{
      
      if (m_needsUpdate)
      {
            m_cachedBC.reset();

            m_needsUpdate = false;

            switch (numVertices())
            {
            case 0:
                        m_cachedValidClosest = false;
                        break;
            case 1:
                  {
                        m_cachedP1 = m_simplexPointsP[0];
                        m_cachedP2 = m_simplexPointsQ[0];
                        m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
                        m_cachedBC.reset();
                        m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
                        m_cachedValidClosest = m_cachedBC.isValid();
                        break;
                  };
            case 2:
                  {
                  //closest point origin from line segment
                              const btVector3& from = m_simplexVectorW[0];
                              const btVector3& to = m_simplexVectorW[1];
                              btVector3 nearest;

                              btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
                              btVector3 diff = p - from;
                              btVector3 v = to - from;
                              btScalar t = v.dot(diff);
                              
                              if (t > 0) {
                                    btScalar dotVV = v.dot(v);
                                    if (t < dotVV) {
                                          t /= dotVV;
                                          diff -= t*v;
                                          m_cachedBC.m_usedVertices.usedVertexA = true;
                                          m_cachedBC.m_usedVertices.usedVertexB = true;
                                    } else {
                                          t = 1;
                                          diff -= v;
                                          //reduce to 1 point
                                          m_cachedBC.m_usedVertices.usedVertexB = true;
                                    }
                              } else
                              {
                                    t = 0;
                                    //reduce to 1 point
                                    m_cachedBC.m_usedVertices.usedVertexA = true;
                              }
                              m_cachedBC.setBarycentricCoordinates(1-t,t);
                              nearest = from + t*v;

                              m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
                              m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
                              m_cachedV = m_cachedP1 - m_cachedP2;
                              
                              reduceVertices(m_cachedBC.m_usedVertices);

                              m_cachedValidClosest = m_cachedBC.isValid();
                              break;
                  }
            case 3: 
                  { 
                        //closest point origin from triangle 
                        btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); 

                        const btVector3& a = m_simplexVectorW[0]; 
                        const btVector3& b = m_simplexVectorW[1]; 
                        const btVector3& c = m_simplexVectorW[2]; 

                        closestPtPointTriangle(p,a,b,c,m_cachedBC); 
                        m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + 
                        m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + 
                        m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; 

                        m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + 
                        m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + 
                        m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; 

                        m_cachedV = m_cachedP1-m_cachedP2; 

                        reduceVertices (m_cachedBC.m_usedVertices); 
                        m_cachedValidClosest = m_cachedBC.isValid(); 

                        break; 
                  }
            case 4:
                  {

                        
                        btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
                        
                        const btVector3& a = m_simplexVectorW[0];
                        const btVector3& b = m_simplexVectorW[1];
                        const btVector3& c = m_simplexVectorW[2];
                        const btVector3& d = m_simplexVectorW[3];

                        bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);

                        if (hasSeperation)
                        {

                              m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
                                    m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
                                    m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
                                    m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];

                              m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
                                    m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
                                    m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
                                    m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];

                              m_cachedV = m_cachedP1-m_cachedP2;
                              reduceVertices (m_cachedBC.m_usedVertices);
                        } else
                        {
//                            printf("sub distance got penetration\n");

                              if (m_cachedBC.m_degenerate)
                              {
                                    m_cachedValidClosest = false;
                              } else
                              {
                                    m_cachedValidClosest = true;
                                    //degenerate case == false, penetration = true + zero
                                    m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
                              }
                              break;
                        }

                        m_cachedValidClosest = m_cachedBC.isValid();

                        //closest point origin from tetrahedron
                        break;
                  }
            default:
                  {
                        m_cachedValidClosest = false;
                  }
            };
      }

      return m_cachedValidClosest;

}

//return/calculate the closest vertex
bool btVoronoiSimplexSolver::closest(btVector3& v)
{
      bool succes = updateClosestVectorAndPoints();
      v = m_cachedV;
      return succes;
}



btScalar btVoronoiSimplexSolver::maxVertex()
{
      int i, numverts = numVertices();
      btScalar maxV = btScalar(0.);
      for (i=0;i<numverts;i++)
      {
            btScalar curLen2 = m_simplexVectorW[i].length2();
            if (maxV < curLen2)
                  maxV = curLen2;
      }
      return maxV;
}



      //return the current simplex
int btVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
{
      int i;
      for (i=0;i<numVertices();i++)
      {
            yBuf[i] = m_simplexVectorW[i];
            pBuf[i] = m_simplexPointsP[i];
            qBuf[i] = m_simplexPointsQ[i];
      }
      return numVertices();
}




bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
{
      bool found = false;
      int i, numverts = numVertices();
      //btScalar maxV = btScalar(0.);
      
      //w is in the current (reduced) simplex
      for (i=0;i<numverts;i++)
      {
            if (m_simplexVectorW[i] == w)
                  found = true;
      }

      //check in case lastW is already removed
      if (w == m_lastW)
            return true;
      
      return found;
}

void btVoronoiSimplexSolver::backup_closest(btVector3& v) 
{
      v = m_cachedV;
}


bool btVoronoiSimplexSolver::emptySimplex() const 
{
      return (numVertices() == 0);

}

void btVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2) 
{
      updateClosestVectorAndPoints();
      p1 = m_cachedP1;
      p2 = m_cachedP2;

}




bool  btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result)
{
      result.m_usedVertices.reset();

    // Check if P in vertex region outside A
    btVector3 ab = b - a;
    btVector3 ac = c - a;
    btVector3 ap = p - a;
    btScalar d1 = ab.dot(ap);
    btScalar d2 = ac.dot(ap);
    if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 
      {
            result.m_closestPointOnSimplex = a;
            result.m_usedVertices.usedVertexA = true;
            result.setBarycentricCoordinates(1,0,0);
            return true;// a; // barycentric coordinates (1,0,0)
      }

    // Check if P in vertex region outside B
    btVector3 bp = p - b;
    btScalar d3 = ab.dot(bp);
    btScalar d4 = ac.dot(bp);
    if (d3 >= btScalar(0.0) && d4 <= d3) 
      {
            result.m_closestPointOnSimplex = b;
            result.m_usedVertices.usedVertexB = true;
            result.setBarycentricCoordinates(0,1,0);

            return true; // b; // barycentric coordinates (0,1,0)
      }
    // Check if P in edge region of AB, if so return projection of P onto AB
    btScalar vc = d1*d4 - d3*d2;
    if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
        btScalar v = d1 / (d1 - d3);
            result.m_closestPointOnSimplex = a + v * ab;
            result.m_usedVertices.usedVertexA = true;
            result.m_usedVertices.usedVertexB = true;
            result.setBarycentricCoordinates(1-v,v,0);
            return true;
        //return a + v * ab; // barycentric coordinates (1-v,v,0)
    }

    // Check if P in vertex region outside C
    btVector3 cp = p - c;
    btScalar d5 = ab.dot(cp);
    btScalar d6 = ac.dot(cp);
    if (d6 >= btScalar(0.0) && d5 <= d6) 
      {
            result.m_closestPointOnSimplex = c;
            result.m_usedVertices.usedVertexC = true;
            result.setBarycentricCoordinates(0,0,1);
            return true;//c; // barycentric coordinates (0,0,1)
      }

    // Check if P in edge region of AC, if so return projection of P onto AC
    btScalar vb = d5*d2 - d1*d6;
    if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
        btScalar w = d2 / (d2 - d6);
            result.m_closestPointOnSimplex = a + w * ac;
            result.m_usedVertices.usedVertexA = true;
            result.m_usedVertices.usedVertexC = true;
            result.setBarycentricCoordinates(1-w,0,w);
            return true;
        //return a + w * ac; // barycentric coordinates (1-w,0,w)
    }

    // Check if P in edge region of BC, if so return projection of P onto BC
    btScalar va = d3*d6 - d5*d4;
    if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
        btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
            
            result.m_closestPointOnSimplex = b + w * (c - b);
            result.m_usedVertices.usedVertexB = true;
            result.m_usedVertices.usedVertexC = true;
            result.setBarycentricCoordinates(0,1-w,w);
            return true;            
       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
    }

    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
    btScalar denom = btScalar(1.0) / (va + vb + vc);
    btScalar v = vb * denom;
    btScalar w = vc * denom;
    
      result.m_closestPointOnSimplex = a + ab * v + ac * w;
      result.m_usedVertices.usedVertexA = true;
      result.m_usedVertices.usedVertexB = true;
      result.m_usedVertices.usedVertexC = true;
      result.setBarycentricCoordinates(1-v-w,v,w);
      
      return true;
//    return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w

}





/// Test if point p and d lie on opposite sides of plane through abc
00428 int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)
{
      btVector3 normal = (b-a).cross(c-a);

    btScalar signp = (p - a).dot(normal); // [AP AB AC]
    btScalar signd = (d - a).dot( normal); // [AD AB AC]

#ifdef CATCH_DEGENERATE_TETRAHEDRON
#ifdef BT_USE_DOUBLE_PRECISION
if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
      {
            return -1;
      }
#else
      if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
      {
//          printf("affine dependent/degenerate\n");//
            return -1;
      }
#endif

#endif
      // Points on opposite sides if expression signs are opposite
    return signp * signd < btScalar(0.);
}


bool  btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult)
{
      btSubSimplexClosestResult tempResult;

    // Start out assuming point inside all halfspaces, so closest to itself
      finalResult.m_closestPointOnSimplex = p;
      finalResult.m_usedVertices.reset();
    finalResult.m_usedVertices.usedVertexA = true;
      finalResult.m_usedVertices.usedVertexB = true;
      finalResult.m_usedVertices.usedVertexC = true;
      finalResult.m_usedVertices.usedVertexD = true;

    int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
      int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
      int   pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
      int   pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);

   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
   {
         finalResult.m_degenerate = true;
         return false;
   }

   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
       {
             return false;
       }


    btScalar bestSqDist = FLT_MAX;
    // If point outside face abc then compute closest point on abc
      if (pointOutsideABC) 
      {
        closestPtPointTriangle(p, a, b, c,tempResult);
            btPoint3 q = tempResult.m_closestPointOnSimplex;
            
        btScalar sqDist = (q - p).dot( q - p);
        // Update best closest point if (squared) distance is less than current best
        if (sqDist < bestSqDist) {
                  bestSqDist = sqDist;
                  finalResult.m_closestPointOnSimplex = q;
                  //convert result bitmask!
                  finalResult.m_usedVertices.reset();
                  finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
                  finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
                  finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
                  finalResult.setBarycentricCoordinates(
                              tempResult.m_barycentricCoords[VERTA],
                              tempResult.m_barycentricCoords[VERTB],
                              tempResult.m_barycentricCoords[VERTC],
                              0
                  );

            }
    }
  

      // Repeat test for face acd
      if (pointOutsideACD) 
      {
        closestPtPointTriangle(p, a, c, d,tempResult);
            btPoint3 q = tempResult.m_closestPointOnSimplex;
            //convert result bitmask!

        btScalar sqDist = (q - p).dot( q - p);
        if (sqDist < bestSqDist) 
            {
                  bestSqDist = sqDist;
                  finalResult.m_closestPointOnSimplex = q;
                  finalResult.m_usedVertices.reset();
                  finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;

                  finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
                  finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
                  finalResult.setBarycentricCoordinates(
                              tempResult.m_barycentricCoords[VERTA],
                              0,
                              tempResult.m_barycentricCoords[VERTB],
                              tempResult.m_barycentricCoords[VERTC]
                  );

            }
    }
    // Repeat test for face adb

      
      if (pointOutsideADB)
      {
            closestPtPointTriangle(p, a, d, b,tempResult);
            btPoint3 q = tempResult.m_closestPointOnSimplex;
            //convert result bitmask!

        btScalar sqDist = (q - p).dot( q - p);
        if (sqDist < bestSqDist) 
            {
                  bestSqDist = sqDist;
                  finalResult.m_closestPointOnSimplex = q;
                  finalResult.m_usedVertices.reset();
                  finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
                  finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
                  
                  finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
                  finalResult.setBarycentricCoordinates(
                              tempResult.m_barycentricCoords[VERTA],
                              tempResult.m_barycentricCoords[VERTC],
                              0,
                              tempResult.m_barycentricCoords[VERTB]
                  );

            }
    }
    // Repeat test for face bdc
    

      if (pointOutsideBDC)
      {
        closestPtPointTriangle(p, b, d, c,tempResult);
            btPoint3 q = tempResult.m_closestPointOnSimplex;
            //convert result bitmask!
        btScalar sqDist = (q - p).dot( q - p);
        if (sqDist < bestSqDist) 
            {
                  bestSqDist = sqDist;
                  finalResult.m_closestPointOnSimplex = q;
                  finalResult.m_usedVertices.reset();
                  //
                  finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
                  finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
                  finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;

                  finalResult.setBarycentricCoordinates(
                              0,
                              tempResult.m_barycentricCoords[VERTA],
                              tempResult.m_barycentricCoords[VERTC],
                              tempResult.m_barycentricCoords[VERTB]
                  );

            }
    }

      //help! we ended up full !
      
      if (finalResult.m_usedVertices.usedVertexA &&
            finalResult.m_usedVertices.usedVertexB &&
            finalResult.m_usedVertices.usedVertexC &&
            finalResult.m_usedVertices.usedVertexD) 
      {
            return true;
      }

    return true;
}


Generated by  Doxygen 1.6.0   Back to index