diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp
index 54403c1..2177810 100644
--- a/src/closest_rotation.cpp
+++ b/src/closest_rotation.cpp
@@ -1,9 +1,44 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/LU>
+#include <iostream>
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+  // // Replace with your code
+  // R = Eigen::Matrix3d::Identity();
+
+	// Compute the SVD of the matrix M
+    Eigen::MatrixXd M_Xd = M;
+	Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+	// Compute the product of the left and right orthogonal matrices
+	Eigen::MatrixXd UVT = svd.matrixU()*svd.matrixV().transpose();
+	double detUVT = UVT.determinant();
+
+	Eigen::Matrix3d Omega;
+	Omega.resize(3,3);
+
+	// Populate the Omega matrix, as per notes
+	for (int ii = 0; ii < 3; ii++)
+	{
+		for (int jj = 0; jj < 3; jj++)
+		{
+			Omega(ii,jj) = 0.0;
+
+			if (ii == jj && ii != 2)
+				Omega(ii,jj) = 1.0;
+			if (ii == 2 && jj == 2)
+				Omega(ii,jj) = detUVT;
+		}
+	}
+
+	// Finally, compute the rotation matrix
+	R = svd.matrixU()*Omega*svd.matrixV().transpose();
+	// std::cout << R << std::endl;
+
+	return;
+
 }
diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp
index 8608964..0cca921 100644
--- a/src/hausdorff_lower_bound.cpp
+++ b/src/hausdorff_lower_bound.cpp
@@ -1,4 +1,7 @@
 #include "hausdorff_lower_bound.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
+#include <iostream>
 
 double hausdorff_lower_bound(
   const Eigen::MatrixXd & VX,
@@ -7,6 +10,20 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
+	// Get the random sampling on X
+	Eigen::MatrixXd X;
+	random_points_on_mesh(n, VX, FX, X);
+
+	// Compute the set of closest points for all X samples
+	Eigen::VectorXd D;
+	Eigen::MatrixXd P, N;
+	point_mesh_distance(X, VY, FY, D, P, N);
+
+	// Now find the maximum of these distances. This is the required bound.
+	int i;
+	double H = D.maxCoeff(&i);
+	// std::cout << H << std::endl;
+
+	return H;
+
 }
diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp
index 1e8fda9..109f177 100644
--- a/src/icp_single_iteration.cpp
+++ b/src/icp_single_iteration.cpp
@@ -1,4 +1,10 @@
 #include "icp_single_iteration.h"
+#include "random_points_on_mesh.h"
+#include "hausdorff_lower_bound.h"
+#include "point_mesh_distance.h"
+#include "point_to_point_rigid_matching.h"
+#include "point_to_plane_rigid_matching.h"
+#include <iostream>
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -10,7 +16,68 @@ void icp_single_iteration(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
+
+  // Set seed for random numbers
+  srand(1);
+
+  // Initial guesses
   R = Eigen::Matrix3d::Identity();
   t = Eigen::RowVector3d::Zero();
+
+  Eigen::MatrixXd VX_updated = VX;
+
+  // Define tolerance based on initial Haussdorf distance
+  double H = hausdorff_lower_bound(VX_updated, FX, VY, FY, num_samples);
+  double tol = 1.0e-3;
+ 
+  int iter = 0;
+  while (H > tol && iter < 10)
+  {
+    iter++;
+
+    H = hausdorff_lower_bound(VX_updated, FX, VY, FY, num_samples);
+    std::cout << "Haussdorf lower bound: " << H << " at iteration " << iter << std::endl;
+
+    // Sample the source mesh
+    Eigen::MatrixXd X;
+    random_points_on_mesh(num_samples, VX_updated, FX, X);
+
+    // Compute the set of closest points for all X samples
+    Eigen::VectorXd D;
+    Eigen::MatrixXd P, N;
+    point_mesh_distance(X, VY, FY, D, P, N);
+
+    if (method == ICP_METHOD_POINT_TO_POINT)
+    {
+      point_to_point_rigid_matching(X, P, R, t);
+
+      // Apply the updated transforms
+      for (int ii = 0; ii < VX_updated.rows(); ii++)
+      {
+        Eigen::MatrixXd temp1 = R*(VX_updated.row(ii).transpose()) + t.transpose();
+        VX_updated(ii,0) = temp1(0,0);
+        VX_updated(ii,1) = temp1(1,0);
+        VX_updated(ii,2) = temp1(2,0);
+      }
+
+    }
+    else
+    {
+      point_to_plane_rigid_matching(X, P, N, R, t);
+
+      // Apply the updated transforms
+      for (int ii = 0; ii < VX_updated.rows(); ii++)
+      {
+        Eigen::MatrixXd temp1 = R*(VX_updated.row(ii).transpose()) + t.transpose();
+        VX_updated(ii,0) = temp1(0,0);
+        VX_updated(ii,1) = temp1(1,0);
+        VX_updated(ii,2) = temp1(2,0);
+      }
+
+    }
+
+
+  }
+
+
 }
diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp
index 2e6a070..3369745 100644
--- a/src/point_mesh_distance.cpp
+++ b/src/point_mesh_distance.cpp
@@ -1,4 +1,7 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <igl/per_face_normals.h>
+#include <iostream>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -8,9 +11,64 @@ void point_mesh_distance(
   Eigen::MatrixXd & P,
   Eigen::MatrixXd & N)
 {
-  // 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();
+  // // 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();
+
+  Eigen::RowVector3d a, b, c, p;
+  a.resize(3); b.resize(3); c.resize(3); p.resize(3);
+  double d;
+
+  P.resize(X.rows(),3);
+  N.resize(X.rows(),3);
+  D.resize(X.rows());
+
+  // Compute all face normals for later use
+  Eigen::MatrixXd allN;
+  igl::per_face_normals(VY, FY, Eigen::Vector3d(1,1,1).normalized(), allN);
+
+  // First, loop through each point in X
+  for (int ii = 0; ii < X.rows(); ii++)
+  {
+    // Extract the current x-point
+    Eigen::RowVector3d x; x(0) = X(ii,0); x(1) = X(ii,1); x(2) = X(ii,2);
+
+    // Now loop through all the triangles in Y and compute the point-to-triangle 
+    // closest points. The smallest of these distances is the one we're after.
+    double x_min_dist = 1.0e10;
+
+    for (int jj = 0; jj < VY.rows(); jj++)
+    {
+        a(0) = VY(FY(jj,0),0); a(1) = VY(FY(jj,0),1); a(2) = VY(FY(jj,0),2);
+        b(0) = VY(FY(jj,1),0); a(1) = VY(FY(jj,1),1); a(2) = VY(FY(jj,1),2);
+        c(0) = VY(FY(jj,2),0); a(1) = VY(FY(jj,2),1); a(2) = VY(FY(jj,2),2);
+
+        point_triangle_distance(x, a, b, c, d, p);
+
+        if (d < x_min_dist)
+        {
+          x_min_dist = d;
+
+          // Pick this closest point to be *the* closest point
+          P(ii,0) = p(0);
+          P(ii,1) = p(1);
+          P(ii,2) = p(2);
+
+          // Pick the right normal that corresponds to this closest triangle
+          N(ii,0) = allN(jj,0);
+          N(ii,1) = allN(jj,1);
+          N(ii,2) = allN(jj,2);
+        }
+
+    }
+
+    D(ii) = x_min_dist;
+
+  }
+
+
+  return;
+
 }
diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp
index 1978510..b50bbfa 100644
--- a/src/point_to_plane_rigid_matching.cpp
+++ b/src/point_to_plane_rigid_matching.cpp
@@ -1,4 +1,9 @@
 #include "point_to_plane_rigid_matching.h"
