done

src/closest_rotation.cpp

@@ -1,9 +1,22 @@
 #include "closest_rotation.h"
+#include <igl/polar_svd3x3.h>
+#include <Eigen/Dense>
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+  Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::Matrix3d V = svd.matrixV();
+  Eigen::Matrix3d U = svd.matrixU();
+  Eigen::Matrix3d omega;
+  Eigen::Matrix3d UVt = U * V.transpose();
+  omega << 1,0,0,
+		   0,1,0,
+           0,0, UVt.determinant();
+
+
+  R = U * omega * V.transpose();
+  //igl::polar_svd3x3(M, R);
 }

src/hausdorff_lower_bound.cpp

@@ -1,4 +1,6 @@
 #include "hausdorff_lower_bound.h"
+#include "point_mesh_distance.h"
+#include "random_points_on_mesh.h"
 
 double hausdorff_lower_bound(
   const Eigen::MatrixXd & VX,
@@ -8,5 +10,21 @@ double hausdorff_lower_bound(
   const int n)
 {
   // Replace with your code
-  return 0;
+  Eigen::MatrixXd X;
+  Eigen::MatrixXd Y;
+  random_points_on_mesh(n, VX, FX, X);
+  random_points_on_mesh(n, VY, FY, Y);
+
+  Eigen::VectorXd DX;
+  Eigen::MatrixXd PX;
+  Eigen::MatrixXd NX;
+  point_mesh_distance(X, VX, FX, DX, PX, NX);
+
+  Eigen::VectorXd DY;
+  Eigen::MatrixXd PY;
+  Eigen::MatrixXd NY;
+  point_mesh_distance(Y, VY, FY, DY, PY, NY);
+
+  double minumum = std::min(DX.minCoeff(), DY.minCoeff());
+  return std::floor(minumum);
 }

src/icp_single_iteration.cpp

@@ -1,4 +1,10 @@
 #include "icp_single_iteration.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
+#include "point_to_plane_rigid_matching.h"
+#include "point_to_point_rigid_matching.h"
+#include "closest_rotation.h"
+#include <iostream>
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -11,6 +17,33 @@ void icp_single_iteration(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+	/*
+	icp V_X, F_X, V_Y, F_Y
+		R, t <- initialize(e.g., set to identity transformation)
+		repeat until convergence
+		X <- sample source mesh(V_X, F_X)
+		P0 <- project all X onto target mesh(V_Y, F_Y)
+		R, t <- update rigid transform to best match X and P0
+		V_X <- rigidly transform original source mesh by R and t
+	*/
+
+  //R = Eigen::Matrix3d::Identity();
+  //t = Eigen::RowVector3d::Zero();
+
+  Eigen::MatrixXd X;
+  random_points_on_mesh(num_samples, VX, FX, X);
+  std::cout << "random points on mesh" << std::endl;
+  Eigen::MatrixXd P; 
+  Eigen::MatrixXd N;
+  Eigen::VectorXd D;
+  point_mesh_distance(X, VY, FY, D, P, N);
+  std::cout << "distance to mesh" << std::endl;
+  Eigen::Matrix3d M;
+  point_to_plane_rigid_matching(X, P, N, M, t);
+  //point_to_point_rigid_matching(X, P, M, t);
+  std::cout << "rigid matching" << std::endl;
+  closest_rotation(M,R);
+  std::cout << "closest rotation" << std::endl;
+  std::cout << "---------" << std::endl;
+
 }

src/point_mesh_distance.cpp

@@ -1,4 +1,7 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <Eigen/Dense>
+#include <iostream>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -11,6 +14,46 @@ 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();
+  D = Eigen::VectorXd(X.rows());
+  //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;
+
+
+  //std::cout << "inside point mesh distance help me" << std::endl;
+  for (int i = 0; i < X.rows(); i++)
+  {
+	  double min_dist = 1000000000000;
+	  Eigen::RowVector3d closest_point(0,0,0);
+	  Eigen::RowVector3d normal(0,0,0);
+
+	  for (int j = 0; j < FY.rows(); j++)
+	  {
+		  int v1_index = FY.row(j)(0);
+		  int v2_index = FY.row(j)(1);
+		  int v3_index = FY.row(j)(2);
+		  point_triangle_distance(X.row(i), VY.row(v1_index), VY.row(v2_index), VY.row(v3_index), d, p);
+		  //std::cout << "min dist before: " << min_dist << std::endl;
+		  //std::cout << "p before: " << closest_point << std::endl;
+		  if (d < min_dist)
+		  {
+			  min_dist = d;
+			  closest_point = p;
+			  Eigen::RowVector3d edge_1 = VY.row(v2_index) - VY.row(v1_index);
+			  Eigen::RowVector3d edge_2 = VY.row(v3_index) - VY.row(v2_index);
+			  normal = edge_1.cross(edge_2);
+			  normal.normalize();
+			  //std::cout << "min dist after: " << min_dist << std::endl;
+			  //std::cout << "p: " << p << std::endl;
+		  }
+	  }
+
+
+	  D(i) = min_dist;
+	  P.row(i) << closest_point;
+	  N.row(i) << normal;
+
+  }
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,6 @@
 #include "point_to_plane_rigid_matching.h"
+#include <igl/cat.h>
+#include <Eigen/src/Jacobi/Jacobi.h>
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -8,6 +10,60 @@ void point_to_plane_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+	Eigen::VectorXd ones = Eigen::VectorXd::Ones(X.rows());
+	Eigen::VectorXd zeros = Eigen::VectorXd::Zero(X.rows());
+
+	Eigen::MatrixXd A(3*X.rows(), 6);
+
+	Eigen::VectorXd X_1 = X.col(0);
+	Eigen::VectorXd X_2 = X.col(1);
+	Eigen::VectorXd X_3 = X.col(2);
+
+	Eigen::VectorXd P_1 = P.col(0);
+	Eigen::VectorXd P_2 = P.col(1);
+	Eigen::VectorXd P_3 = P.col(2);
+
+	Eigen::VectorXd N_1 = N.col(0);
+	Eigen::VectorXd N_2 = N.col(1);
+	Eigen::VectorXd N_3 = N.col(2);
+
+	Eigen::VectorXd P_col(P_1.rows()*3);
+	P_col << P_1, P_2, P_3;
+
+	Eigen::VectorXd X_col(X_1.rows()*3);
+	X_col << X_1, X_2, X_3;
+
+	Eigen::MatrixXd diag_N_1 = Eigen::MatrixXd(N_1.asDiagonal());
+	Eigen::MatrixXd diag_N_2 = Eigen::MatrixXd(N_2.asDiagonal());
+	Eigen::MatrixXd diag_N_3 = Eigen::MatrixXd(N_3.asDiagonal());
+
+	Eigen::MatrixXd big_N_temp(X.rows(), X.rows() * 2);	
+	Eigen::MatrixXd big_N(X.rows(), X.rows() * 3);
+	igl::cat(2, diag_N_1, diag_N_2, big_N_temp);
+	igl::cat(2, big_N_temp, diag_N_3, big_N);
+
+	Eigen::VectorXd D = X_col - P_col;
+
+	A.col(0) << zeros, -X_3, X_2;
+	A.col(1) << X_3, zeros, -X_1;
+	A.col(2) << -X_2, X_1, zeros;
+	A.col(3) << ones, zeros, zeros;
+	A.col(4) << zeros, ones, zeros;
+	A.col(5) << zeros, zeros, ones;
+
+	Eigen::MatrixXd NA = big_N * A;
+	Eigen::VectorXd u;// = -(NA.transpose() * NA).inverse() * (NA.transpose() * (big_N*D));
+	Eigen::MatrixXd C = NA.transpose() * NA;
+	Eigen::VectorXd b = -NA.transpose() * (big_N*D);
+	//u = C.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
+	u = C.llt().solve(b);
+	double alpha = u(0);
+	double beta = u(1);
+	double gamma = u(2);
+	Eigen::Matrix3d r;
+	r << 0, -gamma, beta,
+		gamma, 0, -alpha,
+		-beta, alpha, 0;
+	R = Eigen::Matrix3d::Identity() + r.transpose();
+	t = Eigen::RowVector3d(u(3), u(4), u(5));
 }

