point_simplex_squared_distance now returns the barycentric coordinates

Former-commit-id: ae156ef1ae22fe2e14e3cf9594f6ea00bf87a7a4

include/igl/point_simplex_squared_distance.cpp

@@ -20,26 +20,29 @@ template <
   typename DerivedV,
   typename DerivedEle,
   typename Derivedsqr_d,
-  typename Derivedc>
+  typename Derivedc,
+  typename Derivedb>
 IGL_INLINE void igl::point_simplex_squared_distance(
   const Eigen::MatrixBase<Derivedp> & p,
   const Eigen::MatrixBase<DerivedV> & V,
   const Eigen::MatrixBase<DerivedEle> & Ele,
   const typename DerivedEle::Index primitive,
   Derivedsqr_d & sqr_d,
-  Eigen::PlainObjectBase<Derivedc> & c)
+  Eigen::PlainObjectBase<Derivedc> & c,
+  Eigen::PlainObjectBase<Derivedb> & bary)
 {
   typedef typename Derivedp::Scalar Scalar;
   typedef typename Eigen::Matrix<Scalar,1,DIM> Vector;
   typedef Vector Point;
+  typedef Eigen::PlainObjectBase<Derivedb> BaryPoint;
 
   const auto & Dot = [](const Point & a, const Point & b)->Scalar
   {
     return a.dot(b);
   };
   // Real-time collision detection, Ericson, Chapter 5
-  const auto & ClosestPtPointTriangle = 
-    [&Dot](Point p, Point a, Point b, Point c)->Point 
+  const auto & ClosestBaryPtPointTriangle = 
+    [&Dot](Point p, Point a, Point b, Point c, BaryPoint& bary_out )->Point 
   {
     // Check if P in vertex region outside A
     Vector ab = b - a;
@@ -47,42 +50,61 @@ IGL_INLINE void igl::point_simplex_squared_distance(
     Vector ap = p - a;
     Scalar d1 = Dot(ab, ap);
     Scalar d2 = Dot(ac, ap);
-    if (d1 <= 0.0 && d2 <= 0.0) return a; // barycentric coordinates (1,0,0)
+    if (d1 <= 0.0 && d2 <= 0.0) {
+      // barycentric coordinates (1,0,0)
+      bary_out << 1, 0, 0;
+      return a;
+    }
     // Check if P in vertex region outside B
     Vector bp = p - b;
     Scalar d3 = Dot(ab, bp);
     Scalar d4 = Dot(ac, bp);
-    if (d3 >= 0.0 && d4 <= d3) return b; // barycentric coordinates (0,1,0)
+    if (d3 >= 0.0 && d4 <= d3) {
+      // barycentric coordinates (0,1,0)
+      bary_out << 0, 1, 0;
+      return b;
+    }
     // Check if P in edge region of AB, if so return projection of P onto AB
     Scalar vc = d1*d4 - d3*d2;
     if( a != b)
     {
       if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0) {
         Scalar v = d1 / (d1 - d3);
-        return a + v * ab; // barycentric coordinates (1-v,v,0)
+        // barycentric coordinates (1-v,v,0)
+        bary_out << 1-v, v, 0;
+        return a + v * ab;
       }
     }
     // Check if P in vertex region outside C
     Vector cp = p - c;
     Scalar d5 = Dot(ab, cp);
     Scalar d6 = Dot(ac, cp);
-    if (d6 >= 0.0 && d5 <= d6) return c; // barycentric coordinates (0,0,1)
+    if (d6 >= 0.0 && d5 <= d6) {
+      // barycentric coordinates (0,0,1)
+      bary_out << 0, 0, 1;
+      return c;
+    }
     // Check if P in edge region of AC, if so return projection of P onto AC
     Scalar vb = d5*d2 - d1*d6;
     if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0) {
       Scalar w = d2 / (d2 - d6);
-      return a + w * ac; // barycentric coordinates (1-w,0,w)
+      // barycentric coordinates (1-w,0,w)
+      bary_out << 1-w, 0, w;
+      return a + w * ac;
     }
     // Check if P in edge region of BC, if so return projection of P onto BC
     Scalar va = d3*d6 - d5*d4;
     if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0) {
       Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
-      return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+      // barycentric coordinates (0,1-w,w)
+      bary_out << 0, 1-w, w;
+      return b + w * (c - b);
     }
     // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
     Scalar denom = 1.0 / (va + vb + vc);
     Scalar v = vb * denom;
     Scalar w = vc * denom;
