Add point-to-triangle and point-to-mesh

include/point_mesh_distance.h

@@ -1,6 +1,10 @@
 #ifndef POINT_MESH_DISTANCE_H
 #define POINT_MESH_DISTANCE_H
 #include <Eigen/Core>
+#include <limits>
+#include "point_triangle_distance.h"
+#include "igl/per_face_normals.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`.

include/point_triangle_distance.h

@@ -1,6 +1,8 @@
 #ifndef POINT_TRIANGLE_DISTANCE_H
 #define POINT_TRIANGLE_DISTANCE_H
 #include <Eigen/Core>
+#include <limits>
+
 // Compute the distance `d` between a given point `x` and the closest point `p` on
 // a given triangle with corners `a`, `b`, and `c`.
 //

src/point_mesh_distance.cpp

@@ -1,4 +1,5 @@
 #include "point_mesh_distance.h"
+#include <iostream>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -8,9 +9,43 @@ void point_mesh_distance(
   Eigen::MatrixXd & P,
   Eigen::MatrixXd & N)
 {
-  // Replace with your code
   P.resizeLike(X);
+  D = Eigen::VectorXd(X.rows());
   N = Eigen::MatrixXd::Zero(X.rows(),X.cols());
-  for(int i = 0;i<X.rows();i++) P.row(i) = VY.row(i%VY.rows());
+
+  // Compute face normals
+  Eigen::MatrixXd FN;
+  igl::per_face_normals(VY,FY,Eigen::Vector3d(1,1,1).normalized(),FN);  
+
+  for(int i = 0;i<X.rows();i++){
+    double d = std::numeric_limits<double>::max(), tmp_d;
+    Eigen::RowVector3d closest_point, tmp_point, b;
+    int min_index = 0;
+
+    for(int j = 0;j<FY.rows();j++){
+      point_triangle_distance(
+        X.row(i), 
+        VY.row(FY(j, 0)),
+        VY.row(FY(j, 1)),
+        VY.row(FY(j, 2)),
+        tmp_d, tmp_point
+      );
+
+      // std::cout << i << "," << j << "," << tmp_d << "," << d << "\n";
+
+      if (tmp_d < d){
+        d = tmp_d;
+        closest_point = tmp_point;
+        min_index = j;
+      }
+    }
+
+    P.row(i) = closest_point;
+    D(i) = d;
+    N.row(i) = FN.row(min_index);
+
+    // std::cout << min_index << "," << d << "\n";
+  }
+
   D = (X-P).rowwise().norm();
-}
+}

src/point_triangle_distance.cpp

@@ -8,7 +8,93 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
-}
+  // NOTE: Can be optimized if needed, doing lazily
+
+  // Seems like the proper algorithm using barycentric
+  // coordinates would need to project to two edges anyway
+  // (for obtuse triangles)
+
+  d = std::numeric_limits<double>::max();
+
+  Eigen::RowVector3d ba = b - a;
+  Eigen::RowVector3d ca = c - a;
+  Eigen::RowVector3d xa = x - a;
+  Eigen::RowVector3d n = ba.cross(ca);
+  n.normalize();
+
+  // find point on plane
+  double t = n.dot(xa);
+  Eigen::RowVector3d pop = x + t*n;
+
+  // find barycentric coordinates
+  double d00 = ba.dot(ba);
+  double d01 = ba.dot(ca);
+  double d11 = ca.dot(ca);
+  double d20 = xa.dot(ba);
+  double d21 = xa.dot(ca);
+
+  double denom = d00 * d11 - d01 * d01;
+
+  double beta = (d11 * d20 - d01 * d21) / denom;
+  double gamma = (d00 * d21 - d01 * d20) / denom;
+  double alpha = 1 - beta - gamma; 
+
+  if(alpha > 0 && alpha < 1 && beta > 0 && beta < 1 && gamma > 0 && gamma < 1){
+    p = alpha * a + beta * b + gamma * c;
+    d = (p - x).norm();
+    return;
+  }
+
+  // check three vertices
+  double tmp = (x-a).norm();
+  if (d > tmp){
+    d = tmp;
+    p = a;
+  }
+
+  tmp = (x-b).norm();
+  if (d > tmp){
+    d = tmp;
+    p = b;
+  }
+
+  tmp = (x-c).norm();
+  if (d > tmp){
+    d = tmp;
+    p = c;
+  }
+
+  // check three edges
+  // ab
+  t = (b - a).dot(x - a)/(b-a).dot(b-a);
+  if (t > 0 && t < 1){
+    Eigen::RowVector3d tmp_p = a + t*(b-a);
+    tmp = (tmp_p - x).norm();
+    if (d > tmp){
+      p = tmp_p;
+      d = tmp;
+    }
+  }
+
+  // bc
+  t = (c - b).dot(x - b)/(c-b).dot(c-b);
+  if (t > 0 && t < 1){
+    Eigen::RowVector3d tmp_p = b + t*(c-b);
+    tmp = (tmp_p - x).norm();
+    if (d > tmp){
+      p = tmp_p;
+      d = tmp;
+    }
+  }
+
+  // ca
+  t = (a - c).dot(x - c)/(a-c).dot(a-c);
+  if (t > 0 && t < 1){
+    Eigen::RowVector3d tmp_p = c + t*(a-c);
+    tmp = (tmp_p - x).norm();
+    if (d > tmp){
+      p = tmp_p;
+      d = tmp;
+    }
+  }
+}