src/point_to_point_rigid_matching.cpp

@@ -8,7 +8,43 @@ void point_to_point_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  Eigen::VectorXd ones = Eigen::VectorXd::Ones(X.rows());
+  Eigen::VectorXd zeros = Eigen::VectorXd::Zero(X.rows());
+
+  Eigen::MatrixXd A(3 * X.rows(), 6);
+
+  Eigen::VectorXd X_1 = X.col(0);
+  Eigen::VectorXd X_2 = X.col(1);
+  Eigen::VectorXd X_3 = X.col(2);
+
+  Eigen::VectorXd P_1 = P.col(0);
+  Eigen::VectorXd P_2 = P.col(1);
+  Eigen::VectorXd P_3 = P.col(2);
+
+  Eigen::VectorXd P_col(P_1.rows()*3);
+  P_col << P_1, P_2, P_3;
+  Eigen::VectorXd X_col(X_1.rows()*3);
+  X_col << X_1, X_2, X_3;
+
+  Eigen::VectorXd D = X_col - P_col;
+
+  A.col(0) << zeros, -X_3, X_2;
+  A.col(1) << X_3, zeros, -X_1;
+  A.col(2) << -X_2, X_1, zeros;
+  A.col(3) << ones, zeros, zeros;
+  A.col(4) << zeros, ones, zeros;
+  A.col(5) << zeros, zeros, ones;
+
+  Eigen::VectorXd u = (A.transpose()*A).inverse() * (-A.transpose() * D);
+
+  double alpha = u(0);
+  double beta = u(1);
+  double gamma = u(2);
+  Eigen::Matrix3d r;
+  r << 0, -gamma, beta,
+	  gamma, 0, -alpha,
+	  -beta, alpha, 0;
+  R = Eigen::Matrix3d::Identity() + r.transpose();
+  t = Eigen::RowVector3d(u(3),u(4),u(5));
 }
 

