diff --git a/README.html b/README.html
index 6c13281..c163d77 100644
--- a/README.html
+++ b/README.html
@@ -343,7 +343,7 @@ <h3 id="constantfunctionapproximation">Constant function approximation</h3>
 
 <h3 id="linearfunctionapproximation">Linear function approximation</h3>
 
-<p>If we make make a slightly more appropriate assuming that in the neighborhood
+<p>If we make make a slightly more appropriate assumption that in the neighborhood
 of the <span class="math">\(P_Y(\z₀)\)</span> the surface <span class="math">\(Y\)</span> is a plane, then we can improve this
 approximation while keeping <span class="math">\(\f\)</span> linear in <span class="math">\(\z\)</span>:</p>
 
diff --git a/README.md b/README.md
index bce5dc2..ce58753 100644
--- a/README.md
+++ b/README.md
@@ -263,7 +263,7 @@ derived our gradients geometrically.
 
 ### Linear function approximation
 
-If we make make a slightly more appropriate assuming that in the neighborhood
+If we make make a slightly more appropriate assumption that in the neighborhood
 of the  $P_Y(\z₀)$ the surface $Y$ is a plane, then we can improve this
 approximation while keeping $\f$ linear in $\z$:
 
diff --git a/include/closest_rotation.h b/include/closest_rotation.h
index cc67fd0..641d5f2 100644
--- a/include/closest_rotation.h
+++ b/include/closest_rotation.h
@@ -1,6 +1,9 @@
 #ifndef CLOSEST_ROTATION_H
 #define CLOSEST_ROTATION_H
 #include <Eigen/Core>
+#include <Eigen/SVD>
+#include <Eigen/Dense>
+
 // Given a 3×3 matrix `M`, find the closest rotation matrix `R`.
 //
 // Inputs:
diff --git a/include/hausdorff_lower_bound.h b/include/hausdorff_lower_bound.h
index 15e4db0..efa54e1 100644
--- a/include/hausdorff_lower_bound.h
+++ b/include/hausdorff_lower_bound.h
@@ -1,6 +1,9 @@
 #ifndef HAUSDORFF_LOWER_BOUND_H
 #define HAUSDORFF_LOWER_BOUND_H
 #include <Eigen/Core>
+#include <point_mesh_distance.h>
+#include <random_points_on_mesh.h>
+
 // Compute a lower bound on the _directed_ Hausdorff distance from a given mesh
 // (VX,FX) to another mesh (VY,FY).
 //
diff --git a/include/icp_single_iteration.h b/include/icp_single_iteration.h
index 03e4040..a6e6358 100644
--- a/include/icp_single_iteration.h
+++ b/include/icp_single_iteration.h
@@ -1,6 +1,10 @@
 #ifndef ICP_SINGLE_ITERATION_H
 #define ICP_SINGLE_ITERATION_H
 #include <Eigen/Core>
