427 lines
9.0 KiB
C++
427 lines
9.0 KiB
C++
/*
|
|
* 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;
|
|
}
|