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

src/geometric-tools/GTEngineDEF.h

+// 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

src/geometric-tools/Mathematics/GteDCPQuery.h

+// 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

src/geometric-tools/Mathematics/GteDistLine3Triangle3.h

+// 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

src/geometric-tools/Mathematics/GteDistLineSegment.h

+// 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

src/geometric-tools/Mathematics/GteDistPointTriangle.h

+// 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)
+            {