Commit 2dbd2392 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Add a test on geometric tools and comparison with fcl GJK solver.

parent 4c14cc3e
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)
#pragma once
//----------------------------------------------------------------------------
// The platform specification.
//
// __MSWINDOWS__ : Microsoft Windows (WIN32 or WIN64)
// __APPLE__ : Macintosh OS X
// __LINUX__ : Linux or Cygwin
//----------------------------------------------------------------------------
#if !defined(__LINUX__) && (defined(WIN32) || defined(_WIN64))
#define __MSWINDOWS__
#if !defined(_MSC_VER)
#error Microsoft Visual Studio 2013 or later is required.
#endif
// MSVC 6 is version 12.0
// MSVC 7.0 is version 13.0 (MSVS 2002)
// MSVC 7.1 is version 13.1 (MSVS 2003)
// MSVC 8.0 is version 14.0 (MSVS 2005)
// MSVC 9.0 is version 15.0 (MSVS 2008)
// MSVC 10.0 is version 16.0 (MSVS 2010)
// MSVC 11.0 is version 17.0 (MSVS 2012)
// MSVC 12.0 is version 18.0 (MSVS 2013)
// MSVC 14.0 is version 19.0 (MSVS 2015)
// Currently, projects are provided only for MSVC 12.0 and 14.0.
#if _MSC_VER < 1800
#error Microsoft Visual Studio 2013 or later is required.
#endif
// Debug build values (choose_your_value is 0, 1, or 2)
// 0: Disables checked iterators and disables iterator debugging.
// 1: Enables checked iterators and disables iterator debugging.
// 2: (default) Enables iterator debugging; checked iterators are not relevant.
//
// Release build values (choose_your_value is 0 or 1)
// 0: (default) Disables checked iterators.
// 1: Enables checked iterators; iterator debugging is not relevant.
//
// #define _ITERATOR_DEBUG_LEVEL choose_your_value
#endif // WIN32 or _WIN64
// TODO: Windows DLL configurations have not yet been added to the project,
// but these defines are required to support them (when we do add them).
//
// Add GTE_EXPORT to project preprocessor options for dynamic library
// configurations to export their symbols.
#if defined(GTE_EXPORT)
// For the dynamic library configurations.
#define GTE_IMPEXP __declspec(dllexport)
#else
// For a client of the dynamic library or for the static library
// configurations.
#define GTE_IMPEXP
#endif
// Expose exactly one of these.
#define GTE_USE_ROW_MAJOR
//#define GTE_USE_COL_MAJOR
// Expose exactly one of these.
#define GTE_USE_MAT_VEC
//#define GTE_USE_VEC_MAT
#if (defined(GTE_USE_ROW_MAJOR) && defined(GTE_USE_COL_MAJOR)) || (!defined(GTE_USE_ROW_MAJOR) && !defined(GTE_USE_COL_MAJOR))
#error Exactly one storage order must be specified.
#endif
#if (defined(GTE_USE_MAT_VEC) && defined(GTE_USE_VEC_MAT)) || (!defined(GTE_USE_MAT_VEC) && !defined(GTE_USE_VEC_MAT))
#error Exactly one multiplication convention must be specified.
#endif
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.1 (2018/10/05)
#pragma once
#ifndef GTE_DCPQUERY_H
#define GTE_DCPQUERY_H
#include <Mathematics/GteMath.h>
namespace gte
{
// Distance and closest-point queries.
template <typename Real, typename Type0, typename Type1>
class DCPQuery
{
public:
struct Result
{
// A DCPQuery-base class B must define a B::Result struct with member
// 'Real distance'. A DCPQuery-derived class D must also derive a
// D::Result from B:Result but may have no members. The idea is to
// allow Result to store closest-point information in addition to the
// distance. The operator() is non-const to allow DCPQuery to store
// and modify private state that supports the query.
};
Result operator()(Type0 const& primitive0, Type1 const& primitive1);
};
}
#endif
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)
#pragma once
#ifndef GTE_DIST_LINE3_TRIANGLE3_H
#define GTE_DIST_LINE3_TRIANGLE3_H
#include <Mathematics/GteVector3.h>
#include <Mathematics/GteDistLineSegment.h>
#include <Mathematics/GteTriangle.h>
#include <limits>
namespace gte
{
template <typename Real>
class DCPQuery<Real, Line3<Real>, Triangle3<Real>>
{
public:
struct Result
{
Real distance, sqrDistance;
Real lineParameter, triangleParameter[3];
Vector3<Real> closestPoint[2];
};
Result operator()(Line3<Real> const& line,
Triangle3<Real> const& triangle);
};
template <typename Real>
typename DCPQuery<Real, Line3<Real>, Triangle3<Real>>::Result
DCPQuery<Real, Line3<Real>, Triangle3<Real>>::operator()(
Line3<Real> const& line, Triangle3<Real> const& triangle)
{
Result result;
// Test if line intersects triangle. If so, the squared distance is zero.
Vector3<Real> edge0 = triangle.v[1] - triangle.v[0];
Vector3<Real> edge1 = triangle.v[2] - triangle.v[0];
Vector3<Real> normal = UnitCross(edge0, edge1);
Real NdD = Dot(normal, line.direction);
if (std::abs(NdD) > (Real)0)
{
// The line and triangle are not parallel, so the line intersects
// the plane of the triangle.
Vector3<Real> diff = line.origin - triangle.v[0];
Vector3<Real> basis[3]; // {D, U, V}
basis[0] = line.direction;
ComputeOrthogonalComplement<Real>(1, basis);
Real UdE0 = Dot(basis[1], edge0);
Real UdE1 = Dot(basis[1], edge1);
Real UdDiff = Dot(basis[1], diff);
Real VdE0 = Dot(basis[2], edge0);
Real VdE1 = Dot(basis[2], edge1);
Real VdDiff = Dot(basis[2], diff);
Real invDet = ((Real)1) / (UdE0*VdE1 - UdE1*VdE0);
// Barycentric coordinates for the point of intersection.
Real b1 = (VdE1*UdDiff - UdE1*VdDiff)*invDet;
Real b2 = (UdE0*VdDiff - VdE0*UdDiff)*invDet;
Real b0 = (Real)1 - b1 - b2;
if (b0 >= (Real)0 && b1 >= (Real)0 && b2 >= (Real)0)
{
// Line parameter for the point of intersection.
Real DdE0 = Dot(line.direction, edge0);
Real DdE1 = Dot(line.direction, edge1);
Real DdDiff = Dot(line.direction, diff);
result.lineParameter = b1*DdE0 + b2*DdE1 - DdDiff;
// Barycentric coordinates for the point of intersection.
result.triangleParameter[0] = b0;
result.triangleParameter[1] = b1;
result.triangleParameter[2] = b2;
// The intersection point is inside or on the triangle.
result.closestPoint[0] = line.origin +
result.lineParameter*line.direction;
result.closestPoint[1] = triangle.v[0] + b1*edge0 + b2*edge1;
result.distance = (Real)0;
result.sqrDistance = (Real)0;
return result;
}
}
// Either (1) the line is not parallel to the triangle and the point of
// intersection of the line and the plane of the triangle is outside the
// triangle or (2) the line and triangle are parallel. Regardless, the
// closest point on the triangle is on an edge of the triangle. Compare
// the line to all three edges of the triangle.
result.distance = std::numeric_limits<Real>::max();
result.sqrDistance = std::numeric_limits<Real>::max();
for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
Vector3<Real> segCenter = ((Real)0.5)*(triangle.v[i0] +
triangle.v[i1]);
Vector3<Real> segDirection = triangle.v[i1] - triangle.v[i0];
Real segExtent = ((Real)0.5)*Normalize(segDirection);
Segment3<Real> segment(segCenter, segDirection, segExtent);
DCPQuery<Real, Line3<Real>, Segment3<Real>> query;
auto lsResult = query(line, segment);
if (lsResult.sqrDistance < result.sqrDistance)
{
result.sqrDistance = lsResult.sqrDistance;
result.distance = lsResult.distance;
result.lineParameter = lsResult.parameter[0];
result.triangleParameter[i0] = ((Real)0.5)*((Real)1 -
lsResult.parameter[0] / segExtent);
result.triangleParameter[i1] = (Real)1 -
result.triangleParameter[i0];
result.triangleParameter[3 - i0 - i1] = (Real)0;
result.closestPoint[0] = lsResult.closestPoint[0];
result.closestPoint[1] = lsResult.closestPoint[1];
}
}
return result;
}
}
#endif
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.1 (2018/10/05)
#pragma once
#ifndef GTE_DIST_LINE_SEGMENT_H
#define GTE_DIST_LINE_SEGMENT_H
#include <Mathematics/GteDCPQuery.h>
#include <Mathematics/GteLine.h>
#include <Mathematics/GteSegment.h>
namespace gte
{
template <int N, typename Real>
class DCPQuery<Real, Line<N, Real>, Segment<N, Real>>
{
public:
struct Result
{
Real distance, sqrDistance;
Real parameter[2];
Vector<N, Real> closestPoint[2];
};
// The centered form of the 'segment' is used. Thus, parameter[1] of
// the result is in [-e,e], where e = |segment.p[1] - segment.p[0]|/2.
Result operator()(Line<N, Real> const& line,
Segment<N, Real> const& segment);
};
// Template aliases for convenience.
template <int N, typename Real>
using DCPLineSegment = DCPQuery<Real, Line<N, Real>, Segment<N, Real>>;
template <typename Real>
using DCPLine2Segment2 = DCPLineSegment<2, Real>;
template <typename Real>
using DCPLine3Segment3 = DCPLineSegment<3, Real>;
template <int N, typename Real>
typename DCPQuery<Real, Line<N, Real>, Segment<N, Real>>::Result
DCPQuery<Real, Line<N, Real>, Segment<N, Real>>::operator()(
Line<N, Real> const& line, Segment<N, Real> const& segment)
{
Result result;
Vector<N, Real> segCenter, segDirection;
Real segExtent;
segment.GetCenteredForm(segCenter, segDirection, segExtent);
Vector<N, Real> diff = line.origin - segCenter;
Real a01 = -Dot(line.direction, segDirection);
Real b0 = Dot(diff, line.direction);
Real s0, s1;
if (std::abs(a01) < (Real)1)
{
// The line and segment are not parallel.
Real det = (Real)1 - a01 * a01;
Real extDet = segExtent * det;
Real b1 = -Dot(diff, segDirection);
s1 = a01 * b0 - b1;
if (s1 >= -extDet)
{
if (s1 <= extDet)
{
// Two interior points are closest, one on the line and one
// on the segment.
s0 = (a01 * b1 - b0) / det;
s1 /= det;
}
else
{
// The endpoint e1 of the segment and an interior point of
// the line are closest.
s1 = segExtent;
s0 = -(a01 * s1 + b0);
}
}
else
{
// The endpoint e0 of the segment and an interior point of the
// line are closest.
s1 = -segExtent;
s0 = -(a01 * s1 + b0);
}
}
else
{
// The line and segment are parallel. Choose the closest pair so that
// one point is at segment origin.
s1 = (Real)0;
s0 = -b0;
}
result.parameter[0] = s0;
result.parameter[1] = s1;
result.closestPoint[0] = line.origin + s0 * line.direction;
result.closestPoint[1] = segCenter + s1 * segDirection;
diff = result.closestPoint[0] - result.closestPoint[1];
result.sqrDistance = Dot(diff, diff);
result.distance = std::sqrt(result.sqrDistance);
return result;
}
}
#endif
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.1 (2018/10/05)
#pragma once
#ifndef GTE_DIST_POINT_TRIANGLE_H
#define GTE_DIST_POINT_TRIANGLE_H
#include <Mathematics/GteVector.h>
#include <Mathematics/GteDCPQuery.h>
#include <Mathematics/GteTriangle.h>
namespace gte
{
template <int N, typename Real>
class DCPQuery<Real, Vector<N, Real>, Triangle<N, Real>>
{
public:
struct Result
{
Real distance, sqrDistance;
Real parameter[3]; // barycentric coordinates for triangle.v[3]
Vector<N, Real> closest;
};
Result operator()(Vector<N, Real> const& point,
Triangle<N, Real> const& triangle);
private:
inline void GetMinEdge02(Real const& a11, Real const& b1,
Vector<2, Real>& p) const;
inline void GetMinEdge12(Real const& a01, Real const& a11, Real const& b1,
Real const& f10, Real const& f01, Vector<2, Real>& p) const;
inline void GetMinInterior(Vector<2, Real> const& p0, Real const& h0,
Vector<2, Real> const& p1, Real const& h1, Vector<2, Real>& p) const;
};
// Template aliases for convenience.
template <int N, typename Real>
using DCPPointTriangle =
DCPQuery<Real, Vector<N, Real>, Triangle<N, Real>>;
template <typename Real>
using DCPPoint2Triangle2 = DCPPointTriangle<2, Real>;
template <typename Real>
using DCPPoint3Triangle3 = DCPPointTriangle<3, Real>;
template <int N, typename Real> inline
void DCPQuery<Real, Vector<N, Real>, Triangle<N, Real>>::GetMinEdge02(
Real const& a11, Real const& b1, Vector<2, Real>& p) const
{
p[0] = (Real)0;
if (b1 >= (Real)0)
{
p[1] = (Real)0;
}
else if (a11 + b1 <= (Real)0)
{
p[1] = (Real)1;
}
else
{
p[1] = -b1 / a11;
}
}
template <int N, typename Real> inline
void DCPQuery<Real, Vector<N, Real>, Triangle<N, Real>>::GetMinEdge12(
Real const& a01, Real const& a11, Real const& b1, Real const& f10,
Real const& f01, Vector<2, Real>& p) const
{
Real h0 = a01 + b1 - f10;
if (h0 >= (Real)0)
{
p[1] = (Real)0;
}
else
{
Real h1 = a11 + b1 - f01;
if (h1 <= (Real)0)
{
p[1] = (Real)1;
}
else
{
p[1] = h0 / (h0 - h1);
}
}
p[0] = (Real)1 - p[1];
}
template <int N, typename Real> inline
void DCPQuery<Real, Vector<N, Real>, Triangle<N, Real>>::GetMinInterior(
Vector<2, Real> const& p0, Real const& h0, Vector<2, Real> const& p1,
Real const& h1, Vector<2, Real>& p) const
{
Real z = h0 / (h0 - h1);
p = ((Real)1 - z) * p0 + z * p1;
}
template <int N,typename Real>
typename DCPQuery<Real, Vector<N, Real>, Triangle<N, Real>>::Result
DCPQuery<Real, Vector<N, Real>, Triangle<N, Real>>::operator()(
Vector<N, Real> const& point, Triangle<N, Real> const& triangle)
{
Vector<N, Real> diff = point - triangle.v[0];
Vector<N, Real> edge0 = triangle.v[1] - triangle.v[0];
Vector<N, Real> edge1 = triangle.v[2] - triangle.v[0];
Real a00 = Dot(edge0, edge0);
Real a01 = Dot(edge0, edge1);
Real a11 = Dot(edge1, edge1);
Real b0 = -Dot(diff, edge0);
Real b1 = -Dot(diff, edge1);
Real f00 = b0;
Real f10 = b0 + a00;
Real f01 = b0 + a01;
Vector<2, Real> p0, p1, p;
Real dt1, h0, h1;
// Compute the endpoints p0 and p1 of the segment. The segment is
// parameterized by L(z) = (1-z)*p0 + z*p1 for z in [0,1] and the
// directional derivative of half the quadratic on the segment is
// H(z) = Dot(p1-p0,gradient[Q](L(z))/2), where gradient[Q]/2 = (F,G).
// By design, F(L(z)) = 0 for cases (2), (4), (5), and (6). Cases (1) and
// (3) can correspond to no-intersection or intersection of F = 0 with the
// triangle.
if (f00 >= (Real)0)
{
if (f01 >= (Real)0)
{
// (1) p0 = (0,0), p1 = (0,1), H(z) = G(L(z))
GetMinEdge02(a11, b1, p);
}
else
{
// (2) p0 = (0,t10), p1 = (t01,1-t01), H(z) = (t11 - t10)*G(L(z))
p0[0] = (Real)0;
p0[1] = f00 / (f00 - f01);
p1[0] = f01 / (f01 - f10);
p1[1] = (Real)1 - p1[0];
dt1 = p1[1] - p0[1];
h0 = dt1 * (a11 * p0[1] + b1);
if (h0 >= (Real)0)
{
GetMinEdge02(a11, b1, p);
}
else
{
h1 = dt1 * (a01 * p1[0] + a11 * p1[1] + b1);
if (h1 <= (Real)0)
{
GetMinEdge12(a01, a11, b1, f10, f01, p);
}
else
{
GetMinInterior(p0, h0, p1, h1, p);
}
}
}
}
else if (f01 <= (Real)0)
{
if (f10 <= (Real)0)
{
// (3) p0 = (1,0), p1 = (0,1), H(z) = G(L(z)) - F(L(z))
GetMinEdge12(a01, a11, b1, f10, f01, p);
}
else
{
// (4) p0 = (t00,0), p1 = (t01,1-t01), H(z) = t11*G(L(z))
p0[0] = f00 / (f00 - f10);
p0[1] = (Real)0;
p1[0] = f01 / (f01 - f10);
p1[1] = (Real)1 - p1[0];
h0 = p1[1] * (a01 * p0[0] + b1);
if (h0 >= (Real)0)
{
p = p0; // GetMinEdge01
}
else
{
h1 = p1[1] * (a01 * p1[0] + a11 * p1[1] + b1);
if (h1 <= (Real)0)
{
GetMinEdge12(a01, a11, b1, f10, f01, p);
}
else
{
GetMinInterior(p0, h0, p1, h1, p);
}
}
}
}
else if (f10 <= (Real)0)
{
// (5) p0 = (0,t10), p1 = (t01,1-t01), H(z) = (t11 - t10)*G(L(z))
p0[0] = (Real)0;
p0[1] = f00 / (f00 - f01);
p1[0] = f01 / (f01 - f10);
p1[1] = (Real)1 - p1[0];
dt1 = p1[1] - p0[1];
h0 = dt1 * (a11 * p0[1] + b1);
if (h0 >= (Real)0)
{
GetMinEdge02(a11, b1, p);
}
else
{
h1 = dt1 * (a01 * p1[0] + a11 * p1[1] + b1);
if (h1 <= (Real)0)
{