Logo Search packages:      
Sourcecode: blender version File versions

DT_Convex.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_Convex.h"
#include "GEN_MinMax.h"

//#define DEBUG
#define SAFE_EXIT

#include "DT_GJK.h"
#include "DT_PenDepth.h"

#include <algorithm>
#include <new>

#include "MT_BBox.h"
#include "DT_Sphere.h"
#include "DT_Minkowski.h"

#include "DT_Accuracy.h"

#ifdef STATISTICS
int num_iterations = 0;
int num_irregularities = 0;
#endif

MT_BBox DT_Convex::bbox() const 
{
      MT_Point3 min(-supportH(MT_Vector3(-1.0f, 0.0f, 0.0f)),
                  -supportH(MT_Vector3(0.0f, -1.0f, 0.0f)),
                          -supportH(MT_Vector3(0.0f, 0.0f, -1.0f)));
      MT_Point3 max( supportH(MT_Vector3(1.0f, 0.0f, 0.0f)),
                   supportH(MT_Vector3(0.0f, 1.0f, 0.0f)),
                           supportH(MT_Vector3(0.0f, 0.0f, 1.0f)));

      
    return MT_BBox(min, max);
}

MT_BBox DT_Convex::bbox(const MT_Matrix3x3& basis) const 
{
    MT_Point3 min(-supportH(-basis[0]),
                  -supportH(-basis[1]),
                      -supportH(-basis[2])); 
    MT_Point3 max( supportH( basis[0]),
                   supportH( basis[1]),
                   supportH( basis[2])); 
    return MT_BBox(min, max);
}

MT_BBox DT_Convex::bbox(const MT_Transform& t, MT_Scalar margin) const 
{
    MT_Point3 min(t.getOrigin()[0] - supportH(-t.getBasis()[0]) - margin,
                  t.getOrigin()[1] - supportH(-t.getBasis()[1]) - margin,
                      t.getOrigin()[2] - supportH(-t.getBasis()[2]) - margin); 
    MT_Point3 max(t.getOrigin()[0] + supportH( t.getBasis()[0]) + margin,
                  t.getOrigin()[1] + supportH( t.getBasis()[1]) + margin,
                  t.getOrigin()[2] + supportH( t.getBasis()[2]) + margin); 
    return MT_BBox(min, max);
}

bool DT_Convex::ray_cast(const MT_Point3& source, const MT_Point3& target, MT_Scalar& lambda, MT_Vector3& normal) const
{
      // Still working on this one...
    return false;
}

bool intersect(const DT_Convex& a, const DT_Convex& b, MT_Vector3& v)
{
      DT_GJK gjk;

#ifdef STATISTICS
    num_iterations = 0;
#endif
      MT_Scalar dist2 = MT_INFINITY;

      do
      {
            MT_Point3  p = a.support(-v); 
            MT_Point3  q = b.support(v);
            MT_Vector3 w = p - q; 
            
        if (v.dot(w) > MT_Scalar(0.0)) 
            {
                  return false;
            }
 
            gjk.addVertex(w);

#ifdef STATISTICS
        ++num_iterations;
#endif
        if (!gjk.closest(v)) 
            {
#ifdef STATISTICS
            ++num_irregularities;
#endif
            return false;
        }

#ifdef SAFE_EXIT
            MT_Scalar prev_dist2 = dist2;
#endif

            dist2 = v.length2();

#ifdef SAFE_EXIT
            if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
            {
                  return false;
            }
#endif
    } 
    while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 

    v.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));

    return true;
}




