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..57fc47c 100644
--- a/src/closest_rotation.cpp
+++ b/src/closest_rotation.cpp
@@ -1,9 +1,24 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/Dense>
+#include <iostream>
 
 void closest_rotation(
-  const Eigen::Matrix3d & M,
-  Eigen::Matrix3d & R)
-{
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+        const Eigen::Matrix3d &M,
+        Eigen::Matrix3d &R) {
+
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeThinV | Eigen::ComputeThinU);
+    Eigen::Matrix3d U = svd.matrixU();
+    Eigen::Matrix3d V = svd.matrixV();
+    Eigen::Matrix3d VT = V.transpose();
+    double d = (U * VT).determinant();
+//    std::cout << d << std::endl
+
+    Eigen::Matrix3d O;
+    O << 1, 0, 0,
+            0, 1, 0,
+            0, 0, d;
+
+    R = U * O * VT;
+
 }
diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp
index 8608964..bf9afeb 100644
--- a/src/hausdorff_lower_bound.cpp
+++ b/src/hausdorff_lower_bound.cpp
@@ -1,12 +1,18 @@
 #include "hausdorff_lower_bound.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
 
 double hausdorff_lower_bound(
-  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;
+        const Eigen::MatrixXd &VX,
+        const Eigen::MatrixXi &FX,
+        const Eigen::MatrixXd &VY,
+        const Eigen::MatrixXi &FY,
+        const int n) {
+
+    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..ebcff38 100644
--- a/src/icp_single_iteration.cpp
+++ b/src/icp_single_iteration.cpp
@@ -1,16 +1,42 @@
 #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,
-  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();
+        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) {
+
+    /*
+     *  Algorithm:
+            X ← sample source mesh (V_X,F_X)
+            P0 ← project all X onto target mesh (V_Y,F_Y)
+            R,t ← update rigid transform to best match X and P0
+            V_X ← rigidly transform original source mesh by R and t
+     */
+
+    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);
+
+    switch (method) {
+        case ICPMethod::ICP_METHOD_POINT_TO_PLANE:
+            point_to_plane_rigid_matching(X, P, N, R, t);
+            break;
+        case ICPMethod::ICP_METHOD_POINT_TO_POINT:
+            point_to_point_rigid_matching(X, P, R, t);
+            break;
+        default:
+            assert(true);
+    }
+
+
 }
diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp
index 2e6a070..1d96c1e 100644
--- a/src/point_mesh_distance.cpp
+++ b/src/point_mesh_distance.cpp
@@ -1,16 +1,52 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <iostream>
+#include <igl/per_face_normals.h>
 
 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)
-{
-  // 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();
+        const Eigen::MatrixXd &X,
+        const Eigen::MatrixXd &VY,
+        const Eigen::MatrixXi &FY,
+        Eigen::VectorXd &D,
+        Eigen::MatrixXd &P,
+        Eigen::MatrixXd &N) {
+
+    P.resizeLike(X);
+    D.resize(P.rows(), 1);
+    N.resize(X.rows(), X.cols());
+
+    //computing the normals
+    Eigen::MatrixXd NY;
+    igl::per_face_normals(VY, FY, NY);
+
+    for (int i = 0; i < X.rows(); i++) {
+
+        // min_p and min_d for a given point x_i
+        Eigen::RowVector3d min_p, min_n;
+        double min_d = MAXFLOAT;
+
+        //let's find the minimum for Y for a given x
+        for (int j = 0; j < FY.rows(); j++) {
+            Eigen::RowVector3d p;
+            double d;
+
+            point_triangle_distance(X.row(i),
+                                    VY.row(FY(j, 0)), //a
+                                    VY.row(FY(j, 1)), //b
+                                    VY.row(FY(j, 2)), //c
+                                    d, p);
+
+            if (d < min_d) {
+                min_p = p;
+                min_d = d;
+                min_n = NY.row(j);
+            }
+        }
+        ///--
+        P.row(i) = min_p;
+        D(i) = min_d;
+        N.row(i) = min_n;
+
+    }
+
 }
diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp
index 1978510..5c7b7c8 100644
--- a/src/point_to_plane_rigid_matching.cpp
+++ b/src/point_to_plane_rigid_matching.cpp
@@ -1,13 +1,65 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
+
+void createA(const Eigen::MatrixXd &X, Eigen::MatrixXd &A) {
+    Eigen::VectorXd zero = Eigen::VectorXd::Zero(X.rows());
+    Eigen::VectorXd one = Eigen::VectorXd::Ones(X.rows());
+//-
+    Eigen::VectorXd X_1 = X.col(1);
+    Eigen::VectorXd X_2 = X.col(2);
+    Eigen::VectorXd X_0 = X.col(0);
+
+    A << zero, X_2, -X_1, one, zero, zero,
+            -X_2, zero, X_0, zero, one, zero,
+            X_1, -X_0, zero, zero, zero, one;
+}
+
+Eigen::MatrixXd diagN(const Eigen::MatrixXd &N, int idx) {
+    return N.col(idx).asDiagonal();
+}
 
 void point_to_plane_rigid_matching(
-  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();
+        const Eigen::MatrixXd &X,
+        const Eigen::MatrixXd &P,
+        const Eigen::MatrixXd &N,
+        Eigen::Matrix3d &R,
+        Eigen::RowVector3d &t) {
+
+    Eigen::MatrixXd A(3 * X.rows(), 6);
+    Eigen::MatrixXd d_N(X.rows(), 3 * X.rows());
+
+    //Creating A, diagonal_N and X-P
+    createA(X, A);
+    d_N << diagN(N, 0), diagN(N, 1), diagN(N, 2);
+    Eigen::MatrixXd XP = X - P;
+
+    Eigen::VectorXd B = Eigen::VectorXd::Zero(X.rows() * 3);
+    B << (X - P).col(0), (X - P).col(1), (X - P).col(2);
+
+    //Solving the equation (52) in Readme.
+    A = d_N * A;
+    B = d_N * B;
+
+    //need to be defined dynamically so that JacobiSVD works.
+    Eigen::VectorXd u;
+
+
+    u = (A.transpose() * A).inverse() * (-A.transpose() * B);
+
+    double alpha = u[0];
+    double beta = u[1];
+    double gamma = u[2];
+
+    Eigen::Matrix3d M = Eigen::MatrixXd::Identity(3, 3);
+    Eigen::Matrix3d buff;
+    buff << 0, -gamma, beta,
+            gamma, 0, -alpha,
+            -beta, alpha, 0;
+
+    M = M - buff;
+    closest_rotation(M, R);
+    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..11825ea 100644
--- a/src/point_to_point_rigid_matching.cpp
+++ b/src/point_to_point_rigid_matching.cpp
@@ -1,14 +1,21 @@
 #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,
-  const Eigen::MatrixXd & P,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
-{
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+        const Eigen::MatrixXd &X,
+        const Eigen::MatrixXd &P,
+        Eigen::Matrix3d &R,
+        Eigen::RowVector3d &t) {
+//
+    //Shift to the origin
+    Eigen::MatrixXd X_ = X.rowwise() - X.colwise().mean();
+    Eigen::MatrixXd P_ = P.rowwise() - P.colwise().mean();
+
+    Eigen::MatrixXd M = (P_.transpose() * X_).transpose();
+    closest_rotation(M, R);
+    //compute t
+    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..506eb2c 100644
--- a/src/point_triangle_distance.cpp
+++ b/src/point_triangle_distance.cpp
@@ -1,14 +1,134 @@
 #include "point_triangle_distance.h"
+#include "iostream"
+#include <Eigen/Geometry>
+
+Eigen::RowVector3d PxPy(const Eigen::RowVector3d &x, const Eigen::RowVector3d &y) {
+    return y - x;
+}
+
+Eigen::RowVector3d PxPy_norm(const Eigen::RowVector3d &x, const Eigen::RowVector3d &y) {
+    return PxPy(x, y) / PxPy(x, y).norm();
+}
+
+bool clockwise(double f) {
+    return f < 0;
+}
+
+bool anti_clockwise(double f) {
+    return f > 0;
+}
+
+bool is_outside(const Eigen::RowVector3d &x, const Eigen::RowVector3d &y, const Eigen::RowVector3d &pop,
+                const Eigen::RowVector3d &Np) {
+
+    double val = PxPy(pop, x).cross(PxPy(pop, y)).dot(Np);
+    return val < 0;
+}
+
+void
+getPointDistance(const Eigen::RowVector3d &p1, const Eigen::RowVector3d &p2, const Eigen::RowVector3d &po,
+                 const Eigen::RowVector3d &pop,
+                 const double popop_l, double &d,
+                 Eigen::RowVector3d &p) {
+
+    //P1 and P2 are refereed to equation 7.
+    Eigen::RowVector3d R = PxPy(pop, p2).cross(PxPy(pop, p1)).cross(PxPy(p1, p2));
+    double cos_gamma = PxPy(pop, p1).dot(R) / (PxPy(pop, p1).norm() * R.norm());
+    double poppopp_l = PxPy(pop, p1).norm() * cos_gamma;
+    Eigen::RowVector3d poppopp = poppopp_l * R / R.norm();
+    Eigen::RowVector3d p0pp = pop + poppopp;//eq12
+    Eigen::RowVector3d num = p0pp - p1;
+    Eigen::RowVector3d denom = p2 - p1;
+
+    double t = num.sum() / (denom.sum() + 1e-6);
+
+    if (t >= 0 && t <= 1) {
+        d = sqrt(poppopp_l * poppopp_l + popop_l * popop_l);
+        p = p0pp;
+    } else if (t < 0) {
+        d = PxPy(po, p1).norm();
+        p = p1;
+    } else if (t > 1) {
+        d = PxPy(po, p2).norm();
+        p = p2;
+    }
+}
+
 
 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) {
+
+
+
+    // This implementation follows
+    // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.4264&rep=rep1&type=pdf
+    // 3D Distance from a Point to a Triangle
+
+    //This is a changed of variable to match the equations in the paper.
+    Eigen::RowVector3d p0 = x;
+    Eigen::RowVector3d p1 = a;
+    Eigen::RowVector3d p2 = b;
+    Eigen::RowVector3d p3 = c;
+    ///
+
+
+    // "let's get the perpendicular vector to the triangle plane"
+    Eigen::RowVector3d Np = PxPy(p1, p2).cross(PxPy(p1, p3));
+    double cosalpha = PxPy(p1, p0).dot(Np) / (PxPy(p1, p0).norm() * Np.norm());
+
+
+    // If it were the case that the projection of p0 onto the plane lay within the triangle
+    // this would be the length
+    double p0p0p_l = PxPy(p0, p1).norm() * cosalpha;
+    //--
+    Eigen::RowVector3d p0p0p = (-1.0 * p0p0p_l * Np) / Np.norm();
+    Eigen::RowVector3d pop = p0 + p0p0p; // EQ5
+
+    //--
+    Eigen::RowVector3d v1 = PxPy_norm(p2, p1) + PxPy_norm(p3, p1);
+    Eigen::RowVector3d v2 = PxPy_norm(p3, p2) + PxPy_norm(p1, p2);
+    Eigen::RowVector3d v3 = PxPy_norm(p1, p3) + PxPy_norm(p2, p3);
+
+    //--
+    double f1 = v1.cross(PxPy(p1, pop)).dot(Np);
+    double f2 = v2.cross(PxPy(p2, pop)).dot(Np);
+    double f3 = v3.cross(PxPy(p3, pop)).dot(Np);
+
+    // f1 > 0 if P0 is anticlockwise of V1
+    // f2 > 0 if P0 is anticlockwise of V2
+    // f3 > 0 if P0 is anticlockwise of V3
+
+
+    if (clockwise(f2) && anti_clockwise(f1)) {
+        if (is_outside(p1, p2, pop, Np)) {
+            getPointDistance(p1, p2, p0, pop, p0p0p_l, d, p);
+
+        } else {
+            d = abs(p0p0p_l);
+            p = pop;
+        }
+    } else if (clockwise(f3) && anti_clockwise(f2)) {
+        if (is_outside(p2, p3, pop, Np)) {
+            getPointDistance(p2, p3, p0, pop, p0p0p_l, d, p);
+
+        } else {
+            d = abs(p0p0p_l);
+            p = pop;
+        }
+    } else if (clockwise(f1) && anti_clockwise(f3)) {
+        if (is_outside(p3, p1, pop, Np)) {
+            getPointDistance(p3, p1, p0, pop, p0p0p_l, d, p);
+        } else {
+            d = abs(p0p0p_l);
+            p = pop;
+        }
+    } else {
+        assert(true);
+    }
+//    std::cout << "p:" << p << " d: " << d << std::endl;
 }
diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp
index 6e85d75..d7f3995 100644
--- a/src/random_points_on_mesh.cpp
+++ b/src/random_points_on_mesh.cpp
@@ -1,13 +1,82 @@
 #include "random_points_on_mesh.h"
+#include <iostream>
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+#include <stdlib.h>
+#include <random>
+
+int pick_triangle(Eigen::MatrixXd &M, double gamma) {
+    //This can be done based on binary search, or std::upper_bound,
+    //I am leaving like this for now.
+    for (int i = 0; i < M.rows(); i++)
+        if (M(i, 0) > gamma) {
+            return i;
+        }
+    return -1;
+}
+
+Eigen::VectorXd
+pick_inside_triangle(Eigen::VectorXd &v1, Eigen::VectorXd &v2, Eigen::VectorXd &v3, double alpha, double beta) {
+
+    Eigen::VectorXd x;
+
+    if ((alpha + beta) > 1.0) {
+        alpha = 1. - alpha;
+        beta = 1. - beta;
+    }
+
+    x = v1 + alpha * (v2 - v1) + beta * (v3 - v1);
+//    std::cout << x << std::endl;
+    return x;
+
+}
 
 void random_points_on_mesh(
-  const int n,
-  const Eigen::MatrixXd & V,
-  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());
+        const int n,
+        const Eigen::MatrixXd &V,
+        const Eigen::MatrixXi &F,
+        Eigen::MatrixXd &X) {
+
+    Eigen::MatrixXd area, Ajs;
+
+
+    X.resize(n, 3);
+
+    igl::doublearea(V, F, area);
+
+    //according to documentation double-area computes the area twice.so...
+    area = area / 2.0;
+
+    //total area
+    double Ax = area.sum();
+
+    // Ajs=(Ai,Ai+Ai+1,Ai+Ai+1+Ai+2,....)
+    igl::cumsum(area, 1, Ajs);
+
+    //norm cum sum=Cs
+    Eigen::MatrixXd C = Ajs / Ax;
+
+
+    //source http://www.cplusplus.com/reference/random/uniform_real_distribution/
+    std::random_device rd;
+    std::default_random_engine generator(rd());
+    std::uniform_real_distribution<double> distribution(0.0, 1.0);
+
+    for (int i = 0; i < X.rows(); i++) {
+        double gamma = distribution(generator);
+        int tid = pick_triangle(C, gamma);
+        assert(tid >= 0);
+
+        Eigen::VectorXd v1 = V.row(F(tid, 0));
+        Eigen::VectorXd v2 = V.row(F(tid, 1));
+        Eigen::VectorXd v3 = V.row(F(tid, 2));
+
+
+        X.row(i) = pick_inside_triangle(v1, v2, v3, distribution(generator), distribution(generator));
+
+//        std::cout << X.row(i) << std::endl;
+
+
+    }
 }