From 5bcf569d62bd2e16fc73d607975a9066a6f4b2b8 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 30 Nov 2018 15:11:53 +0100
Subject: [PATCH] [test] Update test on pairs of triangles.
---
src/geometric-tools/GTEngineDEF.h | 80 --
src/geometric-tools/Mathematics/GteDCPQuery.h | 35 -
.../Mathematics/GteDistLine3Triangle3.h | 130 ----
.../Mathematics/GteDistLineSegment.h | 116 ---
.../Mathematics/GteDistPointTriangle.h | 269 -------
.../Mathematics/GteDistSegment3Triangle3.h | 91 ---
.../Mathematics/GteDistTriangle3Triangle3.h | 103 ---
src/geometric-tools/Mathematics/GteLine.h | 115 ---
src/geometric-tools/Mathematics/GteMath.h | 616 ----------------
src/geometric-tools/Mathematics/GteSegment.h | 151 ----
src/geometric-tools/Mathematics/GteTriangle.h | 114 ---
src/geometric-tools/Mathematics/GteVector.h | 695 ------------------
src/geometric-tools/Mathematics/GteVector3.h | 385 ----------
test/CMakeLists.txt | 1 -
test/benchmark/test_fcl_gjk.output | 16 +-
test/test_fcl_gjk.cpp | 234 +++---
16 files changed, 108 insertions(+), 3043 deletions(-)
delete mode 100644 src/geometric-tools/GTEngineDEF.h
delete mode 100644 src/geometric-tools/Mathematics/GteDCPQuery.h
delete mode 100644 src/geometric-tools/Mathematics/GteDistLine3Triangle3.h
delete mode 100644 src/geometric-tools/Mathematics/GteDistLineSegment.h
delete mode 100644 src/geometric-tools/Mathematics/GteDistPointTriangle.h
delete mode 100644 src/geometric-tools/Mathematics/GteDistSegment3Triangle3.h
delete mode 100644 src/geometric-tools/Mathematics/GteDistTriangle3Triangle3.h
delete mode 100644 src/geometric-tools/Mathematics/GteLine.h
delete mode 100644 src/geometric-tools/Mathematics/GteMath.h
delete mode 100644 src/geometric-tools/Mathematics/GteSegment.h
delete mode 100644 src/geometric-tools/Mathematics/GteTriangle.h
delete mode 100644 src/geometric-tools/Mathematics/GteVector.h
delete mode 100644 src/geometric-tools/Mathematics/GteVector3.h
diff --git a/src/geometric-tools/GTEngineDEF.h b/src/geometric-tools/GTEngineDEF.h
deleted file mode 100644
index 88d1869e..00000000
--- a/src/geometric-tools/GTEngineDEF.h
+++ /dev/null
@@ -1,80 +0,0 @@
-// 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
deleted file mode 100644
index 3bf5c6b9..00000000
--- a/src/geometric-tools/Mathematics/GteDCPQuery.h
+++ /dev/null
@@ -1,35 +0,0 @@
-// 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
deleted file mode 100644
index 993bb473..00000000
--- a/src/geometric-tools/Mathematics/GteDistLine3Triangle3.h
+++ /dev/null
@@ -1,130 +0,0 @@
-// 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
deleted file mode 100644
index f002dfab..00000000
--- a/src/geometric-tools/Mathematics/GteDistLineSegment.h
+++ /dev/null
@@ -1,116 +0,0 @@
-// 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
deleted file mode 100644
index 9ab7942b..00000000
--- a/src/geometric-tools/Mathematics/GteDistPointTriangle.h
+++ /dev/null
@@ -1,269 +0,0 @@
-// 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
deleted file mode 100644
index 39c71f56..00000000
--- a/src/geometric-tools/Mathematics/GteDistSegment3Triangle3.h
+++ /dev/null
@@ -1,91 +0,0 @@
-// 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
deleted file mode 100644
index 503c08f0..00000000
--- a/src/geometric-tools/Mathematics/GteDistTriangle3Triangle3.h
+++ /dev/null
@@ -1,103 +0,0 @@
-// 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
deleted file mode 100644
index 106631f9..00000000
--- a/src/geometric-tools/Mathematics/GteLine.h
+++ /dev/null
@@ -1,115 +0,0 @@
-// 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
deleted file mode 100644
index 7a1a40a7..00000000
--- a/src/geometric-tools/Mathematics/GteMath.h
+++ /dev/null
@@ -1,616 +0,0 @@
-// 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
deleted file mode 100644
index 15f31e95..00000000
--- a/src/geometric-tools/Mathematics/GteSegment.h
+++ /dev/null
@@ -1,151 +0,0 @@
-// 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
deleted file mode 100644
index 366a89cf..00000000
--- a/src/geometric-tools/Mathematics/GteTriangle.h
+++ /dev/null
@@ -1,114 +0,0 @@
-// 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
deleted file mode 100644
index 6f96ac72..00000000
--- a/src/geometric-tools/Mathematics/GteVector.h
+++ /dev/null
@@ -1,695 +0,0 @@
-// 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
deleted file mode 100644
index 0b92ff5e..00000000
--- a/src/geometric-tools/Mathematics/GteVector3.h
+++ /dev/null
@@ -1,385 +0,0 @@
-// 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 b13debdc..470afd43 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -18,7 +18,6 @@ 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})
diff --git a/test/benchmark/test_fcl_gjk.output b/test/benchmark/test_fcl_gjk.output
index ee02078d..aaf9b543 100644
--- a/test/benchmark/test_fcl_gjk.output
+++ b/test/benchmark/test_fcl_gjk.output
@@ -1,15 +1,9 @@
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
+nCol = 2840
+nDiff = 0
+Total time gjk: 20020
-- Collisions -------------------------
-Total time gjk: 5219
-Total time gte: 14334
-Number times GJK better than Geometric tools: 2828
+Total time gjk: 5554
-- No collisions -------------------------
-Total time gjk: 16242
-Total time gte: 45164
-Number times GJK better than Geometric tools: 7160
+Total time gjk: 14466
diff --git a/test/test_fcl_gjk.cpp b/test/test_fcl_gjk.cpp
index ef48b55e..65d8e5bd 100644
--- a/test/test_fcl_gjk.cpp
+++ b/test/test_fcl_gjk.cpp
@@ -43,7 +43,6 @@
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes.h>
-#include <Mathematics/GteDistTriangle3Triangle3.h>
using fcl::GJKSolver_indep;
using fcl::TriangleP;
@@ -51,12 +50,11 @@ 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;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> vector_t;
+typedef Eigen::Matrix<FCL_REAL, 6, 1> vector6_t;
+typedef Eigen::Matrix<FCL_REAL, 4, 1> vector4_t;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
struct Result
{
@@ -69,6 +67,8 @@ typedef std::vector <Result> Results_t;
BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
{
+ Eigen::IOFormat format (Eigen::FullPrecision, Eigen::DontAlignCols, ", ",
+ ", ", "np.array ((", "))", "", "");
std::size_t N = 10000;
GJKSolver_indep solver;
Transform3f tf1, tf2;
@@ -86,155 +86,127 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
TriangleP tri1 (P1, P2, P3);
TriangleP tri2 (Q1, Q2, Q3);
+ bool res;
start = clock ();
- bool res = solver.shapeDistance (tri1, tf1, tri2, tf2, &distance, &p1, &p2);
+ 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)));
+ BOOST_CHECK (res == (distance >= 0));
+ if (!res) {
+ ++nCol; distance = 0;
}
-#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];
+ 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));
+ BOOST_CHECK (w1.squaredNorm () > eps*eps);
+ M.col (0) = u1; M.col (1) = v1; M.col (2) = w1;
+ // Compute a1 such that p1 = P1 + a11 u1 + a12 v1 + a13 u1 x v1
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];
+ BOOST_CHECK (w2.squaredNorm () > eps*eps);
+ // Compute a2 such that p2 = Q1 + a21 u2 + a22 v2 + a23 u2 x v2
+ 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);
- // 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;
+
+ // minimal distance and closest points can be considered as a constrained
+ // optimisation problem:
+ //
+ // min f (a1[0],a1[1], a2[0],a2[1])
+ // g1 (a1[0],a1[1], a2[0],a2[1]) <=0
+ // ...
+ // g6 (a1[0],a1[1], a2[0],a2[1]) <=0
+ // with
+ // f (a1[0],a1[1], a2[0],a2[1]) =
+ // 1 2
+ // --- dist (P1 + a1[0] u1 + a1[1] v1, Q1 + a2[0] u2 + a2[1] v2),
+ // 2
+ // g1 (a1[0],a1[1], a2[0],a2[1]) = -a1[0]
+ // g2 (a1[0],a1[1], a2[0],a2[1]) = -a1[1]
+ // g3 (a1[0],a1[1], a2[0],a2[1]) = a1[0] + a1[1] - 1
+ // g4 (a1[0],a1[1], a2[0],a2[1]) = -a2[0]
+ // g5 (a1[0],a1[1], a2[0],a2[1]) = -a2[1]
+ // g6 (a1[0],a1[1], a2[0],a2[1]) = a2[0] + a2[1] - 1
+
+ // Compute gradient of f
+ vector4_t grad_f;
+ grad_f [0] = -(p2 - p1).dot (u1);
+ grad_f [1] = -(p2 - p1).dot (v1);
+ grad_f [2] = (p2 - p1).dot (u2);
+ grad_f [3] = (p2 - p1).dot (v2);
+ vector6_t g;
+ g [0] = -a1 [0];
+ g [1] = -a1 [1];
+ g [2] = a1 [0] + a1 [1] - 1;
+ g [3] = -a2 [0];
+ g [4] = -a2 [1];
+ g [5] = a2 [0] + a2 [1] - 1;
+ matrix_t grad_g (4, 6); grad_g.setZero ();
+ grad_g (0,0) = -1.;
+ grad_g (1,1) = -1;
+ grad_g (0,2) = 1; grad_g (1,2) = 1;
+ grad_g (2,3) = -1;
+ grad_g (3,4) = -1;
+ grad_g (2,5) = 1; grad_g (3,5) = 1;
+ // Check that closest points are on triangles planes
+ // Projection of [P1p1] on line normal to triangle 1 plane is equal to 0
+ BOOST_CHECK (fabs (a1 [2]) < eps);
+ // Projection of [Q1p2] on line normal to triangle 2 plane is equal to 0
+ BOOST_CHECK (fabs (a2 [2]) < eps);
+
+ /* Check Karush–Kuhn–Tucker conditions
+ 6
+ __
+ \
+ -grad f = /_ c grad g
+ i=1 i i
+
+ where c >= 0, and
+ i
+ c g = 0 for i between 1 and 6
+ i i
+ */
+
+ matrix_t Mkkt (4, 6); matrix_t::Index col = 0;
+ // Check that constraints are satisfied
+ for (vector6_t::Index j=0; j<6; ++j) {
+ BOOST_CHECK (g [j] <= eps);
+ // if constraint is saturated, add gradient in matrix
+ if (fabs (g [j]) <= eps) {
+ Mkkt.col (col) = grad_g.col (j); ++col;
+ }
+ }
+ if (col > 0) {
+ Mkkt.conservativeResize (4, col);
+ // Compute KKT coefficients ci by inverting
+ // Mkkt.c = -grad_f
+ Eigen::JacobiSVD <matrix_t> svd
+ (Mkkt, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ vector_t c (svd.solve (-grad_f));
+ for (vector_t::Index j=0; j < c.size (); ++j) {
+ BOOST_CHECK (c [j] >= -eps);
+ }
}
-#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;
+ clock_t totalTimeGjkColl = 0;
+ clock_t totalTimeGjkNoColl = 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;
-
}
--
GitLab