diff --git a/CMakeLists.txt b/CMakeLists.txt
index afd4110..fd9a52d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,6 +20,7 @@ add_executable(pvl
     Utils.hpp
     Assert.hpp
     Math.hpp
+    Optional.hpp
     Vector.hpp
     Matrix.hpp
     Svd.hpp
diff --git a/CloudUtils.hpp b/CloudUtils.hpp
index cb7bcff..99ae054 100644
--- a/CloudUtils.hpp
+++ b/CloudUtils.hpp
@@ -56,9 +56,10 @@ std::vector<Vec3f> estimateNormals(Cloud& cloud) {
         }
         Svd<float> svd = singularValueDecomposition(cov);
 
-        //  std::cout << "singular values = " << svd.S[0] << "," << svd.S[1] << "," << svd.S[2] << "\n";
+        //  std::cout << "singular values = " << svd.S[0] << "," << svd.S[1] << "," << svd.S[2]
+        //  << "\n";
         normals[i] = svd.U.column(argMin(svd.S));
-        normals[i] *= sign(dot(normals[i], cloud[i]));
+        normals[i] *= sign(dotProd(normals[i], cloud[i]));
     }
     return normals;
 }
@@ -111,8 +112,8 @@ void orientNormals(Cloud& cloud, Normals& normals) {
                         continue;
                     }
                     Vec3f e = normalize(cloud[n2] - cloud[n1]);