src/point_triangle_distance.cpp

@@ -1,4 +1,8 @@
 #include "point_triangle_distance.h"
+#include <math.h>
+#include <Eigen/Dense>
+#include <igl/doublearea.h>
+#include <iostream>
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -9,6 +13,73 @@ void point_triangle_distance(
   Eigen::RowVector3d & p)
 {
   // Replace with your code
-  d = 0;
-  p = a;
+  //Edges
+  Eigen::RowVector3d edge_1 = b - a;
+  Eigen::RowVector3d edge_2 = a - c;
+  Eigen::RowVector3d edge_3 = c - b;
+
+  //std::cout << "point triangle distance function" << std::endl;
+
+  // Project x onto the plane containing the triangle (a,b,c)
+  Eigen::RowVector3d plane_normal = edge_1.cross(edge_3);
+  plane_normal.normalize();
+  Eigen::RowVector3d projection = x - x.dot(plane_normal) * plane_normal;
+
+  // Compute barycentric coordinates
+  Eigen::MatrixXd dblA;
+  Eigen::MatrixXd V(4,3);
+  Eigen::MatrixXd F(4,3);
+  V.row(0) << a;
+  V.row(1) << b;
+  V.row(2) << c;
+  V.row(3) << projection;
+
+  F.row(0) << 0,1,2;
+  F.row(1) << 2,0,3;
+  F.row(2) << 0,1,3;
+  F.row(3) << 1,2,3;
+
+  igl::doublearea(V, F, dblA);
+
+  double area_t = dblA(0);
+  double u = dblA(1) / area_t; 
+  double v = dblA(2) / area_t; 
+  double w = dblA(3) / area_t;
+
+  //std::cout << "do i know how barycentric coordinates work?" << std::endl;
+
+  if (u + v + w == 1)
+  {
+	  p = projection-x;
+	  d = (projection-x).norm();
+  }
+  else
+  {
+	  Eigen::RowVector3d ap = x - a;
+	  Eigen::RowVector3d bp = x - b;
+	  Eigen::RowVector3d cp = x - c;
+	  Eigen::RowVector3d edge_1_projection = a + (edge_1.dot(ap) / edge_1.dot(edge_1)) * edge_1;
+	  Eigen::RowVector3d edge_2_projection = c + (edge_2.dot(cp) / edge_2.dot(edge_2)) * edge_2;
+	  Eigen::RowVector3d edge_3_projection = b + (edge_3.dot(bp) / edge_3.dot(edge_3)) * edge_3;
+	  double dist_1 = (x - edge_1_projection).norm();
+	  double dist_2 = (x - edge_2_projection).norm();
+	  double dist_3 = (x - edge_3_projection).norm();
+	  double dist_4 = (x-a).norm();
+	  double dist_5 = (x-b).norm();
+	  double dist_6 = (x-c).norm();
+	  double distances[] = { dist_1, dist_2, dist_3 , dist_4, dist_5, dist_6 };
+	  d = *std::min_element(distances+3, distances + 6);
+	  if (d == dist_1)
+		  p = edge_1_projection-x;
+	  else if (d == dist_2)
+		  p = edge_2_projection-x;
+	  else if (d == dist_3)
+		  p = edge_3_projection-x;
+	  else if (d == dist_4)
+		  p = a;
+	  else if (d == dist_5)
+		  p = b;
+	  else
+		  p = c;
+  }
 }