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/src/closest_rotation.cpp b/src/closest_rotation.cpp
old mode 100644
new mode 100755
index 54403c1..da0038c
--- a/src/closest_rotation.cpp
+++ b/src/closest_rotation.cpp
@@ -1,9 +1,17 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/Dense>
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+  //R = Eigen::Matrix3d::Identity();
+  Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  Eigen::Matrix3d O =  Eigen::Matrix3d::Zero();
+  O(0,0) = 1;
+  O(1,1) = 1;
+  O(2,2) = (svd.matrixU() * (svd.matrixV().transpose())).determinant();
+  R = svd.matrixU() * O * (svd.matrixV().transpose());
 }
diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp
old mode 100644
new mode 100755
index 8608964..07feb40
--- a/src/hausdorff_lower_bound.cpp
+++ b/src/hausdorff_lower_bound.cpp
@@ -1,4 +1,6 @@
 #include "hausdorff_lower_bound.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
 
 double hausdorff_lower_bound(
   const Eigen::MatrixXd & VX,
@@ -8,5 +10,11 @@ double hausdorff_lower_bound(
   const int n)
 {
   // Replace with your code
-  return 0;
+  //return 0;
+  Eigen::MatrixXd X,P,N;
+  Eigen::VectorXd D;
+  random_points_on_mesh(n,VX,FX,X);
+  point_mesh_distance(X,VY,FY,D,P,N);
+  double lowerbound = D.maxCoeff();
+  return lowerbound;
 }
diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp
old mode 100644
new mode 100755
index 1e8fda9..165a7f3
--- a/src/icp_single_iteration.cpp
+++ b/src/icp_single_iteration.cpp
@@ -1,4 +1,8 @@
 #include "icp_single_iteration.h"
+#include "point_to_plane_rigid_matching.h"
+#include "point_to_point_rigid_matching.h"
+#include "point_mesh_distance.h"
+#include "random_points_on_mesh.h"
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -11,6 +15,20 @@ void icp_single_iteration(
   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 X;
+  random_points_on_mesh(num_samples,VX,FX,X);
+
+  Eigen::MatrixXd P,N;
+  Eigen::VectorXd D;
+  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);
+  }
+  else { // ICP_METHOD_POINT_TO_PLANE
+    point_to_plane_rigid_matching(X,P,N,R,t);
+  }
 }
diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp
old mode 100644
new mode 100755
index 2e6a070..11106d3
--- a/src/point_mesh_distance.cpp
+++ b/src/point_mesh_distance.cpp
@@ -1,4 +1,8 @@
 #include "point_mesh_distance.h"
+#include <igl/per_face_normals.h>
+#include "point_triangle_distance.h"
+#include <limits>
+#include <iostream>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -10,7 +14,35 @@ void point_mesh_distance(
 {
   // Replace with your code
   P.resizeLike(X);
+  //N.resize(X.rows(),3);
   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();
+  //for(int i = 0;i<X.rows();i++) P.row(i) = VY.row(i%VY.rows());
+  //D = (X-P).rowwise().norm();
+  //D.resize(X.rows(), 1);
+  Eigen::MatrixXi triangle_F;
+  triangle_F.resize(X.rows(),3);
+
+  for (int i=0; i<X.rows(); i++) {
+    double idist = std::numeric_limits<double>::max();
+    Eigen::RowVector3d x = X.row(i);
+    Eigen::RowVector3d ip;
+
+    for (int j=0; j<FY.rows(); j++) {
+      double cur_dist;
+      Eigen::RowVector3d cur_p,a,b,c;
+      a = VY.row(FY(j,0));
+      b = VY.row(FY(j,1));
+      c = VY.row(FY(j,2));
+      point_triangle_distance(x,a,b,c,cur_dist,cur_p);
+      if (cur_dist < idist) {
+        idist = cur_dist;
+        ip = cur_p;
+        triangle_F.row(i) = FY.row(j);
+      }
+    }
+
+    P.row(i) = ip;
+    D(i) = idist;
+  }
+  igl::per_face_normals(VY,triangle_F,N);
 }
diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp
old mode 100644
new mode 100755
index 1978510..545b542
--- a/src/point_to_plane_rigid_matching.cpp
+++ b/src/point_to_plane_rigid_matching.cpp
@@ -1,4 +1,6 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -8,6 +10,52 @@ void point_to_plane_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  // R = Eigen::Matrix3d::Identity();
+  // t = Eigen::RowVector3d::Zero();
+  // nT
+  Eigen::MatrixXd nT;
+  nT.resize(X.rows(),3*X.rows());
+  nT << Eigen::MatrixXd (N.col(0).asDiagonal()),
+        Eigen::MatrixXd (N.col(1).asDiagonal()),
+        Eigen::MatrixXd (N.col(2).asDiagonal());
+
+  // A
+  Eigen::MatrixXd A;
+  A.resize(3*X.rows(),6);
+  Eigen::VectorXd diag_one = Eigen::VectorXd::Ones(X.rows());
+  Eigen::VectorXd zero = Eigen::VectorXd::Zero(X.rows());
+  A << zero, X.col(2), -X.col(1), diag_one, zero, zero,
+       -X.col(2), zero, X.col(0), zero, diag_one, zero,
+       X.col(1), -X.col(0), zero, zero, zero, diag_one;
+  A = nT * A;
+
+  // X - P
+  Eigen::VectorXd X_minus_P;
+  X_minus_P.resize(3*X.rows());
+  X_minus_P << X.col(0) - P.col(0),
+               X.col(1) - P.col(1),
+               X.col(2) - P.col(2);
+  X_minus_P = nT * X_minus_P;
+
+  // u
+  Eigen::Matrix<double,6,1> u = ((A.transpose()*A).inverse()) * (-A.transpose()*X_minus_P);
+
+  // a,b,r
+  double a, b, r;
+  a = u(0,0);
+  b = u(1,0);
+  r = u(2,0);
+
+  // t
+  t << u(3,0),u(4,0),u(5,0);
+
+  // M
+  Eigen::Matrix3d M;
+  M << 1, -r, b,
+       r, 1, -a,
+       -b, a, 1;
+
+  // R
+  closest_rotation(M, R);
+
 }
diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp
old mode 100644
new mode 100755
index 079151f..99047ef
--- a/src/point_to_point_rigid_matching.cpp
+++ b/src/point_to_point_rigid_matching.cpp
@@ -1,5 +1,7 @@
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
+#include "closest_rotation.h"
+#include <Eigen/Core>
 
 void point_to_point_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -8,7 +10,45 @@ void point_to_point_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  // R = Eigen::Matrix3d::Identity();
+  // t = Eigen::RowVector3d::Zero();
+  // A
+  Eigen::MatrixXd A;
+  A.resize(3*X.rows(),6);
+  Eigen::VectorXd diag_one = Eigen::VectorXd::Ones(X.rows());
+  Eigen::VectorXd zero = Eigen::VectorXd::Zero(X.rows());
+  A << zero, X.col(2), -X.col(1), diag_one, zero, zero,
+       -X.col(2), zero, X.col(0), zero, diag_one, zero,
+       X.col(1), -X.col(0), zero, zero, zero, diag_one;
+
+  // X - P
+  Eigen::MatrixXd X_minus_P;
+  X_minus_P.resize(3*X.rows(),1);
+  X_minus_P << X.col(0) - P.col(0),
+               X.col(1) - P.col(1),
+               X.col(2) - P.col(2);
+
+  // u
+  Eigen::Matrix<double,6,1> u = (A.transpose()*A).inverse() * (-A.transpose()*X_minus_P);
+
+  // a,b,r
+  double a, b, r;
+  a = u(0,0);
+  b = u(1,0);
+  r = u(2,0);
+
+  // t
+  t << u(3,0),u(4,0),u(5,0);
+
+  // M
+  Eigen::Matrix3d M;
+  M << 1, -r, b,
+       r, 1, -a,
+       -b, a, 1;
+
+  // R
+  closest_rotation(M, R);
+
+
 }
 
diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp
old mode 100644
new mode 100755
index 6405100..94ab0d7
--- a/src/point_triangle_distance.cpp
+++ b/src/point_triangle_distance.cpp
@@ -1,4 +1,57 @@
 #include "point_triangle_distance.h"