+#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>
 
 enum ICPMethod
 {
diff --git a/include/point_mesh_distance.h b/include/point_mesh_distance.h
index f542c54..4266e36 100644
--- a/include/point_mesh_distance.h
+++ b/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`.
diff --git a/include/point_to_plane_rigid_matching.h b/include/point_to_plane_rigid_matching.h
index 149620f..f412df2 100644
--- a/include/point_to_plane_rigid_matching.h
+++ b/include/point_to_plane_rigid_matching.h
@@ -1,6 +1,8 @@
 #ifndef POINT_TO_PLANE_RIGID_MATCHING_H
 #define POINT_TO_PLANE_RIGID_MATCHING_H
 #include <Eigen/Core>
+#include <closest_rotation.h>
+
 // Given a set of source points `X` and corresponding target points `P` and
 // normals `N`, find the optimal rigid transformation (`R`,`t`) that aligns `X`
 // to `P`, minimizing the matching energy:
diff --git a/include/point_to_point_rigid_matching.h b/include/point_to_point_rigid_matching.h
index dacc628..d5386b9 100644
--- a/include/point_to_point_rigid_matching.h
+++ b/include/point_to_point_rigid_matching.h
@@ -1,6 +1,8 @@
 #ifndef POINT_TO_POINT_RIGID_MATCHING_H
 #define POINT_TO_POINT_RIGID_MATCHING_H
 #include <Eigen/Core>
+#include <closest_rotation.h>
+
 // Given a set of source points X and corresponding target points P, find the
 // optimal rigid transformation (R,t) that aligns X to P, minimizing the
 // matching energy:
@@ -19,5 +21,4 @@ void point_to_point_rigid_matching(
   const Eigen::MatrixXd & P,
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t);
-#endif
-
+#endif
\ No newline at end of file
diff --git a/include/point_triangle_distance.h b/include/point_triangle_distance.h
index ecdd14e..8c9c4b8 100644
--- a/include/point_triangle_distance.h
+++ b/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`.
 //
diff --git a/include/random_points_on_mesh.h b/include/random_points_on_mesh.h
index 4fe2497..869580b 100644
--- a/include/random_points_on_mesh.h
+++ b/include/random_points_on_mesh.h
@@ -1,6 +1,9 @@
 #ifndef RANDOM_POINTS_ON_MESH_H
 #define RANDOM_POINTS_ON_MESH_H
 #include <Eigen/Core>
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+#include <cstdlib>
 // RANDOM_POINTS_ON_MESH Randomly sample a mesh (V,F) n times.
 //
 // Inputs:
diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp
index 54403c1..27dfcca 100644
--- a/src/closest_rotation.cpp
+++ b/src/closest_rotation.cpp
@@ -4,6 +4,15 @@ 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::ComputeThinU | Eigen::ComputeThinV);
+  Eigen::Matrix3d U = svd.matrixU();
+  Eigen::Matrix3d V = svd.matrixV();
+  Eigen::Matrix3d omega_star;
+  double dt = (U*V.transpose()).determinant();
+
+  omega_star << 1, 0, 0,
+                0, 1, 0,
+                0, 0, dt;
+
+  R = U*omega_star*V.transpose();
+}
\ No newline at end of file
diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp
index 8608964..af7a722 100644
--- a/src/hausdorff_lower_bound.cpp
+++ b/src/hausdorff_lower_bound.cpp
@@ -7,6 +7,11 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
-}
+  Eigen::MatrixXd pointsX, P, N;
+  Eigen::VectorXd D;
+
+  random_points_on_mesh(n, VX, FX, pointsX);
+  point_mesh_distance(pointsX, VY, FY, D, P, N);
+
+  return D.maxCoeff();
+}
\ No newline at end of file
diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp
index 1e8fda9..e7ce569 100644
--- a/src/icp_single_iteration.cpp
+++ b/src/icp_single_iteration.cpp
@@ -10,7 +10,19 @@ void icp_single_iteration(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  // R = Eigen::Matrix3d::Identity();
+  // t = Eigen::RowVector3d::Zero();
+
+  Eigen::MatrixXd pointsX, P, N;
+  Eigen::VectorXd D;
+
+  random_points_on_mesh(num_samples, VX, FX, pointsX);
+  point_mesh_distance(pointsX, VY, FY, D, P, N);
+
+  if(method == ICP_METHOD_POINT_TO_POINT){
+    point_to_point_rigid_matching(pointsX, P, R, t);
+  }
+  else{
+    point_to_plane_rigid_matching(pointsX, P, N, R, t);
+  }
 }
diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp
index 2e6a070..b3f4440 100644
--- a/src/point_mesh_distance.cpp
+++ b/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();
-}
+}
\ No newline at end of file
diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp
index 1978510..fc97b10 100644
--- a/src/point_to_plane_rigid_matching.cpp
+++ b/src/point_to_plane_rigid_matching.cpp
@@ -7,7 +7,52 @@ void point_to_plane_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  int k = X.rows();
+
+  // Set A to zeros
+  Eigen::MatrixXd A;
+  A = Eigen::MatrixXd::Zero(3*k, 6);
+
+  // Build A
+  // Put all the equations corresponding to the rows
+  // together, making it easy to construct A
+  A.col(0).segment(k, k) = -X.col(2);
+  A.col(0).segment(2*k, k) = X.col(1);
+  A.col(1).segment(0, k) = X.col(2);
+  A.col(1).segment(2*k, k) = -X.col(0);
+  A.col(2).segment(0, k) = -X.col(1);
+  A.col(2).segment(k, k) = X.col(0);
+  A.col(3).segment(0, k) = Eigen::VectorXd::Ones(k);
+  A.col(4).segment(k, k) = Eigen::VectorXd::Ones(k);
+  A.col(5).segment(2*k, k) = Eigen::VectorXd::Ones(k);
+
+  // Build X-P
+  Eigen::VectorXd PX;
+  PX = Eigen::VectorXd::Zero(3*k, 1);
+  PX.segment(0, k) = X.col(0) - P.col(0);
+  PX.segment(k, k) = X.col(1) - P.col(1);
+  PX.segment(2*k, k) = X.col(2) - P.col(2);
+
+  // Build N_diag
+  Eigen::MatrixXd N_diag;
+  N_diag = Eigen::MatrixXd::Zero(k, 3*k);
+  N_diag.block(0,0,k,k) = N.col(0).asDiagonal();
+  N_diag.block(0,k,k,k) = N.col(1).asDiagonal();
+  N_diag.block(0,2*k,k,k) = N.col(2).asDiagonal();
+  
+  A = N_diag * A;
+  PX = N_diag * PX;
+
+  // Least squares solution
+  Eigen::Matrix3d M, residual;
+  Eigen::VectorXd u;
+  u = (A.transpose() * A).inverse() * (-A.transpose() * PX);
+  t = Eigen::RowVector3d(u(3),u(4),u(5));
+  
+  residual << 0, -u(2), u(1),
+       u(2), 0, -u(0),
+       -u(1), u(0), 0;
+
+  M = Eigen::Matrix3d::Identity() - residual;
+  closest_rotation(M, R);
 }
diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp
index 079151f..d122775 100644
--- a/src/point_to_point_rigid_matching.cpp
+++ b/src/point_to_point_rigid_matching.cpp
@@ -7,8 +7,14 @@ void point_to_point_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
-}
+  Eigen::RowVector3d X_mean = X.colwise().mean();
+  Eigen::MatrixXd X_cor = X.rowwise() - X_mean;
 
+  Eigen::RowVector3d P_mean = P.colwise().mean();
+  Eigen::MatrixXd P_cor = P.rowwise() - P_mean;
+
+  Eigen::MatrixXd M = (P_cor.transpose() * X_cor).transpose();
+  closest_rotation(M, R);
+  t = (P_mean.transpose() - R*X_mean.transpose()).transpose();
+  // Formula is according to column vectors
+}
\ No newline at end of file
diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp
index 6405100..663808c 100644
--- a/src/point_triangle_distance.cpp
+++ b/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;
+    }
+  }
+}
\ No newline at end of file
diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp
index 6e85d75..6006ae3 100644
--- a/src/random_points_on_mesh.cpp
+++ b/src/random_points_on_mesh.cpp
@@ -1,4 +1,5 @@
 #include "random_points_on_mesh.h"
+#include <iostream>
 
 void random_points_on_mesh(
   const int n,
@@ -6,8 +7,48 @@ 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());
-}
 
+  // create cumulative area vector
+  Eigen::VectorXd areas, cum_areas;
+  igl::doublearea(V, F, areas);
+  igl::cumsum(areas, 1, cum_areas);
+  cum_areas = cum_areas/cum_areas(cum_areas.rows() - 1);
+
+  for(int i = 0;i<X.rows();i++){
+    // Sample face
+    double z = ((double) rand()) / RAND_MAX;
+    
+    int last = F.rows();
+    int first = 0;
+    int face_idx = 0;
+    
+    // Binary search for face_idx
+    while(true){
+      if (first == last || first == last - 1){
+        face_idx = first;
+        break;
+      }
+      face_idx = (first + last)/2;
+      
+      if (cum_areas(face_idx) < z){
+        first = face_idx;
+      }
+      else {
+        last = face_idx;
+      }
+      // std::cout << face_idx << "  ," << first << "  ," << last << "  ," << z << "  ," << cum_areas(face_idx) << "  ," << cum_areas(face_idx + 1) << "\n";
+    }
+
+    // Get random point on face
+    double alpha = ((double) rand()) / RAND_MAX;
+    double beta = ((double) rand()) / RAND_MAX;
+    if (alpha + beta > 1){
+      alpha = 1 - alpha;
+      beta = 1 - beta;
+    }
+
+    Eigen::VectorXd va = V.row(F(face_idx, 0)), vb = V.row(F(face_idx, 1)), vc = V.row(F(face_idx, 2));
+    X.row(i) = va + alpha*(vb - va) + beta*(vc - va);
+  }
+}
\ No newline at end of file