finished distance functions

include/point_mesh_distance.h

@@ -1,6 +1,8 @@
 #ifndef POINT_MESH_DISTANCE_H
 #define POINT_MESH_DISTANCE_H
 #include <Eigen/Core>
+#include <igl/per_face_normals.h>
+#include "point_triangle_distance.h"
 // Compute the distances `D` between a set of given points `X` and their
 // closest points `P` on a given mesh with vertex positions `VY` and face
 // indices `FY`.

src/point_mesh_distance.cpp

@@ -1,5 +1,5 @@
 #include "point_mesh_distance.h"
-
+#include <limits>
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
   const Eigen::MatrixXd & VY,
@@ -11,6 +11,20 @@ void point_mesh_distance(
   // Replace with your code
   P.resizeLike(X);
   N = Eigen::MatrixXd::Zero(X.rows(),X.cols());
-  for(int i = 0;i<X.rows();i++) P.row(i) = VY.row(i%VY.rows());
-  D = (X-P).rowwise().norm();
+  double d;
+  Eigen::RowVector3d p;
+  Eigen::MatrixXd pfn;
+  igl::per_face_normals(VY,FY,Eigen::Vector3d(1,1,1).normalized(),pfn);
+  for(int i = 0;i<X.rows();i++) {
+    D(i) = std::numeric_limits<double>::max();
+    for(int j = 0;j<FY.rows();j++) {
+      point_triangle_distance(X, VY.row(FY(j,0)), VY.row(FY(j,1)), VY.row(FY(j,2)), 
+        d, p);
+      if (D(i) > d) {
+        D(i) = d;
+        P.row(i) = p;
+        N.row(i) = pfn.row(j);
+      }
+    }
+  }
 }

src/point_triangle_distance.cpp

@@ -1,4 +1,5 @@
 #include "point_triangle_distance.h"
+#include <limits>
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -8,7 +9,41 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+  // following algorithm from below link
+  // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.4264&rep=rep1&type=pdf
+  Eigen::RowVector3d ba = b-a;
+  Eigen::RowVector3d cb = c-b;
+  Eigen::RowVector3d ax = a-x;
+  Eigen::RowVector3d n = ba.cross(cb);
+  double area = n.norm();
+  double angle = ax.dot(n) / n.norm();
+  Eigen::RowVector3d projectx = x - angle * n / n.norm();
+
+  Eigen::RowVector3d pb = projectx - b;
+  Eigen::RowVector3d pc = projectx - c;
+  Eigen::RowVector3d pa = projectx - a;
+  double alpha = (pb.cross(pc)).norm() / area; 
+  double beta = (pc.cross(pa)).norm() / area;
+  double gamma = 1 - alpha - beta;
+
+  if(0 <= alpha && alpha <= 1 &&
+  	0 <= beta && beta <= 1 &&
+  	0 <= gamma && gamma <= 1) {
+  	p = projectx;
+  	d = (x - p).norm();
+  	return ;
+  }
+  d = std::numeric_limits<double>::max();
+  if (d > (x - a).norm()) {
+  	d = (x - a).norm();
+  	p = a;
+  }
+  if (d > (x - b).norm()) {
+  	d = (x - b).norm();
+  	p = b;
+  }
+  if (d > (x - c).norm()) {
+  	d = (x - c).norm();
+  	p = c;
+  }
 }