Unverified Commit 22f98db0 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #131 from jmirabel/devel

[EPA] Bug fixes
parents 2720bbe2 00166cb0
......@@ -116,6 +116,10 @@ struct BVNode : public BVNodeBase
static const Matrix3f id3 = Matrix3f::Identity();
return id3;
}
/// \cond
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// \endcond
};
template<>
......
......@@ -85,12 +85,12 @@ struct CollisionRequest;
/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21
/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22
/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23
template<size_t N>
template<short N>
class KDOP
{
private:
/// @brief Origin's distances to N KDOP planes
FCL_REAL dist_[N];
Eigen::Array<FCL_REAL, N, 1> dist_;
public:
......@@ -103,16 +103,15 @@ public:
/// @brief Creating kDOP containing two points
KDOP(const Vec3f& a, const Vec3f& b);
/// @brief Check whether two KDOPs are overlapped
/// @brief Check whether two KDOPs overlap.
bool overlap(const KDOP<N>& other) const;
/// Not implemented
bool overlap(const KDOP<N>& other, const CollisionRequest&,
FCL_REAL& sqrDistLowerBound) const
{
sqrDistLowerBound = sqrt (-1);
return overlap (other);
}
/// @brief Check whether two KDOPs overlap.
/// @return true if collision happens.
/// @retval sqrDistLowerBound squared lower bound on distance between boxes if
/// they do not overlap.
bool overlap(const KDOP<N>& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const;
/// @brief The distance between two KDOP<N>. Not implemented.
FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
......@@ -135,7 +134,7 @@ public:
/// @brief The (AABB) center
inline Vec3f center() const
{
return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
return (dist_.template head<3>() + dist_.template segment<3>(N/2)) / 2;
}
......@@ -163,12 +162,12 @@ public:
return width() * height() * depth();
}
inline FCL_REAL dist(std::size_t i) const
inline FCL_REAL dist(short i) const
{
return dist_[i];
}
inline FCL_REAL& dist(std::size_t i)
inline FCL_REAL& dist(short i)
{
return dist_[i];
}
......@@ -176,16 +175,20 @@ public:
//// @brief Check whether one point is inside the KDOP
bool inside(const Vec3f& p) const;
public:
/// \cond
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// \endcond
};
template<size_t N>
template<short N>
bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/)
{
throw std::logic_error ("not implemented");
}
template<size_t N>
template<short N>
bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
......@@ -194,7 +197,7 @@ bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
}
/// @brief translate the KDOP BV
template<size_t N>
template<short N>
KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
}
......
......@@ -218,7 +218,6 @@ static const size_t EPA_MAX_ITERATIONS = 255;
/// @brief class for EPA algorithm
struct EPA
{
private:
typedef GJK::SimplexV SimplexV;
struct SimplexF
{
......
......@@ -331,6 +331,9 @@ namespace internal
const Matrix3f& Bf,
const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance)
{
FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
if (sinus2 < 1e-6) return false;
const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
......@@ -338,21 +341,10 @@ namespace internal
// We need to divide by the norm || Aia x Bib ||
// As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
if (sinus2 > 1e-6) {
squaredLowerBoundDistance = diff * diff / sinus2;
if (squaredLowerBoundDistance > breakDistance2) {
return true;
}
squaredLowerBoundDistance = diff * diff / sinus2;
if (squaredLowerBoundDistance > breakDistance2) {
return true;
}
/* // or
FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
squaredLowerBoundDistance = diff * diff;
if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
squaredLowerBoundDistance /= sinus2;
return true;
}
// */
}
return false;
}
......
......@@ -39,6 +39,8 @@
#include <limits>
#include <iostream>
#include <hpp/fcl/collision_data.h>
namespace hpp
{
namespace fcl
......@@ -67,7 +69,7 @@ inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv)
/// @brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes
template<std::size_t N>
template<short N>
void getDistances(const Vec3f& p, FCL_REAL* d) {}
/// @brief Specification of getDistances
......@@ -106,39 +108,34 @@ inline void getDistances<9>(const Vec3f& p, FCL_REAL* d)
d[8] = p[1] + p[2] - p[0];
}
template<size_t N>
template<short N>
KDOP<N>::KDOP()
{
FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
for(size_t i = 0; i < N / 2; ++i)
{
dist_[i] = real_max;
dist_[i + N / 2] = -real_max;
}
dist_.template head<N/2>().setConstant( real_max);
dist_.template tail<N/2>().setConstant(-real_max);
}
template<size_t N>
template<short N>
KDOP<N>::KDOP(const Vec3f& v)
{
for(size_t i = 0; i < 3; ++i)
for(short i = 0; i < 3; ++i)
{
dist_[i] = dist_[N / 2 + i] = v[i];
}
FCL_REAL d[(N - 6) / 2];
getDistances<(N - 6) / 2>(v, d);
for(size_t i = 0; i < (N - 6) / 2; ++i)
for(short i = 0; i < (N - 6) / 2; ++i)
{
dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
}
}
template<size_t N>
template<short N>
KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b)
{
for(size_t i = 0; i < 3; ++i)
for(short i = 0; i < 3; ++i)
{
minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
}
......@@ -146,55 +143,69 @@ KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b)
FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
getDistances<(N - 6) / 2>(a, ad);
getDistances<(N - 6) / 2>(b, bd);
for(size_t i = 0; i < (N - 6) / 2; ++i)
for(short i = 0; i < (N - 6) / 2; ++i)
{
minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
}
}
template<size_t N>
template<short N>
bool KDOP<N>::overlap(const KDOP<N>& other) const
{
for(size_t i = 0; i < N / 2; ++i)
{
if(dist_[i] > other.dist_[i + N / 2]) return false;
if(dist_[i + N / 2] < other.dist_[i]) return false;
if ((dist_.template head<N/2>() > other.dist_.template tail<N/2>()).any()) return false;
if ((dist_.template tail<N/2>() < other.dist_.template head<N/2>()).any()) return false;
return true;
}
template<short N>
bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const
{
const FCL_REAL breakDistance (request.break_distance + request.security_margin);
FCL_REAL a = (dist_.template head<N/2>() - other.dist_.template tail<N/2>()).minCoeff();
if (a > breakDistance) {
sqrDistLowerBound = a*a;
return false;
}
FCL_REAL b = (other.dist_.template head<N/2>() - dist_.template tail<N/2>()).minCoeff();
if (b > breakDistance) {
sqrDistLowerBound = b*b;
return false;
}
sqrDistLowerBound = std::min(a, b);
return true;
}
template<size_t N>
template<short N>
bool KDOP<N>::inside(const Vec3f& p) const
{
for(size_t i = 0; i < 3; ++i)
{
if(p[i] < dist_[i] || p[i] > dist_[i + N / 2])
return false;
}
if ((p.array() < dist_.template head<3>()).any()) return false;
if ((p.array() > dist_.template segment<3>(N/2)).any()) return false;
FCL_REAL d[(N - 6) / 2];
getDistances<(N - 6) / 2>(p, d);
for(size_t i = 0; i < (N - 6) / 2; ++i)
{
if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2])
return false;
}
enum { P = ((N-6)/2) };
Eigen::Array<FCL_REAL, P, 1> d;
getDistances<P>(p, d.data());
if ((d < dist_.template segment<P>(3 )).any()) return false;
if ((d > dist_.template segment<P>(3+N/2)).any()) return false;
return true;
}
template<size_t N>
template<short N>
KDOP<N>& KDOP<N>::operator += (const Vec3f& p)
{
for(size_t i = 0; i < 3; ++i)
for(short i = 0; i < 3; ++i)
{
minmax(p[i], dist_[i], dist_[N / 2 + i]);
}
FCL_REAL pd[(N - 6) / 2];
getDistances<(N - 6) / 2>(p, pd);
for(size_t i = 0; i < (N - 6) / 2; ++i)
for(short i = 0; i < (N - 6) / 2; ++i)
{
minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
}
......@@ -202,10 +213,10 @@ KDOP<N>& KDOP<N>::operator += (const Vec3f& p)
return *this;
}
template<size_t N>
template<short N>
KDOP<N>& KDOP<N>::operator += (const KDOP<N>& other)
{
for(size_t i = 0; i < N / 2; ++i)
for(short i = 0; i < N / 2; ++i)
{
dist_[i] = std::min(other.dist_[i], dist_[i]);
dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
......@@ -213,7 +224,7 @@ KDOP<N>& KDOP<N>::operator += (const KDOP<N>& other)
return *this;
}
template<size_t N>
template<short N>
KDOP<N> KDOP<N>::operator + (const KDOP<N>& other) const
{
KDOP<N> res(*this);
......@@ -221,7 +232,7 @@ KDOP<N> KDOP<N>::operator + (const KDOP<N>& other) const
}
template<size_t N>
template<short N>
FCL_REAL KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const
{
std::cerr << "KDOP distance not implemented!" << std::endl;
......@@ -229,22 +240,22 @@ FCL_REAL KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/)
}
template<size_t N>
template<short N>
KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t)
{
KDOP<N> res(bv);
for(size_t i = 0; i < 3; ++i)
for(short i = 0; i < 3; ++i)
{
res.dist(i) += t[i];
res.dist(N / 2 + i) += t[i];
res.dist(short(N / 2 + i)) += t[i];
}
FCL_REAL d[(N - 6) / 2];
getDistances<(N - 6) / 2>(t, d);
for(size_t i = 0; i < (N - 6) / 2; ++i)
for(short i = 0; i < (N - 6) / 2; ++i)
{
res.dist(3 + i) += d[i];
res.dist(3 + i + N / 2) += d[i];
res.dist(short(3 + i)) += d[i];
res.dist(short(3 + i + N / 2)) += d[i];
}
return res;
......
......@@ -1058,25 +1058,26 @@ void EPA::initialize()
bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist)
{
Vec3f ba = b->w - a->w;
Vec3f n_ab = ba.cross(face->n);
Vec3f ab = b->w - a->w;
Vec3f n_ab = ab.cross(face->n);
FCL_REAL a_dot_nab = a->w.dot(n_ab);
if(a_dot_nab < 0) // the origin is on the outside part of ab
{
// following is similar to projectOrigin for two points
// however, as we dont need to compute the parameterization, dont need to compute 0 or 1
FCL_REAL a_dot_ba = a->w.dot(ba);
FCL_REAL b_dot_ba = b->w.dot(ba);
FCL_REAL a_dot_ab = a->w.dot(ab);
FCL_REAL b_dot_ab = b->w.dot(ab);
if(a_dot_ba > 0)
if(a_dot_ab > 0)
dist = a->w.norm();
else if(b_dot_ba < 0)
else if(b_dot_ab < 0)
dist = b->w.norm();
else
{
FCL_REAL a_dot_b = a->w.dot(b->w);
dist = std::sqrt(std::max(a->w.squaredNorm() * b->w.squaredNorm() - a_dot_b * a_dot_b, (FCL_REAL)0));
dist = std::sqrt(std::max(
a->w.squaredNorm() - a_dot_ab * a_dot_ab / ab.squaredNorm(),
0.));
}
return true;
......@@ -1099,16 +1100,17 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
face->n = (b->w - a->w).cross(c->w - a->w);
FCL_REAL l = face->n.norm();
if(l > tolerance)
if(l > Eigen::NumTraits<FCL_REAL>::epsilon())
{
face->n /= l;
if(!(getEdgeDist(face, a, b, face->d) ||
getEdgeDist(face, b, c, face->d) ||
getEdgeDist(face, c, a, face->d)))
{
face->d = a->w.dot(face->n) / l;
face->d = a->w.dot(face->n);
}
face->n /= l;
if(forced || face->d >= -tolerance)
return face;
else
......@@ -1194,46 +1196,35 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
status = Valid;
for(; iterations < max_iterations; ++iterations)
{
if(nextsv < max_vertex_num)
{
SimplexHorizon horizon;
SimplexV* w = &sv_store[nextsv++];
bool valid = true;
best->pass = ++pass;
// At the moment, SimplexF.n is always normalized. This could be revised in the future...
gjk.getSupport(best->n, true, *w);
FCL_REAL wdist = best->n.dot(w->w) - best->d;
if(wdist > tolerance)
{
for(size_t j = 0; (j < 3) && valid; ++j)
{
valid &= expand(pass, w, best->f[j], best->e[j], horizon);
}
if(valid && horizon.nf >= 3)
{
// need to add the edge connectivity between first and last faces
bind(horizon.ff, 2, horizon.cf, 1);
hull.remove(best);
stock.append(best);
best = findBest();
outer = *best;
}
else
{
status = InvalidHull; break;
}
}
else
{
status = AccuracyReached; break;
}
if (nextsv >= max_vertex_num) {
status = OutOfVertices;
break;
}
else
{
status = OutOfVertices; break;
SimplexHorizon horizon;
SimplexV* w = &sv_store[nextsv++];
bool valid = true;
best->pass = ++pass;
// At the moment, SimplexF.n is always normalized. This could be revised in the future...
gjk.getSupport(best->n, true, *w);
FCL_REAL wdist = best->n.dot(w->w) - best->d;
if(wdist <= tolerance) {
status = AccuracyReached;
break;
}
for(size_t j = 0; (j < 3) && valid; ++j)
valid &= expand(pass, w, best->f[j], best->e[j], horizon);
if(!valid || horizon.nf < 3) {
status = InvalidHull;
break;
}
// need to add the edge connectivity between first and last faces
bind(horizon.ff, 2, horizon.cf, 1);
hull.remove(best);
stock.append(best);
best = findBest();
outer = *best;
}
normal = outer.n;
......@@ -1264,45 +1255,44 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon
static const size_t nexti[] = {1, 2, 0};
static const size_t previ[] = {2, 0, 1};
if(f->pass != pass)
if(f->pass == pass)
return false;
const size_t e1 = nexti[e];
// 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->vertex[e]->w) < -tolerance)
{
const size_t e1 = nexti[e];
// 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->vertex[e1], f->vertex[e], w, false);
if(nf)
{
SimplexF* nf = newFace(f->vertex[e1], f->vertex[e], w, false);
if(nf)
{
// add face-face connectivity
bind(nf, 0, f, e);
// if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge.
// This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function.
// Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left)
if(horizon.cf)
bind(nf, 2, horizon.cf, 1);
else
horizon.ff = nf;
horizon.cf = nf;
++horizon.nf;
return true;
}
}
else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face
{
const size_t e2 = previ[e];
f->pass = pass;
if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon))
{
hull.remove(f);
stock.append(f);
return true;
}
// add face-face connectivity
bind(nf, 0, f, e);
// if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge.
// This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function.
// Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left)
if(horizon.cf)
bind(nf, 2, horizon.cf, 1);
else
horizon.ff = nf;
horizon.cf = nf;
++horizon.nf;
return true;
}
return false;
}
// case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face
const size_t e2 = previ[e];
f->pass = pass;
if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon))
{
hull.remove(f);
stock.append(f);
return true;
}
return false;
}
......
......@@ -508,10 +508,10 @@ void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, K
const Vec3f& n = new_s.n;
const FCL_REAL& d = new_s.d;
const std::size_t D = 8;
for(std::size_t i = 0; i < D; ++i)
const short D = 8;
for(short i = 0; i < D; ++i)
bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
for(std::size_t i = D; i < 2 * D; ++i)
for(short i = D; i < 2 * D; ++i)
bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
......@@ -563,11 +563,11 @@ void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, K
const Vec3f& n = new_s.n;
const FCL_REAL& d = new_s.d;
const std::size_t D = 9;
const short D = 9;
for(std::size_t i = 0; i < D; ++i)
for(short i = 0; i < D; ++i)
bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();