+#include <igl/polar_svd.h>
+#include <iostream>
+#include <Eigen/Dense>
+#include <Eigen/QR>
+#include "closest_rotation.h"
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,7 +12,85 @@ void point_to_plane_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  // // Replace with your code
+  // R = Eigen::Matrix3d::Identity();
+  // t = Eigen::RowVector3d::Zero();
+
+	// Based on taking the derivative of the equation to be minimized in the notes,
+	// the only difference here should be factor of two in the numerator of the
+	// right-hand side...although this seems suspicious given that the vector of
+	// normals N would remain unused. But the matrix that contains those diagonal
+	// matrices is a constant w.r.t. the derivative with u, so it should just go
+	// outside of the derivative. And since that matrix cannot be 0, it can be
+	// ignored.
+
+	// Extract the columns of X and P
+	Eigen::MatrixXd A, X1, X2, X3, P1, P2, P3, diffVec, diffVec1, diffVec2, diffVec3, Ivec;
+	X1 = X.col(0);
+	X2 = X.col(1);
+	X3 = X.col(2);
+	P1 = P.col(0);
+	P2 = P.col(1);
+	P3 = P.col(2);
+
+	// Compute the vector Xi - Pi
+	diffVec1 = X1 - P1;
+	diffVec2 = X2 - P2;
+	diffVec3 = X3 - P3;
+
+	diffVec.resize(3*X.rows(), 1);
+	diffVec.block(0, 0, X.rows(), 1) = diffVec1;
+	diffVec.block(X.rows(), 0, X.rows(), 1) = diffVec2;
+	diffVec.block(2*X.rows(), 0, X.rows(), 1) = diffVec3;
+
+	// Create the big matrix A
+	Ivec.setOnes(X.rows(), 1);
+	A.resize(3*X.rows(), 3*X.cols()+3);
+	A.setZero(3*X.rows(), 3*X.cols()+3);
+
+	A.block(0, 1, X.rows(), 1) = X3;
+	A.block(X.rows(), 0, X.rows(), 1) = -X3;
+	A.block(2*X.rows(), 0, X.rows(), 1) = X2;
+	A.block(0, 2, X.rows(), 1) = -X2;
+	A.block(X.rows(), 2, X.rows(), 1) = X1;
+	A.block(2*X.rows(), 1, X.rows(), 1) = -X1;
+	A.block(0, 3, X.rows(), 1) = Ivec;
+	A.block(X.rows(), 4, X.rows(), 1) = Ivec;
+	A.block(2*X.rows(), 5, X.rows(), 1) = Ivec;
+
+	// Compute A^T*A
+	Eigen::MatrixXd bigA = A.transpose()*A;
+
+	// Compute the right hand side
+	Eigen::VectorXd b = -2.0*A.transpose()*diffVec;
+
+	// Solve the system
+	Eigen::ColPivHouseholderQR<Eigen::MatrixXd> QRsolver(bigA);
+	Eigen::VectorXd soln = QRsolver.solve(b);
+
+	double alpha = soln(0);
+	double beta = soln(1);
+	double gamma = soln(2);
+	t(0) = soln(3);
+	t(1) = soln(4);
+	t(2) = soln(5);
+
+	Eigen::Matrix3d M;
+
+	M(0,0) = 1.0;
+	M(1,1) = 1.0;
+	M(2,2) = 1.0;
+	M(0,1) = -gamma;
+	M(0,2) = beta;
+	M(1,0) = gamma;
+	M(2,0) = -beta;
+	M(1,2) = -alpha;
+	M(2,1) = alpha;
+
+	closest_rotation(M, R);
+
+
+	return;
+
 }
+
diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp
index 079151f..5d21b7f 100644
--- a/src/point_to_point_rigid_matching.cpp
+++ b/src/point_to_point_rigid_matching.cpp
@@ -1,5 +1,9 @@
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
+#include <iostream>
+#include <Eigen/Dense>
+#include <Eigen/QR>
+#include "closest_rotation.h"
 
 void point_to_point_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,8 +11,77 @@ void point_to_point_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  // // Replace with your code
+  // R = Eigen::Matrix3d::Identity();
+  // t = Eigen::RowVector3d::Zero();
+
+	// Extract the columns of X and P
+	Eigen::MatrixXd A, X1, X2, X3, P1, P2, P3, diffVec, diffVec1, diffVec2, diffVec3, Ivec;
+	X1 = X.col(0);
+	X2 = X.col(1);
+	X3 = X.col(2);
+	P1 = P.col(0);
+	P2 = P.col(1);
+	P3 = P.col(2);
+
+	// Compute the vector Xi - Pi
+	diffVec1 = X1 - P1;
+	diffVec2 = X2 - P2;
+	diffVec3 = X3 - P3;
+
+	diffVec.resize(3*X.rows(), 1);
+	diffVec.block(0, 0, X.rows(), 1) = diffVec1;
+	diffVec.block(X.rows(), 0, X.rows(), 1) = diffVec2;
+	diffVec.block(2*X.rows(), 0, X.rows(), 1) = diffVec3;
+
+	// Create the big matrix A
+	Ivec.setOnes(X.rows(), 1);
+	A.resize(3*X.rows(), 3*X.cols()+3);
+	A.setZero(3*X.rows(), 3*X.cols()+3);
+
+	A.block(0, 1, X.rows(), 1) = X3;
+	A.block(X.rows(), 0, X.rows(), 1) = -X3;
+	A.block(2*X.rows(), 0, X.rows(), 1) = X2;
+	A.block(0, 2, X.rows(), 1) = -X2;
+	A.block(X.rows(), 2, X.rows(), 1) = X1;
+	A.block(2*X.rows(), 1, X.rows(), 1) = -X1;
+	A.block(0, 3, X.rows(), 1) = Ivec;
+	A.block(X.rows(), 4, X.rows(), 1) = Ivec;
+	A.block(2*X.rows(), 5, X.rows(), 1) = Ivec;
+
+	// Compute A^T*A
+	Eigen::MatrixXd bigA = A.transpose()*A;
+
+	// Compute the right hand side
+	Eigen::VectorXd b = -A.transpose()*diffVec;
+
+	// Solve the system
+	Eigen::ColPivHouseholderQR<Eigen::MatrixXd> QRsolver(bigA);
+	Eigen::VectorXd soln = QRsolver.solve(b);
+
+	double alpha = soln(0);
+	double beta = soln(1);
+	double gamma = soln(2);
+	t(0) = soln(3);
+	t(1) = soln(4);
+	t(2) = soln(5);
+
+	Eigen::Matrix3d M;
+
+	M(0,0) = 1.0;
+	M(1,1) = 1.0;
+	M(2,2) = 1.0;
+	M(0,1) = -gamma;
+	M(0,2) = beta;
+	M(1,0) = gamma;
+	M(2,0) = -beta;
+	M(1,2) = -alpha;
+	M(2,1) = alpha;
+
+	closest_rotation(M, R);
+
+
+	return;
+
 }
 
diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp
index 6405100..c4a69e4 100644
--- a/src/point_triangle_distance.cpp
+++ b/src/point_triangle_distance.cpp
@@ -1,4 +1,5 @@
 #include "point_triangle_distance.h"
+#include <iostream>
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -8,7 +9,51 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+  // // Replace with your code
+  // d = 0;
+  // p = a;
+
+	// Pick number of samples on the given triangle
+	int n = 100;
+
+	// Initialize the output variables
+	p.resize(3);
+	d = 1.0e10;
+
+	for (int ii = 0; ii < n; ii++)
+	{
+		// Define the random constants to sample the triangle
+		double a1 = -((double)rand()/(double)(RAND_MAX + 1));
+		double b1 = -((double)rand()/(double)(RAND_MAX + 1));
+
+	  	if (a1 + b1 > 1.0)
+		{
+			a1 = 1.0 - a1;
+			b1 = 1.0 - b1;
+		}
+
+	  	double xp = a(0) + a1*(b(0) - a(0)) + b1*(c(0) - a(0));
+	  	double yp = a(1) + a1*(b(1) - a(1)) + b1*(c(1) - a(1));
+	  	double zp = a(2) + a1*(b(2) - a(2)) + b1*(c(2) - a(2));
+
+	  	double x_dist = std::abs(xp - x[0]);
+	  	double y_dist = std::abs(yp - x[1]);
+	  	double z_dist = std::abs(zp - x[2]);
+	  	double dist = std::sqrt(x_dist*x_dist + y_dist*y_dist + z_dist*z_dist);
+
+	// std::cout << dist << "; " << xp << ", " << yp << ", " << zp << std::endl;
+
+	  	if (dist < d)
+	  	{
+	  		p[0] = xp; p[1] = yp; p[2] = zp;
+	  		d = dist;
+	  	}
+	}
+
+	// std::cout << d << "; " << p[0] << ", " << p[1] << ", " << p[2] << std::endl;
+
+	return;
+
 }
