Delete necessary comments I think it worked as intended

src/icp_single_iteration.cpp

@@ -28,15 +28,14 @@ void icp_single_iteration(
   Eigen::VectorXd D;
   Eigen::MatrixXd N;
 
-  std::cout << "1" <<std::endl;
+
   random_points_on_mesh(num_samples, VX, FX, X);
-  std::cout << "2" <<std::endl;
+
   point_mesh_distance(X, VY, FY, D, P, N);
-  std::cout << "3" <<std::endl;
+
   if (method == ICP_METHOD_POINT_TO_POINT){
     point_to_point_rigid_matching(X, P, R, t);
   } else if (method == ICP_METHOD_POINT_TO_PLANE){
     point_to_plane_rigid_matching(X, P, N, R, t);
   }
-  std::cout << "4" <<std::endl;
 }

src/point_mesh_distance.cpp

@@ -20,7 +20,6 @@ void point_mesh_distance(
   //D = (X-P).rowwise().norm();
   Eigen::MatrixXi temp_F;
   temp_F.resize(X.rows(),3);
-  std::cout << "2.1" <<std::endl;
   for (int i = 0; i< X.rows(); ++i){
     double min_d =  std::numeric_limits<double>::max();
     Eigen::RowVector3d min_p;
@@ -29,7 +28,6 @@ void point_mesh_distance(
       double temp_d = 0;
       Eigen::RowVector3d temp_p;
       point_triangle_distance(X.row(i), VY.row(FY(j,0)), VY.row(FY(j,1)), VY.row(FY(j,2)), temp_d, temp_p);
-
       if (temp_d < min_d){
         min_d = temp_d;
         min_p = temp_p;
@@ -40,6 +38,5 @@ void point_mesh_distance(
     P.row(i) = min_p;
     temp_F.row(i) = FY.row(min_f);
   }
-  std::cout << "2.2" <<std::endl;
   igl::per_face_normals(VY,temp_F,Eigen::Vector3d(1,1,1).normalized(),N);  
 }

src/point_triangle_distance.cpp

@@ -29,6 +29,19 @@ void closet_to_line(const Eigen::RowVector3d & a, const Eigen::RowVector3d & b,
     }
 }
 
+//https://math.stackexchange.com/questions/544946/determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle
+bool point_in_triangle(const Eigen::RowVector3d & p, const Eigen::RowVector3d & a, const Eigen::RowVector3d & b, const Eigen::RowVector3d & c){
+    auto u = b - a;
+    auto v = c - a;
+    auto n = u.cross(v);
+    auto w = p - a;
+    double a0 = u.cross(w).dot(n)/n.dot(n);
+    double a1 = w.cross(v).dot(n)/n.dot(n);
+    double a2 = 1 - a0 - a1;
+
+    return (a0 >= 0 && a0 <=1) && (a1 >= 0 && a1 <=1)  &&(a2 >= 0 && a2 <=1);
+}
+
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
   const Eigen::RowVector3d & a,
@@ -38,23 +51,16 @@ void point_triangle_distance(
   Eigen::RowVector3d & p)
 {
   // Replace with your code
-    #warning "speed by doing nothing"
-    p = (a+b+c)/3;
-    d = (x - p).norm();
-    return;
+    // #warning "speed by doing nothing"
+    // p = (a+b+c)/3;
+    // d = (x - p).norm();
+    // return;
   auto tempab = b - a;
   auto tempca = c - a;
   auto normal = tempab.cross(tempca).normalized();
   auto intersect = x + (normal.dot(a) - normal.dot(x)) * normal;
 
-  Eigen::Matrix3d A;
-  A.col(0) = a.transpose();
-  A.col(1) = b.transpose();
-  A.col(2) = c.transpose();
-  Eigen::Vector3d coeff = A.colPivHouseholderQr().solve(intersect.transpose());
-  if (!(coeff(0,0) >= 0 &&  coeff(0,0) <= 1
-       && coeff(1,0) >= 0 &&  coeff(1,0) <= 1
-       && coeff(2,0) >= 0 &&  coeff(2,0) <= 1)){ 
+  if (!point_in_triangle(intersect, a,b,c)){ 
     double line_d[3];
     Eigen::RowVector3d line_p[3]; 
     closet_to_line(a, b, x, line_d[0], line_p[0]);

src/random_points_on_mesh.cpp

@@ -44,18 +44,11 @@ void random_points_on_mesh(
 
     igl::doublearea(V, F, areas);
     igl::cumsum(areas, 1, colsum);
-    std::default_random_engine generator;
+    static std::default_random_engine generator;
     std::uniform_real_distribution<double> a(0.0,1.0);
     std::uniform_real_distribution<double> b(0.0,1.0);
     std::uniform_real_distribution<double> c(0.0,1.0);
-    //std::cout << "w1" <<colsum.rows() << " " << colsum.cols() << std::endl;
-      //std::cout << colsum << std::endl;
-    std::cout << colsum.rows() << std::endl;
-    std::cout << areas.rows() << std::endl;
-    std::cout << n << std::endl;
-    std::cout << X.rows() << std::endl;
-    std::cout << V.rows() << std::endl;
-    std::cout << F.rows() << std::endl;
+
     for (int i = 0; i < X.rows(); i++){
         double rand_tri = c(generator);
         int tri_idx = binary_search_larger(colsum, rand_tri);