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

DT_PenDepth.cpp

/*
 * SOLID - Software Library for Interference Detection
 * 
 * Copyright (C) 2001-2003  Dtecta.  All rights reserved.
 *
 * This library may be distributed under the terms of the Q Public License
 * (QPL) as defined by Trolltech AS of Norway and appearing in the file
 * LICENSE.QPL included in the packaging of this file.
 *
 * This library may be distributed and/or modified under the terms of the
 * GNU General Public License (GPL) version 2 as published by the Free Software
 * Foundation and appearing in the file LICENSE.GPL included in the
 * packaging of this file.
 *
 * This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Commercial use or any other use of this library not covered by either 
 * the QPL or the GPL requires an additional license from Dtecta. 
 * Please contact info@dtecta.com for enquiries about the terms of commercial
 * use of this library.
 */

#include "DT_PenDepth.h"

#include "MT_Vector3.h"
#include "MT_Point3.h"
#include "MT_Quaternion.h"
#include "DT_Convex.h"
#include "DT_GJK.h"
#include "DT_Facet.h"

//#define DEBUG

const int       MaxSupportPoints = 1000;
const int       MaxFacets         = 2000;

static MT_Point3  pBuf[MaxSupportPoints];
static MT_Point3  qBuf[MaxSupportPoints];
static MT_Vector3 yBuf[MaxSupportPoints];

static DT_Facet facetBuf[MaxFacets];
static int  freeFacet = 0;
static DT_Facet *facetHeap[MaxFacets];
static int  num_facets;

class DT_FacetComp {
public:
    
    bool operator()(const DT_Facet *face1, const DT_Facet *face2) 
      { 
            return face1->getDist2() > face2->getDist2();
    }
    
} facetComp;

inline DT_Facet *addFacet(int i0, int i1, int i2,
                                      MT_Scalar lower2, MT_Scalar upper2) 
{
    assert(i0 != i1 && i0 != i2 && i1 != i2);
    if (freeFacet < MaxFacets)
      {
            DT_Facet *facet = new(&facetBuf[freeFacet++]) DT_Facet(i0, i1, i2);
#ifdef DEBUG
            std::cout << "Facet " << i0 << ' ' << i1 << ' ' << i2;
#endif
            if (facet->computeClosest(yBuf)) 
            {
                  if (facet->isClosestInternal() && 
                        lower2 <= facet->getDist2() && facet->getDist2() <= upper2) 
                  {
                        facetHeap[num_facets++] = facet;
                        std::push_heap(&facetHeap[0], &facetHeap[num_facets], facetComp);
#ifdef DEBUG
                        std::cout << " accepted" << std::endl;
#endif
                  }
                  else 
                  {
#ifdef DEBUG
                        std::cout << " rejected, ";
                        if (!facet->isClosestInternal()) 
                        {
                              std::cout << "closest point not internal";
                        }
                        else if (lower2 > facet->getDist2()) 
                        {
                              std::cout << "facet is closer than orignal facet";
                        }
                        else 
                        {
                              std::cout << "facet is further than upper bound";
                        }
                        std::cout << std::endl;
#endif
                  }
                  
                  return facet;
            }
    }
    
    return 0;
}

inline bool originInTetrahedron(const MT_Vector3& p1, const MT_Vector3& p2, 
                                                const MT_Vector3& p3, const MT_Vector3& p4)
{
    MT_Vector3 normal1 = (p2 - p1).cross(p3 - p1);
    MT_Vector3 normal2 = (p3 - p2).cross(p4 - p2);
    MT_Vector3 normal3 = (p4 - p3).cross(p1 - p3);
    MT_Vector3 normal4 = (p1 - p4).cross(p2 - p4);
    
    return 
            normal1.dot(p1) > MT_Scalar(0.0) != normal1.dot(p4) > MT_Scalar(0.0) &&
            normal2.dot(p2) > MT_Scalar(0.0) != normal2.dot(p1) > MT_Scalar(0.0) &&
            normal3.dot(p3) > MT_Scalar(0.0) != normal3.dot(p2) > MT_Scalar(0.0) &&
            normal4.dot(p4) > MT_Scalar(0.0) != normal4.dot(p3) > MT_Scalar(0.0);
}