+
+
diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp
index 6e85d75..4137982 100644
--- a/src/random_points_on_mesh.cpp
+++ b/src/random_points_on_mesh.cpp
@@ -1,4 +1,9 @@
 #include "random_points_on_mesh.h"
+#include "stdlib.h"
+#include <iostream>
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+#include "point_triangle_distance.h"
 
 void random_points_on_mesh(
   const int n,
@@ -6,8 +11,72 @@ void random_points_on_mesh(
   const Eigen::MatrixXi & F,
   Eigen::MatrixXd & X)
 {
-  // REPLACE WITH YOUR CODE:
-  X.resize(n,3);
-  for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+  // // REPLACE WITH YOUR CODE:
+  // X.resize(n,3);
+  // for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+
+  	Eigen::MatrixXd dblA, Ci;
+
+  	// Compute triangle areas
+  	igl::doublearea(V, F, dblA);
+
+  	// Convert the areas to relative areas by dividing by total mesh area
+  	double total_area = dblA.sum();
+  	for (int ii = 0; ii < dblA.rows(); ii++)
+  		dblA(ii) = dblA(ii)/total_area;
+
+  	// Cumulative sum for picking triangles
+  	igl::cumsum(dblA, 1, Ci);
+
+  	// std::cout << Ci(Ci.rows()-1) << std::endl;
+  	// std::cout << total_area << ", " << Ci(Ci.rows()-1) << std::endl;
+  	
+   	// Set the size of the output matrix, which is a list a points
+  	X.resize(n,3);
+  	// Generate n random points on the mesh
+  	for (int jj = 0; jj < n; jj++)
+  	{
+		// Define the random constants as per notes
+		double a = -((double)rand()/(double)(RAND_MAX + 1));
+		double b = -((double)rand()/(double)(RAND_MAX + 1));
+
+		if (a + b > 1.0)
+		{
+			a = 1.0 - a;
+			b = 1.0 - b;
+		}
+
+  		double c = -((double)rand()/(double)(RAND_MAX + 1));
+
+  		int flag = 0, kk = 0;
+  		while (flag == 0)
+  		{
+  			if (Ci(kk) > c)
+  			{
+  				flag = 1;
+  				break;
+  			}
+  			kk++;
+  		}
+
+  		// Now kk is the index of the face we've picked, so sample randomly on that face
+  		X(jj,0) = V(F(kk,0),0) + a*(V(F(kk,1),0) - V(F(kk,0),0)) + b*(V(F(kk,2),0) - V(F(kk,0),0));
+  		X(jj,1) = V(F(kk,0),1) + a*(V(F(kk,1),1) - V(F(kk,0),1)) + b*(V(F(kk,2),1) - V(F(kk,0),1));
+  		X(jj,2) = V(F(kk,0),2) + a*(V(F(kk,1),2) - V(F(kk,0),2)) + b*(V(F(kk,2),2) - V(F(kk,0),2));
+
+	  	// std::cout << X(jj,0) << ", " << X(jj,1) << ", " << X(jj,2) << std::endl;
+
+	}
+
+	// Eigen::RowVector3d x; x(0) = 0.0; x(1) = 0.0; x(2) = 0.0;
+	// Eigen::RowVector3d a; a(0) = 1.0; a(1) = 0.0; a(2) = 0.0;
+	// Eigen::RowVector3d b; b(0) = 0.0; b(1) = 1.0; b(2) = 0.0;
+	// Eigen::RowVector3d c; c(0) = 0.0; c(1) = 0.0; c(2) = 1.0;
+	// double d;
+	// Eigen::RowVector3d p;
+	// point_triangle_distance(x, a, b, c, d, p);
+
+	return;
+
 }