+#include <Eigen/Geometry>
+
+bool point_in_triangle(
+  const Eigen::RowVector3d & a,
+  const Eigen::RowVector3d & b,
+  const Eigen::RowVector3d & c,
+  const Eigen::RowVector3d & p)
+{
+  Eigen::RowVector3d pa,pb,pc;
+  pa = (p - a).normalized();
+  pb = (p - b).normalized();
+  pc = (p - c).normalized();
+  double cos_theta = pa.dot(pb) + pa.dot(pc) + pb.dot(pc);
+  return (cos_theta == 1.0);
+}
+
+void point_line_distance (
+  const Eigen::RowVector3d & x,
+  const Eigen::RowVector3d & a,
+  const Eigen::RowVector3d & b,
+  double & d,
+  Eigen::RowVector3d & p
+) 
+{
+  Eigen::RowVector3d ab,xa,n;
+  ab = a - b;
+  xa = x - a;
+  n = ab.cross(xa);
+  if ((n.norm() == 0) && (xa.norm() <= ab.norm())) { // x on ab
+    p = x;
+    d = 0;
+    return;
+  }
+
+  double dist = xa.dot(n);
+  Eigen::RowVector3d p_pontential = x - dist * n;
+
+  if ((p_pontential - a).norm() > ab.norm()) { // p is outside ab
+    p = b;
+    d = (x - b).norm();
+  }
+  
+  else if ((p_pontential - b).norm() > ab.norm())
+  {
+    p = a;
+    d = (x - a).norm();
+  }
+  else {
+    p = p_pontential;
+    d = dist;
+  }
+
+}
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -9,6 +62,30 @@ void point_triangle_distance(
   Eigen::RowVector3d & p)
 {
   // Replace with your code
-  d = 0;
-  p = a;
+  // d = 0;
+  // p = a;
+  Eigen::RowVector3d n = ((a-b).cross(a-c)).normalized();
+  p = x - n.dot(x)*n;
+  // p in triangle
+  if (point_in_triangle(a,b,c,p)) {
+    d = (x - p).norm();
+  }
+  else // p outside triangle
+  {
+    double ad,bd,cd;
+    Eigen::RowVector3d ap,bp,cp;
+    point_line_distance(x,a,b,ad,ap);
+    point_line_distance(x,a,c,cd,cp);
+    point_line_distance(x,b,c,bd,bp);
+    d = ad;
+    p = ap;
+    if (cd > d) {
+      d = cd;
+      p = cp;
+    }
+    if (bd > d) {
+      d = bd;
+      p = bp;
+    }
+  }
 }
diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp
old mode 100644
new mode 100755
index 6e85d75..db1ade0
--- a/src/random_points_on_mesh.cpp
+++ b/src/random_points_on_mesh.cpp
@@ -1,4 +1,62 @@
 #include "random_points_on_mesh.h"
+#include <igl/cumsum.h>
+#include <igl/doublearea.h>
+#include <iostream>
+#include <random>
+
+int find_random_index(Eigen::MatrixXd & C, double r, int start, int end) {
+// identifying the first entry in C whose value is greater than a uniform random variable γ.
+//int start = 0;
+//int end = C.rows() - 1;
+while (start != end) {
+  int mid = (start + end) / 2;
+  double c = C(mid,0);
+  if (c >= r) {
+    //start = start;
+    end = mid;
+    //mid = (start + end) / 2;
+  }
+  else if (c < r)
+  {
+    start = mid;
+    //end = end;
+  //mid = (start + end) / 2;
+  }
+}
+
+if (C(0,0) > r) {
+  return 0;
+} 
+else {
+  return end;
+}
+
+
+// int mid = (start + end) / 2;
+// double c = C(mid,0);
+// if (c > r) {
+//   start = start;
+//   end = mid;
+//   //mid = (start + end) / 2;
+// }
+
+// else if (c < r)
+// {
+//   start = mid;
+//   end = end;
+//   //mid = (start + end) / 2;
+// }
+
+// else {
+//   start = end;
+// }
+
+// if (start < end) {
+//   find_random_index(C,r,start,end);
+// }
+// return mid;
+}
+
 
 void random_points_on_mesh(
   const int n,
@@ -8,6 +66,44 @@ void random_points_on_mesh(
 {
   // REPLACE WITH YOUR CODE:
   X.resize(n,3);
-  for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+  // for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+
+  // Area-weighted random sampling of triangles
+  Eigen::MatrixXd A,C;
+  igl::doublearea(V,F,A);
+  igl::cumsum(A,1,C);
+  C = 1.0 * C / C(C.rows()-1,0);
+
+  // Uniform random sampling of a single triangle
+  // a b
+  std::default_random_engine generator;
+  std::uniform_real_distribution<double> distribution(0.0,1.0);
+  double a;
+  double b;
+  double r;
+  int index;
+ 
+  // random point in a single triangle mesh
+  Eigen::RowVector3d x;
+  Eigen::RowVector3i single;
+  Eigen::RowVector3d v1,v2,v3;
+
+  for (int i=0; i < X.rows(); i++){
+    r = distribution(generator);
+    index = find_random_index(C,r,0,C.rows()-1);
+    single = F.row(index);
+    v1 = V.row(single[0]);
+    v2 = V.row(single[1]);
+    v3 = V.row(single[2]);
+    a = distribution(generator);
+    b = distribution(generator);
+    if (a + b > 1) {
+      a = 1 - a;
+      b = 1 - b;
+    }
+    x = v1 + a * (v2 - v1) + b * (v3 - v1);
+    X.row(i) = x;
+  }
+
 }