Skip to content
Snippets Groups Projects
Commit eb5499e7 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

[detail::GJK] Rename member for clarity 

  Simplex::c -> Simplex::vertex.
  Simplex::p -> Simplex::coefficient.
parent 47fc0b14
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/hpp/fcl/narrowphase/gjk.h
+ 4
− 4
View file @ eb5499e7
......@@ -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();
......
include/hpp/fcl/narrowphase/narrowphase.h
+ 15
− 12
View file @ eb5499e7
......@@ -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();
......
src/narrowphase/gjk.cpp
+ 69
− 45
View file @ eb5499e7
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment