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
index 54403c1..1cb5488 100644
--- a/src/closest_rotation.cpp
+++ b/src/closest_rotation.cpp
@@ -1,9 +1,15 @@
 #include "closest_rotation.h"
+#include <Eigen/Dense>
+#include <Eigen/LU>
 
 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 Vt = (svd.matrixV()).transpose();
+    Eigen::Matrix3d O = Eigen::Matrix3d::Identity();
+    O(2,2) = (U * Vt).determinant();
+    R = U * O * Vt;
 }
diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp
index 8608964..25bcf16 100644
--- a/src/hausdorff_lower_bound.cpp
+++ b/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,
@@ -7,6 +9,10 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
+    Eigen::MatrixXd X;
+    Eigen::VectorXd D;
+    Eigen::MatrixXd P, N;
+    random_points_on_mesh(n, VX, FX, X);
+    point_mesh_distance(X, VY, FY, D, P, N);
+    return D.maxCoeff();
 }
diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp
index 1e8fda9..f34aa36 100644
--- a/src/icp_single_iteration.cpp
+++ b/src/icp_single_iteration.cpp
@@ -1,4 +1,8 @@
 #include "icp_single_iteration.h"
+#include "random_points_on_mesh.h"
+#include "point_to_plane_rigid_matching.h"
+#include "point_to_point_rigid_matching.h"
+#include "point_mesh_distance.h"
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -10,7 +14,14 @@ void icp_single_iteration(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+    Eigen::MatrixXd X, P, N;
+    Eigen::VectorXd D;
+    random_points_on_mesh(num_samples,VX,FX,X);
+    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 {
+        point_to_plane_rigid_matching(X,P,N,R,t);
+    }
 }
diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp
index 2e6a070..3112062 100644
--- a/src/point_mesh_distance.cpp
+++ b/src/point_mesh_distance.cpp
@@ -1,4 +1,6 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <igl/per_face_normals.h>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -8,9 +10,26 @@ 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();
+    P.resizeLike(X);
+    N = Eigen::MatrixXd::Zero(X.rows(),X.cols());
+    D.resize(X.rows());
+    Eigen::MatrixXd normals;
+    igl::per_face_normals(VY, FY, Eigen::Vector3d(1,1,1).normalized(), normals);
+    for(int i = 0;i<X.rows();i++) {
+        double minD = 100000000;
+        double d_;
+        int tri;
+        Eigen::RowVector3d p_, minP;
+        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)), d_, p_);
+            if (d_ < minD) {
+                minD = d_;
+                minP = p_;
+                tri = j;
+            }
+        }
+        P.row(i) = minP;
+        N.row(i) = normals.row(tri);
+        D(i) = minD;
+    }
 }
diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp
index 1978510..d4fb828 100644
--- 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,
@@ -7,7 +9,20 @@ 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 r = X.rows();
+    Eigen::MatrixXd A(r * 3, 6);
+    A << Eigen::VectorXd::Zero(r), X.col(2), -X.col(1), Eigen::VectorXd::Ones(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), -X.col(2), Eigen::VectorXd::Zero(r), X.col(0), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Ones(r), Eigen::VectorXd::Zero(r), X.col(1), -X.col(0), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Ones(r);
+    Eigen::VectorXd B(r * 3);
+    B << X.col(0) - P.col(0), X.col(1) - P.col(1), X.col(2) - P.col(2);
+    Eigen::MatrixXd Nd(r, 3 * r);
+    Eigen::MatrixXd Nd0[] = {N.col(0).asDiagonal(), N.col(1).asDiagonal(), N.col(2).asDiagonal()};
+    Nd << Nd0[0], Nd0[1], Nd0[2];
+    A = Nd * A;
+    B = Nd * B;
+    Eigen::VectorXd u;
+    u = (A.transpose() * A).inverse() * (-A.transpose() * B);
+    Eigen::Matrix3d M(3, 3);
+    M <<  1, u(2), -u(1), -u(2), 1, u(0), u(1), -u(0), 1;
+    closest_rotation(M,R);
+    t = Eigen::Vector3d(u(3), u(4), u(5));
 }
diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp
index 079151f..f4d7581 100644
--- a/src/point_to_point_rigid_matching.cpp
+++ b/src/point_to_point_rigid_matching.cpp
@@ -1,5 +1,6 @@
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
+#include "closest_rotation.h"
 
 void point_to_point_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,8 +8,10 @@ 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::MatrixXd xbar, pbar;
+    xbar = X.rowwise() - X.colwise().mean();
+    pbar = P.rowwise() - P.colwise().mean();
+    Eigen::Matrix3d M = (pbar.transpose() * xbar).transpose();
+    closest_rotation(M, R);
+    t = (P.colwise().mean()).transpose() - R*((X.colwise().mean()).transpose());
 }
-
diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp
index 6405100..b1dea7b 100644
--- a/src/point_triangle_distance.cpp
+++ b/src/point_triangle_distance.cpp
@@ -1,4 +1,7 @@
 #include "point_triangle_distance.h"
+#include <Eigen/Geometry>
+#include <algorithm>
+#include <iostream>
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -8,7 +11,44 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+    Eigen::Hyperplane<double, 3> plane = Eigen::Hyperplane<double, 3>::Through(a, b, c);
+    Eigen::RowVector3d proj = plane.projection(x);
+    if (((b-a).cross(c-a)).dot((b-a).cross(x-a)) >= 0 && ((a-c).cross(b-c)).dot((a-c).cross(x-c)) >= 0 && ((c-b).cross(a-b)).dot((c-b).cross(x-b)) >= 0) {
+        d = (x - proj).norm();
+        p = proj;
+    }
+    else {
+        d = 100000000;
+        double z = (x - a).dot(b - a) / (b - a).squaredNorm();
+        if (z < 0) z = 0;
+        else if (z > 1) z = 1;
+        Eigen::RowVector3d projection = a + z * (b - a);
+        Eigen::RowVector3d abP = projection;
+        double abD = (x - projection).norm();
+        if (abD < d) {
+            d = abD;
+            p = abP;
+        }
+        z = (x - b).dot(c - b) / (c - b).squaredNorm();
+        if (z < 0) z = 0;
+        else if (z > 1) z = 1;
+        projection = b + z * (c - b);
+        Eigen::RowVector3d bcP = projection;
+        double bcD = (x - projection).norm();
+        if (bcD < d) {
+            d = bcD;
+            p = bcP;
+        }
+        z = (x - c).dot(a - c) / (a - c).squaredNorm();
+        if (z < 0) z = 0;
+        else if (z > 1) z = 1;
+        projection = c + z * (a - c);
+        Eigen::RowVector3d caP = projection;
+        double caD = (x - projection).norm();
+        if (caD < d) {
+            d = caD;
+            p = caP;
+        }
+    }
+    
 }
diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp
index 6e85d75..c4523e9 100644
--- a/src/random_points_on_mesh.cpp
+++ b/src/random_points_on_mesh.cpp
@@ -1,4 +1,7 @@
 #include "random_points_on_mesh.h"
+#include <igl/cumsum.h>
+#include <igl/doublearea.h>
+#include <random>
 
 void random_points_on_mesh(
   const int n,
@@ -6,8 +9,26 @@ 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());
+    X.resize(n,3);
+    Eigen::VectorXd area, cumarea;
+    igl::doublearea(V, F, area);
+    igl::cumsum(area * 0.5, 1, cumarea);
+    std::random_device gen;
+    std::uniform_real_distribution<> distribution(0.0, 1.0);
+    for(int i = 0;i<X.rows();i++) {
+        double ra = distribution(gen) * cumarea(cumarea.rows()-1);
+        int tri = 0;
+        while (cumarea(tri) < ra) {
+            tri++;
+        }
+        Eigen::Vector3d v[] = {V.row(F(tri,0)), V.row(F(tri,1)), V.row(F(tri,2))};
+        double alpha = distribution(gen);
+        double beta = distribution(gen);
+        if(alpha + beta > 1) {
+            alpha = 1 - beta;
+            beta = 1 - alpha;
+        }
+        X.row(i) = alpha * (v[1] - v[0]) + beta * (v[2] - v[0]) + v[0];
+    }
 }