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..93a7621 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/LU> 
+#include <Eigen/SVD>
+
 // Given a 3×3 matrix `M`, find the closest rotation matrix `R`.
 //
 // Inputs:
diff --git a/main.cpp b/main.cpp
index 3115cc9..69a6887 100644
--- a/main.cpp
+++ b/main.cpp
@@ -85,7 +85,7 @@ int main(int argc, char *argv[])
       Eigen::RowVector3d t;
       icp_single_iteration(VX,FX,VY,FY,num_samples,method,R,t);
       // Apply transformation to source mesh
-      VX = ((VX*R).rowwise() + t).eval();
+      VX = ((VX*R.transpose()).rowwise() + t).eval();
       set_meshes();
       if(show_samples)
       {
diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp
index 54403c1..462fd1b 100644
--- a/src/closest_rotation.cpp
+++ b/src/closest_rotation.cpp
@@ -1,9 +1,14 @@
 #include "closest_rotation.h"
 
 void closest_rotation(
-  const Eigen::Matrix3d & M,
-  Eigen::Matrix3d & R)
+    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::MatrixXd U = svd.matrixU();
+    Eigen::MatrixXd V = svd.matrixV().transpose();
+    Eigen::MatrixXd UV = U * V;
+    Eigen::Vector3d omega(1, 1, UV.determinant());
+    Eigen::Matrix3d Omega = omega.asDiagonal();
+    R = U * Omega * V;
 }
diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp
index 8608964..2fd18c8 100644
--- a/src/hausdorff_lower_bound.cpp
+++ b/src/hausdorff_lower_bound.cpp
@@ -1,12 +1,17 @@
 #include "hausdorff_lower_bound.h"