+    bary_out << 1.0-v-w, v, w;
     return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w
   };
 
@@ -91,16 +113,38 @@ IGL_INLINE void igl::point_simplex_squared_distance(
   assert(Ele.cols() <= DIM+1);
   assert(Ele.cols() <= 3 && "Only simplices up to triangles are considered");
 
-  c = ClosestPtPointTriangle(
+  bary.resize( DIM, 1 );
+  c = ClosestBaryPtPointTriangle(
     p,
     V.row(Ele(primitive,0)),
     V.row(Ele(primitive,1%Ele.cols())),
-    V.row(Ele(primitive,2%Ele.cols())));
+    V.row(Ele(primitive,2%Ele.cols())),
+    bary);
   sqr_d = (p-c).squaredNorm();
 }
+template <
+  int DIM,
+  typename Derivedp,
+  typename DerivedV,
+  typename DerivedEle,
+  typename Derivedsqr_d,
+  typename Derivedc>
+IGL_INLINE void igl::point_simplex_squared_distance(
+  const Eigen::MatrixBase<Derivedp> & p,
+  const Eigen::MatrixBase<DerivedV> & V,
+  const Eigen::MatrixBase<DerivedEle> & Ele,
+  const typename DerivedEle::Index primitive,
+  Derivedsqr_d & sqr_d,
+  Eigen::PlainObjectBase<Derivedc> & c)
+{
+    Eigen::PlainObjectBase<Derivedc> b;
+    point_simplex_squared_distance<DIM>( p, V, Ele, primitive, sqr_d, c, b );
+}
 
 #ifdef IGL_STATIC_LIBRARY
 // Explicit template instanciation
 template void igl::point_simplex_squared_distance<3, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
 template void igl::point_simplex_squared_distance<2, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&);
+template void igl::point_simplex_squared_distance<3, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
+template void igl::point_simplex_squared_distance<2, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&);
 #endif

include/igl/point_simplex_squared_distance.h

@@ -36,6 +36,35 @@ namespace igl
     const typename DerivedEle::Index i,
     Derivedsqr_d & sqr_d,
     Eigen::PlainObjectBase<Derivedc> & c);
+  // Determine squared distance from a point to linear simplex.
+  // Also return barycentric coordinate of closest point. 
+  //
+  // Inputs:
+  //   p  d-long query point
+  //   V  #V by d list of vertices
+  //   Ele  #Ele by ss<=d+1 list of simplex indices into V
+  //   i  index into Ele of simplex
+  // Outputs:
+  //   sqr_d  squared distance of Ele(i) to p
+  //   c  closest point on Ele(i) 
+  //   b  barycentric coordinates of closest point on Ele(i) 
+  //
+  template <
+    int DIM,
+    typename Derivedp,
+    typename DerivedV,
+    typename DerivedEle,
+    typename Derivedsqr_d,
+    typename Derivedc,
+    typename Derivedb>
+  IGL_INLINE void point_simplex_squared_distance(
+    const Eigen::MatrixBase<Derivedp> & p,
+    const Eigen::MatrixBase<DerivedV> & V,
+    const Eigen::MatrixBase<DerivedEle> & Ele,
+    const typename DerivedEle::Index i,
+    Derivedsqr_d & sqr_d,
+    Eigen::PlainObjectBase<Derivedc> & c,
+    Eigen::PlainObjectBase<Derivedb> & b);
 }
 #ifndef IGL_STATIC_LIBRARY
 #  include "point_simplex_squared_distance.cpp"