diff --git a/src/geometric-tools/GTEngineDEF.h b/src/geometric-tools/GTEngineDEF.h
new file mode 100644
index 0000000000000000000000000000000000000000..88d1869e2b65ba82d5c055109d72f7cdca221b62
--- /dev/null
+++ b/src/geometric-tools/GTEngineDEF.h
@@ -0,0 +1,80 @@
+// 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
diff --git a/src/geometric-tools/Mathematics/GteDCPQuery.h b/src/geometric-tools/Mathematics/GteDCPQuery.h
new file mode 100644
index 0000000000000000000000000000000000000000..3bf5c6b97b40d4eaf369911382bda2fe3c8def10
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteDCPQuery.h
@@ -0,0 +1,35 @@
+// 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
diff --git a/src/geometric-tools/Mathematics/GteDistLine3Triangle3.h b/src/geometric-tools/Mathematics/GteDistLine3Triangle3.h
new file mode 100644
index 0000000000000000000000000000000000000000..993bb4736afb1e13293c13be5ab25e3c4cfce5f7
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteDistLine3Triangle3.h
@@ -0,0 +1,130 @@
+// 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
diff --git a/src/geometric-tools/Mathematics/GteDistLineSegment.h b/src/geometric-tools/Mathematics/GteDistLineSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..f002dfabb1c3145149d6a68de01fafb4a7fe4048
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteDistLineSegment.h
@@ -0,0 +1,116 @@
+// 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
diff --git a/src/geometric-tools/Mathematics/GteDistPointTriangle.h b/src/geometric-tools/Mathematics/GteDistPointTriangle.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ab7942bd2f8c49c446071178bd854eb8cd08cc5
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteDistPointTriangle.h
@@ -0,0 +1,269 @@
+// 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)
+ {
+ GetMinEdge12(a01, a11, b1, f10, f01, p);
+ }
+ else
+ {
+ GetMinInterior(p0, h0, p1, h1, p);
+ }
+ }
+ }
+ else
+ {
+ // (6) p0 = (t00,0), p1 = (0,t11), H(z) = t11*G(L(z))
+ p0[0] = f00 / (f00 - f10);
+ p0[1] = (Real)0;
+ p1[0] = (Real)0;
+ p1[1] = f00 / (f00 - f01);
+ h0 = p1[1] * (a01 * p0[0] + b1);
+ if (h0 >= (Real)0)
+ {
+ p = p0; // GetMinEdge01
+ }
+ else
+ {
+ h1 = p1[1] * (a11 * p1[1] + b1);
+ if (h1 <= (Real)0)
+ {
+ GetMinEdge02(a11, b1, p);
+ }
+ else
+ {
+ GetMinInterior(p0, h0, p1, h1, p);
+ }
+ }
+ }
+
+ Result result;
+ result.parameter[0] = (Real)1 - p[0] - p[1];
+ result.parameter[1] = p[0];
+ result.parameter[2] = p[1];
+ result.closest = triangle.v[0] + p[0] * edge0 + p[1] * edge1;
+ diff = point - result.closest;
+ result.sqrDistance = Dot(diff, diff);
+ result.distance = std::sqrt(result.sqrDistance);
+ return result;
+}
+
+
+}
+#endif
diff --git a/src/geometric-tools/Mathematics/GteDistSegment3Triangle3.h b/src/geometric-tools/Mathematics/GteDistSegment3Triangle3.h
new file mode 100644
index 0000000000000000000000000000000000000000..39c71f56383926ba70b3d2d0b9c1574fdb2e4b4a
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteDistSegment3Triangle3.h
@@ -0,0 +1,91 @@
+// 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_SEGMENT3_TRIANGLE3_H
+#define GTE_DIST_SEGMENT3_TRIANGLE3_H
+
+#include <Mathematics/GteDistLine3Triangle3.h>
+#include <Mathematics/GteDistPointTriangle.h>
+#include <Mathematics/GteSegment.h>
+
+namespace gte
+{
+
+template <typename Real>
+class DCPQuery<Real, Segment3<Real>, Triangle3<Real>>
+{
+public:
+ struct Result
+ {
+ Real distance, sqrDistance;
+ Real segmentParameter, triangleParameter[3];
+ Vector3<Real> closestPoint[2];
+ };
+
+ Result operator()(Segment3<Real> const& segment,
+ Triangle3<Real> const& triangle);
+};
+
+
+template <typename Real>
+typename DCPQuery<Real, Segment3<Real>, Triangle3<Real>>::Result
+DCPQuery<Real, Segment3<Real>, Triangle3<Real>>::operator()(
+ Segment3<Real> const& segment, Triangle3<Real> const& triangle)
+{
+ Result result;
+
+ Vector3<Real> segCenter, segDirection;
+ Real segExtent;
+ segment.GetCenteredForm(segCenter, segDirection, segExtent);
+
+ Line3<Real> line(segCenter, segDirection);
+ DCPQuery<Real, Line3<Real>, Triangle3<Real>> ltQuery;
+ auto ltResult = ltQuery(line, triangle);
+
+ if (ltResult.lineParameter >= -segExtent)
+ {
+ if (ltResult.lineParameter <= segExtent)
+ {
+ result.distance = ltResult.distance;
+ result.sqrDistance = ltResult.sqrDistance;
+ result.segmentParameter = ltResult.lineParameter;
+ result.triangleParameter[0] = ltResult.triangleParameter[0];
+ result.triangleParameter[1] = ltResult.triangleParameter[1];
+ result.triangleParameter[2] = ltResult.triangleParameter[2];
+ result.closestPoint[0] = ltResult.closestPoint[0];
+ result.closestPoint[1] = ltResult.closestPoint[1];
+ }
+ else
+ {
+ DCPQuery<Real, Vector3<Real>, Triangle3<Real>> ptQuery;
+ Vector3<Real> point = segCenter + segExtent*segDirection;
+ auto ptResult = ptQuery(point, triangle);
+ result.sqrDistance = ptResult.sqrDistance;
+ result.distance = ptResult.distance;
+ result.segmentParameter = segExtent;
+ result.closestPoint[0] = point;
+ result.closestPoint[1] = ptResult.closest;
+ }
+ }
+ else
+ {
+ DCPQuery<Real, Vector3<Real>, Triangle3<Real>> ptQuery;
+ Vector3<Real> point = segCenter - segExtent*segDirection;
+ auto ptResult = ptQuery(point, triangle);
+ result.sqrDistance = ptResult.sqrDistance;
+ result.distance = ptResult.distance;
+ result.segmentParameter = segExtent;
+ result.closestPoint[0] = point;
+ result.closestPoint[1] = ptResult.closest;
+ }
+ return result;
+}
+
+
+}
+#endif
diff --git a/src/geometric-tools/Mathematics/GteDistTriangle3Triangle3.h b/src/geometric-tools/Mathematics/GteDistTriangle3Triangle3.h
new file mode 100644
index 0000000000000000000000000000000000000000..503c08f002c65c4b76895524ee5df6f81353d48e
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteDistTriangle3Triangle3.h
@@ -0,0 +1,103 @@
+// 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_TRIANGLE3_TRIANGLE3_H
+#define GTE_DIST_TRIANGLE3_TRIANGLE3_H
+
+#include <Mathematics/GteDistSegment3Triangle3.h>
+
+namespace gte
+{
+
+template <typename Real>
+class DCPQuery<Real, Triangle3<Real>, Triangle3<Real>>
+{
+public:
+ struct Result
+ {
+ Real distance, sqrDistance;
+ Real triangle0Parameter[3], triangle1Parameter[3];
+ Vector3<Real> closestPoint[2];
+ };
+
+ Result operator()(Triangle3<Real> const& triangle0,
+ Triangle3<Real> const& triangle1);
+};
+
+
+template <typename Real>
+typename DCPQuery<Real, Triangle3<Real>, Triangle3<Real>>::Result
+DCPQuery<Real, Triangle3<Real>, Triangle3<Real>>::operator()(
+ Triangle3<Real> const& triangle0, Triangle3<Real> const& triangle1)
+{
+ Result result;
+
+ DCPQuery<Real, Segment3<Real>, Triangle3<Real>> stQuery;
+ typename DCPQuery<Real, Segment3<Real>, Triangle3<Real>>::Result
+ stResult;
+ result.sqrDistance = std::numeric_limits<Real>::max();
+
+ // Compare edges of triangle0 to the interior of triangle1.
+ for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
+ {
+ Vector3<Real> segCenter = ((Real)0.5)*(triangle0.v[i0] +
+ triangle0.v[i1]);
+ Vector3<Real> segDirection = triangle0.v[i1] - triangle0.v[i0];
+ Real segExtent = ((Real)0.5)*Normalize(segDirection);
+ Segment3<Real> edge(segCenter, segDirection, segExtent);
+
+ stResult = stQuery(edge, triangle1);
+ if (stResult.sqrDistance < result.sqrDistance)
+ {
+ result.distance = stResult.distance;
+ result.sqrDistance = stResult.sqrDistance;
+ Real ratio = stResult.segmentParameter / segExtent; // in [-1,1]
+ result.triangle0Parameter[i0] = ((Real)0.5)*((Real)1 - ratio);
+ result.triangle0Parameter[i1] =
+ (Real)1 - result.triangle0Parameter[i0];
+ result.triangle0Parameter[3 - i0 - i1] = (Real)0;
+ result.triangle1Parameter[0] = stResult.triangleParameter[0];
+ result.triangle1Parameter[1] = stResult.triangleParameter[1];
+ result.triangle1Parameter[2] = stResult.triangleParameter[2];
+ result.closestPoint[0] = stResult.closestPoint[0];
+ result.closestPoint[1] = stResult.closestPoint[1];
+ }
+ }
+
+ // Compare edges of triangle1 to the interior of triangle0.
+ for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
+ {
+ Vector3<Real> segCenter = ((Real)0.5)*(triangle1.v[i0] +
+ triangle1.v[i1]);
+ Vector3<Real> segDirection = triangle1.v[i1] - triangle1.v[i0];
+ Real segExtent = ((Real)0.5)*Normalize(segDirection);
+ Segment3<Real> edge(segCenter, segDirection, segExtent);
+
+ stResult = stQuery(edge, triangle0);
+ if (stResult.sqrDistance < result.sqrDistance)
+ {
+ result.distance = stResult.distance;
+ result.sqrDistance = stResult.sqrDistance;
+ Real ratio = stResult.segmentParameter / segExtent; // in [-1,1]
+ result.triangle0Parameter[0] = stResult.triangleParameter[0];
+ result.triangle0Parameter[1] = stResult.triangleParameter[1];
+ result.triangle0Parameter[2] = stResult.triangleParameter[2];
+ result.triangle1Parameter[i0] = ((Real)0.5)*((Real)1 - ratio);
+ result.triangle1Parameter[i1] =
+ (Real)1 - result.triangle0Parameter[i0];
+ result.triangle1Parameter[3 - i0 - i1] = (Real)0;
+ result.closestPoint[0] = stResult.closestPoint[0];
+ result.closestPoint[1] = stResult.closestPoint[1];
+ }
+ }
+ return result;
+}
+
+
+}
+#endif
diff --git a/src/geometric-tools/Mathematics/GteLine.h b/src/geometric-tools/Mathematics/GteLine.h
new file mode 100644
index 0000000000000000000000000000000000000000..106631f93120d6f4f8c1711fbda621590e456636
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteLine.h
@@ -0,0 +1,115 @@
+// 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_LINE_H
+#define GTE_LINE_H
+
+#include <Mathematics/GteVector.h>
+
+// The line is represented by P+t*D, where P is an origin point, D is a
+// unit-length direction vector, and t is any real number. The user must
+// ensure that D is unit length.
+
+namespace gte
+{
+
+template <int N, typename Real>
+class Line
+{
+public:
+ // Construction and destruction. The default constructor sets the origin
+ // to (0,...,0) and the line direction to (1,0,...,0).
+ Line();
+ Line(Vector<N, Real> const& inOrigin, Vector<N, Real> const& inDirection);
+
+ // Public member access. The direction must be unit length.
+ Vector<N, Real> origin, direction;
+
+public:
+ // Comparisons to support sorted containers.
+ bool operator==(Line const& line) const;
+ bool operator!=(Line const& line) const;
+ bool operator< (Line const& line) const;
+ bool operator<=(Line const& line) const;
+ bool operator> (Line const& line) const;
+ bool operator>=(Line const& line) const;
+};
+
+// Template aliases for convenience.
+template <typename Real>
+using Line2 = Line<2, Real>;
+
+template <typename Real>
+using Line3 = Line<3, Real>;
+
+
+template <int N, typename Real>
+Line<N, Real>::Line()
+{
+ origin.MakeZero();
+ direction.MakeUnit(0);
+}
+
+template <int N, typename Real>
+Line<N, Real>::Line(Vector<N, Real> const& inOrigin,
+ Vector<N, Real> const& inDirection)
+ :
+ origin(inOrigin),
+ direction(inDirection)
+{
+}
+
+template <int N, typename Real>
+bool Line<N, Real>::operator==(Line const& line) const
+{
+ return origin == line.origin && direction == line.direction;
+}
+
+template <int N, typename Real>
+bool Line<N, Real>::operator!=(Line const& line) const
+{
+ return !operator==(line);
+}
+
+template <int N, typename Real>
+bool Line<N, Real>::operator<(Line const& line) const
+{
+ if (origin < line.origin)
+ {
+ return true;
+ }
+
+ if (origin > line.origin)
+ {
+ return false;
+ }
+
+ return direction < line.direction;
+}
+
+template <int N, typename Real>
+bool Line<N, Real>::operator<=(Line const& line) const
+{
+ return operator<(line) || operator==(line);
+}
+
+template <int N, typename Real>
+bool Line<N, Real>::operator>(Line const& line) const
+{
+ return !operator<=(line);
+}
+
+template <int N, typename Real>
+bool Line<N, Real>::operator>=(Line const& line) const
+{
+ return !operator<(line);
+}
+
+
+}
+#endif
diff --git a/src/geometric-tools/Mathematics/GteMath.h b/src/geometric-tools/Mathematics/GteMath.h
new file mode 100644
index 0000000000000000000000000000000000000000..7a1a40a730ae14f695bb834fd3ebac75a5cebe78
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteMath.h
@@ -0,0 +1,616 @@
+// 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.17.0 (2018/10/05)
+
+#pragma once
+#ifndef GTE_MATH_H
+#define GTE_MATH_H
+
+// This file extends the <cmath> support to include convenient constants and
+// functions. The shared constants for CPU, Intel SSE and GPU lead to
+// correctly rounded approximations of the constants when using 'float' or
+// 'double'.
+
+#include <GTEngineDEF.h>
+#include <cmath>
+#include <limits>
+
+// Maximum number of iterations for bisection before a subinterval
+// degenerates to a single point. TODO: Verify these. I used the formula:
+// 3 + std::numeric_limits<T>::digits - std::numeric_limits<T>::min_exponent.
+// IEEEBinary16: digits = 11, min_exponent = -13
+// float: digits = 27, min_exponent = -125
+// double: digits = 53, min_exponent = -1021
+// BSNumber and BSRational use std::numeric_limits<unsigned int>::max(),
+// but maybe these should be set to a large number or be user configurable.
+// The MAX_BISECTIONS_GENERIC is an arbitrary choice for now and is used
+// in template code where Real is the template parameter.
+#define GTE_C_MAX_BISECTIONS_FLOAT16 27u
+#define GTE_C_MAX_BISECTIONS_FLOAT32 155u
+#define GTE_C_MAX_BISECTIONS_FLOAT64 1077u
+#define GTE_C_MAX_BISECTIONS_BSNUMBER 0xFFFFFFFFu
+#define GTE_C_MAX_BISECTIONS_BSRATIONAL 0xFFFFFFFFu
+#define GTE_C_MAX_BISECTIONS_GENERIC 2048u
+
+// Constants involving pi.
+#define GTE_C_PI 3.1415926535897931
+#define GTE_C_HALF_PI 1.5707963267948966
+#define GTE_C_QUARTER_PI 0.7853981633974483
+#define GTE_C_TWO_PI 6.2831853071795862
+#define GTE_C_INV_PI 0.3183098861837907
+#define GTE_C_INV_TWO_PI 0.1591549430918953
+#define GTE_C_INV_HALF_PI 0.6366197723675813
+
+// Conversions between degrees and radians.
+#define GTE_C_DEG_TO_RAD 0.0174532925199433
+#define GTE_C_RAD_TO_DEG 57.295779513082321
+
+// Common constants.
+#define GTE_C_SQRT_2 1.4142135623730951
+#define GTE_C_INV_SQRT_2 0.7071067811865475
+#define GTE_C_LN_2 0.6931471805599453
+#define GTE_C_INV_LN_2 1.4426950408889634
+#define GTE_C_LN_10 2.3025850929940459
+#define GTE_C_INV_LN_10 0.43429448190325176
+
+// Constants for minimax polynomial approximations to sqrt(x).
+// The algorithm minimizes the maximum absolute error on [1,2].
+#define GTE_C_SQRT_DEG1_C0 +1.0
+#define GTE_C_SQRT_DEG1_C1 +4.1421356237309505e-01
+#define GTE_C_SQRT_DEG1_MAX_ERROR 1.7766952966368793e-2
+
+#define GTE_C_SQRT_DEG2_C0 +1.0
+#define GTE_C_SQRT_DEG2_C1 +4.8563183076125260e-01
+#define GTE_C_SQRT_DEG2_C2 -7.1418268388157458e-02
+#define GTE_C_SQRT_DEG2_MAX_ERROR 1.1795695163108744e-3
+
+#define GTE_C_SQRT_DEG3_C0 +1.0
+#define GTE_C_SQRT_DEG3_C1 +4.9750045320242231e-01
+#define GTE_C_SQRT_DEG3_C2 -1.0787308044477850e-01
+#define GTE_C_SQRT_DEG3_C3 +2.4586189615451115e-02
+#define GTE_C_SQRT_DEG3_MAX_ERROR 1.1309620116468910e-4
+
+#define GTE_C_SQRT_DEG4_C0 +1.0
+#define GTE_C_SQRT_DEG4_C1 +4.9955939832918816e-01
+#define GTE_C_SQRT_DEG4_C2 -1.2024066151943025e-01
+#define GTE_C_SQRT_DEG4_C3 +4.5461507257698486e-02
+#define GTE_C_SQRT_DEG4_C4 -1.0566681694362146e-02
+#define GTE_C_SQRT_DEG4_MAX_ERROR 1.2741170151556180e-5
+
+#define GTE_C_SQRT_DEG5_C0 +1.0
+#define GTE_C_SQRT_DEG5_C1 +4.9992197660031912e-01
+#define GTE_C_SQRT_DEG5_C2 -1.2378506719245053e-01
+#define GTE_C_SQRT_DEG5_C3 +5.6122776972699739e-02
+#define GTE_C_SQRT_DEG5_C4 -2.3128836281145482e-02
+#define GTE_C_SQRT_DEG5_C5 +5.0827122737047148e-03
+#define GTE_C_SQRT_DEG5_MAX_ERROR 1.5725568940708201e-6
+
+#define GTE_C_SQRT_DEG6_C0 +1.0
+#define GTE_C_SQRT_DEG6_C1 +4.9998616695784914e-01
+#define GTE_C_SQRT_DEG6_C2 -1.2470733323278438e-01
+#define GTE_C_SQRT_DEG6_C3 +6.0388587356982271e-02
+#define GTE_C_SQRT_DEG6_C4 -3.1692053551807930e-02
+#define GTE_C_SQRT_DEG6_C5 +1.2856590305148075e-02
+#define GTE_C_SQRT_DEG6_C6 -2.6183954624343642e-03
+#define GTE_C_SQRT_DEG6_MAX_ERROR 2.0584155535630089e-7
+
+#define GTE_C_SQRT_DEG7_C0 +1.0
+#define GTE_C_SQRT_DEG7_C1 +4.9999754817809228e-01
+#define GTE_C_SQRT_DEG7_C2 -1.2493243476353655e-01
+#define GTE_C_SQRT_DEG7_C3 +6.1859954146370910e-02
+#define GTE_C_SQRT_DEG7_C4 -3.6091595023208356e-02
+#define GTE_C_SQRT_DEG7_C5 +1.9483946523450868e-02
+#define GTE_C_SQRT_DEG7_C6 -7.5166134568007692e-03
+#define GTE_C_SQRT_DEG7_C7 +1.4127567687864939e-03
+#define GTE_C_SQRT_DEG7_MAX_ERROR 2.8072302919734948e-8
+
+#define GTE_C_SQRT_DEG8_C0 +1.0
+#define GTE_C_SQRT_DEG8_C1 +4.9999956583056759e-01
+#define GTE_C_SQRT_DEG8_C2 -1.2498490369914350e-01
+#define GTE_C_SQRT_DEG8_C3 +6.2318494667579216e-02
+#define GTE_C_SQRT_DEG8_C4 -3.7982961896432244e-02
+#define GTE_C_SQRT_DEG8_C5 +2.3642612312869460e-02
+#define GTE_C_SQRT_DEG8_C6 -1.2529377587270574e-02
+#define GTE_C_SQRT_DEG8_C7 +4.5382426960713929e-03
+#define GTE_C_SQRT_DEG8_C8 -7.8810995273670414e-04
+#define GTE_C_SQRT_DEG8_MAX_ERROR 3.9460605685825989e-9
+
+// Constants for minimax polynomial approximations to 1/sqrt(x).
+// The algorithm minimizes the maximum absolute error on [1,2].
+#define GTE_C_INVSQRT_DEG1_C0 +1.0
+#define GTE_C_INVSQRT_DEG1_C1 -2.9289321881345254e-01
+#define GTE_C_INVSQRT_DEG1_MAX_ERROR 3.7814314552701983e-2
+
+#define GTE_C_INVSQRT_DEG2_C0 +1.0
+#define GTE_C_INVSQRT_DEG2_C1 -4.4539812104566801e-01
+#define GTE_C_INVSQRT_DEG2_C2 +1.5250490223221547e-01
+#define GTE_C_INVSQRT_DEG2_MAX_ERROR 4.1953446330581234e-3
+
+#define GTE_C_INVSQRT_DEG3_C0 +1.0
+#define GTE_C_INVSQRT_DEG3_C1 -4.8703230993068791e-01
+#define GTE_C_INVSQRT_DEG3_C2 +2.8163710486669835e-01
+#define GTE_C_INVSQRT_DEG3_C3 -8.7498013749463421e-02
+#define GTE_C_INVSQRT_DEG3_MAX_ERROR 5.6307702007266786e-4
+
+#define GTE_C_INVSQRT_DEG4_C0 +1.0
+#define GTE_C_INVSQRT_DEG4_C1 -4.9710061558048779e-01
+#define GTE_C_INVSQRT_DEG4_C2 +3.4266247597676802e-01
+#define GTE_C_INVSQRT_DEG4_C3 -1.9106356536293490e-01
+#define GTE_C_INVSQRT_DEG4_C4 +5.2608486153198797e-02
+#define GTE_C_INVSQRT_DEG4_MAX_ERROR 8.1513919987605266e-5
+
+#define GTE_C_INVSQRT_DEG5_C0 +1.0
+#define GTE_C_INVSQRT_DEG5_C1 -4.9937760586004143e-01
+#define GTE_C_INVSQRT_DEG5_C2 +3.6508741295133973e-01
+#define GTE_C_INVSQRT_DEG5_C3 -2.5884890281853501e-01
+#define GTE_C_INVSQRT_DEG5_C4 +1.3275782221320753e-01
+#define GTE_C_INVSQRT_DEG5_C5 -3.2511945299404488e-02
+#define GTE_C_INVSQRT_DEG5_MAX_ERROR 1.2289367475583346e-5
+
+#define GTE_C_INVSQRT_DEG6_C0 +1.0
+#define GTE_C_INVSQRT_DEG6_C1 -4.9987029229547453e-01
+#define GTE_C_INVSQRT_DEG6_C2 +3.7220923604495226e-01
+#define GTE_C_INVSQRT_DEG6_C3 -2.9193067713256937e-01
+#define GTE_C_INVSQRT_DEG6_C4 +1.9937605991094642e-01
+#define GTE_C_INVSQRT_DEG6_C5 -9.3135712130901993e-02
+#define GTE_C_INVSQRT_DEG6_C6 +2.0458166789566690e-02
+#define GTE_C_INVSQRT_DEG6_MAX_ERROR 1.9001451223750465e-6
+
+#define GTE_C_INVSQRT_DEG7_C0 +1.0
+#define GTE_C_INVSQRT_DEG7_C1 -4.9997357250704977e-01
+#define GTE_C_INVSQRT_DEG7_C2 +3.7426216884998809e-01
+#define GTE_C_INVSQRT_DEG7_C3 -3.0539882498248971e-01
+#define GTE_C_INVSQRT_DEG7_C4 +2.3976005607005391e-01
+#define GTE_C_INVSQRT_DEG7_C5 -1.5410326351684489e-01
+#define GTE_C_INVSQRT_DEG7_C6 +6.5598809723041995e-02
+#define GTE_C_INVSQRT_DEG7_C7 -1.3038592450470787e-02
+#define GTE_C_INVSQRT_DEG7_MAX_ERROR 2.9887724993168940e-7
+
+#define GTE_C_INVSQRT_DEG8_C0 +1.0
+#define GTE_C_INVSQRT_DEG8_C1 -4.9999471066120371e-01
+#define GTE_C_INVSQRT_DEG8_C2 +3.7481415745794067e-01
+#define GTE_C_INVSQRT_DEG8_C3 -3.1023804387422160e-01
+#define GTE_C_INVSQRT_DEG8_C4 +2.5977002682930106e-01
+#define GTE_C_INVSQRT_DEG8_C5 -1.9818790717727097e-01
+#define GTE_C_INVSQRT_DEG8_C6 +1.1882414252613671e-01
+#define GTE_C_INVSQRT_DEG8_C7 -4.6270038088550791e-02
+#define GTE_C_INVSQRT_DEG8_C8 +8.3891541755747312e-03
+#define GTE_C_INVSQRT_DEG8_MAX_ERROR 4.7596926146947771e-8
+
+// Constants for minimax polynomial approximations to sin(x).
+// The algorithm minimizes the maximum absolute error on [-pi/2,pi/2].
+#define GTE_C_SIN_DEG3_C0 +1.0
+#define GTE_C_SIN_DEG3_C1 -1.4727245910375519e-01
+#define GTE_C_SIN_DEG3_MAX_ERROR 1.3481903639145865e-2
+
+#define GTE_C_SIN_DEG5_C0 +1.0
+#define GTE_C_SIN_DEG5_C1 -1.6600599923812209e-01
+#define GTE_C_SIN_DEG5_C2 +7.5924178409012000e-03
+#define GTE_C_SIN_DEG5_MAX_ERROR 1.4001209384639779e-4
+
+#define GTE_C_SIN_DEG7_C0 +1.0
+#define GTE_C_SIN_DEG7_C1 -1.6665578084732124e-01
+#define GTE_C_SIN_DEG7_C2 +8.3109378830028557e-03
+#define GTE_C_SIN_DEG7_C3 -1.8447486103462252e-04
+#define GTE_C_SIN_DEG7_MAX_ERROR 1.0205878936686563e-6
+
+#define GTE_C_SIN_DEG9_C0 +1.0
+#define GTE_C_SIN_DEG9_C1 -1.6666656235308897e-01
+#define GTE_C_SIN_DEG9_C2 +8.3329962509886002e-03
+#define GTE_C_SIN_DEG9_C3 -1.9805100675274190e-04
+#define GTE_C_SIN_DEG9_C4 +2.5967200279475300e-06
+#define GTE_C_SIN_DEG9_MAX_ERROR 5.2010746265374053e-9
+
+#define GTE_C_SIN_DEG11_C0 +1.0
+#define GTE_C_SIN_DEG11_C1 -1.6666666601721269e-01
+#define GTE_C_SIN_DEG11_C2 +8.3333303183525942e-03
+#define GTE_C_SIN_DEG11_C3 -1.9840782426250314e-04
+#define GTE_C_SIN_DEG11_C4 +2.7521557770526783e-06
+#define GTE_C_SIN_DEG11_C5 -2.3828544692960918e-08
+#define GTE_C_SIN_DEG11_MAX_ERROR 1.9295870457014530e-11
+
+// Constants for minimax polynomial approximations to cos(x).
+// The algorithm minimizes the maximum absolute error on [-pi/2,pi/2].
+#define GTE_C_COS_DEG2_C0 +1.0
+#define GTE_C_COS_DEG2_C1 -4.0528473456935105e-01
+#define GTE_C_COS_DEG2_MAX_ERROR 5.4870946878404048e-2
+
+#define GTE_C_COS_DEG4_C0 +1.0
+#define GTE_C_COS_DEG4_C1 -4.9607181958647262e-01
+#define GTE_C_COS_DEG4_C2 +3.6794619653489236e-02
+#define GTE_C_COS_DEG4_MAX_ERROR 9.1879932449712154e-4
+
+#define GTE_C_COS_DEG6_C0 +1.0
+#define GTE_C_COS_DEG6_C1 -4.9992746217057404e-01
+#define GTE_C_COS_DEG6_C2 +4.1493920348353308e-02
+#define GTE_C_COS_DEG6_C3 -1.2712435011987822e-03
+#define GTE_C_COS_DEG6_MAX_ERROR 9.2028470133065365e-6
+
+#define GTE_C_COS_DEG8_C0 +1.0
+#define GTE_C_COS_DEG8_C1 -4.9999925121358291e-01
+#define GTE_C_COS_DEG8_C2 +4.1663780117805693e-02
+#define GTE_C_COS_DEG8_C3 -1.3854239405310942e-03
+#define GTE_C_COS_DEG8_C4 +2.3154171575501259e-05
+#define GTE_C_COS_DEG8_MAX_ERROR 5.9804533020235695e-8
+
+#define GTE_C_COS_DEG10_C0 +1.0
+#define GTE_C_COS_DEG10_C1 -4.9999999508695869e-01
+#define GTE_C_COS_DEG10_C2 +4.1666638865338612e-02
+#define GTE_C_COS_DEG10_C3 -1.3888377661039897e-03
+#define GTE_C_COS_DEG10_C4 +2.4760495088926859e-05
+#define GTE_C_COS_DEG10_C5 -2.6051615464872668e-07
+#define GTE_C_COS_DEG10_MAX_ERROR 2.7006769043325107e-10
+
+// Constants for minimax polynomial approximations to tan(x).
+// The algorithm minimizes the maximum absolute error on [-pi/4,pi/4].
+#define GTE_C_TAN_DEG3_C0 1.0
+#define GTE_C_TAN_DEG3_C1 4.4295926544736286e-01
+#define GTE_C_TAN_DEG3_MAX_ERROR 1.1661892256204731e-2
+
+#define GTE_C_TAN_DEG5_C0 1.0
+#define GTE_C_TAN_DEG5_C1 3.1401320403542421e-01
+#define GTE_C_TAN_DEG5_C2 2.0903948109240345e-01
+#define GTE_C_TAN_DEG5_MAX_ERROR 5.8431854390143118e-4
+
+#define GTE_C_TAN_DEG7_C0 1.0
+#define GTE_C_TAN_DEG7_C1 3.3607213284422555e-01
+#define GTE_C_TAN_DEG7_C2 1.1261037305184907e-01
+#define GTE_C_TAN_DEG7_C3 9.8352099470524479e-02
+#define GTE_C_TAN_DEG7_MAX_ERROR 3.5418688397723108e-5
+
+#define GTE_C_TAN_DEG9_C0 1.0
+#define GTE_C_TAN_DEG9_C1 3.3299232843941784e-01
+#define GTE_C_TAN_DEG9_C2 1.3747843432474838e-01
+#define GTE_C_TAN_DEG9_C3 3.7696344813028304e-02
+#define GTE_C_TAN_DEG9_C4 4.6097377279281204e-02
+#define GTE_C_TAN_DEG9_MAX_ERROR 2.2988173242199927e-6
+
+#define GTE_C_TAN_DEG11_C0 1.0
+#define GTE_C_TAN_DEG11_C1 3.3337224456224224e-01
+#define GTE_C_TAN_DEG11_C2 1.3264516053824593e-01
+#define GTE_C_TAN_DEG11_C3 5.8145237645931047e-02
+#define GTE_C_TAN_DEG11_C4 1.0732193237572574e-02
+#define GTE_C_TAN_DEG11_C5 2.1558456793513869e-02
+#define GTE_C_TAN_DEG11_MAX_ERROR 1.5426257940140409e-7
+
+#define GTE_C_TAN_DEG13_C0 1.0
+#define GTE_C_TAN_DEG13_C1 3.3332916426394554e-01
+#define GTE_C_TAN_DEG13_C2 1.3343404625112498e-01
+#define GTE_C_TAN_DEG13_C3 5.3104565343119248e-02
+#define GTE_C_TAN_DEG13_C4 2.5355038312682154e-02
+#define GTE_C_TAN_DEG13_C5 1.8253255966556026e-03
+#define GTE_C_TAN_DEG13_C6 1.0069407176615641e-02
+#define GTE_C_TAN_DEG13_MAX_ERROR 1.0550264249037378e-8
+
+// Constants for minimax polynomial approximations to acos(x), where the
+// approximation is of the form acos(x) = sqrt(1 - x)*p(x) with p(x) a
+// polynomial. The algorithm minimizes the maximum error
+// |acos(x)/sqrt(1-x) - p(x)| on [0,1]. At the same time we get an
+// approximation for asin(x) = pi/2 - acos(x).
+#define GTE_C_ACOS_DEG1_C0 +1.5707963267948966
+#define GTE_C_ACOS_DEG1_C1 -1.5658276442180141e-01
+#define GTE_C_ACOS_DEG1_MAX_ERROR 1.1659002803738105e-2
+
+#define GTE_C_ACOS_DEG2_C0 +1.5707963267948966
+#define GTE_C_ACOS_DEG2_C1 -2.0347053865798365e-01
+#define GTE_C_ACOS_DEG2_C2 +4.6887774236182234e-02
+#define GTE_C_ACOS_DEG2_MAX_ERROR 9.0311602490029258e-4
+
+#define GTE_C_ACOS_DEG3_C0 +1.5707963267948966
+#define GTE_C_ACOS_DEG3_C1 -2.1253291899190285e-01
+#define GTE_C_ACOS_DEG3_C2 +7.4773789639484223e-02
+#define GTE_C_ACOS_DEG3_C3 -1.8823635069382449e-02
+#define GTE_C_ACOS_DEG3_MAX_ERROR 9.3066396954288172e-5
+
+#define GTE_C_ACOS_DEG4_C0 +1.5707963267948966
+#define GTE_C_ACOS_DEG4_C1 -2.1422258835275865e-01
+#define GTE_C_ACOS_DEG4_C2 +8.4936675142844198e-02
+#define GTE_C_ACOS_DEG4_C3 -3.5991475120957794e-02
+#define GTE_C_ACOS_DEG4_C4 +8.6946239090712751e-03
+#define GTE_C_ACOS_DEG4_MAX_ERROR 1.0930595804481413e-5
+
+#define GTE_C_ACOS_DEG5_C0 +1.5707963267948966
+#define GTE_C_ACOS_DEG5_C1 -2.1453292139805524e-01
+#define GTE_C_ACOS_DEG5_C2 +8.7973089282889383e-02
+#define GTE_C_ACOS_DEG5_C3 -4.5130266382166440e-02
+#define GTE_C_ACOS_DEG5_C4 +1.9467466687281387e-02
+#define GTE_C_ACOS_DEG5_C5 -4.3601326117634898e-03
+#define GTE_C_ACOS_DEG5_MAX_ERROR 1.3861070257241426-6
+
+#define GTE_C_ACOS_DEG6_C0 +1.5707963267948966
+#define GTE_C_ACOS_DEG6_C1 -2.1458939285677325e-01
+#define GTE_C_ACOS_DEG6_C2 +8.8784960563641491e-02
+#define GTE_C_ACOS_DEG6_C3 -4.8887131453156485e-02
+#define GTE_C_ACOS_DEG6_C4 +2.7011519960012720e-02
+#define GTE_C_ACOS_DEG6_C5 -1.1210537323478320e-02
+#define GTE_C_ACOS_DEG6_C6 +2.3078166879102469e-03
+#define GTE_C_ACOS_DEG6_MAX_ERROR 1.8491291330427484e-7
+
+#define GTE_C_ACOS_DEG7_C0 +1.5707963267948966
+#define GTE_C_ACOS_DEG7_C1 -2.1459960076929829e-01
+#define GTE_C_ACOS_DEG7_C2 +8.8986946573346160e-02
+#define GTE_C_ACOS_DEG7_C3 -5.0207843052845647e-02
+#define GTE_C_ACOS_DEG7_C4 +3.0961594977611639e-02
+#define GTE_C_ACOS_DEG7_C5 -1.7162031184398074e-02
+#define GTE_C_ACOS_DEG7_C6 +6.7072304676685235e-03
+#define GTE_C_ACOS_DEG7_C7 -1.2690614339589956e-03
+#define GTE_C_ACOS_DEG7_MAX_ERROR 2.5574620927948377e-8
+
+#define GTE_C_ACOS_DEG8_C0 +1.5707963267948966
+#define GTE_C_ACOS_DEG8_C1 -2.1460143648688035e-01
+#define GTE_C_ACOS_DEG8_C2 +8.9034700107934128e-02
+#define GTE_C_ACOS_DEG8_C3 -5.0625279962389413e-02
+#define GTE_C_ACOS_DEG8_C4 +3.2683762943179318e-02
+#define GTE_C_ACOS_DEG8_C5 -2.0949278766238422e-02
+#define GTE_C_ACOS_DEG8_C6 +1.1272900916992512e-02
+#define GTE_C_ACOS_DEG8_C7 -4.1160981058965262e-03
+#define GTE_C_ACOS_DEG8_C8 +7.1796493341480527e-04
+#define GTE_C_ACOS_DEG8_MAX_ERROR 3.6340015129032732e-9
+
+// Constants for minimax polynomial approximations to atan(x).
+// The algorithm minimizes the maximum absolute error on [-1,1].
+#define GTE_C_ATAN_DEG3_C0 +1.0
+#define GTE_C_ATAN_DEG3_C1 -2.1460183660255172e-01
+#define GTE_C_ATAN_DEG3_MAX_ERROR 1.5970326392614240e-2
+
+#define GTE_C_ATAN_DEG5_C0 +1.0
+#define GTE_C_ATAN_DEG5_C1 -3.0189478312144946e-01
+#define GTE_C_ATAN_DEG5_C2 +8.7292946518897740e-02
+#define GTE_C_ATAN_DEG5_MAX_ERROR 1.3509832247372636e-3
+
+#define GTE_C_ATAN_DEG7_C0 +1.0
+#define GTE_C_ATAN_DEG7_C1 -3.2570157599356531e-01
+#define GTE_C_ATAN_DEG7_C2 +1.5342994884206673e-01
+#define GTE_C_ATAN_DEG7_C3 -4.2330209451053591e-02
+#define GTE_C_ATAN_DEG7_MAX_ERROR 1.5051227215514412e-4
+
+#define GTE_C_ATAN_DEG9_C0 +1.0
+#define GTE_C_ATAN_DEG9_C1 -3.3157878236439586e-01
+#define GTE_C_ATAN_DEG9_C2 +1.8383034738018011e-01
+#define GTE_C_ATAN_DEG9_C3 -8.9253037587244677e-02
+#define GTE_C_ATAN_DEG9_C4 +2.2399635968909593e-02
+#define GTE_C_ATAN_DEG9_MAX_ERROR 1.8921598624582064e-5
+
+#define GTE_C_ATAN_DEG11_C0 +1.0
+#define GTE_C_ATAN_DEG11_C1 -3.3294527685374087e-01
+#define GTE_C_ATAN_DEG11_C2 +1.9498657165383548e-01
+#define GTE_C_ATAN_DEG11_C3 -1.1921576270475498e-01
+#define GTE_C_ATAN_DEG11_C4 +5.5063351366968050e-02
+#define GTE_C_ATAN_DEG11_C5 -1.2490720064867844e-02
+#define GTE_C_ATAN_DEG11_MAX_ERROR 2.5477724974187765e-6
+
+#define GTE_C_ATAN_DEG13_C0 +1.0
+#define GTE_C_ATAN_DEG13_C1 -3.3324998579202170e-01
+#define GTE_C_ATAN_DEG13_C2 +1.9856563505717162e-01
+#define GTE_C_ATAN_DEG13_C3 -1.3374657325451267e-01
+#define GTE_C_ATAN_DEG13_C4 +8.1675882859940430e-02
+#define GTE_C_ATAN_DEG13_C5 -3.5059680836411644e-02
+#define GTE_C_ATAN_DEG13_C6 +7.2128853633444123e-03
+#define GTE_C_ATAN_DEG13_MAX_ERROR 3.5859104691865484e-7
+
+// Constants for minimax polynomial approximations to exp2(x) = 2^x.
+// The algorithm minimizes the maximum absolute error on [0,1].
+#define GTE_C_EXP2_DEG1_C0 1.0
+#define GTE_C_EXP2_DEG1_C1 1.0
+#define GTE_C_EXP2_DEG1_MAX_ERROR 8.6071332055934313e-2
+
+#define GTE_C_EXP2_DEG2_C0 1.0
+#define GTE_C_EXP2_DEG2_C1 6.5571332605741528e-01
+#define GTE_C_EXP2_DEG2_C2 3.4428667394258472e-01
+#define GTE_C_EXP2_DEG2_MAX_ERROR 3.8132476831060358e-3
+
+#define GTE_C_EXP2_DEG3_C0 1.0
+#define GTE_C_EXP2_DEG3_C1 6.9589012084456225e-01
+#define GTE_C_EXP2_DEG3_C2 2.2486494900110188e-01
+#define GTE_C_EXP2_DEG3_C3 7.9244930154334980e-02
+#define GTE_C_EXP2_DEG3_MAX_ERROR 1.4694877755186408e-4
+
+#define GTE_C_EXP2_DEG4_C0 1.0
+#define GTE_C_EXP2_DEG4_C1 6.9300392358459195e-01
+#define GTE_C_EXP2_DEG4_C2 2.4154981722455560e-01
+#define GTE_C_EXP2_DEG4_C3 5.1744260331489045e-02
+#define GTE_C_EXP2_DEG4_C4 1.3701998859367848e-02
+#define GTE_C_EXP2_DEG4_MAX_ERROR 4.7617792624521371e-6
+
+#define GTE_C_EXP2_DEG5_C0 1.0
+#define GTE_C_EXP2_DEG5_C1 6.9315298010274962e-01
+#define GTE_C_EXP2_DEG5_C2 2.4014712313022102e-01
+#define GTE_C_EXP2_DEG5_C3 5.5855296413199085e-02
+#define GTE_C_EXP2_DEG5_C4 8.9477503096873079e-03
+#define GTE_C_EXP2_DEG5_C5 1.8968500441332026e-03
+#define GTE_C_EXP2_DEG5_MAX_ERROR 1.3162098333463490e-7
+
+#define GTE_C_EXP2_DEG6_C0 1.0
+#define GTE_C_EXP2_DEG6_C1 6.9314698914837525e-01
+#define GTE_C_EXP2_DEG6_C2 2.4023013440952923e-01
+#define GTE_C_EXP2_DEG6_C3 5.5481276898206033e-02
+#define GTE_C_EXP2_DEG6_C4 9.6838443037086108e-03
+#define GTE_C_EXP2_DEG6_C5 1.2388324048515642e-03
+#define GTE_C_EXP2_DEG6_C6 2.1892283501756538e-04
+#define GTE_C_EXP2_DEG6_MAX_ERROR 3.1589168225654163e-9
+
+#define GTE_C_EXP2_DEG7_C0 1.0
+#define GTE_C_EXP2_DEG7_C1 6.9314718588750690e-01
+#define GTE_C_EXP2_DEG7_C2 2.4022637363165700e-01
+#define GTE_C_EXP2_DEG7_C3 5.5505235570535660e-02
+#define GTE_C_EXP2_DEG7_C4 9.6136265387940512e-03
+#define GTE_C_EXP2_DEG7_C5 1.3429234504656051e-03
+#define GTE_C_EXP2_DEG7_C6 1.4299202757683815e-04
+#define GTE_C_EXP2_DEG7_C7 2.1662892777385423e-05
+#define GTE_C_EXP2_DEG7_MAX_ERROR 6.6864513925679603e-11
+
+// Constants for minimax polynomial approximations to log2(x).
+// The algorithm minimizes the maximum absolute error on [1,2].
+// The polynomials all have constant term zero.
+#define GTE_C_LOG2_DEG1_C1 +1.0
+#define GTE_C_LOG2_DEG1_MAX_ERROR 8.6071332055934202e-2
+
+#define GTE_C_LOG2_DEG2_C1 +1.3465553856377803
+#define GTE_C_LOG2_DEG2_C2 -3.4655538563778032e-01
+#define GTE_C_LOG2_DEG2_MAX_ERROR 7.6362868906658110e-3
+
+#define GTE_C_LOG2_DEG3_C1 +1.4228653756681227
+#define GTE_C_LOG2_DEG3_C2 -5.8208556916449616e-01
+#define GTE_C_LOG2_DEG3_C3 +1.5922019349637218e-01
+#define GTE_C_LOG2_DEG3_MAX_ERROR 8.7902902652883808e-4
+
+#define GTE_C_LOG2_DEG4_C1 +1.4387257478171547
+#define GTE_C_LOG2_DEG4_C2 -6.7778401359918661e-01
+#define GTE_C_LOG2_DEG4_C3 +3.2118898377713379e-01
+#define GTE_C_LOG2_DEG4_C4 -8.2130717995088531e-02
+#define GTE_C_LOG2_DEG4_MAX_ERROR 1.1318551355360418e-4
+
+#define GTE_C_LOG2_DEG5_C1 +1.4419170408633741
+#define GTE_C_LOG2_DEG5_C2 -7.0909645927612530e-01
+#define GTE_C_LOG2_DEG5_C3 +4.1560609399164150e-01
+#define GTE_C_LOG2_DEG5_C4 -1.9357573729558908e-01
+#define GTE_C_LOG2_DEG5_C5 +4.5149061716699634e-02
+#define GTE_C_LOG2_DEG5_MAX_ERROR 1.5521274478735858e-5
+
+#define GTE_C_LOG2_DEG6_C1 +1.4425449435950917
+#define GTE_C_LOG2_DEG6_C2 -7.1814525675038965e-01
+#define GTE_C_LOG2_DEG6_C3 +4.5754919692564044e-01
+#define GTE_C_LOG2_DEG6_C4 -2.7790534462849337e-01
+#define GTE_C_LOG2_DEG6_C5 +1.2179791068763279e-01
+#define GTE_C_LOG2_DEG6_C6 -2.5841449829670182e-02
+#define GTE_C_LOG2_DEG6_MAX_ERROR 2.2162051216689793e-6
+
+#define GTE_C_LOG2_DEG7_C1 +1.4426664401536078
+#define GTE_C_LOG2_DEG7_C2 -7.2055423726162360e-01
+#define GTE_C_LOG2_DEG7_C3 +4.7332419162501083e-01
+#define GTE_C_LOG2_DEG7_C4 -3.2514018752954144e-01
+#define GTE_C_LOG2_DEG7_C5 +1.9302965529095673e-01
+#define GTE_C_LOG2_DEG7_C6 -7.8534970641157997e-02
+#define GTE_C_LOG2_DEG7_C7 +1.5209108363023915e-02
+#define GTE_C_LOG2_DEG7_MAX_ERROR 3.2546531700261561e-7
+
+#define GTE_C_LOG2_DEG8_C1 +1.4426896453621882
+#define GTE_C_LOG2_DEG8_C2 -7.2115893912535967e-01
+#define GTE_C_LOG2_DEG8_C3 +4.7861716616785088e-01
+#define GTE_C_LOG2_DEG8_C4 -3.4699935395019565e-01
+#define GTE_C_LOG2_DEG8_C5 +2.4114048765477492e-01
+#define GTE_C_LOG2_DEG8_C6 -1.3657398692885181e-01
+#define GTE_C_LOG2_DEG8_C7 +5.1421382871922106e-02
+#define GTE_C_LOG2_DEG8_C8 -9.1364020499895560e-03
+#define GTE_C_LOG2_DEG8_MAX_ERROR 4.8796219218050219e-8
+
+// These functions are convenient for some applications. The classes
+// BSNumber, BSRational and IEEEBinary16 have implementations that
+// (for now) use typecasting to call the 'float' or 'double' versions.
+namespace gte
+{
+ inline float atandivpi(float x)
+ {
+ return std::atan(x) * (float)GTE_C_INV_PI;
+ }
+
+ inline float atan2divpi(float y, float x)
+ {
+ return std::atan2(y, x) * (float)GTE_C_INV_PI;
+ }
+
+ inline float clamp(float x, float xmin, float xmax)
+ {
+ return (x <= xmin ? xmin : (x >= xmax ? xmax : x));
+ }
+
+ inline float cospi(float x)
+ {
+ return std::cos(x * (float)GTE_C_PI);
+ }
+
+ inline float exp10(float x)
+ {
+ return std::exp(x * (float)GTE_C_LN_10);
+ }
+
+ inline float invsqrt(float x)
+ {
+ return 1.0f / std::sqrt(x);
+ }
+
+ inline int isign(float x)
+ {
+ return (x > 0.0f ? 1 : (x < 0.0f ? -1 : 0));
+ }
+
+ inline float saturate(float x)
+ {
+ return (x <= 0.0f ? 0.0f : (x >= 1.0f ? 1.0f : x));
+ }
+
+ inline float sign(float x)
+ {
+ return (x > 0.0f ? 1.0f : (x < 0.0f ? -1.0f : 0.0f));
+ }
+
+ inline float sinpi(float x)
+ {
+ return std::sin(x * (float)GTE_C_PI);
+ }
+
+ inline float sqr(float x)
+ {
+ return x * x;
+ }
+
+
+ inline double atandivpi(double x)
+ {
+ return std::atan(x) * GTE_C_INV_PI;
+ }
+
+ inline double atan2divpi(double y, double x)
+ {
+ return std::atan2(y, x) * GTE_C_INV_PI;
+ }
+
+ inline double clamp(double x, double xmin, double xmax)
+ {
+ return (x <= xmin ? xmin : (x >= xmax ? xmax : x));
+ }
+
+ inline double cospi(double x)
+ {
+ return std::cos(x * GTE_C_PI);
+ }
+
+ inline double exp10(double x)
+ {
+ return std::exp(x * GTE_C_LN_10);
+ }
+
+ inline double invsqrt(double x)
+ {
+ return 1.0 / std::sqrt(x);
+ }
+
+ inline double sign(double x)
+ {
+ return (x > 0.0 ? 1.0 : (x < 0.0 ? -1.0 : 0.0f));
+ }
+
+ inline int isign(double x)
+ {
+ return (x > 0.0 ? 1 : (x < 0.0 ? -1 : 0));
+ }
+
+ inline double saturate(double x)
+ {
+ return (x <= 0.0 ? 0.0 : (x >= 1.0 ? 1.0 : x));
+ }
+
+ inline double sinpi(double x)
+ {
+ return std::sin(x * GTE_C_PI);
+ }
+
+ inline double sqr(double x)
+ {
+ return x * x;
+ }
+}
+#endif
diff --git a/src/geometric-tools/Mathematics/GteSegment.h b/src/geometric-tools/Mathematics/GteSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..15f31e95a8ea2c1ea398a697b8c2d47b44d634e5
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteSegment.h
@@ -0,0 +1,151 @@
+// 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_SEGMENT_H
+#define GTE_SEGMENT_H
+
+#include <Mathematics/GteVector.h>
+
+// The segment is represented by (1-t)*P0 + t*P1, where P0 and P1 are the
+// endpoints of the segment and 0 <= t <= 1. Some algorithms prefer a
+// centered representation that is similar to how oriented bounding boxes are
+// defined. This representation is C + s*D, where C = (P0 + P1)/2 is the
+// center of the segment, D = (P1 - P0)/|P1 - P0| is a unit-length direction
+// vector for the segment, and |t| <= e. The value e = |P1 - P0|/2 is the
+// extent (or radius or half-length) of the segment.
+
+namespace gte
+{
+
+template <int N, typename Real>
+class Segment
+{
+public:
+ // Construction and destruction. The default constructor sets p0 to
+ // (-1,0,...,0) and p1 to (1,0,...,0). NOTE: If you set p0 and p1;
+ // compute C, D, and e; and then recompute q0 = C-e*D and q1 = C+e*D,
+ // numerical round-off errors can lead to q0 not exactly equal to p0
+ // and q1 not exactly equal to p1.
+ Segment();
+ Segment(Vector<N, Real> const& p0, Vector<N, Real> const& p1);
+ Segment(std::array<Vector<N, Real>, 2> const& inP);
+ Segment(Vector<N, Real> const& center, Vector<N, Real> const& direction,
+ Real extent);
+
+ // Manipulation via the centered form.
+ void SetCenteredForm(Vector<N, Real> const& center,
+ Vector<N, Real> const& direction, Real extent);
+
+ void GetCenteredForm(Vector<N, Real>& center, Vector<N, Real>& direction,
+ Real& extent) const;
+
+ // Public member access.
+ std::array<Vector<N, Real>, 2> p;
+
+public:
+ // Comparisons to support sorted containers.
+ bool operator==(Segment const& segment) const;
+ bool operator!=(Segment const& segment) const;
+ bool operator< (Segment const& segment) const;
+ bool operator<=(Segment const& segment) const;
+ bool operator> (Segment const& segment) const;
+ bool operator>=(Segment const& segment) const;
+};
+
+// Template aliases for convenience.
+template <typename Real>
+using Segment2 = Segment<2, Real>;
+
+template <typename Real>
+using Segment3 = Segment<3, Real>;
+
+
+template <int N, typename Real>
+Segment<N, Real>::Segment()
+{
+ p[1].MakeUnit(0);
+ p[0] = -p[1];
+}
+
+template <int N, typename Real>
+Segment<N, Real>::Segment(Vector<N, Real> const& p0,
+ Vector<N, Real> const& p1)
+{
+ p[0] = p0;
+ p[1] = p1;
+}
+
+template <int N, typename Real>
+Segment<N, Real>::Segment(std::array<Vector<N, Real>, 2> const& inP)
+{
+ p = inP;
+}
+
+template <int N, typename Real>
+Segment<N, Real>::Segment(Vector<N, Real> const& center,
+ Vector<N, Real> const& direction, Real extent)
+{
+ SetCenteredForm(center, direction, extent);
+}
+
+template <int N, typename Real>
+void Segment<N, Real>::SetCenteredForm(Vector<N, Real> const& center,
+ Vector<N, Real> const& direction, Real extent)
+{
+ p[0] = center - extent * direction;
+ p[1] = center + extent * direction;
+}
+
+template <int N, typename Real>
+void Segment<N, Real>::GetCenteredForm(Vector<N, Real>& center,
+ Vector<N, Real>& direction, Real& extent) const
+{
+ center = ((Real)0.5)*(p[0] + p[1]);
+ direction = p[1] - p[0];
+ extent = ((Real)0.5)*Normalize(direction);
+}
+
+template <int N, typename Real>
+bool Segment<N, Real>::operator==(Segment const& segment) const
+{
+ return p == segment.p;
+}
+
+template <int N, typename Real>
+bool Segment<N, Real>::operator!=(Segment const& segment) const
+{
+ return !operator==(segment);
+}
+
+template <int N, typename Real>
+bool Segment<N, Real>::operator<(Segment const& segment) const
+{
+ return p < segment.p;
+}
+
+template <int N, typename Real>
+bool Segment<N, Real>::operator<=(Segment const& segment) const
+{
+ return operator<(segment) || operator==(segment);
+}
+
+template <int N, typename Real>
+bool Segment<N, Real>::operator>(Segment const& segment) const
+{
+ return !operator<=(segment);
+}
+
+template <int N, typename Real>
+bool Segment<N, Real>::operator>=(Segment const& segment) const
+{
+ return !operator<(segment);
+}
+
+
+}
+#endif
diff --git a/src/geometric-tools/Mathematics/GteTriangle.h b/src/geometric-tools/Mathematics/GteTriangle.h
new file mode 100644
index 0000000000000000000000000000000000000000..366a89cfdd554bbca7b24a15e294b062fd2dd4ff
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteTriangle.h
@@ -0,0 +1,114 @@
+// 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_TRIANGLE_H
+#define GTE_TRIANGLE_H
+
+#include <Mathematics/GteVector.h>
+
+// The triangle is represented as an array of three vertices. The dimension
+// N must be 2 or larger.
+
+namespace gte
+{
+
+template <int N, typename Real>
+class Triangle
+{
+public:
+ // Construction and destruction. The default constructor sets the
+ // vertices to (0,..,0), (1,0,...,0), and (0,1,0,...,0).
+ Triangle();
+ Triangle(Vector<N, Real> const& v0, Vector<N, Real> const& v1,
+ Vector<N, Real> const& v2);
+ Triangle(std::array<Vector<N, Real>, 3> const& inV);
+
+ // Public member access.
+ std::array<Vector<N, Real>, 3> v;
+
+public:
+ // Comparisons to support sorted containers.
+ bool operator==(Triangle const& triangle) const;
+ bool operator!=(Triangle const& triangle) const;
+ bool operator< (Triangle const& triangle) const;
+ bool operator<=(Triangle const& triangle) const;
+ bool operator> (Triangle const& triangle) const;
+ bool operator>=(Triangle const& triangle) const;
+};
+
+// Template aliases for convenience.
+template <typename Real>
+using Triangle2 = Triangle<2, Real>;
+
+template <typename Real>
+using Triangle3 = Triangle<3, Real>;
+
+
+template <int N, typename Real>
+Triangle<N, Real>::Triangle()
+{
+ v[0].MakeZero();
+ v[1].MakeUnit(0);
+ v[2].MakeUnit(1);
+}
+
+template <int N, typename Real>
+Triangle<N, Real>::Triangle(Vector<N, Real> const& v0,
+ Vector<N, Real> const& v1, Vector<N, Real> const& v2)
+{
+ v[0] = v0;
+ v[1] = v1;
+ v[2] = v2;
+}
+
+template <int N, typename Real>
+Triangle<N, Real>::Triangle(std::array<Vector<N, Real>, 3> const& inV)
+ :
+ v(inV)
+{
+}
+
+template <int N, typename Real>
+bool Triangle<N, Real>::operator==(Triangle const& triangle) const
+{
+ return v == triangle.v;
+}
+
+template <int N, typename Real>
+bool Triangle<N, Real>::operator!=(Triangle const& triangle) const
+{
+ return !operator==(triangle);
+}
+
+template <int N, typename Real>
+bool Triangle<N, Real>::operator<(Triangle const& triangle) const
+{
+ return v < triangle.v;
+}
+
+template <int N, typename Real>
+bool Triangle<N, Real>::operator<=(Triangle const& triangle) const
+{
+ return operator<(triangle) || operator==(triangle);
+}
+
+template <int N, typename Real>
+bool Triangle<N, Real>::operator>(Triangle const& triangle) const
+{
+ return !operator<=(triangle);
+}
+
+template <int N, typename Real>
+bool Triangle<N, Real>::operator>=(Triangle const& triangle) const
+{
+ return !operator<(triangle);
+}
+
+
+}
+#endif
diff --git a/src/geometric-tools/Mathematics/GteVector.h b/src/geometric-tools/Mathematics/GteVector.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f96ac7246c110ed9f385686ddf51122ebcbec8a
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteVector.h
@@ -0,0 +1,695 @@
+// 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.3 (2018/10/05)
+
+#pragma once
+#ifndef GTE_VECTOR_H
+#define GTE_VECTOR_H
+
+#include <GTEngineDEF.h>
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <initializer_list>
+
+namespace gte
+{
+
+template <int N, typename Real>
+class Vector
+{
+public:
+ // The tuple is uninitialized.
+ Vector();
+
+ // The tuple is fully initialized by the inputs.
+ Vector(std::array<Real, N> const& values);
+
+ // At most N elements are copied from the initializer list, setting any
+ // remaining elements to zero. Create the zero vector using the syntax
+ // Vector<N,Real> zero{(Real)0};
+ // WARNING: The C++ 11 specification states that
+ // Vector<N,Real> zero{};
+ // will lead to a call of the default constructor, not the initializer
+ // constructor!
+ Vector(std::initializer_list<Real> values);
+
+ // For 0 <= d < N, element d is 1 and all others are 0. If d is invalid,
+ // the zero vector is created. This is a convenience for creating the
+ // standard Euclidean basis vectors; see also MakeUnit(int) and Unit(int).
+ Vector(int d);
+
+ // The copy constructor, destructor, and assignment operator are generated
+ // by the compiler.
+
+ // Member access. The first operator[] returns a const reference rather
+ // than a Real value. This supports writing via standard file operations
+ // that require a const pointer to data.
+ inline int GetSize() const; // returns N
+ inline Real const& operator[](int i) const;
+ inline Real& operator[](int i);
+
+ // Comparisons for sorted containers and geometric ordering.
+ inline bool operator==(Vector const& vec) const;
+ inline bool operator!=(Vector const& vec) const;
+ inline bool operator< (Vector const& vec) const;
+ inline bool operator<=(Vector const& vec) const;
+ inline bool operator> (Vector const& vec) const;
+ inline bool operator>=(Vector const& vec) const;
+
+ // Special vectors.
+ void MakeZero(); // All components are 0.
+ void MakeUnit(int d); // Component d is 1, all others are zero.
+ static Vector Zero();
+ static Vector Unit(int d);
+
+protected:
+ // This data structure takes advantage of the built-in operator[],
+ // range checking, and visualizers in MSVS.
+ std::array<Real, N> mTuple;
+};
+
+// Unary operations.
+template <int N, typename Real>
+Vector<N, Real> operator+(Vector<N, Real> const& v);
+
+template <int N, typename Real>
+Vector<N, Real> operator-(Vector<N, Real> const& v);
+
+// Linear-algebraic operations.
+template <int N, typename Real>
+Vector<N, Real>
+operator+(Vector<N, Real> const& v0, Vector<N, Real> const& v1);
+
+template <int N, typename Real>
+Vector<N, Real>
+operator-(Vector<N, Real> const& v0, Vector<N, Real> const& v1);
+
+template <int N, typename Real>
+Vector<N, Real> operator*(Vector<N, Real> const& v, Real scalar);
+
+template <int N, typename Real>
+Vector<N, Real> operator*(Real scalar, Vector<N, Real> const& v);
+
+template <int N, typename Real>
+Vector<N, Real> operator/(Vector<N, Real> const& v, Real scalar);
+
+template <int N, typename Real>
+Vector<N, Real>& operator+=(Vector<N, Real>& v0, Vector<N, Real> const& v1);
+
+template <int N, typename Real>
+Vector<N, Real>& operator-=(Vector<N, Real>& v0, Vector<N, Real> const& v1);
+
+template <int N, typename Real>
+Vector<N, Real>& operator*=(Vector<N, Real>& v, Real scalar);
+
+template <int N, typename Real>
+Vector<N, Real>& operator/=(Vector<N, Real>& v, Real scalar);
+
+// Componentwise algebraic operations.
+template <int N, typename Real>
+Vector<N, Real>
+operator*(Vector<N, Real> const& v0, Vector<N, Real> const& v1);
+
+template <int N, typename Real>
+Vector<N, Real>
+operator/(Vector<N, Real> const& v0, Vector<N, Real> const& v1);
+
+template <int N, typename Real>
+Vector<N, Real>& operator*=(Vector<N, Real>& v0, Vector<N, Real> const& v1);
+
+template <int N, typename Real>
+Vector<N, Real>& operator/=(Vector<N, Real>& v0, Vector<N, Real> const& v1);
+
+// Geometric operations. The functions with 'robust' set to 'false' use the
+// standard algorithm for normalizing a vector by computing the length as a
+// square root of the squared length and dividing by it. The results can be
+// infinite (or NaN) if the length is zero. When 'robust' is set to 'true',
+// the algorithm is designed to avoid floating-point overflow and sets the
+// normalized vector to zero when the length is zero.
+template <int N, typename Real>
+Real Dot(Vector<N, Real> const& v0, Vector<N, Real> const& v1);
+
+template <int N, typename Real>
+Real Length(Vector<N, Real> const& v, bool robust = false);
+
+template <int N, typename Real>
+Real Normalize(Vector<N, Real>& v, bool robust = false);
+
+// Gram-Schmidt orthonormalization to generate orthonormal vectors from the
+// linearly independent inputs. The function returns the smallest length of
+// the unnormalized vectors computed during the process. If this value is
+// nearly zero, it is possible that the inputs are linearly dependent (within
+// numerical round-off errors). On input, 1 <= numElements <= N and v[0]
+// through v[numElements-1] must be initialized. On output, the vectors
+// v[0] through v[numElements-1] form an orthonormal set.
+template <int N, typename Real>
+Real Orthonormalize(int numElements, Vector<N, Real>* v, bool robust = false);
+
+// Construct a single vector orthogonal to the nonzero input vector. If the
+// maximum absolute component occurs at index i, then the orthogonal vector
+// U has u[i] = v[i+1], u[i+1] = -v[i], and all other components zero. The
+// index addition i+1 is computed modulo N.
+template <int N, typename Real>
+Vector<N, Real> GetOrthogonal(Vector<N, Real> const& v, bool unitLength);
+
+// Compute the axis-aligned bounding box of the vectors. The return value is
+// 'true' iff the inputs are valid, in which case vmin and vmax have valid
+// values.
+template <int N, typename Real>
+bool ComputeExtremes(int numVectors, Vector<N, Real> const* v,
+ Vector<N, Real>& vmin, Vector<N, Real>& vmax);
+
+// Lift n-tuple v to homogeneous (n+1)-tuple (v,last).
+template <int N, typename Real>
+Vector<N + 1, Real> HLift(Vector<N, Real> const& v, Real last);
+
+// Project homogeneous n-tuple v = (u,v[n-1]) to (n-1)-tuple u.
+template <int N, typename Real>
+Vector<N - 1, Real> HProject(Vector<N, Real> const& v);
+
+// Lift n-tuple v = (w0,w1) to (n+1)-tuple u = (w0,u[inject],w1). By
+// inference, w0 is a (inject)-tuple [nonexistent when inject=0] and w1 is a
+// (n-inject)-tuple [nonexistent when inject=n].
+template <int N, typename Real>
+Vector<N + 1, Real> Lift(Vector<N, Real> const& v, int inject, Real value);
+
+// Project n-tuple v = (w0,v[reject],w1) to (n-1)-tuple u = (w0,w1). By
+// inference, w0 is a (reject)-tuple [nonexistent when reject=0] and w1 is a
+// (n-1-reject)-tuple [nonexistent when reject=n-1].
+template <int N, typename Real>
+Vector<N - 1, Real> Project(Vector<N, Real> const& v, int reject);
+
+
+template <int N, typename Real>
+Vector<N, Real>::Vector()
+{
+ // Uninitialized.
+}
+
+template <int N, typename Real>
+Vector<N, Real>::Vector(std::array<Real, N> const& values)
+ :
+ mTuple(values)
+{
+}
+template <int N, typename Real>
+Vector<N, Real>::Vector(std::initializer_list<Real> values)
+{
+ int const numValues = static_cast<int>(values.size());
+ if (N == numValues)
+ {
+ std::copy(values.begin(), values.end(), mTuple.begin());
+ }
+ else if (N > numValues)
+ {
+ std::copy(values.begin(), values.end(), mTuple.begin());
+ std::fill(mTuple.begin() + numValues, mTuple.end(), (Real)0);
+ }
+ else // N < numValues
+ {
+ std::copy(values.begin(), values.begin() + N, mTuple.begin());
+ }
+}
+
+template <int N, typename Real>
+Vector<N, Real>::Vector(int d)
+{
+ MakeUnit(d);
+}
+
+template <int N, typename Real> inline
+int Vector<N, Real>::GetSize() const
+{
+ return N;
+}
+
+template <int N, typename Real> inline
+Real const& Vector<N, Real>::operator[](int i) const
+{
+ return mTuple[i];
+}
+
+template <int N, typename Real> inline
+Real& Vector<N, Real>::operator[](int i)
+{
+ return mTuple[i];
+}
+
+template <int N, typename Real> inline
+bool Vector<N, Real>::operator==(Vector const& vec) const
+{
+ return mTuple == vec.mTuple;
+}
+
+template <int N, typename Real> inline
+bool Vector<N, Real>::operator!=(Vector const& vec) const
+{
+ return mTuple != vec.mTuple;
+}
+
+template <int N, typename Real> inline
+bool Vector<N, Real>::operator<(Vector const& vec) const
+{
+ return mTuple < vec.mTuple;
+}
+
+template <int N, typename Real> inline
+bool Vector<N, Real>::operator<=(Vector const& vec) const
+{
+ return mTuple <= vec.mTuple;
+}
+
+template <int N, typename Real> inline
+bool Vector<N, Real>::operator>(Vector const& vec) const
+{
+ return mTuple > vec.mTuple;
+}
+
+template <int N, typename Real> inline
+bool Vector<N, Real>::operator>=(Vector const& vec) const
+{
+ return mTuple >= vec.mTuple;
+}
+
+template <int N, typename Real>
+void Vector<N, Real>::MakeZero()
+{
+ std::fill(mTuple.begin(), mTuple.end(), (Real)0);
+}
+
+template <int N, typename Real>
+void Vector<N, Real>::MakeUnit(int d)
+{
+ std::fill(mTuple.begin(), mTuple.end(), (Real)0);
+ if (0 <= d && d < N)
+ {
+ mTuple[d] = (Real)1;
+ }
+}
+
+template <int N, typename Real>
+Vector<N, Real> Vector<N, Real>::Zero()
+{
+ Vector<N, Real> v;
+ v.MakeZero();
+ return v;
+}
+
+template <int N, typename Real>
+Vector<N, Real> Vector<N, Real>::Unit(int d)
+{
+ Vector<N, Real> v;
+ v.MakeUnit(d);
+ return v;
+}
+
+
+
+template <int N, typename Real>
+Vector<N, Real> operator+(Vector<N, Real> const& v)
+{
+ return v;
+}
+
+template <int N, typename Real>
+Vector<N, Real> operator-(Vector<N, Real> const& v)
+{
+ Vector<N, Real> result;
+ for (int i = 0; i < N; ++i)
+ {
+ result[i] = -v[i];
+ }
+ return result;
+}
+
+template <int N, typename Real>
+Vector<N, Real> operator+(Vector<N, Real> const& v0,
+ Vector<N, Real> const& v1)
+{
+ Vector<N, Real> result = v0;
+ return result += v1;
+}
+
+template <int N, typename Real>
+Vector<N, Real> operator-(Vector<N, Real> const& v0,
+ Vector<N, Real> const& v1)
+{
+ Vector<N, Real> result = v0;
+ return result -= v1;
+}
+
+template <int N, typename Real>
+Vector<N, Real> operator*(Vector<N, Real> const& v, Real scalar)
+{
+ Vector<N, Real> result = v;
+ return result *= scalar;
+}
+
+template <int N, typename Real>
+Vector<N, Real> operator*(Real scalar, Vector<N, Real> const& v)
+{
+ Vector<N, Real> result = v;
+ return result *= scalar;
+}
+
+template <int N, typename Real>
+Vector<N, Real> operator/(Vector<N, Real> const& v, Real scalar)
+{
+ Vector<N, Real> result = v;
+ return result /= scalar;
+}
+
+template <int N, typename Real>
+Vector<N, Real>& operator+=(Vector<N, Real>& v0, Vector<N, Real> const& v1)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ v0[i] += v1[i];
+ }
+ return v0;
+}
+
+template <int N, typename Real>
+Vector<N, Real>& operator-=(Vector<N, Real>& v0, Vector<N, Real> const& v1)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ v0[i] -= v1[i];
+ }
+ return v0;
+}
+
+template <int N, typename Real>
+Vector<N, Real>& operator*=(Vector<N, Real>& v, Real scalar)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ v[i] *= scalar;
+ }
+ return v;
+}
+
+template <int N, typename Real>
+Vector<N, Real>& operator/=(Vector<N, Real>& v, Real scalar)
+{
+ if (scalar != (Real)0)
+ {
+ Real invScalar = ((Real)1) / scalar;
+ for (int i = 0; i < N; ++i)
+ {
+ v[i] *= invScalar;
+ }
+ }
+ else
+ {
+ for (int i = 0; i < N; ++i)
+ {
+ v[i] = (Real)0;
+ }
+ }
+ return v;
+}
+
+template <int N, typename Real>
+Vector<N, Real> operator*(Vector<N, Real> const& v0,
+ Vector<N, Real> const& v1)
+{
+ Vector<N, Real> result = v0;
+ return result *= v1;
+}
+
+template <int N, typename Real>
+Vector<N, Real> operator/(Vector<N, Real> const& v0,
+ Vector<N, Real> const& v1)
+{
+ Vector<N, Real> result = v0;
+ return result /= v1;
+}
+
+template <int N, typename Real>
+Vector<N, Real>& operator*=(Vector<N, Real>& v0, Vector<N, Real> const& v1)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ v0[i] *= v1[i];
+ }
+ return v0;
+}
+
+template <int N, typename Real>
+Vector<N, Real>& operator/=(Vector<N, Real>& v0, Vector<N, Real> const& v1)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ v0[i] /= v1[i];
+ }
+ return v0;
+}
+
+template <int N, typename Real>
+Real Dot(Vector<N, Real> const& v0, Vector<N, Real> const& v1)
+{
+ Real dot = v0[0] * v1[0];
+ for (int i = 1; i < N; ++i)
+ {
+ dot += v0[i] * v1[i];
+ }
+ return dot;
+}
+
+template <int N, typename Real>
+Real Length(Vector<N, Real> const& v, bool robust)
+{
+ if (robust)
+ {
+ Real maxAbsComp = std::abs(v[0]);
+ for (int i = 1; i < N; ++i)
+ {
+ Real absComp = std::abs(v[i]);
+ if (absComp > maxAbsComp)
+ {
+ maxAbsComp = absComp;
+ }
+ }
+
+ Real length;
+ if (maxAbsComp > (Real)0)
+ {
+ Vector<N, Real> scaled = v / maxAbsComp;
+ length = maxAbsComp * std::sqrt(Dot(scaled, scaled));
+ }
+ else
+ {
+ length = (Real)0;
+ }
+ return length;
+ }
+ else
+ {
+ return std::sqrt(Dot(v, v));
+ }
+}
+
+template <int N, typename Real>
+Real Normalize(Vector<N, Real>& v, bool robust)
+{
+ if (robust)
+ {
+ Real maxAbsComp = std::abs(v[0]);
+ for (int i = 1; i < N; ++i)
+ {
+ Real absComp = std::abs(v[i]);
+ if (absComp > maxAbsComp)
+ {
+ maxAbsComp = absComp;
+ }
+ }
+
+ Real length;
+ if (maxAbsComp > (Real)0)
+ {
+ v /= maxAbsComp;
+ length = std::sqrt(Dot(v, v));
+ v /= length;
+ length *= maxAbsComp;
+ }
+ else
+ {
+ length = (Real)0;
+ for (int i = 0; i < N; ++i)
+ {
+ v[i] = (Real)0;
+ }
+ }
+ return length;
+ }
+ else
+ {
+ Real length = std::sqrt(Dot(v, v));
+ if (length > (Real)0)
+ {
+ v /= length;
+ }
+ else
+ {
+ for (int i = 0; i < N; ++i)
+ {
+ v[i] = (Real)0;
+ }
+ }
+ return length;
+ }
+}
+
+template <int N, typename Real>
+Real Orthonormalize(int numInputs, Vector<N, Real>* v, bool robust)
+{
+ if (v && 1 <= numInputs && numInputs <= N)
+ {
+ Real minLength = Normalize(v[0], robust);
+ for (int i = 1; i < numInputs; ++i)
+ {
+ for (int j = 0; j < i; ++j)
+ {
+ Real dot = Dot(v[i], v[j]);
+ v[i] -= v[j] * dot;
+ }
+ Real length = Normalize(v[i], robust);
+ if (length < minLength)
+ {
+ minLength = length;
+ }
+ }
+ return minLength;
+ }
+
+ return (Real)0;
+}
+
+template <int N, typename Real>
+Vector<N, Real> GetOrthogonal(Vector<N, Real> const& v, bool unitLength)
+{
+ Real cmax = std::abs(v[0]);
+ int imax = 0;
+ for (int i = 1; i < N; ++i)
+ {
+ Real c = std::abs(v[i]);
+ if (c > cmax)
+ {
+ cmax = c;
+ imax = i;
+ }
+ }
+
+ Vector<N, Real> result;
+ result.MakeZero();
+ int inext = imax + 1;
+ if (inext == N)
+ {
+ inext = 0;
+ }
+ result[imax] = v[inext];
+ result[inext] = -v[imax];
+ if (unitLength)
+ {
+ Real sqrDistance = result[imax] * result[imax] + result[inext] * result[inext];
+ Real invLength = ((Real)1) / std::sqrt(sqrDistance);
+ result[imax] *= invLength;
+ result[inext] *= invLength;
+ }
+ return result;
+}
+
+template <int N, typename Real>
+bool ComputeExtremes(int numVectors, Vector<N, Real> const* v,
+ Vector<N, Real>& vmin, Vector<N, Real>& vmax)
+{
+ if (v && numVectors > 0)
+ {
+ vmin = v[0];
+ vmax = vmin;
+ for (int j = 1; j < numVectors; ++j)
+ {
+ Vector<N, Real> const& vec = v[j];
+ for (int i = 0; i < N; ++i)
+ {
+ if (vec[i] < vmin[i])
+ {
+ vmin[i] = vec[i];
+ }
+ else if (vec[i] > vmax[i])
+ {
+ vmax[i] = vec[i];
+ }
+ }
+ }
+ return true;
+ }
+
+ return false;
+}
+
+template <int N, typename Real>
+Vector<N + 1, Real> HLift(Vector<N, Real> const& v, Real last)
+{
+ Vector<N + 1, Real> result;
+ for (int i = 0; i < N; ++i)
+ {
+ result[i] = v[i];
+ }
+ result[N] = last;
+ return result;
+}
+
+template <int N, typename Real>
+Vector<N - 1, Real> HProject(Vector<N, Real> const& v)
+{
+ static_assert(N >= 2, "Invalid dimension.");
+ Vector<N - 1, Real> result;
+ for (int i = 0; i < N - 1; ++i)
+ {
+ result[i] = v[i];
+ }
+ return result;
+}
+
+template <int N, typename Real>
+Vector<N + 1, Real> Lift(Vector<N, Real> const& v, int inject, Real value)
+{
+ Vector<N + 1, Real> result;
+ int i;
+ for (i = 0; i < inject; ++i)
+ {
+ result[i] = v[i];
+ }
+ result[i] = value;
+ int j = i;
+ for (++j; i < N; ++i, ++j)
+ {
+ result[j] = v[i];
+ }
+ return result;
+}
+
+template <int N, typename Real>
+Vector<N - 1, Real> Project(Vector<N, Real> const& v, int reject)
+{
+ static_assert(N >= 2, "Invalid dimension.");
+ Vector<N - 1, Real> result;
+ for (int i = 0, j = 0; i < N - 1; ++i, ++j)
+ {
+ if (j == reject)
+ {
+ ++j;
+ }
+ result[i] = v[j];
+ }
+ return result;
+}
+
+}
+#endif
diff --git a/src/geometric-tools/Mathematics/GteVector3.h b/src/geometric-tools/Mathematics/GteVector3.h
new file mode 100644
index 0000000000000000000000000000000000000000..0b92ff5e1aa7507e66708fd8e1456ef41b5eeda2
--- /dev/null
+++ b/src/geometric-tools/Mathematics/GteVector3.h
@@ -0,0 +1,385 @@
+// 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.2 (2018/10/04)
+
+#pragma once
+#ifndef GTE_VECTOR3_H
+#define GTE_VECTOR3_H
+
+#include <Mathematics/GteVector.h>
+
+namespace gte
+{
+
+// Template alias for convenience.
+template <typename Real>
+using Vector3 = Vector<3, Real>;
+
+// Cross, UnitCross, and DotCross have a template parameter N that should
+// be 3 or 4. The latter case supports affine vectors in 4D (last component
+// w = 0) when you want to use 4-tuples and 4x4 matrices for affine algebra.
+
+// Compute the cross product using the formal determinant:
+// cross = det{{e0,e1,e2},{x0,x1,x2},{y0,y1,y2}}
+// = (x1*y2-x2*y1, x2*y0-x0*y2, x0*y1-x1*y0)
+// where e0 = (1,0,0), e1 = (0,1,0), e2 = (0,0,1), v0 = (x0,x1,x2), and
+// v1 = (y0,y1,y2).
+template <int N, typename Real>
+Vector<N,Real> Cross(Vector<N,Real> const& v0, Vector<N,Real> const& v1);
+
+// Compute the normalized cross product.
+template <int N, typename Real>
+Vector<N, Real> UnitCross(Vector<N,Real> const& v0, Vector<N,Real> const& v1,
+ bool robust = false);
+
+// Compute Dot((x0,x1,x2),Cross((y0,y1,y2),(z0,z1,z2)), the triple scalar
+// product of three vectors, where v0 = (x0,x1,x2), v1 = (y0,y1,y2), and
+// v2 is (z0,z1,z2).
+template <int N, typename Real>
+Real DotCross(Vector<N,Real> const& v0, Vector<N,Real> const& v1,
+ Vector<N,Real> const& v2);
+
+// Compute a right-handed orthonormal basis for the orthogonal complement
+// of the input vectors. The function returns the smallest length of the
+// unnormalized vectors computed during the process. If this value is nearly
+// zero, it is possible that the inputs are linearly dependent (within
+// numerical round-off errors). On input, numInputs must be 1 or 2 and
+// v[0] through v[numInputs-1] must be initialized. On output, the
+// vectors v[0] through v[2] form an orthonormal set.
+template <typename Real>
+Real ComputeOrthogonalComplement(int numInputs, Vector3<Real>* v, bool robust = false);
+
+// Compute the barycentric coordinates of the point P with respect to the
+// tetrahedron <V0,V1,V2,V3>, P = b0*V0 + b1*V1 + b2*V2 + b3*V3, where
+// b0 + b1 + b2 + b3 = 1. The return value is 'true' iff {V0,V1,V2,V3} is
+// a linearly independent set. Numerically, this is measured by
+// |det[V0 V1 V2 V3]| <= epsilon. The values bary[] are valid only when the
+// return value is 'true' but set to zero when the return value is 'false'.
+template <typename Real>
+bool ComputeBarycentrics(Vector3<Real> const& p, Vector3<Real> const& v0,
+ Vector3<Real> const& v1, Vector3<Real> const& v2, Vector3<Real> const& v3,
+ Real bary[4], Real epsilon = (Real)0);
+
+// Get intrinsic information about the input array of vectors. The return
+// value is 'true' iff the inputs are valid (numVectors > 0, v is not null,
+// and epsilon >= 0), in which case the class members are valid.
+template <typename Real>
+class IntrinsicsVector3
+{
+public:
+ // The constructor sets the class members based on the input set.
+ IntrinsicsVector3(int numVectors, Vector3<Real> const* v, Real inEpsilon);
+
+ // A nonnegative tolerance that is used to determine the intrinsic
+ // dimension of the set.
+ Real epsilon;
+
+ // The intrinsic dimension of the input set, computed based on the
+ // nonnegative tolerance mEpsilon.
+ int dimension;
+
+ // Axis-aligned bounding box of the input set. The maximum range is
+ // the larger of max[0]-min[0], max[1]-min[1], and max[2]-min[2].
+ Real min[3], max[3];
+ Real maxRange;
+
+ // Coordinate system. The origin is valid for any dimension d. The
+ // unit-length direction vector is valid only for 0 <= i < d. The
+ // extreme index is relative to the array of input points, and is also
+ // valid only for 0 <= i < d. If d = 0, all points are effectively
+ // the same, but the use of an epsilon may lead to an extreme index
+ // that is not zero. If d = 1, all points effectively lie on a line
+ // segment. If d = 2, all points effectively line on a plane. If
+ // d = 3, the points are not coplanar.
+ Vector3<Real> origin;
+ Vector3<Real> direction[3];
+
+ // The indices that define the maximum dimensional extents. The
+ // values extreme[0] and extreme[1] are the indices for the points
+ // that define the largest extent in one of the coordinate axis
+ // directions. If the dimension is 2, then extreme[2] is the index
+ // for the point that generates the largest extent in the direction
+ // perpendicular to the line through the points corresponding to
+ // extreme[0] and extreme[1]. Furthermore, if the dimension is 3,
+ // then extreme[3] is the index for the point that generates the
+ // largest extent in the direction perpendicular to the triangle
+ // defined by the other extreme points. The tetrahedron formed by the
+ // points V[extreme[0]], V[extreme[1]], V[extreme[2]], and
+ // V[extreme[3]] is clockwise or counterclockwise, the condition
+ // stored in extremeCCW.
+ int extreme[4];
+ bool extremeCCW;
+};
+
+
+template <int N, typename Real>
+Vector<N, Real> Cross(Vector<N, Real> const& v0, Vector<N, Real> const& v1)
+{
+ static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
+
+ Vector<N, Real> result;
+ result.MakeZero();
+ result[0] = v0[1] * v1[2] - v0[2] * v1[1];
+ result[1] = v0[2] * v1[0] - v0[0] * v1[2];
+ result[2] = v0[0] * v1[1] - v0[1] * v1[0];
+ return result;
+}
+
+template <int N, typename Real>
+Vector<N, Real> UnitCross(Vector<N, Real> const& v0, Vector<N, Real> const& v1, bool robust)
+{
+ static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
+
+ Vector<N, Real> unitCross = Cross(v0, v1);
+ Normalize(unitCross, robust);
+ return unitCross;
+}
+
+template <int N, typename Real>
+Real DotCross(Vector<N, Real> const& v0, Vector<N, Real> const& v1,
+ Vector<N, Real> const& v2)
+{
+ static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
+
+ return Dot(v0, Cross(v1, v2));
+}
+
+template <typename Real>
+Real ComputeOrthogonalComplement(int numInputs, Vector3<Real>* v, bool robust)
+{
+ if (numInputs == 1)
+ {
+ if (std::abs(v[0][0]) > std::abs(v[0][1]))
+ {
+ v[1] = { -v[0][2], (Real)0, +v[0][0] };
+ }
+ else
+ {
+ v[1] = { (Real)0, +v[0][2], -v[0][1] };
+ }
+ numInputs = 2;
+ }
+
+ if (numInputs == 2)
+ {
+ v[2] = Cross(v[0], v[1]);
+ return Orthonormalize<3, Real>(3, v, robust);
+ }
+
+ return (Real)0;
+}
+
+template <typename Real>
+bool ComputeBarycentrics(Vector3<Real> const& p, Vector3<Real> const& v0,
+ Vector3<Real> const& v1, Vector3<Real> const& v2, Vector3<Real> const& v3,
+ Real bary[4], Real epsilon)
+{
+ // Compute the vectors relative to V3 of the tetrahedron.
+ Vector3<Real> diff[4] = { v0 - v3, v1 - v3, v2 - v3, p - v3 };
+
+ Real det = DotCross(diff[0], diff[1], diff[2]);
+ if (det < -epsilon || det > epsilon)
+ {
+ Real invDet = ((Real)1) / det;
+ bary[0] = DotCross(diff[3], diff[1], diff[2]) * invDet;
+ bary[1] = DotCross(diff[3], diff[2], diff[0]) * invDet;
+ bary[2] = DotCross(diff[3], diff[0], diff[1]) * invDet;
+ bary[3] = (Real)1 - bary[0] - bary[1] - bary[2];
+ return true;
+ }
+
+ for (int i = 0; i < 4; ++i)
+ {
+ bary[i] = (Real)0;
+ }
+ return false;
+}
+
+template <typename Real>
+IntrinsicsVector3<Real>::IntrinsicsVector3(int numVectors,
+ Vector3<Real> const* v, Real inEpsilon)
+ :
+ epsilon(inEpsilon),
+ dimension(0),
+ maxRange((Real)0),
+ origin({ (Real)0, (Real)0, (Real)0 }),
+ extremeCCW(false)
+{
+ min[0] = (Real)0;
+ min[1] = (Real)0;
+ min[2] = (Real)0;
+ direction[0] = { (Real)0, (Real)0, (Real)0 };
+ direction[1] = { (Real)0, (Real)0, (Real)0 };
+ direction[2] = { (Real)0, (Real)0, (Real)0 };
+ extreme[0] = 0;
+ extreme[1] = 0;
+ extreme[2] = 0;
+ extreme[3] = 0;
+
+ if (numVectors > 0 && v && epsilon >= (Real)0)
+ {
+ // Compute the axis-aligned bounding box for the input vectors. Keep
+ // track of the indices into 'vectors' for the current min and max.
+ int j, indexMin[3], indexMax[3];
+ for (j = 0; j < 3; ++j)
+ {
+ min[j] = v[0][j];
+ max[j] = min[j];
+ indexMin[j] = 0;
+ indexMax[j] = 0;
+ }
+
+ int i;
+ for (i = 1; i < numVectors; ++i)
+ {
+ for (j = 0; j < 3; ++j)
+ {
+ if (v[i][j] < min[j])
+ {
+ min[j] = v[i][j];
+ indexMin[j] = i;
+ }
+ else if (v[i][j] > max[j])
+ {
+ max[j] = v[i][j];
+ indexMax[j] = i;
+ }
+ }
+ }
+
+ // Determine the maximum range for the bounding box.
+ maxRange = max[0] - min[0];
+ extreme[0] = indexMin[0];
+ extreme[1] = indexMax[0];
+ Real range = max[1] - min[1];
+ if (range > maxRange)
+ {
+ maxRange = range;
+ extreme[0] = indexMin[1];
+ extreme[1] = indexMax[1];
+ }
+ range = max[2] - min[2];
+ if (range > maxRange)
+ {
+ maxRange = range;
+ extreme[0] = indexMin[2];
+ extreme[1] = indexMax[2];
+ }
+
+ // The origin is either the vector of minimum x0-value, vector of
+ // minimum x1-value, or vector of minimum x2-value.
+ origin = v[extreme[0]];
+
+ // Test whether the vector set is (nearly) a vector.
+ if (maxRange <= epsilon)
+ {
+ dimension = 0;
+ for (j = 0; j < 3; ++j)
+ {
+ extreme[j + 1] = extreme[0];
+ }
+ return;
+ }
+
+ // Test whether the vector set is (nearly) a line segment. We need
+ // {direction[2],direction[3]} to span the orthogonal complement of
+ // direction[0].
+ direction[0] = v[extreme[1]] - origin;
+ Normalize(direction[0], false);
+ if (std::abs(direction[0][0]) > std::abs(direction[0][1]))
+ {
+ direction[1][0] = -direction[0][2];
+ direction[1][1] = (Real)0;
+ direction[1][2] = +direction[0][0];
+ }
+ else
+ {
+ direction[1][0] = (Real)0;
+ direction[1][1] = +direction[0][2];
+ direction[1][2] = -direction[0][1];
+ }
+ Normalize(direction[1], false);
+ direction[2] = Cross(direction[0], direction[1]);
+
+ // Compute the maximum distance of the points from the line
+ // origin+t*direction[0].
+ Real maxDistance = (Real)0;
+ Real distance, dot;
+ extreme[2] = extreme[0];
+ for (i = 0; i < numVectors; ++i)
+ {
+ Vector3<Real> diff = v[i] - origin;
+ dot = Dot(direction[0], diff);
+ Vector3<Real> proj = diff - dot * direction[0];
+ distance = Length(proj, false);
+ if (distance > maxDistance)
+ {
+ maxDistance = distance;
+ extreme[2] = i;
+ }
+ }
+
+ if (maxDistance <= epsilon*maxRange)
+ {
+ // The points are (nearly) on the line origin+t*direction[0].
+ dimension = 1;
+ extreme[2] = extreme[1];
+ extreme[3] = extreme[1];
+ return;
+ }
+
+ // Test whether the vector set is (nearly) a planar polygon. The
+ // point v[extreme[2]] is farthest from the line: origin +
+ // t*direction[0]. The vector v[extreme[2]]-origin is not
+ // necessarily perpendicular to direction[0], so project out the
+ // direction[0] component so that the result is perpendicular to
+ // direction[0].
+ direction[1] = v[extreme[2]] - origin;
+ dot = Dot(direction[0], direction[1]);
+ direction[1] -= dot * direction[0];
+ Normalize(direction[1], false);
+
+ // We need direction[2] to span the orthogonal complement of
+ // {direction[0],direction[1]}.
+ direction[2] = Cross(direction[0], direction[1]);
+
+ // Compute the maximum distance of the points from the plane
+ // origin+t0*direction[0]+t1*direction[1].
+ maxDistance = (Real)0;
+ Real maxSign = (Real)0;
+ extreme[3] = extreme[0];
+ for (i = 0; i < numVectors; ++i)
+ {
+ Vector3<Real> diff = v[i] - origin;
+ distance = Dot(direction[2], diff);
+ Real sign = (distance >(Real)0 ? (Real)1 :
+ (distance < (Real)0 ? (Real)-1 : (Real)0));
+ distance = std::abs(distance);
+ if (distance > maxDistance)
+ {
+ maxDistance = distance;
+ maxSign = sign;
+ extreme[3] = i;
+ }
+ }
+
+ if (maxDistance <= epsilon * maxRange)
+ {
+ // The points are (nearly) on the plane origin+t0*direction[0]
+ // +t1*direction[1].
+ dimension = 2;
+ extreme[3] = extreme[2];
+ return;
+ }
+
+ dimension = 3;
+ extremeCCW = (maxSign > (Real)0);
+ return;
+ }
+}
+
+}
+#endif
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index f81a4de87a4b4898e054b8cdbd415197c6918ef5..b13debdca5fab5007507cb0ad190822bf4dae013 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -18,6 +18,7 @@ macro(add_fcl_test test_name)
endmacro(add_fcl_test)
include_directories(${CMAKE_CURRENT_BINARY_DIR})
+include_directories(${PROJECT_SOURCE_DIR}/src/geometric-tools)
include_directories(${Boost_INCLUDE_DIRS})
@@ -46,6 +47,8 @@ add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_profiling test_fcl_profiling.cpp test_fcl_utility.cpp)
PKG_CONFIG_USE_DEPENDENCY(test_fcl_profiling assimp)
+add_fcl_test(test_fcl_gjk test_fcl_gjk.cpp)
+
## Benchmark
add_executable(benchmark benchmark.cpp test_fcl_utility.cpp)
target_link_libraries(benchmark hpp-fcl ${Boost_LIBRARIES})
diff --git a/test/benchmark/test_fcl_gjk.output b/test/benchmark/test_fcl_gjk.output
new file mode 100644
index 0000000000000000000000000000000000000000..ee02078dcc52a67836eacf5618e616c7d27cd911
--- /dev/null
+++ b/test/benchmark/test_fcl_gjk.output
@@ -0,0 +1,15 @@
+Result after running test_fcl_gjk in Release mode.
+
+nCol = 2831
+nDiff = 41
+Total time gjk: 21461
+Total time gte: 59498
+Number times GJK better than Geometric tools: 9988
+-- Collisions -------------------------
+Total time gjk: 5219
+Total time gte: 14334
+Number times GJK better than Geometric tools: 2828
+-- No collisions -------------------------
+Total time gjk: 16242
+Total time gte: 45164
+Number times GJK better than Geometric tools: 7160
diff --git a/test/gjk-geometric-tools-benchmark b/test/gjk-geometric-tools-benchmark
new file mode 100644
index 0000000000000000000000000000000000000000..4104d7511f78c3d9f22b4b16fe63395dcacdd5e5
--- /dev/null
+++ b/test/gjk-geometric-tools-benchmark
@@ -0,0 +1,15 @@
+Result after running test_fcl_gjk.cpp in Release mode.
+
+nCol = 2831
+nDiff = 41
+Total time gjk: 21461
+Total time gte: 59498
+Number times GJK better than Geometric tools: 9988
+-- Collisions -------------------------
+Total time gjk: 5219
+Total time gte: 14334
+Number times GJK better than Geometric tools: 2828
+-- No collisions -------------------------
+Total time gjk: 16242
+Total time gte: 45164
+Number times GJK better than Geometric tools: 7160
diff --git a/test/test_fcl_gjk.cpp b/test/test_fcl_gjk.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef48b55e630445d782e2a2035dbc53dbbb355c69
--- /dev/null
+++ b/test/test_fcl_gjk.cpp
@@ -0,0 +1,240 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, CNRS-LAAS.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Florent Lamiraux <florent@laas.fr> */
+
+#define BOOST_TEST_MODULE FCL_GJK
+#define BOOST_TEST_DYN_LINK
+
+#include <time.h>
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+#include <hpp/fcl/narrowphase/narrowphase.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+#include <Mathematics/GteDistTriangle3Triangle3.h>
+
+using fcl::GJKSolver_indep;
+using fcl::TriangleP;
+using fcl::Vec3f;
+using fcl::Transform3f;
+using fcl::Matrix3f;
+using fcl::FCL_REAL;
+using gte::DCPQuery;
+using gte::Triangle3;
+using gte::Vector3;
+
+typedef DCPQuery <FCL_REAL, Triangle3 <FCL_REAL>, Triangle3 <FCL_REAL> >
+Query_t;
+
+struct Result
+{
+ bool collision;
+ clock_t timeGjk;
+ clock_t timeGte;
+}; // struct benchmark
+
+typedef std::vector <Result> Results_t;
+
+BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
+{
+ std::size_t N = 10000;
+ GJKSolver_indep solver;
+ Transform3f tf1, tf2;
+ Vec3f p1, p2, a1, a2;
+ Matrix3f M;
+ FCL_REAL distance (sqrt (-1));
+ clock_t start, end;
+
+ std::size_t nCol = 0, nDiff = 0;
+ FCL_REAL eps = 1e-7;
+ Results_t results (N);
+ for (std::size_t i=0; i<N; ++i) {
+ Vec3f P1 (Vec3f::Random ()), P2 (Vec3f::Random ()), P3 (Vec3f::Random ());
+ Vec3f Q1 (Vec3f::Random ()), Q2 (Vec3f::Random ()), Q3 (Vec3f::Random ());
+ TriangleP tri1 (P1, P2, P3);
+ TriangleP tri2 (Q1, Q2, Q3);
+
+ start = clock ();
+ bool res = solver.shapeDistance (tri1, tf1, tri2, tf2, &distance, &p1, &p2);
+ end = clock ();
+ results [i].timeGjk = end - start;
+ results [i].collision = !res;
+ assert (res != (distance < 0));
+ if (distance >= 0) {
+ // Compute vectors between vertices
+ Vec3f u1 (P2 - P1);
+ Vec3f v1 (P3 - P1);
+ Vec3f w1 (u1.cross (v1));
+ Vec3f u2 (Q2 - Q1);
+ Vec3f v2 (Q3 - Q1);
+ Vec3f w2 (u2.cross (v2));
+ // Check that closest points are on triangles
+ // Compute a1 such that p1 = P1 + a11 u1 + b11 v1 + c11 u1 x v1
+ assert (w1.squaredNorm () > eps*eps);
+ M.col (0) = u1; M.col (1) = v1; M.col (2) = w1;
+ a1 = M.inverse () * (p1 - P1);
+ assert (fabs (a1 [2]) < eps);
+ assert (a1 [0] >= -eps);
+ assert (a1 [1] >= -eps);
+ assert (a1 [0] + a1 [1] <= 1+eps);
+ assert (w2.squaredNorm () > eps*eps);
+ M.col (0) = u2; M.col (1) = v2; M.col (2) = w2;
+ a2 = M.inverse () * (p2 - Q1);
+ assert (fabs (a2 [2]) < eps);
+ assert (a2 [0] >= -eps);
+ assert (a2 [1] >= -eps);
+ assert (a2 [0] + a2 [1] <= 1+eps);
+ } else {
+ ++nCol;
+ }
+ // Using geometric tools
+ Query_t query;
+ Vector3 <FCL_REAL> gteP1 = {P1 [0], P1 [1], P1 [2]};
+ Vector3 <FCL_REAL> gteP2 = {P2 [0], P2 [1], P2 [2]};
+ Vector3 <FCL_REAL> gteP3 = {P3 [0], P3 [1], P3 [2]};
+ Vector3 <FCL_REAL> gteQ1 = {Q1 [0], Q1 [1], Q1 [2]};
+ Vector3 <FCL_REAL> gteQ2 = {Q2 [0], Q2 [1], Q2 [2]};
+ Vector3 <FCL_REAL> gteQ3 = {Q3 [0], Q3 [1], Q3 [2]};
+ Triangle3 <FCL_REAL> gteTri1 (gteP1, gteP2, gteP3);
+ Triangle3 <FCL_REAL> gteTri2 (gteQ1, gteQ2, gteQ3);
+ start = clock ();
+ Query_t::Result gteRes = query (gteTri1, gteTri2);
+ end = clock ();
+ results [i].timeGte = end - start;
+
+ Vector3 <FCL_REAL> gte_p1 = gteRes.closestPoint [0];
+ Vector3 <FCL_REAL> gte_p2 = gteRes.closestPoint [1];
+ Vector3 <FCL_REAL> gte_p2_p1 = gte_p2 - gte_p1;
+ // assert (((distance < 0) && (gteRes.distance == 0)) ||
+ // ((distance > 0) && (gteRes.distance > 0)));
+ assert (fabs (gteRes.sqrDistance - Dot (gte_p2_p1, gte_p2_p1)) < eps);
+ if ((distance >= 0) && (fabs (distance - gteRes.distance) >= eps)) {
+ ++nDiff;
+ } else {
+ assert ((distance < 0) ||
+ ((fabs (gte_p1 [0] - p1 [0]) < eps) &&
+ (fabs (gte_p1 [1] - p1 [1]) < eps) &&
+ (fabs (gte_p1 [2] - p1 [2]) < eps) &&
+ (fabs (gte_p2 [0] - p2 [0]) < eps) &&
+ (fabs (gte_p2 [1] - p2 [1]) < eps) &&
+ (fabs (gte_p2 [2] - p2 [2]) < eps)) ||
+ ((fabs (gte_p1 [0] - p2 [0]) < eps) &&
+ (fabs (gte_p1 [1] - p2 [1]) < eps) &&
+ (fabs (gte_p1 [2] - p2 [2]) < eps) &&
+ (fabs (gte_p2 [0] - p1 [0]) < eps) &&
+ (fabs (gte_p2 [1] - p1 [1]) < eps) &&
+ (fabs (gte_p2 [2] - p1 [2]) < eps)));
+ }
+#if 0
+ // Compute vectors between vertices
+ Vector3 <FCL_REAL> gte_u1 (gteP2 - gteP1);
+ Vector3 <FCL_REAL> gte_v1 (gteP3 - gteP1);
+ Vector3 <FCL_REAL> gte_w1 (Cross (gte_u1, gte_v1));
+ Vector3 <FCL_REAL> gte_u2 (gteQ2 - gteQ1);
+ Vector3 <FCL_REAL> gte_v2 (gteQ3 - gteQ1);
+ Vector3 <FCL_REAL> gte_w2 (Cross (gte_u2, gte_v2));
+ // Check that closest points are on triangles
+ // Compute a1 such that p1 = P1 + a11 u1 + b11 v1 + c11 u1 x v1
+ assert (std::sqrt(Dot(gte_w1, gte_w1)) > eps*eps);
+ M (0,0) = gte_u1 [0]; M (1,0) = gte_u1 [1]; M (2,0) = gte_u1 [2];
+ M (0,1) = gte_v1 [0]; M (1,1) = gte_v1 [1]; M (2,1) = gte_v1 [2];
+ M (0,2) = gte_w1 [0]; M (1,2) = gte_w1 [1]; M (2,2) = gte_w1 [2];
+ p1 [0] = gte_p1 [0]; p1 [1] = gte_p1 [1]; p1 [2] = gte_p1 [2];
+ a1 = M.inverse () * (p1 - P1);
+ assert (fabs (a1 [2]) < eps);
+ assert (a1 [0] >= -eps);
+ assert (a1 [1] >= -eps);
+ assert (a1 [0] + a1 [1] <= 1+eps);
+ // Check whether closest point is on triangle edges
+ if ((a1 [0] > eps) && (a1 [1] > eps) && (a1 [0] + a1 [1] < 1 - eps)) {
+ std::cerr << "a1 = " << a1.transpose () << std::endl;
+ }
+ assert (std::sqrt(Dot(gte_w2, gte_w2)) > eps*eps);
+ M (0,0) = gte_u2 [0]; M (1,0) = gte_u2 [1]; M (2,0) = gte_u2 [2];
+ M (0,1) = gte_v2 [0]; M (1,1) = gte_v2 [1]; M (2,1) = gte_v2 [2];
+ M (0,2) = gte_w2 [0]; M (1,2) = gte_w2 [1]; M (2,2) = gte_w2 [2];
+ p2 [0] = gte_p2 [0]; p2 [1] = gte_p2 [1]; p2 [2] = gte_p2 [2];
+ a2 = M.inverse () * (p2 - Q1);
+ assert (fabs (a2 [2]) < eps);
+ assert (a2 [0] >= -eps);
+ assert (a2 [1] >= -eps);
+ assert (a2 [0] + a2 [1] <= 1+eps);
+ // Check whether closest point is on triangle edges
+ if ((a2 [0] > eps) && (a2 [1] > eps) && (a2 [0] + a2 [1] < 1 - eps)) {
+ std::cerr << "a2 = " << a2.transpose () << std::endl;
+ }
+#endif
+ }
+ std::cerr << "nCol = " << nCol << std::endl;
+ std::cerr << "nDiff = " << nDiff << std::endl;
+ // statistics
+ clock_t totalTimeGjk = 0, totalTimeGte = 0;
+ clock_t totalTimeGjkColl = 0, totalTimeGteColl = 0;
+ clock_t totalTimeGjkNoColl = 0, totalTimeGteNoColl = 0;
+ std::size_t nGjkBetterThanGteColl = 0;
+ std::size_t nGjkBetterThanGteNoColl = 0;
+ for (std::size_t i=0; i<N; ++i) {
+ if (results [i].collision) {
+ totalTimeGjkColl += results [i].timeGjk;
+ totalTimeGteColl += results [i].timeGte;
+ if ((results [i].timeGjk < results [i].timeGte)) {
+ ++ nGjkBetterThanGteColl;
+ }
+ } else {
+ totalTimeGjkNoColl += results [i].timeGjk;
+ totalTimeGteNoColl += results [i].timeGte;
+ if ((results [i].timeGjk < results [i].timeGte)) {
+ ++ nGjkBetterThanGteNoColl;
+ }
+ }
+ }
+ std::cerr << "Total time gjk: " << totalTimeGjkNoColl + totalTimeGjkColl
+ << std::endl;
+ std::cerr << "Total time gte: " << totalTimeGteNoColl + totalTimeGteColl
+ << std::endl;
+ std::cerr << "Number times GJK better than Geometric tools: "
+ << nGjkBetterThanGteNoColl + nGjkBetterThanGteColl << std::endl;
+ std::cerr << "-- Collisions -------------------------" << std::endl;
+ std::cerr << "Total time gjk: " << totalTimeGjkColl << std::endl;
+ std::cerr << "Total time gte: " << totalTimeGteColl << std::endl;
+ std::cerr << "Number times GJK better than Geometric tools: "
+ << nGjkBetterThanGteColl << std::endl;
+ std::cerr << "-- No collisions -------------------------" << std::endl;
+ std::cerr << "Total time gjk: " << totalTimeGjkNoColl << std::endl;
+ std::cerr << "Total time gte: " << totalTimeGteNoColl << std::endl;
+ std::cerr << "Number times GJK better than Geometric tools: "
+ << nGjkBetterThanGteNoColl << std::endl;
+
+}