+#include "point_mesh_distance.h"
+#include "random_points_on_mesh.h"
 
 double hausdorff_lower_bound(
-  const Eigen::MatrixXd & VX,
-  const Eigen::MatrixXi & FX,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  const int n)
+    const Eigen::MatrixXd & VX,
+    const Eigen::MatrixXi & FX,
+    const Eigen::MatrixXd & VY,
+    const Eigen::MatrixXi & FY,
+    const int n)
 {
-  // Replace with your code
-  return 0;
+    Eigen::VectorXd D;
+    Eigen::MatrixXd X, 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..5856b51 100644
--- a/src/icp_single_iteration.cpp
+++ b/src/icp_single_iteration.cpp
@@ -1,16 +1,30 @@
 #include "icp_single_iteration.h"
+#include "point_mesh_distance.h"
+#include "point_to_plane_rigid_matching.h"
+#include "point_to_point_rigid_matching.h"
+#include "random_points_on_mesh.h"
 
 void icp_single_iteration(
-  const Eigen::MatrixXd & VX,
-  const Eigen::MatrixXi & FX,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  const int num_samples,
-  const ICPMethod method,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+    const Eigen::MatrixXd & VX,
+    const Eigen::MatrixXi & FX,
+    const Eigen::MatrixXd & VY,
+    const Eigen::MatrixXi & FY,
+    const int num_samples,
+    const ICPMethod method,
+    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..962c74b 100644
--- a/src/point_mesh_distance.cpp
+++ b/src/point_mesh_distance.cpp
@@ -1,16 +1,101 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <unsupported/Eigen/BVH>
+#include <igl/per_face_normals.h> 
+#include <vector>
+
+struct Triangle 
+{
+    // each row is one vertex on this triangle
+    Eigen::Matrix3d vertices;
+
+    // row in FY that defines this triangle
+    int frow;
+};
+
+Eigen::AlignedBox3d bounding_box(Triangle t) 
+{
+        Eigen::Vector3d corner1 = t.vertices.colwise().minCoeff();
+        Eigen::Vector3d corner2 = t.vertices.colwise().maxCoeff();
+        return Eigen::AlignedBox3d(corner1, corner2);
+}
+
+struct PointTriangleMinimizer
+{
+    typedef double Scalar;
+    Eigen::RowVector3d query_point;
+
+    double shortest_dist = -1;
+    Eigen::RowVector3d closest_point;
+
+    // the row in FY that defines the triangle on which closest_point lies
+    int frow;
+
+    PointTriangleMinimizer(Eigen::RowVector3d x) 
+    {
+        query_point = x;
+    }
+
+    // Shortest distance from query_point to bounding box b
+    double minimumOnVolume(const Eigen::AlignedBox3d & b) 
+    {
+        Eigen::Matrix3d D;
+        D << b.corner(b.BottomLeftFloor) - query_point.transpose(),
+            Eigen::Vector3d::Zero(),
+            query_point.transpose() - b.corner(b.TopRightCeil);
+        return D.rowwise().maxCoeff().norm();
+    }
+
+    // Shortest distance from query_point to triangle t
+    double minimumOnObject(const Triangle & t) 
+    {
+        double d;
+        Eigen::RowVector3d p;
+        point_triangle_distance(query_point, t.vertices.row(0), t.vertices.row(1), t.vertices.row(2), d, p);
+
+        if (d < shortest_dist || shortest_dist == -1) 
+        {
+            shortest_dist = d;
+            closest_point = p;
+            frow = t.frow;
+        }
+
+        return d;
+    }
+};
+
 
 void point_mesh_distance(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  Eigen::VectorXd & D,
-  Eigen::MatrixXd & P,
-  Eigen::MatrixXd & N)
+    const Eigen::MatrixXd & X,
+    const Eigen::MatrixXd & VY,
+    const Eigen::MatrixXi & FY,
+    Eigen::VectorXd & D,
+    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();
+    Eigen::MatrixXd all_normals;
+    igl::per_face_normals(VY, FY, all_normals);
+
+    D.resize(X.rows());
+    P.resize(X.rows(), 3);
+    N.resize(X.rows(), 3);
+
+    std::vector<Triangle> triangles;
+    for (int j = 0; j < FY.rows(); j++) {
+        Eigen::RowVector3i fy = FY.row(j);
+        Triangle t;
+        t.vertices << VY.row(fy[0]), VY.row(fy[1]), VY.row(fy[2]);
+        t.frow = j;
+        triangles.push_back(t);
+    }
+
+    Eigen::KdBVH<double, 3, Triangle> triangle_tree(triangles.begin(), triangles.end());
+
+    for (int i = 0; i < X.rows(); i++) 
+    {
+        PointTriangleMinimizer minimizer(X.row(i));
+        D[i] = Eigen::BVMinimize(triangle_tree, minimizer);
+        P.row(i) = minimizer.closest_point;
+        N.row(i) = all_normals.row(minimizer.frow);
+    }
 }
diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp
index 1978510..34d93ad 100644
--- a/src/point_to_plane_rigid_matching.cpp
+++ b/src/point_to_plane_rigid_matching.cpp
@@ -1,13 +1,42 @@
+#include "closest_rotation.h"
 #include "point_to_plane_rigid_matching.h"
+#include <Eigen/SVD>
 
 void point_to_plane_rigid_matching(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & P,
-  const Eigen::MatrixXd & N,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+    const Eigen::MatrixXd & X,
+    const Eigen::MatrixXd & P,
+    const Eigen::MatrixXd & N,
+    Eigen::Matrix3d & R,
+    Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+    Eigen::MatrixXd XN(X.rows(), 3);
+    for (int i = 0; i < X.rows(); i++) 
+    {
+        Eigen::RowVector3d x = X.row(i);
+        Eigen::RowVector3d n = N.row(i);
+        XN.row(i) = x.cross(n);
+    }
+
+    Eigen::MatrixXd A(P.rows(), 6);
+    A << XN, N;
+
+    Eigen::VectorXd b(X.rows());
+    for (int i = 0; i < X.rows(); i++) {
+        b[i] = -(X.row(i) - P.row(i)).dot(N.row(i));
+    }
+
+    Eigen::VectorXd u = A.colPivHouseholderQr().solve(b);
+    Eigen::Matrix3d U;
+
+    // first three elements of u define the angles of rotation
+    // about the x, y, and z axis, in that order
+    U << Eigen::RowVector3d(0, -u[2], u[1]),
+        Eigen::RowVector3d(u[2], 0, -u[0]),
+        Eigen::RowVector3d(-u[1], u[0], 0);
+
+    Eigen::MatrixXd M = Eigen::Matrix3d::Identity() + U;
+    closest_rotation(M, R);
+
+    // last three elements of u define the translation
+    t << 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..241f11b 100644
--- a/src/point_to_point_rigid_matching.cpp
+++ b/src/point_to_point_rigid_matching.cpp
@@ -1,14 +1,20 @@
+#include "closest_rotation.h"
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
 
 void point_to_point_rigid_matching(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & P,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+    const Eigen::MatrixXd & X,
+    const Eigen::MatrixXd & P,
+    Eigen::Matrix3d & R,
+    Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+    Eigen::Vector3d x_centroid = X.colwise().mean();
+    Eigen::Vector3d p_centroid = P.colwise().mean();
+
+    Eigen::MatrixXd X_bar = (X.rowwise() - x_centroid.transpose()).eval();
+    Eigen::MatrixXd P_bar = (P.rowwise() - p_centroid.transpose()).eval();
+
+    closest_rotation(P_bar.transpose() * X_bar, R);
+    t = p_centroid - R * x_centroid;
 }
 
diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp
index 6405100..cf59ed8 100644
--- a/src/point_triangle_distance.cpp
+++ b/src/point_triangle_distance.cpp
@@ -1,14 +1,104 @@
 #include "point_triangle_distance.h"
+#include <Eigen/Dense>
+
+// Returns area of a triangle with vertices v1, v2, and v3.
+double triangle_area(
+    const Eigen::Vector3d & v1,
+    const Eigen::Vector3d & v2,
+    const Eigen::Vector3d & v3) 
+{
+    return ((v1 - v2).cross(v1 - v3)).norm() / 2;
+}
+
+// Returns true if the projection of x onto the plane
+// defined by a triangle with vertices a, b, and c 
+// is inside the triangle, false otherwise. Outputs
+// the coordinates of the projection proj regardless.
+bool projection_in_triangle(
+    const Eigen::Vector3d & x,
+    const Eigen::Vector3d & a,
+    const Eigen::Vector3d & b,
+    const Eigen::Vector3d & c,
+    Eigen::Vector3d & proj) 
+{
+    Eigen::Vector3d n = (c - a).cross(b - a); // normal to plane
+    Eigen::Vector3d u = n / n.norm(); // unit normal
+    double d = u.dot(x - a);
+
+    proj = x - d * u;
+
+    return triangle_area(a, b, c) >= 
+        triangle_area(proj, b, c) + 
+        triangle_area(a, proj, c) + 
+        triangle_area(a, b, proj);
+}
+
+// Outputs the shortest distance d from x to the line
+// segment defined by vectors v1 and v2, and the coordinates
+// of the closest point p.
+void point_line_distance(
+    const Eigen::RowVector3d x,
+    const Eigen::RowVector3d v1,
+    const Eigen::RowVector3d v2,
+    double & d,
+    Eigen::RowVector3d & p)
+{
+    Eigen::RowVector3d u = (v1 - v2) / (v1 - v2).norm();
+    p = (v1 - x).dot(u) * u;
+    double dist = (p - v1).dot(u);
+
+    if (dist < 0 || dist > (v1 - v2).norm())
+    {
+        // p is not on the line segment, so we set it to the the nearest vertex
+        p = (x - v1).norm() < (x - v2).norm() ? v1 : v2;
+    }
+
+    d = (x - p).norm();
+}
 
 void point_triangle_distance(
-  const Eigen::RowVector3d & x,
-  const Eigen::RowVector3d & a,
-  const Eigen::RowVector3d & b,
-  const Eigen::RowVector3d & c,
-  double & d,
-  Eigen::RowVector3d & p)
-{
-  // Replace with your code
-  d = 0;
-  p = a;
+    const Eigen::RowVector3d & x,
+    const Eigen::RowVector3d & a,
+    const Eigen::RowVector3d & b,
+    const Eigen::RowVector3d & c,
+    double & d,
+    Eigen::RowVector3d & p)
+{
+    double da = (x - a).norm();
+    
+    // If x is more than 5 triangle side lengths away 
+    // from the triangle, approximate distances will suffice
+    // so we choose an arbitrary point.
+    if (da > 5 * (a - b).norm())
+    {
+        d = da;
+        p = a;
+        return;
+    }
+
+    Eigen::Vector3d proj;
+    if (projection_in_triangle(x.transpose(), a.transpose(), b.transpose(), c.transpose(), proj)) 
+    {
+        p = proj.transpose();
+        d = (x - p).norm();
+    }
+    else 
+    {
+        double dists[3];
+        Eigen::RowVector3d points[3];
+
+        point_line_distance(x, a, b, dists[0], points[0]);
+        point_line_distance(x, a, c, dists[1], points[1]);
+        point_line_distance(x, b, c, dists[2], points[2]);
+
+        d = -1;
+        for (int i = 0; i < 3; i++) 
+        {
+            if (d == -1 || d > dists[i]) 
+            {
+                d = dists[i];
+                p = points[i];
+            }
+        }
+    }
 }
diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp
index 6e85d75..9be1d0b 100644
--- a/src/random_points_on_mesh.cpp
+++ b/src/random_points_on_mesh.cpp
@@ -1,13 +1,62 @@
 #include "random_points_on_mesh.h"
+#include <igl/cumsum.h>
+#include <igl/doublearea.h>
 
-void random_points_on_mesh(
-  const int n,
-  const Eigen::MatrixXd & V,
-  const Eigen::MatrixXi & F,
-  Eigen::MatrixXd & X)
+int random_triangle(Eigen::VectorXd cumsum) 
+{
+    double gamma = (double)rand() / RAND_MAX;
+    int L = 0;
+    int R = cumsum.size() - 1;
+    while (L != R) 
+    {
+        int m = (L + R) / 2;
+        if (cumsum(m) <= gamma) 
+        {
+            L = m + 1;
+        }
+        else 
+        {
+            R = m;
+        }
+    }
+
+    return L;
+}
+
+Eigen::Vector3d random_point_in_triangle(
+    Eigen::Vector3d v1,
+    Eigen::Vector3d v2,
+    Eigen::Vector3d v3) 
 {
-  // REPLACE WITH YOUR CODE:
-  X.resize(n,3);
-  for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+    double alpha = (double)rand() / RAND_MAX;
+    double beta = (double)rand() / RAND_MAX;
+
+    if (alpha + beta > 1) 
+    {
+        alpha = 1 - alpha;
+        beta = 1 - beta;
+    }
+
+    return v1 + alpha * (v2 - v1) + beta * (v3 - v1);
 }
 
+void random_points_on_mesh(
+    const int n,
+    const Eigen::MatrixXd & V,
+    const Eigen::MatrixXi & F,
+    Eigen::MatrixXd & X)
+{
+    Eigen::VectorXd dblA, cumsum;
+    igl::doublearea(V, F, dblA);
+    igl::cumsum(dblA, 1, cumsum);
+    double totalA = dblA.sum();
+    cumsum = cumsum / totalA;
+
+    X.resize(n, 3);
+
+    for (int i = 0; i < n; i++) 
+    {
+        Eigen::Vector3i t = F.row(random_triangle(cumsum));
+        X.row(i) = random_point_in_triangle(V.row(t[0]), V.row(t[1]), V.row(t[2]));
+    }
+}