From eb5499e737fc323ac949103a8efe29e1247ba78d Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Thu, 29 Nov 2018 17:09:01 +0100
Subject: [PATCH] [detail::GJK] Rename member for clarity
Simplex::c -> Simplex::vertex.
Simplex::p -> Simplex::coefficient.
---
include/hpp/fcl/narrowphase/gjk.h | 8 +-
include/hpp/fcl/narrowphase/narrowphase.h | 27 ++---
src/narrowphase/gjk.cpp | 114 +++++++++++++---------
3 files changed, 88 insertions(+), 61 deletions(-)
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 8ad919ae..b960c042 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -137,9 +137,9 @@ struct GJK
struct Simplex
{
/// @brief simplex vertex
- SimplexV* c[4];
+ SimplexV* vertex[4];
/// @brief weight
- FCL_REAL p[4];
+ FCL_REAL coefficient[4];
/// @brief size of simplex (number of vertices)
size_t rank;
@@ -217,7 +217,7 @@ private:
{
Vec3f n;
FCL_REAL d;
- SimplexV* c[3]; // a face has three vertices
+ SimplexV* vertex[3]; // a face has three vertices
SimplexF* f[3]; // a face has three adjacent faces
SimplexF* l[2]; // the pre and post faces in the list
size_t e[3];
@@ -300,7 +300,7 @@ public:
bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
- SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
+ SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex, bool forced);
/// @brief Find the best polytope face to split
SimplexF* findBest();
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 7554b56a..b0e8d3a7 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -79,7 +79,8 @@ struct GJKSolver_indep
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ w0 += shape.support(epa.result.vertex[i]->d, 0) *
+ epa.result.coefficient[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = tf2.getRotation() * epa.normal;
@@ -128,7 +129,8 @@ struct GJKSolver_indep
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ w0 += shape.support(epa.result.vertex[i]->d, 0) *
+ epa.result.coefficient[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = -epa.normal;
@@ -177,7 +179,8 @@ struct GJKSolver_indep
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ w0 += shape.support(epa.result.vertex[i]->d, 0) *
+ epa.result.coefficient[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = -epa.normal;
@@ -218,9 +221,9 @@ struct GJKSolver_indep
Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- FCL_REAL p = gjk.getSimplex()->p[i];
- w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
- w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
+ FCL_REAL p = gjk.getSimplex()->coefficient[i];
+ w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
}
if(distance) *distance = (w0 - w1).norm();
@@ -283,9 +286,9 @@ struct GJKSolver_indep
Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- FCL_REAL p = gjk.getSimplex()->p[i];
- w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
- w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
+ FCL_REAL p = gjk.getSimplex()->coefficient[i];
+ w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
}
if(distance) *distance = (w0 - w1).norm();
@@ -333,9 +336,9 @@ struct GJKSolver_indep
Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- FCL_REAL p = gjk.getSimplex()->p[i];
- w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
- w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
+ FCL_REAL p = gjk.getSimplex()->coefficient[i];
+ w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
}
if(distance) *distance = (w0 - w1).norm();
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 1a705c61..aa71cbc4 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -204,8 +204,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
// appendVertex(simplices[0], (ray.squaredNorm() > 0) ? -ray : Vec3f(1, 0, 0));
if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray);
else appendVertex(simplices[0], Vec3f(1, 0, 0));
- simplices[0].p[0] = 1;
- ray = simplices[0].c[0]->w;
+ simplices[0].coefficient[0] = 1;
+ ray = simplices[0].vertex[0]->w;
lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray; // cache previous support points, the new support point will compare with it to avoid too close support points
do
@@ -225,7 +225,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
appendVertex(curr_simplex, -ray); // see below, ray points away from origin
// check B: when the new support point is close to previous support points, stop (as the new simplex is degenerated)
- Vec3f& w = curr_simplex.c[curr_simplex.rank - 1]->w;
+ Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
bool found = false;
for(size_t i = 0; i < 4; ++i)
{
@@ -258,11 +258,19 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
switch(curr_simplex.rank)
{
case 2:
- project_res = Project::projectLineOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w); break;
+ project_res = Project::projectLineOrigin(curr_simplex.vertex[0]->w,
+ curr_simplex.vertex[1]->w);
+ break;
case 3:
- project_res = Project::projectTriangleOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w); break;
+ project_res = Project::projectTriangleOrigin(curr_simplex.vertex[0]->w,
+ curr_simplex.vertex[1]->w,
+ curr_simplex.vertex[2]->w); break;
case 4:
- project_res = Project::projectTetrahedraOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w); break;
+ project_res = Project::projectTetrahedraOrigin(curr_simplex.vertex[0]->w,
+ curr_simplex.vertex[1]->w,
+ curr_simplex.vertex[2]->w,
+ curr_simplex.vertex[3]->w);
+ break;
}
if(project_res.sqr_distance >= 0)
@@ -274,12 +282,15 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
{
if(project_res.encode & (1 << i))
{
- next_simplex.c[next_simplex.rank] = curr_simplex.c[i];
- next_simplex.p[next_simplex.rank++] = project_res.parameterization[i]; // weights[i];
- ray += curr_simplex.c[i]->w * project_res.parameterization[i]; // weights[i];
+ next_simplex.vertex[next_simplex.rank] = curr_simplex.vertex[i];
+ // weights[i]
+ next_simplex.coefficient[next_simplex.rank++] =
+ project_res.parameterization[i];
+ // weights[i]
+ ray += curr_simplex.vertex[i]->w * project_res.parameterization[i];
}
else
- free_v[nfree++] = curr_simplex.c[i];
+ free_v[nfree++] = curr_simplex.vertex[i];
}
if(project_res.encode == 15) status = Inside; // the origin is within the 4-simplex, collision
}
@@ -317,14 +328,14 @@ void GJK::getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const
void GJK::removeVertex(Simplex& simplex)
{
- free_v[nfree++] = simplex.c[--simplex.rank];
+ free_v[nfree++] = simplex.vertex[--simplex.rank];
}
void GJK::appendVertex(Simplex& simplex, const Vec3f& v)
{
- simplex.p[simplex.rank] = 0; // initial weight 0
- simplex.c[simplex.rank] = free_v[--nfree]; // set the memory
- getSupport(v, *simplex.c[simplex.rank++]);
+ simplex.coefficient[simplex.rank] = 0; // initial weight 0
+ simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
+ getSupport(v, *simplex.vertex[simplex.rank++]);
}
bool GJK::encloseOrigin()
@@ -348,7 +359,7 @@ bool GJK::encloseOrigin()
break;
case 2:
{
- Vec3f d = simplex->c[1]->w - simplex->c[0]->w;
+ Vec3f d = simplex->vertex[1]->w - simplex->vertex[0]->w;
for(size_t i = 0; i < 3; ++i)
{
Vec3f axis(0,0,0);
@@ -368,7 +379,8 @@ bool GJK::encloseOrigin()
break;
case 3:
{
- Vec3f n = (simplex->c[1]->w - simplex->c[0]->w).cross(simplex->c[2]->w - simplex->c[0]->w);
+ Vec3f n = (simplex->vertex[1]->w - simplex->vertex[0]->w).cross
+ (simplex->vertex[2]->w - simplex->vertex[0]->w);
if(n.squaredNorm() > 0)
{
appendVertex(*simplex, n);
@@ -382,7 +394,9 @@ bool GJK::encloseOrigin()
break;
case 4:
{
- if(std::abs(triple(simplex->c[0]->w - simplex->c[3]->w, simplex->c[1]->w - simplex->c[3]->w, simplex->c[2]->w - simplex->c[3]->w)) > 0)
+ if(std::abs(triple(simplex->vertex[0]->w - simplex->vertex[3]->w,
+ simplex->vertex[1]->w - simplex->vertex[3]->w,
+ simplex->vertex[2]->w - simplex->vertex[3]->w)) > 0)
return true;
}
break;
@@ -441,9 +455,9 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
stock.remove(face);
hull.append(face);
face->pass = 0;
- face->c[0] = a;
- face->c[1] = b;
- face->c[2] = c;
+ face->vertex[0] = a;
+ face->vertex[1] = b;
+ face->vertex[2] = c;
face->n = (b->w - a->w).cross(c->w - a->w);
FCL_REAL l = face->n.norm();
@@ -506,21 +520,27 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
status = Valid;
nextsv = 0;
- if((simplex.c[0]->w - simplex.c[3]->w).dot((simplex.c[1]->w - simplex.c[3]->w).cross(simplex.c[2]->w - simplex.c[3]->w)) < 0)
+ if((simplex.vertex[0]->w - simplex.vertex[3]->w).dot
+ ((simplex.vertex[1]->w - simplex.vertex[3]->w).cross
+ (simplex.vertex[2]->w - simplex.vertex[3]->w)) < 0)
{
- SimplexV* tmp = simplex.c[0];
- simplex.c[0] = simplex.c[1];
- simplex.c[1] = tmp;
+ SimplexV* tmp = simplex.vertex[0];
+ simplex.vertex[0] = simplex.vertex[1];
+ simplex.vertex[1] = tmp;
- FCL_REAL tmpv = simplex.p[0];
- simplex.p[0] = simplex.p[1];
- simplex.p[1] = tmpv;
+ FCL_REAL tmpv = simplex.coefficient[0];
+ simplex.coefficient[0] = simplex.coefficient[1];
+ simplex.coefficient[1] = tmpv;
}
- SimplexF* tetrahedron[] = {newFace(simplex.c[0], simplex.c[1], simplex.c[2], true),
- newFace(simplex.c[1], simplex.c[0], simplex.c[3], true),
- newFace(simplex.c[2], simplex.c[1], simplex.c[3], true),
- newFace(simplex.c[0], simplex.c[2], simplex.c[3], true) };
+ SimplexF* tetrahedron[] = {newFace(simplex.vertex[0], simplex.vertex[1],
+ simplex.vertex[2], true),
+ newFace(simplex.vertex[1], simplex.vertex[0],
+ simplex.vertex[3], true),
+ newFace(simplex.vertex[2], simplex.vertex[1],
+ simplex.vertex[3], true),
+ newFace(simplex.vertex[0], simplex.vertex[2],
+ simplex.vertex[3], true) };
if(hull.count == 4)
{
@@ -585,17 +605,21 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
normal = outer.n;
depth = outer.d;
result.rank = 3;
- result.c[0] = outer.c[0];
- result.c[1] = outer.c[1];
- result.c[2] = outer.c[2];
- result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).norm();
- result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).norm();
- result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).norm();
-
- FCL_REAL sum = result.p[0] + result.p[1] + result.p[2];
- result.p[0] /= sum;
- result.p[1] /= sum;
- result.p[2] /= sum;
+ result.vertex[0] = outer.vertex[0];
+ result.vertex[1] = outer.vertex[1];
+ result.vertex[2] = outer.vertex[2];
+ result.coefficient[0] = ((outer.vertex[1]->w - projection).cross
+ (outer.vertex[2]->w - projection)).norm();
+ result.coefficient[1] = ((outer.vertex[2]->w - projection).cross
+ (outer.vertex[0]->w - projection)).norm();
+ result.coefficient[2] = ((outer.vertex[0]->w - projection).cross
+ (outer.vertex[1]->w - projection)).norm();
+
+ FCL_REAL sum = result.coefficient[0] + result.coefficient[1] +
+ result.coefficient[2];
+ result.coefficient[0] /= sum;
+ result.coefficient[1] /= sum;
+ result.coefficient[2] /= sum;
return status;
}
}
@@ -607,8 +631,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
else normal = Vec3f(1, 0, 0);
depth = 0;
result.rank = 1;
- result.c[0] = simplex.c[0];
- result.p[0] = 1;
+ result.vertex[0] = simplex.vertex[0];
+ result.coefficient[0] = 1;
return status;
}
@@ -626,7 +650,7 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon
// case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
if(f->n.dot(w->w) - f->d < -tolerance)
{
- SimplexF* nf = newFace(f->c[e1], f->c[e], w, false);
+ SimplexF* nf = newFace(f->vertex[e1], f->vertex[e], w, false);
if(nf)
{
// add face-face connectivity
--
GitLab