Merge pull request #131 from jmirabel/devel

[EPA] Bug fixes

include/hpp/fcl/BV/BV_node.h

@@ -116,6 +116,10 @@ struct BVNode : public BVNodeBase
     static const Matrix3f id3 = Matrix3f::Identity();
     return id3;
   }
+
+  /// \cond
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  /// \endcond
 };
 
 template<>

include/hpp/fcl/BV/kDOP.h

@@ -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);
 
 }

include/hpp/fcl/narrowphase/gjk.h

@@ -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
   {

src/BV/OBB.cpp

@@ -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;
     }

src/BV/kDOP.cpp

@@ -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;

src/narrowphase/gjk.cpp

@@ -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;
 }
 

src/shape/geometric_shapes_utility.cpp

@@ -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();
-