-                    Vec3f ndash = normals[n2] - 2 * e * dot(e, normals[n2]);
-                    if (dot(normals[n1], ndash) < 0.8) {
+                    Vec3f ndash = normals[n2] - 2 * e * dotProd(e, normals[n2]);
+                    if (dotProd(normals[n1], ndash) < 0.8) {
                         // do not count as neighbours
                         continue;
                     }
@@ -144,8 +145,8 @@ void orientNormals(Cloud& cloud, Normals& normals) {
             for (int j : neighs) {
                 if (indices[j] == 0) {
                     Vec3f e = normalize(cloud[j] - cloud[i]);
-                    Vec3f ndash = normals[j] - 2 * e * dot(e, normals[j]);
-                    votes += sign(dot(normals[i], ndash));
+                    Vec3f ndash = normals[j] - 2 * e * dotProd(e, normals[j]);
+                    votes += sign(dotProd(normals[i], ndash));
                 }
             }
         }
diff --git a/Graph.hpp b/Graph.hpp
index af5f7cd..5fdc3b8 100644
--- a/Graph.hpp
+++ b/Graph.hpp
@@ -63,15 +63,12 @@ struct HalfEdge {
     HalfEdge(VertexHandle to)
         : to(to) {
         left = FaceHandle(-1);
+        opposite = HalfEdgeHandle(-1);
     }
 
     bool boundary() const {
         return opposite < 0;
     }
-
-    /* bool removed() const {
-          return left < 0;
-      }*/
 };
 
 struct Vertex {
@@ -88,10 +85,6 @@ struct Vertex {
     Vertex(HalfEdgeHandle eh)
         : edge(eh) {}
 
-    /*bool valid() const {
-        return edge >= 0;
-    }*/
-
     struct EndTag {};
 
     class IteratorBase {
@@ -187,10 +180,6 @@ struct Face {
     Face(HalfEdgeHandle eh)
         : edge(eh) {}
 
-    /* bool valid() const {
-         return edge >= 0;
-     }*/
-
     struct EndTag {};
 
     class IteratorBase {
@@ -260,6 +249,7 @@ struct Face {
 
 struct Edge {};
 
+// template<typename Vertex, typename HalfEdge, typename Face>
 class Graph {
     friend class Vertex;
     friend class Face;
@@ -317,14 +307,15 @@ class Graph {
         PVL_ASSERT(!boundary(eh));
         return halfEdges_[opposite(eh)].left;
     }
-    HalfEdgeHandle from(VertexHandle vh) const {
+
+    /*HalfEdgeHandle from(VertexHandle vh) const {
         PVL_ASSERT(valid(vh));
         return vertices_[vh].edge;
     }
     HalfEdgeHandle to(VertexHandle vh) const {
         PVL_ASSERT(valid(vh));
         return prev(from(vh));
-    }
+    }*/
     // returns halfedge from vh1 to vh2, or -1 if not exists
     HalfEdgeHandle halfEdge(VertexHandle vh1, VertexHandle vh2) const {
         PVL_ASSERT(valid(vh1) && valid(vh2));
@@ -357,17 +348,35 @@ class Graph {
         return eh;
     }
     EdgeHandle edge(VertexHandle vh1, VertexHandle vh2) const {
+        /*  PVL_ASSERT(valid(vh1) && valid(vh2));
+          if (vh1 > vh2) {
+              std::swap(vh1, vh2);
+          }
+          HalfEdgeHandle eh = halfEdge(vh1, vh2);
+          if (!valid(eh)) {
+              // might still be a boundary edge
+              eh = halfEdge(vh2, vh1);
+              PVL_ASSERT(!valid(eh) || boundary(eh));
+              if (boundary(eh)) {
+                  return edge(eh);
+              }
+          }
+          return edge(eh);*/
         PVL_ASSERT(valid(vh1) && valid(vh2));
-        if (vh1 > vh2) {
-            std::swap(vh1, vh2);
-        }
-        HalfEdgeHandle eh = halfEdge(vh1, vh2);
-        if (!valid(eh)) {
-            // might still be a boundary edge
-            eh = halfEdge(vh2, vh1);
-            PVL_ASSERT(!valid(eh) || boundary(eh));
+        for (EdgeHandle eh : edgeRing(vh1)) {
+            HalfEdgeHandle heh = halfEdge(eh);
+            if ((from(heh) == vh1 && to(heh) == vh2) || (to(heh) == vh1 && from(heh) == vh2)) {
+                return eh;
+            }
         }
-        return edge(eh);
+        return EdgeHandle(-1);
+    }
+    HalfEdgeHandle emanating(VertexHandle vh) const {
+        PVL_ASSERT(valid(vh));
+        return vertices_[vh].edge;
+    }
+    HalfEdgeHandle incoming(VertexHandle vh) const {
+        return prev(emanating(vh));
     }
     bool valid(FaceHandle fh) const {
         PVL_ASSERT(fh < int(faces_.size()));
@@ -534,17 +543,12 @@ class Graph {
             Face::FaceIterator(*this, fh, Face::EndTag{}) };
     }
 
-    std::array<int, 3> faceIndices(FaceHandle fh) const {
-        if (!valid(fh)) {
-            return { 0, 0, 0 };
-        }
-        std::array<int, 3> indices;
-        int i = 0;
-        for (VertexHandle vh : vertexRing(fh)) {
-            indices[i] = vh.index();
-            ++i;
-        }
-        return indices;
+    std::array<VertexHandle, 3> faceVertices(FaceHandle fh) const {
+        PVL_ASSERT(valid(fh));
+        std::array<VertexHandle, 3> vertices;
+        Face::VertexRange ring = vertexRing(fh);
+        std::copy(ring.begin(), ring.end(), vertices.begin());
+        return vertices;
     }
 
     VertexHandle addVertex() {
@@ -639,8 +643,8 @@ class Graph {
                 }
             }
             {
-                HalfEdgeHandle e = from(vh[i]);
-                HalfEdgeHandle e0 = from(vh[i]);
+                HalfEdgeHandle e = vertices_[vh[i]].edge;
+                HalfEdgeHandle e0 = e;
                 if (!boundary(e)) {
                     // find boundary
                     do {
@@ -676,11 +680,19 @@ class Graph {
 
 
     // collapse to to from
-    void collapse(HalfEdgeHandle heh) {
-        PVL_ASSERT(collapseAllowed(heh));
+    void collapse(EdgeHandle edge) {
+        PVL_ASSERT(collapseAllowed(edge));
         // check();
-        CollapseContext context(*this, edge(heh));
-        HalfEdgeHandle oheh = opposite(heh);
+        CollapseContext context(*this, edge);
+
+        HalfEdgeHandle heh = context.edge;
+        bool onesided = boundary(heh);
+        HalfEdgeHandle oheh = !onesided ? opposite(heh) : HalfEdgeHandle(-1);
+
+        VertexHandle v0 = context.remaining;
+        VertexHandle v1 = context.removed;
+        VertexHandle vL = to(next(heh));
+        VertexHandle vR = !onesided ? to(next(oheh)) : VertexHandle(-1);
 
         /*if (vertices_[context.remaining].edge == heh) {
             HalfEdgeHandle l = opposite(prev(heh));
@@ -689,15 +701,45 @@ class Graph {
             vertices_[context.remaining].edge = opposite(prev(heh));
         }*/
 
-        VertexHandle v0 = context.remaining;
-        VertexHandle vl = to(next(heh));
-        VertexHandle vr = to(next(oheh));
-
-        /// \todo move only if necessary?
+        HalfEdgeHandle ev0, evL, evR;
         /// \todo fix boundary case
-        vertices_[v0].edge = opposite(prev(heh));
-        vertices_[vl].edge = next(opposite(prev(heh)));
-        vertices_[vr].edge = opposite(next(oheh));
+        PVL_ASSERT(!boundary(next(heh)));
+        if (vertices_[v1].edge != next(heh) && vertices_[v1].edge != oheh) {
+            // unless the halfedge gets removed, simply use the emanating edge of the vertex to
+            // be removed
+            ev0 = vertices_[v1].edge;
+        } else {
+            PVL_ASSERT(!onesided);
+            ev0 = opposite(prev(heh));
+        }
+        std::cout << "Assigning emanating halfedge for v0 - " << from(ev0) << "-" << to(ev0)
+                  << std::endl;
+        PVL_ASSERT(valid(ev0));
+
+        if (vertices_[vL].edge == prev(heh)) {
+            // move to any other halfedge, it cannot be a boundary
+            PVL_ASSERT(!boundary(prev(heh)));
+            evL = next(opposite(prev(heh)));
+            std::cout << "Assigning emanating halfedge for evL - " << from(evL) << "-"
+                      << to(evL) << std::endl;
+        } else {
+            // keep the edge as it might be boundary
+            evL = vertices_[vL].edge;
+        }
+        PVL_ASSERT(evL);
+
+        if (!onesided) {
+            if (vertices_[vR].edge == prev(oheh)) {
+                PVL_ASSERT(!boundary(prev(oheh)));
+                evR = opposite(next(oheh));
+                std::cout << "Assigning emanating halfedge for evR - " << from(evR) << "-"
+                          << to(evR) << std::endl;
+            } else {
+                evR = vertices_[vR].edge;
+            }
+            PVL_ASSERT(valid(evR));
+        }
+
 
         /*for (HalfEdgeHandle neh : halfEdgeRing(to(heh))) {
             if (to(neh) == from(heh)) {
@@ -707,21 +749,46 @@ class Graph {
         }*/
         HalfEdgeHandle neh = heh;
         do {
-            neh = opposite(next(neh));
+            neh = next(neh);
+            if (boundary(neh)) {
+                PVL_ASSERT(boundary(heh));
+                break;
+            }
+            neh = opposite(neh);
 
             /*std::cout << "Reassigning 'to' for edge " << from(neh) << "-" << to(neh)
                       << " from " << halfEdges_[neh].to << " to " << from(heh) << std::endl;*/
-            halfEdges_[neh].to = from(heh);
+            halfEdges_[neh].to = v0;
         } while (neh != heh);
         /*edges_[prev(eh)].next = next(opposite(next(eh)));
         edges_[prev(opposite(next(eh)))].next = from(eh);
         edges_[next(opposite(prev(oeh)))].next = next(oeh);*/
         connect(opposite(next(heh)), opposite(prev(heh)));
-        connect(opposite(next(oheh)), opposite(prev(oheh)));
+        if (!onesided) {
+            connect(opposite(next(oheh)), opposite(prev(oheh)));
+        }
+
+        vertices_[v1].edge = HalfEdgeHandle(-1);
+        vertices_[v0].edge = ev0;
+        vertices_[vL].edge = evL;
 
-        vertices_[context.removed].edge = HalfEdgeHandle(-1);
         remove(context.left);
-        remove(context.right);
+        if (!onesided) {
+            vertices_[vR].edge = evR;
+            remove(context.right);
+        }
+
+        PVL_ASSERT(valid(v0));
+        PVL_ASSERT(valid(vL));
+        PVL_ASSERT(onesided || valid(vR));
+        PVL_ASSERT(from(vertices_[v0].edge) == v0);
+        PVL_ASSERT(to(vertices_[v0].edge) != v1);
+        PVL_ASSERT(from(vertices_[vL].edge) == vL);
+        PVL_ASSERT(to(vertices_[vL].edge) != v1);
+        if (!onesided) {
+            PVL_ASSERT(from(vertices_[vR].edge) == vR);
+            PVL_ASSERT(to(vertices_[vR].edge) != v1);
+        }
 
         // check();
         /*remove(opposite(next(heh)));
@@ -730,44 +797,58 @@ class Graph {
         // remove(to(eh));
     }
 
-    bool collapseAllowed(HalfEdgeHandle eh) {
-        PVL_ASSERT(valid(eh));
-        if (boundary(eh)) { /// \todo enable boundary collapse
+    bool collapseAllowed(EdgeHandle edge) {
+        PVL_ASSERT(valid(edge));
+        HalfEdgeHandle eh = halfEdge(edge);
+
+        /*if (boundary(eh)) { /// \todo enable boundary collapse
             return false;
-        }
+        }*/
 
         if (boundary(to(eh)) != boundary(from(eh))) {
             // disallow contraction of boundary vertex and inner vertex
             return false;
         }
+        if (boundary(next(eh)) || boundary(prev(eh))) {
+            // disallow contraction of triangle with >1 boundary edge
+            return false;
+        }
         std::set<VertexHandle> ring1;
         for (VertexHandle vh : vertexRing(to(eh))) {
-            // std::cout << "circ " << vh << std::endl;
             if (vh != from(eh)) {
+                // std::cout << "ring1 " << vh << std::endl;
                 ring1.insert(vh);
             }
-            for (HalfEdgeHandle neh : halfEdgeRing(vh)) {
-                if (boundary(neh)) {
-                    return false;
+            /*if (!boundary(eh)) {
+                for (HalfEdgeHandle neh : halfEdgeRing(vh)) {
+                    if (boundary(neh)) {
+                        return false;
+                    }
                 }
-            }
+            }*/
         }
         std::set<VertexHandle> ring2;
         for (VertexHandle vh : vertexRing(from(eh))) {
             if (vh != to(eh)) {
+                // std::cout << "ring2 " << vh << std::endl;
                 ring2.insert(vh);
             }
-            for (HalfEdgeHandle neh : halfEdgeRing(vh)) {
-                if (boundary(neh)) {
-                    return false;
+            /*if (!boundary(eh)) {
+                for (HalfEdgeHandle neh : halfEdgeRing(vh)) {
+                    if (boundary(neh)) {
+                        return false;
+                    }
                 }
-            }
+            }*/
         }
         std::vector<VertexHandle> is;
         std::set_intersection(
             ring1.begin(), ring1.end(), ring2.begin(), ring2.end(), std::back_inserter(is));
-        PVL_ASSERT(is.size() >= 2);
-        return is.size() == 2;
+        VertexHandle vl = to(next(eh));
+        VertexHandle vr = !boundary(eh) ? to(next(opposite(eh))) : VertexHandle(-1);
+        return std::all_of(
+            is.begin(), is.end(), [vl, vr](VertexHandle vh) { return vh == vl || vh == vr; });
+        // return is.size() == 2;
         // if (is.size() != 2) {
         //  return false;
         //}
diff --git a/Matrix.hpp b/Matrix.hpp
index 3b1e73d..5aca720 100644
--- a/Matrix.hpp
+++ b/Matrix.hpp
@@ -51,14 +51,6 @@ class Matrix {
         return col;
     }
 
-    Vector<T, Rows> transform(const Vector<T, Cols>& v) const {
-        Vector<T, Rows> res;
-        for (int i = 0; i < Rows; ++i) {
-            res[i] = dot(row(i), v);
-        }
-        return res;
-    }
-
     static Matrix null() {
         return Matrix(Vector<T, Cols>(0), Vector<T, Cols>(0), Vector<T, Cols>(0));
     }
@@ -129,7 +121,7 @@ class Matrix {
         Matrix<T, Rows, N> result;
         for (int r = 0; r < N; ++r) {
             for (int c = 0; c < Rows; ++c) {
-                result(c, r) = dot(row(r), other.column(c));
+                result(c, r) = dotProd(row(r), other.column(c));
             }
         }
         return result;
@@ -139,6 +131,15 @@ class Matrix {
 using Mat33f = Matrix<float, 3, 3>;
 using Mat44f = Matrix<float, 4, 4>;
 
+template <typename T, int Cols, int Rows>
+Vector<T, Rows> prod(const Matrix<T, Cols, Rows>& m, const Vector<T, Cols>& v) {
+    Vector<T, Rows> res;
+    for (int i = 0; i < Rows; ++i) {
+        res[i] = dotProd(m.row(i), v);
+    }
+    return res;
+}
+
 template <typename T, int N, int M>
 Matrix<T, N, M> outerProd(const Vector<T, N>& v1, const Vector<T, M>& v2) {
     Matrix<T, N, M> res;
@@ -159,6 +160,17 @@ inline T trace(const Matrix<T, N, N>& m) {
     return tr;
 }
 
+template <typename T, int Cols, int Rows>
+Matrix<T, Rows, Cols> transpose(const Matrix<T, Cols, Rows>& m) {
+    Matrix<T, Rows, Cols> tr;
+    for (int i = 0; i < Cols; ++i) {
+        for (int j = 0; j < Rows; ++j) {
+            tr(j, i) = m(i, j);
+        }
+    }
+    return tr;
+}
+
 inline Mat33f invert(const Mat33f& m) {
     double det = m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) -
                  m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
diff --git a/MemorylessDecimator.hpp b/MemorylessDecimator.hpp
index de6a07d..88bd9bf 100644
--- a/MemorylessDecimator.hpp
+++ b/MemorylessDecimator.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "Matrix.hpp"
+#include "Optional.hpp"
 #include "TriangleMesh.hpp"
 
 namespace Pvl {
@@ -12,48 +13,40 @@ static double SQR_SIN_ALPHA = sqr(std::sin(ALPHA));
 static constexpr double MAX_PHI = 90. * M_PI / 180.;
 static double MIN_COS_PHI = std::cos(MAX_PHI);
 
-/*class OnScopeExit : boost::noncopyable {
-    std::function<void()> closure_;
-
-public:
-    OnScopeExit(std::function<void()>&& closure)
-        : closure_(std::move(closure)) {}
-
-    ~OnScopeExit() {
-        closure_();
-    }
-};*/
-
+template <typename Float>
 class LindstromTurkConstraints {
-    Mat33f A_;
-    Vec3f b_;
+    using Vec = Vector<Float, 3>;
+    using Mat = Matrix<Float, 3, 3>;
+
+    Mat A_;
+    Vec b_;
     int n_ = 0;
 
 public:
-    bool addConstraint(const Vec3f& p, const double b) {
+    bool addConstraint(const Vec& p, const Float b) {
         if (full()) {
             return false;
         }
 
         if (isCompatible(p)) {
-            const double distSqr = dot(p, p);
-            const double dist = sqrt(distSqr);
-            const Vec3f pn = p / dist;
-            const double bn = b / dist;
+            const Float distSqr = dotProd(p, p);
+            const Float dist = sqrt(distSqr);
+            const Vec pn = p / dist;
+            const Float bn = b / dist;
 
             A_.row(n_) = pn;
             b_[n_] = bn;
             n_++;
             return true;
         } else {
-            std::cout << "Constraint not compatible" << std::endl;
+            // std::cout << "Constraint not compatible" << std::endl;
             return false;
         }
     }
 
     /// Used for problems formulated as quadratic minimization,
     /// i.e. finds point where gradient Hv + c = 0.
-    int addQuadraticConstraint(const Mat33f& H, const Vec3f& c) {
+    int addQuadraticConstraint(const Mat& H, const Vec& c) {
         int added = 0;
         switch (n_) {
         case 0:
@@ -62,11 +55,11 @@ class LindstromTurkConstraints {
             added += int(addConstraint(H.row(2), -c[2]));
             break;
         case 1: {
-            const Vec3f r0 = A_.row(0);
-            std::array<double, 3> abs_r0 = {
+            const Vec r0 = A_.row(0);
+            std::array<Float, 3> abs_r0 = {
                 std::abs(r0[0]), std::abs(r0[1]), std::abs(r0[2])
             };
-            Vec3f q0;
+            Vec q0;
             const int maxIdx =
                 int(std::max_element(abs_r0.begin(), abs_r0.end()) - abs_r0.begin());
             switch (maxIdx) {
@@ -80,22 +73,22 @@ class LindstromTurkConstraints {
                 q0 = { 1., 0., -r0[0] / r0[2] };
                 break;
             }
-            const Vec3f q1 = cross(r0, q0);
-            const Vec3f p0 = H.transform(q0);
-            const Vec3f p1 = H.transform(q1);
-            const double b0 = -dot(q0, c);
-            const double b1 = -dot(q1, c);
+            const Vec q1 = crossProd(r0, q0);
+            const Vec p0 = prod(H, q0);
+            const Vec p1 = prod(H, q1);
+            const Float b0 = -dotProd(q0, c);
+            const Float b1 = -dotProd(q1, c);
 
             added += int(addConstraint(p0, b0));
             added += int(addConstraint(p1, b1));
             break;
         }
         case 2: {
-            const Vec3f r0 = A_.row(0);
-            const Vec3f r1 = A_.row(1);
-            const Vec3f n = cross(r0, r1);
-            const Vec3f p = H.transform(n);
-            const double b = -dot(n, c);
+            const Vec r0 = A_.row(0);
+            const Vec r1 = A_.row(1);
+            const Vec n = crossProd(r0, r1);
+            const Vec p = prod(H, n);
+            const Float b = -dotProd(n, c);
             added += int(addConstraint(p, b));
             break;
         }
@@ -106,9 +99,9 @@ class LindstromTurkConstraints {
         return added;
     }
 
-    Vec3f getPlacement() const {
-        const Mat33f Ainv = invert(A_);
-        return Ainv.transform(b_);
+    Vec getPlacement() const {
+        const Mat Ainv = invert(A_);
+        return prod(Ainv, b_);
     }
 
     bool full() const {
@@ -121,8 +114,8 @@ class LindstromTurkConstraints {
 
 private:
     /// Contraint compatibility rules (see Sec. 4.2 in LT paper)
-    bool isCompatible(const Vec3f& p) const {
-        const double distSqr = normSqr(p);
+    bool isCompatible(const Vec& p) const {
+        const Float distSqr = normSqr(p);
         if (distSqr < 1.e-20) {
             return false;
         }
@@ -130,21 +123,21 @@ class LindstromTurkConstraints {
         case 0:
             return true;
         case 1: {
-            const Vec3f r0 = A_.row(0);
-            const double pr0 = dot(r0, p);
-            const double r0sqr = dot(r0, r0);
-            const double lhs = sqr(pr0);
-            const double rhs = r0sqr * distSqr * SQR_COS_ALPHA;
+            const Vec r0 = A_.row(0);
+            const Float pr0 = dotProd(r0, p);
+            const Float r0sqr = dotProd(r0, r0);
+            const Float lhs = sqr(pr0);
+            const Float rhs = r0sqr * distSqr * SQR_COS_ALPHA;
             return lhs < rhs;
         }
         case 2: {
-            const Vec3f r0 = A_.row(0);
-            const Vec3f r1 = A_.row(1);
-            const Vec3f n = cross(r0, r1);
-            const double np = dot(n, p);
-            const double nsqr = dot(n, n);
-            const double lhs = sqr(np);
-            const double rhs = nsqr * distSqr * SQR_SIN_ALPHA;
+            const Vec r0 = A_.row(0);
+            const Vec r1 = A_.row(1);
+            const Vec n = crossProd(r0, r1);
+            const Float np = dotProd(n, p);
+            const Float nsqr = dotProd(n, n);
+            const Float lhs = sqr(np);
+            const Float rhs = nsqr * distSqr * SQR_SIN_ALPHA;
             return lhs > rhs;
         }
         default:
@@ -153,86 +146,95 @@ class LindstromTurkConstraints {
     }
 };
 
+template <typename Mesh>
 class MemorylessDecimator {
-    using Mesh = TriangleMesh<Vec3f, int>;
+    using Float = typename Mesh::Float;
+    using Vec = Vector<Float, 3>;
+    using Mat = Matrix<Float, 3, 3>;
+    using Constraints = LindstromTurkConstraints<Float>;
 
 public:
-    float cost(const Mesh& mesh, const Graph::CollapseContext& context) const {
-        const Vec3f p0 = mesh.point(context.remaining);
-        const Vec3f p1 = mesh.point(context.removed);
-        const double lengthSqr = normSqr(p0 - p1);
+    Optional<float> cost(const Mesh& mesh, const Graph::CollapseContext& context) const {
+        const Vec p0 = mesh.point(context.remaining);
+        const Vec p1 = mesh.point(context.removed);
+        const Float lengthSqr = normSqr(p0 - p1);
         std::set<FaceHandle> triangles = getTriangles(mesh, context);
         std::set<VertexHandle> ring = getVertexRing(mesh, context);
         std::set<HalfEdgeHandle> boundary; //= getBoundary(mesh, context);
-        Vec3f p = computePlacement(mesh, triangles, ring, boundary, lengthSqr);
-        /*if (!p || !isCollapseAllowed(ci, *p)) {
-            return Base::ILLEGAL_COLLAPSE;
-        }*/
+        Optional<Vec> dp = computePlacement(mesh, p0, triangles, ring, boundary, lengthSqr);
+        if (dp) {
+            Vec p = p0 + dp.value();
 
-        const double volumeCost = computeVolumeCost(mesh, triangles, p);
-        const double boundaryCost = computeBoundaryCost(mesh, boundary, p);
+            const Float volumeCost = computeVolumeCost(mesh, triangles, p);
+            const Float boundaryCost = computeBoundaryCost(mesh, boundary, p);
 
-        const double totalCost = 0.5 * boundaryCost * lengthSqr + 0.5 * volumeCost;
-        PVL_ASSERT(totalCost >= 0.);
+            const Float totalCost = 0.5 * boundaryCost * lengthSqr + 0.5 * volumeCost;
+            PVL_ASSERT(totalCost >= 0.);
 
-        return totalCost;
+            return totalCost;
+        } else {
+            return NONE;
+        }
     }
 
-    Vec3f placement(const Mesh& mesh, const Graph::CollapseContext& context) const {
-        const Vec3f p0 = mesh.point(context.remaining);
-        const Vec3f p1 = mesh.point(context.removed);
-        const double lengthSqr = normSqr(p0 - p1);
+    Optional<Vec> placement(const Mesh& mesh, const Graph::CollapseContext& context) const {
+        const Vec p0 = mesh.point(context.remaining);
+        const Vec p1 = mesh.point(context.removed);
+        const Float lengthSqr = normSqr(p0 - p1);
         std::set<FaceHandle> faces = getTriangles(mesh, context);
         std::set<VertexHandle> ring = getVertexRing(mesh, context);
         std::set<HalfEdgeHandle> boundary; //= getBoundary(ci);
-        return computePlacement(mesh, faces, ring, boundary, lengthSqr);
+        Optional<Vec> dp = computePlacement(mesh, p0, faces, ring, boundary, lengthSqr);
+        if (dp) {
+            return p0 + dp.value();
+        } else {
+            return NONE;
+        }
     }
 
     void postprocess(const Mesh&, const Graph::CollapseContext&) {}
 
 private:
-    inline Mat33f crossProductMatrixSqr(const Vec3f& p) const {
+    inline Mat crossProductMatrixSqr(const Vec& p) const {
         return {
-            Vec3f(p[1] * p[1] + p[2] * p[2], -p[0] * p[1], -p[0] * p[2]),
-            Vec3f(-p[0] * p[1], p[0] * p[0] + p[2] * p[2], -p[1] * p[2]),
-            Vec3f(-p[0] * p[2], -p[1] * p[2], p[0] * p[0] + p[1] * p[1]),
+            Vec(p[1] * p[1] + p[2] * p[2], -p[0] * p[1], -p[0] * p[2]),
+            Vec(-p[0] * p[1], p[0] * p[0] + p[2] * p[2], -p[1] * p[2]),
+            Vec(-p[0] * p[2], -p[1] * p[2], p[0] * p[0] + p[1] * p[1]),
         };
     }
 
-    Vec3f computePlacement(const Mesh& mesh,
+    Vec computePlacement(const Mesh& mesh,
+        const Vec& pivot,
         const std::set<FaceHandle>& triangles,
         const std::set<VertexHandle>& ring,
         const std::set<HalfEdgeHandle>& boundary,
-        const double lengthSqr) const {
-        LindstromTurkConstraints constraints;
+        const Float lengthSqr) const {
+        Constraints constraints;
 
-        addVolumeAndBoundaryOptimizationConstraint(
-            mesh, constraints, triangles, boundary, lengthSqr);
-        std::cout << "after optimization - " << constraints.count() << " constraints"
-                  << std::endl;
         addBoundaryConstraint(mesh, constraints, boundary);
-        addVolumeConstraint(mesh, constraints, triangles);
-        std::cout << "after volume - " << constraints.count() << " constraints" << std::endl;
-        addShapeConstraint(mesh, constraints, ring);
-        std::cout << "after shape - " << constraints.count() << " constraints" << std::endl;
+        addVolumeConstraint(mesh, pivot, constraints, triangles);
+        addVolumeAndBoundaryOptimizationConstraint(
+            mesh, pivot, constraints, triangles, boundary, lengthSqr);
+        addShapeConstraint(mesh, pivot, constraints, ring);
 
-        Vec3f p = constraints.getPlacement();
-        return p;
+        return constraints.getPlacement();
     }
 
     void addVolumeConstraint(const Mesh& mesh,
-        LindstromTurkConstraints& constraints,
+        const Vec& pivot,
+        Constraints& constraints,
         const std::set<FaceHandle>& faces) const {
         if (constraints.full()) {
             return;
         }
-        Vec3f sumN(0);
-        double sumL = 0.;
+        Vec sumN(0);
+        Float sumL = 0.;
 
         for (FaceHandle fh : faces) {
-            std::array<Vec3f, 3> tri = mesh.triangle(fh);
-            const Vec3f n = cross(tri[1] - tri[0], tri[2] - tri[0]);
-            const double l = dot(cross(tri[0], tri[1]), tri[2]);
+            /// \todo add pivot to triangle(fh) ?
+            std::array<Vec, 3> tri = mesh.triangle(fh);
+            const Vec n = crossProd(tri[1] - tri[0], tri[2] - tri[0]);
+            const Float l = dotProd(crossProd(tri[0] - pivot, tri[1] - pivot), tri[2] - pivot);
             sumN += n;
             sumL += l;
         }
@@ -241,80 +243,82 @@ class MemorylessDecimator {
     }
 
     void addBoundaryConstraint(const Mesh& mesh,
-        LindstromTurkConstraints& constraints,
+        Constraints& constraints,
         const std::set<HalfEdgeHandle>& edges) const {
         if (edges.empty() || constraints.full()) {
             return;
         }
 
-        Vec3f e1(0);
-        Vec3f e2(0);
+        Vec e1(0);
+        Vec e2(0);
 
         for (HalfEdgeHandle eh : edges) {
-            const Vec3f from = mesh.point(mesh.from(eh));
-            const Vec3f to = mesh.point(mesh.to(eh));
+            const Vec from = mesh.point(mesh.from(eh));
+            const Vec to = mesh.point(mesh.to(eh));
             e1 += from - to;
-            e2 += cross(from, to);
+            e2 += crossProd(from, to);
         }
 
-        const Mat33f H = crossProductMatrixSqr(e1);
-        const Vec3f c = cross(e1, e2);
+        const Mat H = crossProductMatrixSqr(e1);
+        const Vec c = crossProd(e1, e2);
 
         constraints.addQuadraticConstraint(H, c);
     }
 
     void addShapeConstraint(const Mesh& mesh,
-        LindstromTurkConstraints& constraints,
+        const Vec& pivot,
+        Constraints& constraints,
         const std::set<VertexHandle>& ring) const {
         if (constraints.full()) {
             return;
         }
-        Mat33f H = Mat33f::identity() * double(ring.size());
-        Vec3f c(0);
+        Mat H = Mat::identity() * Float(ring.size());
+        Vec c(0);
         for (VertexHandle v : ring) {
-            c -= mesh.point(v);
+            c -= (mesh.point(v) - pivot);
         }
         constraints.addQuadraticConstraint(H, c);
     }
 
     void addVolumeAndBoundaryOptimizationConstraint(const Mesh& mesh,
-        LindstromTurkConstraints& constraints,
+        const Vec& pivot,
+        Constraints& constraints,
         const std::set<FaceHandle>& faces,
         const std::set<HalfEdgeHandle>& boundary,
-        const double lengthSqr) const {
+        const Float lengthSqr) const {
         if (constraints.full()) {
             std::cout << "Skipping optimization contraint" << std::endl;
             return;
         }
 
-        Mat33f H = Mat33f::null();
-        Vec3f c(0, 0, 0);
+        Mat H = Mat::null();
+        Vec c(0, 0, 0);
 
         for (FaceHandle face : faces) {
-            std::array<Vec3f, 3> tri = mesh.triangle(face);
-            const Vec3f n = cross(tri[1] - tri[0], tri[2] - tri[0]);
-            const double l = dot(cross(tri[0], tri[1]), tri[2]);
+            std::array<Vec, 3> tri = mesh.triangle(face);
+            const Vec n = crossProd(tri[1] - tri[0], tri[2] - tri[0]);
+            const Float l = dotProd(crossProd(tri[0] - pivot, tri[1] - pivot), tri[2] - pivot);
 
             H += outerProd(n, n);
             c -= n * l;
         }
 
         if (!boundary.empty()) {
-            Mat33f Hb = Mat33f::null();
-            Vec3f cb(0, 0, 0);
+            Mat Hb = Mat::null();
+            Vec cb(0, 0, 0);
             for (HalfEdgeHandle edge : boundary) {
-                const Vec3f from = mesh.point(mesh.from(edge));
-                const Vec3f to = mesh.point(mesh.to(edge));
-                const Vec3f n = cross(from, to);
-                const Vec3f dir = from - to;
+                const Vec from = mesh.point(mesh.from(edge));
+                const Vec to = mesh.point(mesh.to(edge));
+                const Vec n = crossProd(from, to);
+                const Vec dir = from - to;
 
                 Hb += crossProductMatrixSqr(dir);
-                cb += cross(dir, n);
+                cb += crossProd(dir, n);
             }
             // 9 * boundary weight * homogenizing factor
-            const double volumeWeight = 0.5;
-            const double boundaryWeight = 0.5;
-            const double w = 9 * boundaryWeight * lengthSqr;
+            const Float volumeWeight = 0.5;
+            const Float boundaryWeight = 0.5;
+            const Float w = 9 * boundaryWeight * lengthSqr;
 
             H = H * volumeWeight + Hb * w;
             c = c * volumeWeight + cb * w;
@@ -368,33 +372,33 @@ class MemorylessDecimator {
         return ring;
     }
 
-    double computeVolumeCost(const Mesh& mesh,
+    Float computeVolumeCost(const Mesh& mesh,
         const std::set<FaceHandle>& faces,
-        const Vec3f& p) const {
-        double cost = 0.;
+        const Vec& p) const {
+        Float cost = 0.;
         for (FaceHandle face : faces) {
-            std::array<Vec3f, 3> tri = mesh.triangle(face);
+            std::array<Vec, 3> tri = mesh.triangle(face);
 
-            const Vec3f v01 = tri[1] - tri[0];
-            const Vec3f v02 = tri[2] - tri[0];
-            const Vec3f n = cross(v01, v02);
-            const double l = dot(cross(tri[0], tri[1]), tri[2]);
-            cost += sqr(dot(n, p) - l);
+            const Vec v01 = tri[1] - tri[0];
+            const Vec v02 = tri[2] - tri[0];
+            const Vec n = crossProd(v01, v02);
+            const Float l = dotProd(crossProd(tri[0], tri[1]), tri[2]);
+            cost += sqr(dotProd(n, p) - l);
         }
         return cost / 36.;
     }
 
-    double computeBoundaryCost(const Mesh& mesh,
+    Float computeBoundaryCost(const Mesh& mesh,
         const std::set<HalfEdgeHandle>& edges,
-        const Vec3f& p) const {
-        double cost = 0.;
+        const Vec& p) const {
+        Float cost = 0.;
         for (HalfEdgeHandle edge : edges) {
-            Vec3f from = mesh.point(mesh.from(edge));
-            Vec3f to = mesh.point(mesh.to(edge));
+            Vec from = mesh.point(mesh.from(edge));
+            Vec to = mesh.point(mesh.to(edge));
 
-            const Vec3f dir = from - to;
-            const Vec3f c = cross(dir, from - p);
-            cost += dot(c, c);
+            const Vec dir = from - to;
+            const Vec c = crossProd(dir, from - p);
+            cost += dotProd(c, c);
         }
         return cost / 4.;
     }
@@ -416,7 +420,7 @@ class MemorylessDecimator {
                   if (face != ci.fl && face != ci.fr) {
                       typename MeshT::Normal n1 = mesh_.normal(face);
                       typename MeshT::Normal n2 = mesh_.calc_face_normal(face);
-                      const double cosPhi = dot(n1, n2);
+                      const Float cosPhi = dot(n1, n2);
                       if (cosPhi < MIN_COS_PHI) {
                           return false;
                       }
diff --git a/Optional.hpp b/Optional.hpp
new file mode 100644
index 0000000..b37dbf4
--- /dev/null
+++ b/Optional.hpp
@@ -0,0 +1,98 @@
+#pragma once
+#include "Assert.hpp"
+#include <type_traits>
+
+namespace Pvl {
+
+struct OptionalNone {};
+const OptionalNone NONE;
+
+template <typename T>
+class Optional {
+    typename std::aligned_storage<sizeof(T), alignof(T)>::type data_;
+    bool used_;
+
+public:
+    Optional()
+        : used_(false) {}
+
+    Optional(OptionalNone)
+        : used_(false) {}
+
+    Optional(const T& value)
+        : used_(true) {
+        new (&data_) T(value);
+    }
+
+    Optional(T&& value)
+        : used_(true) {
+        new (&data_) T(std::move(value));
+    }
+
+    ~Optional() {
+        if (used_) {
+            value().~T();
+        }
+    }
+
+    T& value() {
+        PVL_ASSERT(used_);
+        return reinterpret_cast<T&>(data_);
+    }
+
+    const T& value() const {
+        PVL_ASSERT(used_);
+        return reinterpret_cast<const T&>(data_);
+    }
+
+    template <typename Alt>
+    T valueOr(Alt&& alt) {
+        if (used_) {
+            return value();
+        } else {
+            return std::forward<Alt>(alt);
+        }
+    }
+
+    template <typename Exception, typename... TArgs>
+    T valueOrThrow(TArgs&&... args) {
+        if (used_) {
+            return value();
+        } else {
+            throw Exception(std::forward<TArgs>(args)...);
+        }
+    }
+
+    T* operator->() const {
+        PVL_ASSERT(used_);
+        return &value();
+    }
+
+    explicit operator bool() const {
+        return used_;
+    }
+
+    bool operator!() const {
+        return !used_;
+    }
+};
+
+template <typename T>
+bool hasValue(const T&) {
+    return true;
+}
+template <typename T>
+bool hasValue(const Optional<T>& opt) {
+    return bool(opt);
+}
+template <typename T>
+const T& value(const T& v) {
+    return v;
+}
+template <typename T>
+const T& value(const Optional<T>& opt) {
+    return opt.value();
+}
+
+
+} // namespace Pvl
diff --git a/PlyWriter.hpp b/PlyWriter.hpp
index 04b5a8f..a6f2fde 100644
--- a/PlyWriter.hpp
+++ b/PlyWriter.hpp
@@ -80,7 +80,7 @@ class PlyWriter {
             if (!mesh.valid(FaceHandle(i))) {
                 continue;
             }
-            auto f = mesh.faceIndices(FaceHandle(i));
+            auto f = mesh.faceVertices(FaceHandle(i));
             out_ << "3 " << f[0] << " " << f[1] << " " << f[2] << "\n";
             validCnt++;
         }
diff --git a/QuadricDecimator.hpp b/QuadricDecimator.hpp
index 3015522..737b47a 100644
--- a/QuadricDecimator.hpp
+++ b/QuadricDecimator.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "Matrix.hpp"
+#include "Optional.hpp"
 #include "TriangleMesh.hpp"
 #include <iostream>
 #include <map>
@@ -9,65 +10,72 @@
 namespace Pvl {
 
 /// \todo make callable with both Context and EdgeHandle? (distinguish using traits)
+template <typename Mesh>
 class QuadricDecimator {
-    using Mesh = TriangleMesh<Vec3f, int>;
+public:
+    using Float = typename Mesh::Float;
+    using Point = Vector<Float, 3>;
+    using Plane = Vector<Float, 4>;
+    using Quadric = Matrix<Float, 4, 4>;
 
-    std::map<VertexHandle, Mat44f> quadrics_;
+private:
+    std::map<VertexHandle, Quadric> quadrics_;
 
 public:
     QuadricDecimator(Mesh& mesh) {
         for (VertexHandle vh : mesh.vertexRange()) {
-            quadrics_[vh] = Mat44f::null();
+            quadrics_[vh] = Quadric::null();
         }
 
         for (FaceHandle fh : mesh.faceRange()) {
-            Vec3f n = mesh.normal(fh);
-            Vec3f p0 = mesh.triangle(fh)[0];
-            Vec4f plane;
+            Point n = mesh.normal(fh);
+            Point p0 = mesh.triangle(fh)[0]; /// \todo fix
+            Plane plane;
             plane[0] = n[0];
             plane[1] = n[1];
             plane[2] = n[2];
-            plane[3] = -dot(p0, n);
+            plane[3] = -dotProd(p0, n);
 
             /// \todo optimize - compute together with normal
-            const float area = mesh.area(fh);
-            PVL_ASSERT(area > 0.f);
+            const Float area = mesh.area(fh);
+            PVL_ASSERT(area > 0);
 
-            Mat44f Q = outerProd(plane, plane) * area;
+            Quadric Q = outerProd(plane, plane) * area;
 
-            std::array<int, 3> idxs = mesh.faceIndices(fh);
-            for (int i : idxs) {
-                quadrics_[VertexHandle(i)] += Q;
+            for (VertexHandle vh : mesh.vertexRing(fh)) {
+                quadrics_[vh] += Q;
             }
         }
     }
 
     float cost(const Mesh& mesh, const Graph::CollapseContext& context) const {
-        /*Vec3f v1 = mesh.point(context.remaining);
-        Vec3f v2 = mesh.point(context.removed);*/
-        Mat44f Q = quadrics_.at(context.remaining) + quadrics_.at(context.removed);
-        Vec3f target = placement(mesh, context);
-        Vec4f v = homogeneous(target);
-        float result = dot(Q.transform(v), v);
-        // PVL_ASSERT(result >= 0.f);
+        Quadric Q = quadrics_.at(context.remaining) + quadrics_.at(context.removed);
+        Point target = placement(mesh, context);
+        Plane v = homogeneous(target);
+        Float result = dotProd(prod(Q, v), v);
+        PVL_ASSERT(std::isfinite(result));
         return result;
     }
 
-    Vec3f placement(const Mesh& mesh, const Graph::CollapseContext& context) const {
-        Mat44f Q = quadrics_.at(context.remaining) + quadrics_.at(context.removed);
-        float eps = 1.e-6 * pow<4>(trace(Q));
+    Point placement(const Mesh& mesh, const Graph::CollapseContext& context) const {
+        Quadric Q = quadrics_.at(context.remaining) + quadrics_.at(context.removed);
+        Float eps = 1.e-6 * pow<4>(trace(Q)); /// \todo eps dependent on type of Float?
 
         Q(0, 3) = Q(1, 3) = Q(2, 3) = 0;
         Q(3, 3) = 1;
         // bit of magic to get dimensionless eps
-        float det = std::abs(determinant(Q));
+        Float det = std::abs(determinant(Q));
         if (det > eps) {
-            Mat44f Qinv = invert(Q);
-            return Vec3f(Qinv(3, 0), Qinv(3, 1), Qinv(3, 2));
+            Quadric Qinv = invert(Q);
+            return Point(Qinv(3, 0), Qinv(3, 1), Qinv(3, 2));
         } else {
-            Vec3f v1 = mesh.point(context.remaining);
-            Vec3f v2 = mesh.point(context.removed);
-            return 0.5 * (v1 + v2);
+            // select from the endpoints
+            Plane v1 = homogeneous(mesh.point(context.remaining));
+            Plane v2 = homogeneous(mesh.point(context.removed));
+            Float cost1 = dotProd(prod(Q, v1), v1);
+            Float cost2 = dotProd(prod(Q, v2), v2);
+            Plane v = (cost1 < cost2) ? v1 : v2;
+            return euclidean(v);
         }
     }
 
diff --git a/Simplification.hpp b/Simplification.hpp
index 8f062e8..d8468bc 100644
--- a/Simplification.hpp
+++ b/Simplification.hpp
@@ -1,31 +1,78 @@
 #pragma once
 
+#include "Optional.hpp"
 #include "PlyWriter.hpp"
 #include "TriangleMesh.hpp"
 #include <queue>
 
 namespace Pvl {
 
-template <typename Vec>
+template <typename Mesh>
 struct SimpleDecimator {
-    template <typename Mesh>
-    typename Vec::Float cost(const Mesh& mesh, const Graph::CollapseContext& context) const {
-        Vec v1 = mesh.point(context.remaining);
-        Vec v2 = mesh.point(context.removed);
+    using Point = typename Mesh::Point;
+
+    typename Point::Float cost(const Mesh& mesh, const Graph::CollapseContext& context) const {
+        Point v1 = mesh.point(context.remaining);
+        Point v2 = mesh.point(context.removed);
         return normSqr(v1 - v2);
     }
 
-    template <typename Mesh>
-    Vec placement(const Mesh& mesh, const Graph::CollapseContext& context) const {
-        Vec v1 = mesh.point(context.remaining);
-        Vec v2 = mesh.point(context.removed);
+    Optional<Point> placement(const Mesh& mesh, const Graph::CollapseContext& context) const {
+        Point v1 = mesh.point(context.remaining);
+        Point v2 = mesh.point(context.removed);
         return 0.5 * (v1 + v2);
     }
 
-    template <typename Mesh>
     void postprocess(const Mesh&, const Graph::CollapseContext&) const {}
 };
 
+template <typename Decimator>
+struct PreventFaceFoldDecorator : public Decimator {
+    using Point = typename Decimator::Point;
+
+    using Decimator::Decimator;
+
+    template <typename Mesh>
+    Optional<Point> placement(Mesh& mesh, const Graph::CollapseContext& context) const {
+        Optional<Point> target = Decimator::placement(mesh, context);
+        if (!target) {
+            return NONE;
+        }
+
+        std::map<FaceHandle, Point> faces;
+        for (FaceHandle fh : mesh.faceRing(context.remaining)) {
+            if (fh != context.left && fh != context.right) {
+                faces[fh] = mesh.normal(fh);
+            }
+        }
+        for (FaceHandle fh : mesh.faceRing(context.removed)) {
+            if (fh != context.left && fh != context.right) {
+                faces[fh] = mesh.normal(fh);
+            }
+        }
+        const Point p1 = mesh.points[context.remaining];
+        const Point p2 = mesh.points[context.removed];
+
+        // simulate collapse
+        mesh.points[context.remaining] = mesh.points[context.removed] = target.value();
+
+        for (const auto& p : faces) {
+            const Point normal = mesh.normal(p.first);
+            if (dotProd(normal, p.second) < 0) {
+                // revert collapse
+                mesh.points[context.remaining] = p1;
+                mesh.points[context.removed] = p2;
+                return NONE;
+            }
+        }
+        // revert collapse
+        mesh.points[context.remaining] = p1;
+        mesh.points[context.removed] = p2;
+        return target;
+    }
+};
+
+
 class EdgeCountStop {
     std::size_t count_;
 
@@ -87,6 +134,7 @@ class CollapseQueue {
     }
 };
 
+/*
 template <typename Vec, typename Index>
 void savePatch(const TriangleMesh<Vec, Index>& mesh, HalfEdgeHandle eh) {
     TriangleMesh<Vec, Index> patch;
@@ -145,26 +193,20 @@ void savePatch(const TriangleMesh<Vec, Index>& mesh, HalfEdgeHandle eh) {
         out << p[0] << " " << p[1] << " " << p[2] << " " << c[0] << " " << c[1] << " " << c[2]
             << "\n";
     }
-}
+}*/
 
 /// \todo add stop to decimator? (cost stop)
 template <typename Vec, typename Index, typename Decimator, typename Stop>
 void simplify(TriangleMesh<Vec, Index>& mesh, Decimator& decimator, const Stop& stop) {
+    using Float = typename Vec::Float;
+
     CollapseQueue queue;
     for (EdgeHandle eh : mesh.edgeRange()) {
-        float cost = decimator.cost(mesh, Graph::CollapseContext(mesh, eh));
-        queue.insert(eh, cost);
-    }
-
-    /*for (HalfEdgeHandle heh : mesh.halfEdgeRange()) {
-        if (!mesh.valid(heh)) {
-            continue;
+        Optional<Float> cost = decimator.cost(mesh, Graph::CollapseContext(mesh, eh));
+        if (cost) {
+            queue.insert(eh, cost.value());
         }
-        EdgeHandle eh = mesh.edge(heh);
-        HalfEdgeHandle h = mesh.halfEdge(eh);
-        float cost = decimator.cost(mesh, Graph::CollapseContext(mesh, heh));
-        queue.update(h, cost);
-    }*/
+    }
 
     std::size_t cnt = 0;
     EdgeHandle collapsedEdge;
@@ -174,9 +216,13 @@ void simplify(TriangleMesh<Vec, Index>& mesh, Decimator& decimator, const Stop&
         if (mesh.removed(collapsedEdge)) {
             continue;
         }
-        Graph::CollapseContext context(mesh, collapsedEdge);
-        if (mesh.collapseAllowed(context.edge)) {
-            Vec target = decimator.placement(mesh, context);
+        if (mesh.collapseAllowed(collapsedEdge)) {
+            Graph::CollapseContext context(mesh, collapsedEdge);
+            Optional<Vec> target = decimator.placement(mesh, context);
+            if (!target) {
+                // collapse not allowed
+                continue;
+            }
             /*std::vector<HalfEdgeHandle> ring;
             for (HalfEdgeHandle eh : mesh.halfEdgeRing(mesh.from(collapsedEdge))) {
                 ring.push_back(eh);
@@ -198,8 +244,8 @@ void simplify(TriangleMesh<Vec, Index>& mesh, Decimator& decimator, const Stop&
                     queue.remove(mesh.edge(vh, context.removed));
                 }
             }
-            std::cout << "# " << cnt << " Collapsing " << mesh.to(context.edge) << " into "
-                      << mesh.from(context.edge) << ", cost = " << c << std::endl;
+            /*std::cout << "# " << cnt << " Collapsing " << mesh.to(context.edge) << " into "
+                      << mesh.from(context.edge) << ", cost = " << c << std::endl;*/
             /*std::cout << "ring:\n";
             for (HalfEdgeHandle eh : ring) {
                 std::cout << mesh.from(eh) << "-" << mesh.to(eh) << "\n";
@@ -211,7 +257,7 @@ void simplify(TriangleMesh<Vec, Index>& mesh, Decimator& decimator, const Stop&
             }
             std::cout << std::endl;
              savePatch(mesh, collapsedEdge);*/
-            mesh.collapse(context.edge, target);
+            mesh.collapse(collapsedEdge, target.value());
             for (VertexHandle vh : ring) {
                 EdgeHandle eh = mesh.edge(context.remaining, vh);
                 PVL_ASSERT(!mesh.removed(eh));
@@ -219,8 +265,10 @@ void simplify(TriangleMesh<Vec, Index>& mesh, Decimator& decimator, const Stop&
 
                 // HalEdgeHandle heh1 = mesh.halfEdge(mesh.edge(heh));
                 // PVL_ASSERT(!mesh.removed(heh1));
-                float cost = decimator.cost(mesh, Graph::CollapseContext(mesh, eh));
-                queue.insert(eh, cost);
+                Optional<Float> cost = decimator.cost(mesh, Graph::CollapseContext(mesh, eh));
+                if (cost) {
+                    queue.insert(eh, cost.value());
+                }
             }
             decimator.postprocess(mesh, context);
             if (stop(cnt++)) {
diff --git a/TriangleMesh.hpp b/TriangleMesh.hpp
index 63cba79..4c4ae3c 100644
--- a/TriangleMesh.hpp
+++ b/TriangleMesh.hpp
@@ -9,6 +9,7 @@ namespace Pvl {
 template <typename Vec, typename Index>
 class TriangleMesh : public Graph {
 public:
+    using Point = Vec;
     using Float = typename Vec::Float;
     static constexpr int Dim = Vec::size();
 
@@ -17,31 +18,32 @@ class TriangleMesh : public Graph {
 
     std::vector<Vec> points;
 
-    Vec3f point(VertexHandle vh) const {
+    Point point(VertexHandle vh) const {
         return points[vh];
     }
 
-    std::array<Vec3f, 3> triangle(FaceHandle fh) const {
-        std::array<int, 3> idxs = faceIndices(fh);
-        Vec3f p0 = points[idxs[0]];
-        Vec3f p1 = points[idxs[1]];
-        Vec3f p2 = points[idxs[2]];
+    std::array<Point, 3> triangle(FaceHandle fh) const {
+        std::array<VertexHandle, 3> idxs = faceVertices(fh);
+        Point p0 = point(idxs[0]);
+        Point p1 = point(idxs[1]);
+        Point p2 = point(idxs[2]);
         return { p0, p1, p2 };
     }
 
-    Vec3f normal(FaceHandle fh) const {
-        std::array<Vec3f, 3> tr = triangle(fh);
-        return normalize(cross(tr[1] - tr[0], tr[2] - tr[0]));
+    Point normal(FaceHandle fh) const {
+        std::array<Point, 3> tr = triangle(fh);
+        return normalize(crossProd(tr[1] - tr[0], tr[2] - tr[0]));
     }
 
     float area(FaceHandle fh) const {
         std::array<Vec3f, 3> tr = triangle(fh);
-        return 0.5f * norm(cross(tr[1] - tr[0], tr[2] - tr[0]));
+        return 0.5f * norm(crossProd(tr[1] - tr[0], tr[2] - tr[0]));
     }
 
     // to -> from
-    void collapse(HalfEdgeHandle eh, const Vec& placement) {
-        points[to(eh)] = points[from(eh)] = placement;
+    void collapse(EdgeHandle eh, const Vec& placement) {
+        HalfEdgeHandle heh = halfEdge(eh);
+        points[to(heh)] = points[from(heh)] = placement;
         Graph::collapse(eh);
     }
 };
diff --git a/Vector.hpp b/Vector.hpp
index 8e3ba93..dfcbae1 100644
--- a/Vector.hpp
+++ b/Vector.hpp
@@ -151,7 +151,7 @@ using Vec3i = Vector<int, 3>;
 using Vec4i = Vector<int, 4>;
 
 template <typename T, int Dim>
-T dot(const Vector<T, Dim>& v1, const Vector<T, Dim>& v2) {
+T dotProd(const Vector<T, Dim>& v1, const Vector<T, Dim>& v2) {
     T res = 0.f;
     for (int i = 0; i < Dim; ++i) {
         res += v1[i] * v2[i];
@@ -161,7 +161,7 @@ T dot(const Vector<T, Dim>& v1, const Vector<T, Dim>& v2) {
 
 template <typename T, int Dim>
 T normSqr(const Vector<T, Dim>& v) {
-    return dot(v, v);
+    return dotProd(v, v);
 }
 
 template <typename T, int Dim>
@@ -183,8 +183,10 @@ Vector<T, Dim> normalize(const Vector<T, Dim>& v) {
     return v / norm(v);
 }
 
-inline Vec3f cross(const Vec3f& v1, const Vec3f& v2) {
-    return Vec3f(v1[1] * v2[2] - v1[2] * v2[1], v1[2] * v2[0] - v1[0] * v2[2], v1[0] * v2[1] - v1[1] * v2[0]);
+inline Vec3f crossProd(const Vec3f& v1, const Vec3f& v2) {
+    return Vec3f(v1[1] * v2[2] - v1[2] * v2[1],
+        v1[2] * v2[0] - v1[0] * v2[2],
+        v1[0] * v2[1] - v1[1] * v2[0]);
 }
 
 template <typename T1, typename T2, int Dim>
@@ -255,4 +257,9 @@ inline Vec4f homogeneous(const Vec3f& v) {
     return Vec4f(v[0], v[1], v[2], 1.f);
 }
 
+inline Vec3f euclidean(const Vec4f& v) {
+    PVL_ASSERT(v[3] != 0);
+    return Vec3f(v[0] / v[3], v[1] / v[3], v[2] / v[3]);
+}
+
 } // namespace Pvl
diff --git a/main.cpp b/main.cpp
index 72a7c23..ff1eb97 100644
--- a/main.cpp
+++ b/main.cpp
@@ -2,8 +2,10 @@
 #include "Graph.hpp"
 #include "KdTree.hpp"
 #include "Kernels.hpp"
+#include "MemorylessDecimator.hpp"
 #include "PlyReader.hpp"
 #include "PlyWriter.hpp"
+#include "QuadricDecimator.hpp"
 #include "Refinement.hpp"
 #include "Simplification.hpp"
 #include "Svd.hpp"
@@ -127,7 +129,7 @@ TEST_CASE("collapse allowed", "[simplify]") {
 
     HalfEdgeHandle eh = graph.halfEdge(vA, vB);
     REQUIRE_FALSE(graph.removed(eh));
-    REQUIRE_FALSE(graph.collapseAllowed(eh));
+    REQUIRE_FALSE(graph.collapseAllowed(graph.edge(eh)));
 }
 
 VertexHandle operator"" _vh(unsigned long long int i) {
@@ -146,14 +148,17 @@ struct SimpleGraphFixture {
     Graph graph;
     VertexHandle vA;
     VertexHandle vB;
-    std::array<VertexHandle, 8> vs;
+    std::set<VertexHandle> ring;
+
 
     SimpleGraphFixture() {
+        std::array<VertexHandle, 8> vs;
         for (int i = 0; i < 8; ++i) {
             vs[i] = graph.addVertex();
         }
         vA = graph.addVertex();
         vB = graph.addVertex();
+        ring.insert(vs.begin(), vs.end());
 
         graph.addFace(vs[0], vA, vs[1]);
         graph.addFace(vA, vs[2], vs[1]);
@@ -168,13 +173,27 @@ struct SimpleGraphFixture {
     }
 };
 
-TEST_CASE_METHOD(SimpleGraphFixture, "halfedge", "[graph]") {
+TEST_CASE_METHOD(SimpleGraphFixture, "get halfedge from vertices", "[graph]") {
     HalfEdgeHandle eh = graph.halfEdge(vA, vB);
     REQUIRE(graph.valid(eh));
     REQUIRE(graph.from(eh) == vA);
     REQUIRE(graph.to(eh) == vB);
 }
 
+
+TEST_CASE_METHOD(SimpleGraphFixture, "get edge from vertices", "[graph]") {
+    EdgeHandle eh1 = graph.edge(vA, vB);
+    REQUIRE(graph.valid(eh1));
+    EdgeHandle eh2 = graph.edge(vB, vA);
+    REQUIRE(graph.valid(eh2));
+    REQUIRE(eh1 == eh2);
+    HalfEdgeHandle heh = graph.halfEdge(eh1);
+    REQUIRE(graph.valid(heh));
+    REQUIRE(graph.from(heh) == vA);
+    REQUIRE(graph.to(heh) == vB);
+}
+
+
 TEST_CASE_METHOD(SimpleGraphFixture, "edge range", "[graph]") {
     Graph::EdgeRange edges = graph.edgeRange();
     REQUIRE(std::distance(edges.begin(), edges.end()) == 19);
@@ -188,31 +207,71 @@ TEST_CASE_METHOD(SimpleGraphFixture, "edge range", "[graph]") {
     REQUIRE(visited == 1);
 }
 
-TEST_CASE_METHOD(SimpleGraphFixture, "collapse simple", "[simplify]") {
+TEST_CASE_METHOD(SimpleGraphFixture, "collapse inner edge", "[simplify]") {
     Graph::EdgeRange edges = graph.edgeRange();
-    HalfEdgeHandle collapsible = graph.halfEdge(vA, vB);
+    EdgeHandle collapsible = graph.edge(vA, vB);
 
-    REQUIRE(std::all_of(edges.begin(), edges.end(), [&](EdgeHandle eh) {
-        HalfEdgeHandle heh = graph.halfEdge(eh);
-        if (heh == collapsible) {
-            return graph.collapseAllowed(heh);
+    /*REQUIRE(std::all_of(edges.begin(), edges.end(), [&](EdgeHandle eh) {
+        if (eh == collapsible) {
+            return graph.collapseAllowed(eh);
         } else {
-            return !graph.collapseAllowed(heh);
+            return !graph.collapseAllowed(eh);
         }
-    }));
+    }));*/
 
+    REQUIRE(graph.collapseAllowed(collapsible));
     graph.collapse(collapsible);
 
-    Graph::EdgeRange edges2 = graph.edgeRange();
-    REQUIRE(std::all_of(edges2.begin(), edges2.end(), [&](EdgeHandle eh) {
-        if (!graph.valid(eh)) {
-            // removed
-            return true;
-        } else {
-            HalfEdgeHandle heh = graph.halfEdge(eh);
-            return !graph.collapseAllowed(heh);
+    Vertex::VertexRange vertices = graph.vertexRing(vA);
+    REQUIRE(std::distance(vertices.begin(), vertices.end()) == ring.size());
+    REQUIRE(ring == std::set<VertexHandle>(vertices.begin(), vertices.end()));
+
+    /*  Graph::EdgeRange edges2 = graph.edgeRange();
+      REQUIRE(std::all_of(edges2.begin(), edges2.end(), [&](EdgeHandle eh) {
+          if (!graph.valid(eh)) {
+              // removed
+              return true;
+          } else {
+              return !graph.collapseAllowed(eh);
+          }
+      }));*/
+}
+
+TEST_CASE_METHOD(SimpleGraphFixture, "collapse inner-to-boundary edge", "[simplify]") {
+    for (VertexHandle vh : ring) {
+        EdgeHandle eh1 = graph.edge(vA, vh);
+        if (eh1 != EdgeHandle(-1)) {
+            REQUIRE_FALSE(graph.collapseAllowed(eh1));
+        }
+        EdgeHandle eh2 = graph.edge(vB, vh);
+        if (eh2 != EdgeHandle(-1)) {
+            REQUIRE_FALSE(graph.collapseAllowed(eh2));
         }
-    }));
+    }
+}
+
+TEST_CASE_METHOD(SimpleGraphFixture, "collapse boundary edge", "[simplify]") {
+    VertexHandle v0 = *ring.begin();                 // 0
+    VertexHandle v1 = graph.to(graph.emanating(v0)); // 7
+    REQUIRE(graph.boundary(v0));
+    REQUIRE(graph.boundary(v1));
+    EdgeHandle eh = graph.edge(v0, v1);
+    REQUIRE(graph.boundary(eh));
+    REQUIRE(graph.collapseAllowed(eh));
+
+    graph.collapse(eh);
+    REQUIRE(!graph.valid(v1));
+    REQUIRE(graph.collapseAllowed(graph.edge(vA, vB)));
+    std::set<VertexHandle> ringA;
+    for (VertexHandle vh : graph.vertexRing(vA)) {
+        ringA.insert(vh);
+    }
+    std::set<VertexHandle> ringB;
+    for (VertexHandle vh : graph.vertexRing(vB)) {
+        ringB.insert(vh);
+    }
+    REQUIRE(ringA == std::set<VertexHandle>({ 0_vh, 1_vh, 2_vh, 6_vh, 9_vh }));
+    REQUIRE(ringB == std::set<VertexHandle>({ 2_vh, 3_vh, 4_vh, 5_vh, 6_vh, 8_vh }));
 }
 
 TEST_CASE("collapse simple 2", "[simplify]") {
@@ -244,7 +303,7 @@ TEST_CASE("collapse simple 2", "[simplify]") {
         std::cout << std::endl;
     }
 
-    HalfEdgeHandle eh = graph.halfEdge(vA, vB);
+    EdgeHandle eh = graph.edge(vA, vB);
     graph.collapse(eh);
 
     REQUIRE(graph.valid(vA));
@@ -305,7 +364,7 @@ TEST_CASE("simplify simple", "[simplify]") {
     mesh.addFace(vs[6], vB, vA);
     mesh.addFace(vs[0], vs[7], vA);
 
-    Pvl::HalfEdgeHandle eh = mesh.halfEdge(vA, vB);
+    Pvl::EdgeHandle eh = mesh.edge(vA, vB);
     std::cout << "A-B edge = " << eh << std::endl;
     {
         std::ofstream ofs("base.ply");
@@ -361,12 +420,53 @@ TEST_CASE("simplify bunny", "[simplify]") {
     auto bunny = reader.readMesh();
 
     std::cout << "Simplifying bunny " << std::endl;
-    Pvl::simplify(bunny,
-        Pvl::EdgeLengthCost<Pvl::Vec3f>{},
-        Pvl::MidpointPlacement<Pvl::Vec3f>{},
-        Pvl::EdgeCountStop{ 33000 });
+    Pvl::SimpleDecimator<decltype(bunny)> decimator;
+    Pvl::simplify(bunny, decimator, Pvl::EdgeCountStop{ 32500 });
+    /*for (int cnt : { 6000, 6000, 6000, 6000, 6000, 3000 }) {
+        Pvl::SimpleDecimator<Pvl::Vec3f> decimator;
+        Pvl::simplify(bunny, decimator, Pvl::EdgeCountStop{ cnt });
+    }*/
     {
-        std::ofstream ofs("simplified.ply");
+        std::ofstream ofs("simplified-simple.ply");
+        Pvl::PlyWriter writer(ofs);
+        writer << bunny;
+    }
+}
+
+TEST_CASE("simplify bunny 2", "[simplify]") {
+    std::ifstream ifs("/home/pavel/projects/pvl/data/bunny-fixed.ply");
+    PlyReader reader(ifs);
+    auto bunny = reader.readMesh();
+
+    std::cout << "Simplifying bunny " << std::endl;
+
+    PreventFaceFoldDecorator<QuadricDecimator<TriangleMesh<Vec3f, int>>> decimator(bunny);
+    simplify(bunny, decimator, Pvl::EdgeCountStop{ 32500 });
+
+
+    {
+        std::ofstream ofs("simplified-quadrics.ply");
+        PlyWriter writer(ofs);
+        writer << bunny;
+    }
+}
+
+TEST_CASE("simplify bunny 3", "[simplify]") {
+    std::ifstream ifs("/home/pavel/projects/pvl/data/bunny-fixed.ply");
+    Pvl::PlyReader reader(ifs);
+    auto bunny = reader.readMesh();
+
+    std::cout << "Simplifying bunny " << std::endl;
+
+    Pvl::MemorylessDecimator<decltype(bunny)> decimator; //(bunny);
+    Pvl::simplify(bunny, decimator, Pvl::EdgeCountStop{ 32500 });
+    /*for (int cnt : { 6000, 6000, 6000, 6000, 6000, 3000 }) {
+        Pvl::QuadricDecimator decimator(bunny);
+        Pvl::simplify(bunny, decimator, Pvl::EdgeCountStop{ cnt });
+    }*/
+
+    {
+        std::ofstream ofs("simplified-lt.ply");
         Pvl::PlyWriter writer(ofs);
         writer << bunny;
     }