From bef75add3a6fa8fe9682595eba786896438db47b Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 30 Aug 2019 15:53:21 +0200
Subject: [PATCH] [GJK] Force orientation of triangles.
---
src/narrowphase/gjk.cpp | 39 ++++++++++++++++-----------------------
1 file changed, 16 insertions(+), 23 deletions(-)
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index a92be05b..2d7a9ad8 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -465,7 +465,8 @@ 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]);
+ //Remove check because the triangle might be inverted
+ //assert (next_simplex.vertex[k] == curr_simplex.vertex[i]);
_ray += curr_simplex.vertex[i]->w * project_res.parameterization[i];
++k;
}
@@ -642,15 +643,21 @@ inline void originToSegment (
inline void originToTriangle (
const GJK::Simplex& current,
int a, int b, int c,
- const Vec3f& A, const Vec3f& AB, const Vec3f& AC,
+ const Vec3f& A,
const Vec3f& ABC,
GJK::Simplex& next,
Vec3f& ray)
{
+ bool aboveTri (ABC.dot(-A));
ray = ABC;
- next.vertex[0] = current.vertex[c];
- next.vertex[1] = current.vertex[b];
+ if (aboveTri) {
+ next.vertex[0] = current.vertex[c];
+ next.vertex[1] = current.vertex[b];
+ } else {
+ next.vertex[0] = current.vertex[b];
+ next.vertex[1] = current.vertex[c];
+ }
next.vertex[2] = current.vertex[a];
next.rank = 3;
@@ -701,12 +708,10 @@ FCL_REAL GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
if (towardsB < 0) { // Region 5
// A is the closest to the origin
originToPoint (current, a, A, next, ray);
- free_v[nfree++] = current.vertex[c];
free_v[nfree++] = current.vertex[b];
- } else { // Region 4
+ } else // Region 4
originToSegment (current, a, b, A, B, AB, towardsB, next, ray);
- free_v[nfree++] = current.vertex[c];
- }
+ free_v[nfree++] = current.vertex[c];
}
} else {
FCL_REAL edgeAB2o = AB.cross(ABC).dot (-A);
@@ -715,24 +720,12 @@ FCL_REAL GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
if (towardsB < 0) { // Region 5
// A is the closest to the origin
originToPoint (current, a, A, next, ray);
- free_v[nfree++] = current.vertex[c];
free_v[nfree++] = current.vertex[b];
- } else { // Region 4
+ } else // Region 4
originToSegment (current, a, b, A, B, AB, towardsB, next, ray);
- free_v[nfree++] = current.vertex[c];
- }
+ free_v[nfree++] = current.vertex[c];
} else {
- // I don't think there is any need to check whether we are above or below
- // the triangle.
- originToTriangle (current, a, b, c, A, AB, AC, ABC, next, ray);
- /*
- FCL_REAL aboveTri = ABC.dot(-A);
- if (aboveTri) { // Region 2
- originToTriangle (current, a, b, c, A, AB, AC, ABC, next, ray);
- } else { // Region 3
- // Flip the ray
- }
- */
+ originToTriangle (current, a, b, c, A, ABC, next, ray);
}
}
return ray.squaredNorm();
--
GitLab