From 175a0a8871e159a633040da54c31f0d6d5a0d47e Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 29 Aug 2019 18:15:51 +0200
Subject: [PATCH] [GJK] Handle simplex of rank 3 + checks of backward compat.
---
include/hpp/fcl/narrowphase/gjk.h | 3 +
src/narrowphase/gjk.cpp | 180 +++++++++++++++++++++++++++++-
2 files changed, 178 insertions(+), 5 deletions(-)
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index c018747c..42f39c1a 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -190,6 +190,9 @@ private:
/// @brief Project origin (0) onto line a-b
FCL_REAL projectLineOrigin(const Simplex& current, Simplex& next);
+ /// @brief Project origin (0) onto triangle a-b-c
+ FCL_REAL projectTriangleOrigin(const Simplex& current, Simplex& next);
+
};
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index a0d321a3..d1964e35 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -378,6 +378,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
break;
}
+ // This has been rewritten thanks to the excellent video:
+ // https://youtu.be/Qupqu1xe7Io
switch(curr_simplex.rank)
{
case 2:
@@ -395,10 +397,11 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
{
if(project_res.encode & (1 << i))
{
- assert (next_simplex.vertex[k++] == curr_simplex.vertex[i]);
+ assert (next_simplex.vertex[k] == curr_simplex.vertex[i]);
assert (
fabs(next_simplex.coefficient[k]-project_res.parameterization[i]) < 1e-10);
_ray += curr_simplex.vertex[i]->w * project_res.parameterization[i];
+ ++k;
}
}
//assert (_ray.isApprox (ray));
@@ -407,9 +410,31 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
}
break;
case 3:
- project_res = Project::projectTriangleOrigin(curr_simplex.vertex[0]->w,
- curr_simplex.vertex[1]->w,
- curr_simplex.vertex[2]->w); break;
+ project_res.sqr_distance = projectTriangleOrigin (curr_simplex, next_simplex);
+ { // This only checks that, so far, the behaviour did not change.
+ FCL_REAL d = project_res.sqr_distance;
+ project_res = Project::projectTriangleOrigin(curr_simplex.vertex[0]->w,
+ curr_simplex.vertex[1]->w,
+ curr_simplex.vertex[2]->w);
+ assert (fabs(d-project_res.sqr_distance) < 1e-10);
+
+ Vec3f _ray(0,0,0);
+ size_t k = 0;
+ for(size_t i = 0; i < curr_simplex.rank; ++i)
+ {
+ if(project_res.encode & (1 << i))
+ {
+ assert (next_simplex.vertex[k] == curr_simplex.vertex[i]);
+ assert (
+ fabs(next_simplex.coefficient[k]-project_res.parameterization[i]) < 1e-10);
+ _ray += curr_simplex.vertex[i]->w * project_res.parameterization[i];
+ ++k;
+ }
+ }
+ assert ((_ray - ray).array().abs().maxCoeff() < 1e-10);
+ assert (k = next_simplex.rank);
+ }
+ break;
case 4:
project_res = Project::projectTetrahedraOrigin(curr_simplex.vertex[0]->w,
curr_simplex.vertex[1]->w,
@@ -420,7 +445,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
if(project_res.sqr_distance >= 0)
{
current = next;
- if (curr_simplex.rank != 2) {
+ if (curr_simplex.rank > 3) {
next_simplex.rank = 0;
ray = Vec3f(0,0,0);
for(size_t i = 0; i < curr_simplex.rank; ++i)
@@ -587,6 +612,151 @@ FCL_REAL GJK::projectLineOrigin(const Simplex& current, Simplex& next)
}
}
+FCL_REAL GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
+{
+ const int a = 2, b = 1, c = 0;
+ // A is the last point we added.
+ const Vec3f& A = current.vertex[a]->w,
+ B = current.vertex[b]->w,
+ C = current.vertex[c]->w;
+
+ const Vec3f AB = B - A,
+ AC = C - A,
+ ABC = AB.cross(AC);
+
+ FCL_REAL edgeAC2o = ABC.cross(AC).dot (-A);
+ if (edgeAC2o >= 0) {
+ FCL_REAL towardsC = AC.dot (-A);
+ if (towardsC >= 0) { // Region 1
+ // ray = - ( AC ^ AO ) ^ AC = (AC.C) A + (-AC.A) C
+ ray = AC.dot(C) * A + towardsC * C;
+ next.vertex[0] = current.vertex[c];
+ next.vertex[1] = current.vertex[a];
+ next.rank = 2;
+ free_v[nfree++] = current.vertex[b];
+ // To ensure backward compatibility
+
+ FCL_REAL alpha = towardsC / AC.squaredNorm();
+ next.coefficient[1] = 1 - alpha; // A
+ next.coefficient[0] = alpha; // C
+ ray /= AC.squaredNorm();
+ return ray.squaredNorm();
+ } else { // Region 4 or 5
+ FCL_REAL towardsB = AB.dot(-A);
+ if (towardsB < 0) { // Region 5
+ // A is the closest to the origin
+ ray = A;
+ next.vertex[0] = current.vertex[a];
+ next.rank = 1;
+ free_v[nfree++] = current.vertex[c];
+ free_v[nfree++] = current.vertex[b];
+
+ // To ensure backward compatibility
+ next.coefficient[0] = 1;
+ return A.squaredNorm();
+ } else { // Region 4
+ // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
+ ray = AB.dot(B) * A + towardsB * B;
+
+ next.vertex[0] = current.vertex[b];
+ next.vertex[1] = current.vertex[a];
+ next.rank = 2;
+ free_v[nfree++] = current.vertex[c];
+
+ // To ensure backward compatibility
+ FCL_REAL alpha = towardsB / AB.squaredNorm();
+ next.coefficient[1] = 1 - alpha; // A
+ next.coefficient[0] = alpha; // B
+ ray /= AB.squaredNorm();
+
+ return ray.squaredNorm();
+ }
+ }
+ } else {
+ FCL_REAL edgeAB2o = AB.cross(ABC).dot (-A);
+ if (edgeAB2o >= 0) { // Region 4 or 5
+ FCL_REAL towardsB = AB.dot(-A);
+ if (towardsB < 0) { // Region 5
+ // A is the closest to the origin
+ ray = A;
+ next.vertex[0] = current.vertex[a];
+ next.rank = 1;
+ free_v[nfree++] = current.vertex[c];
+ free_v[nfree++] = current.vertex[b];
+
+ // To ensure backward compatibility
+ next.coefficient[0] = 1;
+ return A.squaredNorm();
+ } else { // Region 4
+ // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
+ ray = AB.dot(B) * A + towardsB * B;
+
+ next.vertex[0] = current.vertex[b];
+ next.vertex[1] = current.vertex[a];
+ next.rank = 2;
+ free_v[nfree++] = current.vertex[c];
+
+ // To ensure backward compatibility
+ FCL_REAL alpha = towardsB / AB.squaredNorm();
+ next.coefficient[1] = 1 - alpha; // A
+ next.coefficient[0] = alpha; // B
+ ray /= AB.squaredNorm();
+
+ return ray.squaredNorm();
+ }
+ } else {
+ FCL_REAL aboveTri = ABC.dot(-A);
+ if (aboveTri) { // Region 2
+ ray = ABC;
+
+ next.vertex[0] = current.vertex[c];
+ next.vertex[1] = current.vertex[b];
+ next.vertex[2] = current.vertex[a];
+ next.rank = 3;
+
+ // To ensure backward compatibility
+ FCL_REAL s = ABC.squaredNorm();
+ ray *= A.dot(ABC) / s;
+ Vec3f AQ (ray - A);
+ Vec3f m (ABC.cross(AB));
+ FCL_REAL _u2 = 1/AB.squaredNorm();
+ FCL_REAL wu_u2 = AQ.dot (AB) * _u2;
+ FCL_REAL vu_u2 = AC.dot (AB) * _u2;
+ FCL_REAL wm_vm = AQ.dot(m) / AC.dot (m);
+
+ next.coefficient[0] = wm_vm; // C
+ next.coefficient[1] = wu_u2 - wm_vm*vu_u2; // B
+ next.coefficient[2] = 1 - next.coefficient[0] - next.coefficient[1]; // A
+ assert (0. <= next.coefficient[0] && next.coefficient[0] <= 1.);
+ assert (0. <= next.coefficient[1] && next.coefficient[1] <= 1.);
+ assert (0. <= next.coefficient[2] && next.coefficient[2] <= 1.);
+
+ return ray.squaredNorm();
+ } else { // Region 3
+ ray = ABC;
+
+ //next.vertex[0] = current.vertex[b];
+ next.vertex[0] = current.vertex[c];
+ next.vertex[1] = current.vertex[b];
+ next.vertex[2] = current.vertex[a];
+ next.rank = 3;
+
+ // To ensure backward compatibility
+ FCL_REAL s = ABC.squaredNorm();
+ ray *= A.dot(ABC) / s;
+ next.coefficient[2] = -1;
+ next.coefficient[1] = -1;
+ next.coefficient[0] = -1;
+ assert (0. <= next.coefficient[0] && next.coefficient[0] <= 1.);
+ assert (0. <= next.coefficient[1] && next.coefficient[1] <= 1.);
+ assert (0. <= next.coefficient[2] && next.coefficient[2] <= 1.);
+
+ return ray.squaredNorm();
+ }
+ }
+ }
+}
+
void EPA::initialize()
{
sv_store = new SimplexV[max_vertex_num];
--
GitLab