bool penDepth(const DT_GJK& gjk, const DT_Convex& a, const DT_Convex& b,
                    MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
{
      
    int num_verts = gjk.getSimplex(pBuf, qBuf, yBuf);
    
    switch (num_verts) 
      {
      case 1:
          // Touching contact. Yes, we have a collision,
          // but no penetration.
          return false;
      case 2:     
      {
          // We have a line segment inside the Minkowski sum containing the
          // origin. Blow it up by adding three additional support points.
          
          MT_Vector3 dir  = (yBuf[1] - yBuf[0]).normalized();
          int        axis = dir.furthestAxis();
          
          static MT_Scalar sin_60 = MT_sqrt(MT_Scalar(3.0)) * MT_Scalar(0.5);
          
          MT_Quaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, MT_Scalar(0.5));
          MT_Matrix3x3 rot_mat(rot);
          
          MT_Vector3 aux1 = dir.cross(MT_Vector3(axis == 0, axis == 1, axis == 2));
          MT_Vector3 aux2 = rot_mat * aux1;
          MT_Vector3 aux3 = rot_mat * aux2;
          
          pBuf[2] = a.support(aux1);
          qBuf[2] = b.support(-aux1);
          yBuf[2] = pBuf[2] - qBuf[2];
          
          pBuf[3] = a.support(aux2);
          qBuf[3] = b.support(-aux2);
          yBuf[3] = pBuf[3] - qBuf[3];
          
          pBuf[4] = a.support(aux3);
          qBuf[4] = b.support(-aux3);
          yBuf[4] = pBuf[4] - qBuf[4];
          
          if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4])) 
            {
                  pBuf[1] = pBuf[4];
                  qBuf[1] = qBuf[4];
                  yBuf[1] = yBuf[4];
          }
          else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4])) 
            {
                  pBuf[0] = pBuf[4];
                  qBuf[0] = qBuf[4];
                  yBuf[0] = yBuf[4];
          } 
          else 
            {
                  // Origin not in initial polytope
                  return false;
          }
          
          num_verts = 4;
          
          break;
      }
      case 3: 
      {
          // We have a triangle inside the Minkowski sum containing
          // the origin. First blow it up.
          
          MT_Vector3 v1     = yBuf[1] - yBuf[0];
          MT_Vector3 v2     = yBuf[2] - yBuf[0];
          MT_Vector3 vv     = v1.cross(v2);
          
          pBuf[3] = a.support(vv);
          qBuf[3] = b.support(-vv);
          yBuf[3] = pBuf[3] - qBuf[3];
          pBuf[4] = a.support(-vv);
          qBuf[4] = b.support(vv);
          yBuf[4] = pBuf[4] - qBuf[4];
          
         
          if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4])) 
            {
                  pBuf[3] = pBuf[4];
                  qBuf[3] = qBuf[4];
                  yBuf[3] = yBuf[4];
          }
          else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
            { 
                  // Origin not in initial polytope
                  return false;
          }
          
          num_verts = 4;
          
          break;
      }
    }
    
    // We have a tetrahedron inside the Minkowski sum containing
    // the origin (if GJK did it's job right ;-)
      
    
    if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) 
      {
            //    assert(false);
            return false;
      }
    
      num_facets = 0;
    freeFacet = 0;

    DT_Facet *f0 = addFacet(0, 1, 2, MT_Scalar(0.0), MT_INFINITY);
    DT_Facet *f1 = addFacet(0, 3, 1, MT_Scalar(0.0), MT_INFINITY);
    DT_Facet *f2 = addFacet(0, 2, 3, MT_Scalar(0.0), MT_INFINITY);
    DT_Facet *f3 = addFacet(1, 3, 2, MT_Scalar(0.0), MT_INFINITY);
    
    if (!f0 || f0->getDist2() == MT_Scalar(0.0) ||
            !f1 || f1->getDist2() == MT_Scalar(0.0) ||
            !f2 || f2->getDist2() == MT_Scalar(0.0) ||
            !f3 || f3->getDist2() == MT_Scalar(0.0)) 
      {
            return false;
    }
    
    f0->link(0, f1, 2);
    f0->link(1, f3, 2);
    f0->link(2, f2, 0);
    f1->link(0, f2, 2);
    f1->link(1, f3, 0);
    f2->link(1, f3, 1);
    
    if (num_facets == 0) 
      {
            return false;
    }
    
    // at least one facet on the heap.    
    
    DT_EdgeBuffer edgeBuffer(20);

    DT_Facet *facet = 0;
    
    MT_Scalar upper_bound2 = MT_INFINITY;       
    
    do {
        facet = facetHeap[0];
        std::pop_heap(&facetHeap[0], &facetHeap[num_facets], facetComp);
        --num_facets;
            
            if (!facet->isObsolete()) 
            {
                  assert(facet->getDist2() > MT_Scalar(0.0));
                  
                  if (num_verts == MaxSupportPoints)
                  {
#ifdef DEBUG
                        std::cout << "Ouch, no convergence!!!" << std::endl;
#endif 
                        assert(false);    
                        break;
                  }
                  
                  pBuf[num_verts] = a.support(facet->getClosest());
                  qBuf[num_verts] = b.support(-facet->getClosest());
                  yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts];
                  
                  int index = num_verts++;
                  MT_Scalar far_dist2 = yBuf[index].dot(facet->getClosest());
                  
                  // Make sure the support mapping is OK.
                  assert(far_dist2 > MT_Scalar(0.0));
                  
                  GEN_set_min(upper_bound2, far_dist2 * far_dist2 / facet->getDist2());
                  
                  if (upper_bound2 <= DT_Accuracy::depth_tolerance * facet->getDist2()
#define CHECK_NEW_SUPPORT
#ifdef CHECK_NEW_SUPPORT
                        || yBuf[index] == yBuf[(*facet)[0]] 
                        || yBuf[index] == yBuf[(*facet)[1]]
                        || yBuf[index] == yBuf[(*facet)[2]]
#endif
                        ) 
                  {
                        break;
                  }
                  
                  // Compute the silhouette cast by the new vertex
                  // Note that the new vertex is on the positive side
                  // of the current facet, so the current facet is will
                  // not be in the convex hull. Start local search
                  // from this facet.
                  
                  facet->silhouette(yBuf[index], edgeBuffer);
                  
                  if (edgeBuffer.empty()) 
                  {
                        return false;
                  }
                  
                  DT_EdgeBuffer::const_iterator it = edgeBuffer.begin();
                  DT_Facet *firstFacet = 
                        addFacet((*it).getTarget(), (*it).getSource(),
                                     index, facet->getDist2(), upper_bound2);
                  
                  if (!firstFacet) 
                  {
                        break;
                  }
                  
                  firstFacet->link(0, (*it).getFacet(), (*it).getIndex());
                  DT_Facet *lastFacet = firstFacet;
                  
                  ++it;
                  for (; it != edgeBuffer.end(); ++it) 
                  {
                        DT_Facet *newFacet = 
                              addFacet((*it).getTarget(), (*it).getSource(),
                                           index, facet->getDist2(), upper_bound2);
                        
                        if (!newFacet) 
                        {
                              break;
                        }
                        
                        if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex())) 
                        {
                              break;
                        }
                        
                        if (!newFacet->link(2, lastFacet, 1)) 
                        {
                              break;
                        }
                        
                        lastFacet = newFacet;                     
                  }
                  if (it != edgeBuffer.end()) 
                  {
                        break;
                  }
                  
                  firstFacet->link(2, lastFacet, 1);
            }
    }
    while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2);
      
#ifdef DEBUG    
    std::cout << "#facets left = " << num_facets << std::endl;
#endif
    
    v = facet->getClosest();
    pa = facet->getClosestPoint(pBuf);    
    pb = facet->getClosestPoint(qBuf);    
    return true;
}


Generated by  Doxygen 1.6.0   Back to index