bool common_point(const DT_Convex& a, const DT_Convex& b,
                  MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
{
      DT_GJK gjk;

#ifdef STATISTICS
    num_iterations = 0;
#endif

      MT_Scalar dist2 = MT_INFINITY;

    do
      {
            MT_Point3  p = a.support(-v); 
            MT_Point3  q = b.support(v);
            MT_Vector3 w = p - q; 
            
        if (v.dot(w) > MT_Scalar(0.0)) 
            {
                  return false;
            }
 
            gjk.addVertex(w, p, q);

#ifdef STATISTICS
        ++num_iterations;
#endif
        if (!gjk.closest(v)) 
            {
#ifdef STATISTICS
            ++num_irregularities;
#endif
            return false;
        }         
            
#ifdef SAFE_EXIT
            MT_Scalar prev_dist2 = dist2;
#endif

            dist2 = v.length2();

#ifdef SAFE_EXIT
            if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
            {
                  return false;
            }
#endif
    }
    while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
    
      gjk.compute_points(pa, pb);

    v.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));

    return true;
}






      
bool penetration_depth(const DT_Convex& a, const DT_Convex& b,
                       MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
{
      DT_GJK gjk;

#ifdef STATISTICS
    num_iterations = 0;
#endif

      MT_Scalar dist2 = MT_INFINITY;

    do
      {
            MT_Point3  p = a.support(-v); 
            MT_Point3  q = b.support(v);
            MT_Vector3 w = p - q; 
            
        if (v.dot(w) > MT_Scalar(0.0)) 
            {
                  return false;
            }
 
            gjk.addVertex(w, p, q);

#ifdef STATISTICS
        ++num_iterations;
#endif
        if (!gjk.closest(v)) 
            {
#ifdef STATISTICS
            ++num_irregularities;
#endif
            return false;
        }         
            
#ifdef SAFE_EXIT
            MT_Scalar prev_dist2 = dist2;
#endif

            dist2 = v.length2();

#ifdef SAFE_EXIT
            if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
            {
                  return false;
            }
#endif
    }
    while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
    

      return penDepth(gjk, a, b, v, pa, pb);

}

bool hybrid_penetration_depth(const DT_Convex& a, MT_Scalar a_margin, 
                                            const DT_Convex& b, MT_Scalar b_margin,
                              MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
{
      MT_Scalar margin = a_margin + b_margin;
      if (margin > MT_Scalar(0.0))
      {
            MT_Scalar margin2 = margin * margin;

            DT_GJK gjk;

#ifdef STATISTICS
            num_iterations = 0;
#endif
            MT_Scalar dist2 = MT_INFINITY;

            do
            {
                  MT_Point3  p = a.support(-v); 
                  MT_Point3  q = b.support(v);
                  
                  MT_Vector3 w = p - q; 
                  
                  MT_Scalar delta = v.dot(w);
                  
                  if (delta > MT_Scalar(0.0) && delta * delta > dist2 * margin2) 
                  {
                        return false;
                  }
                  
                  if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2)
                  {
                        gjk.compute_points(pa, pb);
                        MT_Scalar s = MT_sqrt(dist2);
                        assert(s > MT_Scalar(0.0));
                        pa -= v * (a_margin / s);
                        pb += v * (b_margin / s);
                        return true;
                  }
                  
                  gjk.addVertex(w, p, q);
                  
#ifdef STATISTICS
                  ++num_iterations;
#endif
                  if (!gjk.closest(v)) 
                  {
#ifdef STATISTICS
                        ++num_irregularities;
#endif
                        gjk.compute_points(pa, pb);
                        MT_Scalar s = MT_sqrt(dist2);
                        assert(s > MT_Scalar(0.0));
                        pa -= v * (a_margin / s);
                        pb += v * (b_margin / s);
                        return true;
                  }
                  
#ifdef SAFE_EXIT
                  MT_Scalar prev_dist2 = dist2;
#endif
                  
                  dist2 = v.length2();
                  
#ifdef SAFE_EXIT
                  if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
                  { 
                        gjk.backup_closest(v);
                        dist2 = v.length2();
                        gjk.compute_points(pa, pb);
                        MT_Scalar s = MT_sqrt(dist2);
                        assert(s > MT_Scalar(0.0));
                        pa -= v * (a_margin / s);
                        pb += v * (b_margin / s);
                        return true;
                  }
#endif
            }
            while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
            
      }
      // Second GJK phase. compute points on the boundary of the offset object
      
      return penetration_depth((a_margin > MT_Scalar(0.0) ? 
                                            static_cast<const DT_Convex&>(DT_Minkowski(a, DT_Sphere(a_margin))) : 
                                            static_cast<const DT_Convex&>(a)), 
                                           (b_margin > MT_Scalar(0.0) ? 
                                            static_cast<const DT_Convex&>(DT_Minkowski(b, DT_Sphere(b_margin))) : 
                                            static_cast<const DT_Convex&>(b)), v, pa, pb);
}


MT_Scalar closest_points(const DT_Convex& a, const DT_Convex& b, MT_Scalar max_dist2,
                         MT_Point3& pa, MT_Point3& pb) 
{
      MT_Vector3 v(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));
      
    DT_GJK gjk;

#ifdef STATISTICS
    num_iterations = 0;
#endif

      MT_Scalar dist2 = MT_INFINITY;

    do
      {
            MT_Point3  p = a.support(-v); 
            MT_Point3  q = b.support(v);
            MT_Vector3 w = p - q; 

            MT_Scalar delta = v.dot(w);
            if (delta > MT_Scalar(0.0) && delta * delta > dist2 * max_dist2) 
            {
                  return MT_INFINITY;
            }

            if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2) 
            {
            break;
            }

            gjk.addVertex(w, p, q);

#ifdef STATISTICS
        ++num_iterations;
        if (num_iterations > 1000) 
            {
                  std::cout << "v: " << v << " w: " << w << std::endl;
            }
#endif
        if (!gjk.closest(v)) 
            {
#ifdef STATISTICS
            ++num_irregularities;
#endif
            break;
        }

#ifdef SAFE_EXIT
            MT_Scalar prev_dist2 = dist2;
#endif

            dist2 = v.length2();

#ifdef SAFE_EXIT
            if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
            {
         gjk.backup_closest(v);
         dist2 = v.length2();
                  break;
            }
#endif
    }
    while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 

      assert(!gjk.emptySimplex());
      
      if (dist2 <= max_dist2)
      {
            gjk.compute_points(pa, pb);
      }
      
      return dist2;
}

Generated by  Doxygen 1.6.0   Back to index