diff --git a/Assert.hpp b/Assert.hpp
new file mode 100644
index 0000000..ca5f5ce
--- /dev/null
+++ b/Assert.hpp
@@ -0,0 +1,5 @@
+#pragma once
+
+#include <cassert>
+
+#define ASSERT(x, ...) assert(x)
diff --git a/Box.hpp b/Box.hpp
new file mode 100644
index 0000000..413f2ce
--- /dev/null
+++ b/Box.hpp
@@ -0,0 +1,79 @@
+#pragma once
+
+#include "Vector.hpp"
+
+namespace Pvl {
+
+template <typename T, int Dim>
+class BoundingBox {
+    Vector<T, Dim> lower_;
+    Vector<T, Dim> upper_;
+
+public:
+    BoundingBox()
+        : lower_(std::numeric_limits<T>::max())
+        , upper_(std::numeric_limits<T>::lowest()) {}
+
+    BoundingBox(const Vector<T, Dim>& lower, const Vector<T, Dim>& upper)
+        : lower_(lower)
+        , upper_(upper) {}
+
+    Vector<T, Dim>& lower() {
+        return lower_;
+    }
+
+    const Vector<T, Dim>& lower() const {
+        return lower_;
+    }
+
+    Vector<T, Dim>& upper() {
+        return upper_;
+    }
+
+    const Vector<T, Dim>& upper() const {
+        return upper_;
+    }
+
+    Vector<T, Dim> size() const {
+        return upper_ - lower_;
+    }
+
+    Vector<T, Dim> center() const {
+        return T(0.5) * (upper_ + lower_);
+    }
+
+
+    bool contains(const Vector<T, Dim>& p) const {
+        for (int i = 0; i < Dim; ++i) {
+            if (p[i] < lower_[i] || p[i] > upper_[i]) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    void extend(const Vector<T, Dim>& p) {
+        lower_ = min(lower_, p);
+        upper_ = max(upper_, p);
+    }
+};
+
+/// \brief Splits the box along given coordinate.
+///
+/// The splitting plane must pass through the box.
+template <typename T, int Dim>
+std::pair<BoundingBox<T, Dim>, BoundingBox<T, Dim>> splitBox(const BoundingBox<T, Dim>& box,
+    const int dim,
+    const T x) {
+    /*ASSERT(isValid());*/
+    ASSERT(dim < Dim);
+    ASSERT(x >= box.lower()[dim] && x <= box.upper()[dim]);
+    BoundingBox<T, Dim> b1 = box;
+    BoundingBox<T, Dim> b2 = box;
+    b1.lower()[dim] = x;
+    b2.upper()[dim] = x;
+    return std::make_pair(b1, b2);
+}
+
+
+} // namespace Pvl
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..e5dc210
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(pvl LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(TBB REQUIRED tbb)
+
+add_executable(pvl
+    main.cpp
+    Utils.hpp
+    Assert.hpp
+    Math.hpp
+    Vector.hpp
+    Matrix.hpp
+    Svd.hpp
+    Box.hpp
+    KdTree.h KdTree.inl.h
+    PlyWriter.hpp PlyReader.hpp
+    Cloud.hpp
+    Kernels.hpp
+    TriangleMesh.hpp
+    MeshUtils.hpp
+    Range.hpp
+    Graph.hpp Graph.inl.hpp
+    UniformGrid.hpp
+    OctreeGrid.hpp
+    MarchingCubes.hpp MarchingCubes.cpp
+    )
+
+target_link_libraries(pvl PRIVATE tbb)
diff --git a/Cloud.hpp b/Cloud.hpp
new file mode 100644
index 0000000..0abadad
--- /dev/null
+++ b/Cloud.hpp
@@ -0,0 +1,3 @@
+#pragma once
+
+namespace Pvl {}
diff --git a/Graph.hpp b/Graph.hpp
new file mode 100644
index 0000000..238b2ab
--- /dev/null
+++ b/Graph.hpp
@@ -0,0 +1,445 @@
+#pragma once
+
+#include "Assert.hpp"
+#include "Range.hpp"
+#include <algorithm>
+#include <array>
+#include <deque>
+#include <iostream>
+#include <iterator>
+#include <map>
+#include <set>
+#include <vector>
+
+
+namespace Pvl {
+
+template <typename TObject, typename Index = int>
+class Handle {
+    Index idx_;
+
+public:
+    using Object = TObject;
+
+    Handle() = default;
+
+    explicit Handle(const Index idx)
+        : idx_(idx) {}
+
+    Index index() const {
+        return idx_;
+    }
+
+    operator Index() const {
+        return idx_;
+    }
+};
+
+class HalfEdge;
+class Vertex;
+class Face;
+class Graph;
+
+using HalfEdgeHandle = Handle<HalfEdge>;
+using VertexHandle = Handle<Vertex>;
+using FaceHandle = Handle<Face>;
+
+struct HalfEdge {
+    using Handle = HalfEdgeHandle;
+
+    VertexHandle to;
+    FaceHandle left;
+    HalfEdgeHandle opposite;
+    HalfEdgeHandle next;
+    HalfEdgeHandle prev;
+
+
+    HalfEdge(VertexHandle to)
+        : to(to) {}
+
+    bool boundary() const {
+        return opposite < 0;
+    }
+};
+
+struct Vertex {
+    using Handle = VertexHandle;
+
+    HalfEdgeHandle edge; // emanating vertex
+
+    Vertex() {
+        edge = HalfEdgeHandle(-1);
+    }
+
+    Vertex(HalfEdgeHandle eh)
+        : edge(eh) {}
+
+    bool valid() const {
+        return edge >= 0;
+    }
+
+    struct EndTag {};
+
+    class IteratorBase {
+    protected:
+        const Graph& graph_;
+        HalfEdgeHandle eh_;
+
+        enum class Status {
+            BEGIN,
+            ITERS,
+            END,
+        } status_;
+
+    public:
+        IteratorBase(const Graph& graph, VertexHandle vh);
+
+        IteratorBase(const Graph& graph, VertexHandle vh, EndTag);
+
+        IteratorBase& operator++();
+
+        bool operator!=(const IteratorBase& other) const;
+    };
+
+    class HalfEdgeIterator : public IteratorBase {
+    public:
+        using IteratorBase::IteratorBase;
+
+        HalfEdgeHandle operator*() const;
+    };
+
+    class VertexIterator : public IteratorBase {
+    public:
+        using IteratorBase::IteratorBase;
+
+        VertexHandle operator*() const;
+    };
+
+    class FaceIterator : public IteratorBase {
+    public:
+        using IteratorBase::IteratorBase;
+
+        FaceHandle operator*() const;
+    };
+
+
+    using HalfEdgeRange = Range<HalfEdgeIterator>;
+    using VertexRange = Range<VertexIterator>;
+    using FaceRange = Range<FaceIterator>;
+};
+
+static_assert(sizeof(Vertex) == sizeof(Handle<Vertex>), "error");
+
+struct Face {
+    HalfEdgeHandle edge;
+
+    Face() = default;
+
+    Face(HalfEdgeHandle eh)
+        : edge(eh) {}
+
+    struct EndTag {};
+
+    class IteratorBase {
+    protected:
+        const Graph& graph_;
+        HalfEdgeHandle eh_;
+        int steps_;
+
+    public:
+        IteratorBase(const Graph& graph, FaceHandle fh);
+
+        IteratorBase(const Graph& graph, FaceHandle fh, EndTag);
+
+        IteratorBase& operator++();
+
+        bool operator!=(const IteratorBase& other) const;
+    };
+
+    class HalfEdgeIterator : public IteratorBase {
+    public:
+        using IteratorBase::IteratorBase;
+
+        HalfEdgeHandle operator*() const;
+    };
+
+    class VertexIterator : public IteratorBase {
+    public:
+        using IteratorBase::IteratorBase;
+
+        VertexHandle operator*() const;
+    };
+
+    class FaceIterator : public IteratorBase {
+    public:
+        FaceIterator(const Graph& graph, FaceHandle fh);
+
+        FaceIterator(const Graph& graph, FaceHandle fh, EndTag);
+
+        FaceHandle operator*() const;
+
+        FaceIterator& operator++();
+    };
+
+
+    using HalfEdgeRange = Range<HalfEdgeIterator>;
+    using VertexRange = Range<VertexIterator>;
+    using FaceRange = Range<FaceIterator>;
+};
+
+class Graph {
+    friend class Vertex;
+    friend class Face;
+
+    std::vector<Vertex> vertices_;
+    std::vector<Face> faces_;
+    std::vector<HalfEdge> edges_;
+
+    // maps vertex to the set of emanating boundary edges
+    std::map<VertexHandle, std::set<HalfEdgeHandle>> boundaryEdges_;
+
+
+public:
+    HalfEdgeHandle next(HalfEdgeHandle eh) const {
+        return edges_[eh].next;
+    }
+    HalfEdgeHandle prev(HalfEdgeHandle eh) const {
+        return edges_[eh].prev;
+    }
+    HalfEdgeHandle opposite(HalfEdgeHandle eh) const {
+        ASSERT(!boundary(eh));
+        return edges_[eh].opposite;
+    }
+    bool boundary(HalfEdgeHandle eh) const {
+        return edges_[eh].boundary();
+    }
+    bool boundary(VertexHandle vh) const {
+        return boundary(vertices_[vh].edge);
+    }
+    VertexHandle to(HalfEdgeHandle eh) const {
+        return edges_[eh].to;
+    }
+    VertexHandle from(HalfEdgeHandle eh) const {
+        return edges_[prev(eh)].to;
+    }
+    FaceHandle left(HalfEdgeHandle eh) const {
+        return edges_[eh].left;
+    }
+    FaceHandle right(HalfEdgeHandle eh) const {
+        ASSERT(!edges_[eh].boundary());
+        return edges_[opposite(eh)].left;
+    }
+    HalfEdgeHandle from(VertexHandle vh) const {
+        return vertices_[vh].edge;
+    }
+    HalfEdgeHandle to(VertexHandle vh) const {
+        return prev(from(vh));
+    }
+
+    template <typename THandle>
+    class HandleIterator {
+        THandle handle_;
+
+    public:
+        using iterator_category = std::forward_iterator_tag;
+        using value_type = THandle;
+        using difference_type = int;
+        using reference = THandle&;
+        using pointer = THandle*;
+
+        HandleIterator(THandle handle)
+            : handle_(handle) {}
+
+        THandle operator*() const {
+            return handle_;
+        }
+
+        HandleIterator& operator++() {
+            handle_ = THandle(handle_ + 1);
+            return *this;
+        }
+
+        bool operator==(const HandleIterator& other) const {
+            return handle_ == other.handle_;
+        }
+
+        bool operator!=(const HandleIterator& other) const {
+            return handle_ != other.handle_;
+        }
+    };
+
+    using HalfEdgeIterator = HandleIterator<HalfEdgeHandle>;
+    using VertexIterator = HandleIterator<VertexHandle>;
+    using FaceIterator = HandleIterator<FaceHandle>;
+
+    using HalfEdgeRange = Range<HalfEdgeIterator>;
+    using VertexRange = Range<VertexIterator>;
+    using FaceRange = Range<FaceIterator>;
+
+    /// \todo rename to Iterators and Circulators
+    HalfEdgeRange halfEdgeRange() const {
+        return { HalfEdgeHandle(0), HalfEdgeHandle(edges_.size()) };
+    }
+
+    VertexRange vertexRange() const {
+        return { VertexHandle(0), VertexHandle(vertices_.size()) };
+    }
+
+    FaceRange faceRange() const {
+        return { FaceHandle(0), FaceHandle(faces_.size()) };
+    }
+
+    Vertex::HalfEdgeRange halfEdgeRing(VertexHandle vh) const {
+        return { Vertex::HalfEdgeIterator(*this, vh), Vertex::HalfEdgeIterator(*this, vh, Vertex::EndTag{}) };
+    }
+
+    Vertex::VertexRange vertexRing(VertexHandle vh) const {
+        return { Vertex::VertexIterator(*this, vh), Vertex::VertexIterator(*this, vh, Vertex::EndTag{}) };
+    }
+
+    Vertex::FaceRange faceRing(VertexHandle vh) const {
+        return { Vertex::FaceIterator(*this, vh), Vertex::FaceIterator(*this, vh, Vertex::EndTag{}) };
+    }
+
+    Face::HalfEdgeRange halfEdgeRing(FaceHandle fh) const {
+        return { Face::HalfEdgeIterator(*this, fh), Face::HalfEdgeIterator(*this, fh, Face::EndTag{}) };
+    }
+
+    Face::VertexRange vertexRing(FaceHandle fh) const {
+        return { Face::VertexIterator(*this, fh), Face::VertexIterator(*this, fh, Face::EndTag{}) };
+    }
+
+    Face::FaceRange faceRing(FaceHandle fh) const {
+        return { Face::FaceIterator(*this, fh), Face::FaceIterator(*this, fh, Face::EndTag{}) };
+    }
+
+    std::array<int, 3> faceIndices(FaceHandle fh) const {
+        std::array<int, 3> indices;
+        int i = 0;
+        for (VertexHandle vh : vertexRing(fh)) {
+            indices[i] = vh.index();
+            ++i;
+        }
+        return indices;
+    }
+
+    VertexHandle addVertex() {
+        vertices_.emplace_back(HalfEdgeHandle(-1));
+        return VertexHandle(vertices_.size() - 1);
+    }
+
+    FaceHandle addFace(VertexHandle vh1, VertexHandle vh2, VertexHandle vh3) {
+        // std::cout << "Adding face " << vh1 << ", " << vh2 << ", " << vh3 << std::endl;
+        faces_.emplace_back();
+        FaceHandle fh = FaceHandle(faces_.size() - 1);
+        Face& f = faces_[fh];
+
+        edges_.emplace_back(vh2);
+        edges_.emplace_back(vh3);
+        edges_.emplace_back(vh1);
+
+        std::array<HalfEdgeHandle, 3> eh = {
+            HalfEdgeHandle(edges_.size() - 3),
+            HalfEdgeHandle(edges_.size() - 2),
+            HalfEdgeHandle(edges_.size() - 1),
+        };
+        std::array<VertexHandle, 3> vh = { vh1, vh2, vh3 };
+
+        // set face edge
+        f.edge = eh[0];
+
+        for (int i = 0; i < 3; ++i) {
+            // set emanating edge
+            if (!vertices_[vh[i]].valid()) {
+                vertices_[vh[i]].edge = eh[i];
+            }
+
+            // set next and previous edge
+            HalfEdge& e = edges_[eh[i]];
+            e.left = fh;
+            e.next = eh[(i + 1) % 3];
+            e.prev = eh[(i + 2) % 3];
+            e.opposite = HalfEdgeHandle(-1);
+        }
+
+        for (int i = 0; i < 3; ++i) {
+            for (HalfEdgeHandle neh : halfEdgeRing(to(eh[i]))) {
+                // std::cout << "Visiting " << neh << "\n";
+                if (to(neh) == from(eh[i])) {
+                    if (!edges_[neh].boundary()) {
+                        std::cout << "Complex edge!\n";
+                        break;
+                    }
+                    edges_[neh].opposite = eh[i];
+                    edges_[eh[i]].opposite = neh;
+                    break;
+                }
+            }
+
+            if (edges_[eh[i]].boundary()) {
+                // opposite not found by circulation
+                std::set<HalfEdgeHandle> removed;
+                for (HalfEdgeHandle neh : boundaryEdges_[to(eh[i])]) {
+                    if (to(neh) == from(eh[i])) {
+                        if (!edges_[neh].boundary()) {
+                            std::cout << "Complex edge!\n";
+                        }
+                        edges_[neh].opposite = eh[i];
+                        edges_[eh[i]].opposite = neh;
+                        removed.insert(neh);
+                        break;
+                    }
+                }
+                for (HalfEdgeHandle neh : removed) {
+                    boundaryEdges_[to(eh[i])].erase(neh);
+                }
+                if (boundaryEdges_[to(eh[i])].empty()) {
+                    boundaryEdges_.erase(to(eh[i]));
+                }
+            }
+        }
+        for (int i = 0; i < 3; ++i) {
+            if (edges_[eh[i]].boundary()) {
+                boundaryEdges_[from(eh[i])].insert(eh[i]);
+            }
+
+            {
+                VertexHandle vh = from(eh[i]);
+                if (!boundary(eh[i])) {
+                    // find boundary
+                    HalfEdgeHandle e = eh[i];
+                    do {
+                        e = next(opposite(e));
+                    } while (e != eh[i] && !boundary(e));
+                    vertices_[vh].edge = e;
+                }
+            }
+            {
+                HalfEdgeHandle e = from(vh[i]);
+                HalfEdgeHandle e0 = from(vh[i]);
+                if (!boundary(e)) {
+                    // find boundary
+                    do {
+                        e = next(opposite(e));
+                    } while (e != e0 && !boundary(e));
+                    vertices_[vh[i]].edge = e;
+                }
+            }
+        }
+
+        return fh;
+    }
+
+
+    std::size_t numVertices() const {
+        return vertices_.size();
+    }
+
+    std::size_t numFaces() const {
+        return faces_.size();
+    }
+}; // namespace Nevim
+
+} // namespace Pvl
+
+#include "Graph.inl.hpp"
diff --git a/Graph.inl.hpp b/Graph.inl.hpp
new file mode 100644
index 0000000..93ae296
--- /dev/null
+++ b/Graph.inl.hpp
@@ -0,0 +1,110 @@
+#pragma once
+
+#include "Graph.hpp"
+
+namespace Pvl {
+
+inline Vertex::IteratorBase::IteratorBase(const Graph& graph, VertexHandle vh)
+    : graph_(graph) {
+    eh_ = graph_.vertices_[vh].edge;
+    if (eh_ != HalfEdgeHandle(-1)) {
+        status_ = Status::BEGIN;
+    } else {
+        status_ = Status::END; // unreferenced vertex
+    }
+}
+
+inline Vertex::IteratorBase::IteratorBase(const Graph& graph, VertexHandle vh, EndTag)
+    : graph_(graph) {
+    eh_ = graph_.vertices_[vh].edge;
+    status_ = Status::END;
+}
+
+inline Vertex::IteratorBase& Vertex::IteratorBase::operator++() {
+    const HalfEdgeHandle prev = graph_.prev(eh_);
+    if (!graph_.boundary(prev)) {
+        eh_ = graph_.opposite(prev);
+        status_ = Status::ITERS;
+    } else {
+        status_ = Status::END;
+    }
+    ASSERT(eh_ != HalfEdgeHandle(-1));
+    /*  std::cout << "++ - " << eh_ << ", status = "
+                << (status_ == Status::BEGIN ? "begin" : (status_ == Status::ITERS ? "iters" : "end"))
+                << std::endl;
+  */
+    return *this;
+}
+
+inline bool Vertex::IteratorBase::operator!=(const IteratorBase& other) const {
+    /// \todo proper
+    return (status_ != Status::END || other.status_ != Status::END) &&
+           (eh_ != other.eh_ || status_ == Status::BEGIN);
+}
+
+inline HalfEdgeHandle Vertex::HalfEdgeIterator::operator*() const {
+    return eh_;
+}
+
+inline VertexHandle Vertex::VertexIterator::operator*() const {
+    return graph_.to(eh_);
+}
+
+inline FaceHandle Vertex::FaceIterator::operator*() const {
+    return graph_.left(eh_);
+}
+
+inline Face::IteratorBase::IteratorBase(const Graph& graph, FaceHandle fh)
+    : graph_(graph) {
+    eh_ = graph_.faces_[fh].edge;
+    steps_ = 0;
+}
+
+inline Face::IteratorBase::IteratorBase(const Graph& graph, FaceHandle fh, EndTag)
+    : graph_(graph) {
+    eh_ = graph_.faces_[fh].edge;
+    steps_ = 3;
+}
+
+inline Face::IteratorBase& Face::IteratorBase::operator++() {
+    eh_ = graph_.next(eh_);
+    ++steps_;
+    return *this;
+}
+
+inline bool Face::IteratorBase::operator!=(const IteratorBase& other) const {
+    return eh_ != other.eh_ || steps_ != other.steps_;
+}
+
+inline HalfEdgeHandle Face::HalfEdgeIterator::operator*() const {
+    return eh_;
+}
+
+inline VertexHandle Face::VertexIterator::operator*() const {
+    return graph_.to(eh_);
+}
+
+inline Face::FaceIterator::FaceIterator(const Graph& graph, FaceHandle fh)
+    : IteratorBase(graph, fh) {
+    while (graph_.boundary(eh_) && steps_ < 3) {
+        eh_ = graph_.next(eh_);
+        ++steps_;
+    }
+}
+
+inline Face::FaceIterator::FaceIterator(const Graph& graph, FaceHandle fh, EndTag t)
+    : IteratorBase(graph, fh, t) {}
+
+inline FaceHandle Face::FaceIterator::operator*() const {
+    return graph_.right(eh_);
+}
+
+inline Face::FaceIterator& Face::FaceIterator::operator++() {
+    do {
+        eh_ = graph_.next(eh_);
+        ++steps_;
+    } while (graph_.boundary(eh_) && steps_ < 3);
+    return *this;
+}
+
+} // namespace Pvl
diff --git a/Kernels.hpp b/Kernels.hpp
new file mode 100644
index 0000000..ec978bd
--- /dev/null
+++ b/Kernels.hpp
@@ -0,0 +1,258 @@
+#pragma once
+
+#include "Math.hpp"
+#include "Utils.hpp"
+#include "Vector.hpp"
+
+namespace Pvl {
+
+/// \brief Base class for all SPH kernels.
+///
+/// Provides an interface for computing kernel values and gradients. All derived class must implement method
+/// <code>valueImpl</code> and <code>gradImpl</code>. Both function take SQUARED value of dimensionless
+/// distance q as a parameter. Function value returns the kernel value, grad returns gradient DIVIDED BY q.
+template <typename TDerived, typename Float, int Dim>
+class Kernel : public Noncopyable {
+public:
+    using Vec = Vector<Float, Dim>;
+
+    Kernel() = default;
+
+    /// Value of kernel at given point
+    /// this should be called only once for a pair of particles as there is expensive division
+    /// \todo Potentially precompute the 3rd power ...
+    Float value(const Vec& r, const Float h) const noexcept {
+        ASSERT(h > 0.f);
+        const Float hInv = 1.f / h;
+        return pow<Dim>(hInv) * impl().valueImpl(normSqr(r) * sqr(hInv));
+    }
+
+    Vec grad(const Vec& r, const Float h) const noexcept {
+        ASSERT(h > 0.f);
+        const Float hInv = 1.f / h;
+        return r * pow<Dim + 2>(hInv) * impl().gradImpl(normSqr(r) * sqr(hInv));
+    }
+
+private:
+    const TDerived& impl() const noexcept {
+        return static_cast<const TDerived&>(*this);
+    }
+};
+
+
+/// \brief A look-up table approximation of the kernel.
+///
+/// Can be constructed from any SPH kernel. Use this class exclusively for any high-performance computations,
+/// it is always faster than using kernel functions directly (except for trivial kerneles, such as \ref
+/// TriangleKernel). The precision difference is about 1.e-6.
+template <typename Float, int Dim>
+class LutKernel : public Kernel<LutKernel<Float, Dim>, Float, Dim> {
+private:
+    static constexpr int NEntries = 40000;
+
+    /// \todo replace with StaticArray?
+    Float values[NEntries];
+    Float grads[NEntries];
+
+    Float rad = 0.f;
+    Float qSqrToIdx = 0.f;
+
+public:
+    LutKernel() {
+        /// \todo initialize, otherwise compiler complains about using uninitialized values
+        qSqrToIdx = NAN;
+        for (Float& v : values) {
+            v = NAN;
+        }
+        for (Float& g : grads) {
+            g = NAN;
+        }
+    }
+
+    LutKernel(const LutKernel& other) {
+        *this = other;
+    }
+
+    LutKernel& operator=(const LutKernel& other) {
+        rad = other.rad;
+        qSqrToIdx = other.qSqrToIdx;
+        for (int i = 0; i < NEntries; ++i) {
+            values[i] = other.values[i];
+            grads[i] = other.grads[i];
+        }
+        return *this;
+    }
+
+    /// \brief Constructs LUT kernel given an exact SPH kernel.
+    template <typename TKernel,
+        typename = std::enable_if_t<!std::is_same<std::decay_t<TKernel>, LutKernel>::value>>
+    LutKernel(TKernel&& source) {
+        rad = source.radius();
+
+        ASSERT(rad > 0.f);
+        const Float radInvSqr = 1.f / (rad * rad);
+        qSqrToIdx = Float(NEntries) * radInvSqr;
+        for (int i = 0; i < NEntries; ++i) {
+            const Float qSqr = Float(i) / qSqrToIdx;
+            values[i] = source.valueImpl(qSqr);
+            grads[i] = source.gradImpl(qSqr);
+        }
+        /// \todo re-normalize?
+    }
+
+    bool initialized() const {
+        return rad > 0.f;
+    }
+
+    Float radius() const noexcept {
+        return rad;
+    }
+
+    Float valueImpl(const Float qSqr) const noexcept {
+        ASSERT(qSqr >= 0.f);
+        ASSERT(initialized());
+        if (qSqr >= sqr(rad)) {
+            // outside of kernel support
+            return 0.f;
+        }
+        // linear interpolation of stored values
+        const Float floatIdx = qSqrToIdx * qSqr;
+        const int idx1 = Size(floatIdx);
+        ASSERT(idx1 < NEntries);
+        const int idx2 = idx1 + 1;
+        const Float ratio = floatIdx - Float(idx1);
+        ASSERT(ratio >= 0.f && ratio < 1.f);
+
+        return values[idx1] * (1.f - ratio) + (int(idx2 < NEntries) * values[idx2]) * ratio;
+    }
+
+    Float gradImpl(const Float qSqr) const noexcept {
+        ASSERT(qSqr >= 0.f);
+        ASSERT(initialized());
+        if (qSqr >= sqr(rad)) {
+            // outside of kernel support
+            return 0.f;
+        }
+        const Float floatIdx = qSqrToIdx * qSqr;
+        const int idx1 = Size(floatIdx);
+        ASSERT(unsigned(idx1) < unsigned(NEntries));
+        const int idx2 = idx1 + 1;
+        const Float ratio = floatIdx - Float(idx1);
+        ASSERT(ratio >= 0.f && ratio < 1.f);
+
+        return grads[idx1] * (1.f - ratio) + (int(idx2 < NEntries) * grads[idx2]) * ratio;
+    }
+};
+
+
+/// \brief Cubic spline (M4) kernel
+template <typename Float, int Dim>
+class CubicSpline : public Kernel<CubicSpline<Float, Dim>, Float, Dim> {
+private:
+    const Float normalization[3] = { 2.f / 3.f, 10.f / (7.f * PI), 1.f / PI };
+
+public:
+    Float radius() const {
+        return 2.f;
+    }
+
+    // template <bool TApprox = false>
+    Float valueImpl(const Float qSqr) const {
+        const Float q = sqrt(qSqr);
+        ASSERT(q >= 0);
+        if (q < 1.f) {
+            return normalization[Dim - 1] * (0.25f * pow<3>(2.f - q) - pow<3>(1.f - q));
+        }
+        if (q < 2.f) {
+            return normalization[Dim - 1] * (0.25f * pow<3>(2.f - q));
+        }
+        return 0.f; // compact within 2r radius
+    }
+
+    // template <bool TApprox = false>
+    Float gradImpl(const Float qSqr) const {
+        const Float q = sqrt(qSqr);
+        if (q == 0.f) {
+            // gradient of kernel is 0 at q = 0, but here we divide by q,
+            // the expression grad/q has a finite limit for q->0
+            return -3.f * normalization[Dim - 1];
+        }
+        if (q < 1.f) {
+            return (1.f / q) * normalization[Dim - 1] * (-0.75f * pow<2>(2.f - q) + 3.f * pow<2>(1.f - q));
+        }
+        if (q < 2.f) {
+            return (1.f / q) * normalization[Dim - 1] * (-0.75f * pow<2>(2.f - q));
+        }
+        return 0.f;
+    }
+};
+/*
+/// \brief Gaussian kernel
+///
+/// Clamped to zero at radius 5, the error is therefore about exp(-5^2) = 10^-11.
+template <Size D>
+class Gaussian : public Kernel<Gaussian<D>, D> {
+private:
+    const Float normalization[3] = { 1.f / sqrt(PI), 1.f / PI, 1.f / (PI * sqrt(PI)) };
+
+public:
+    Float radius() const {
+        return 5.f;
+    }
+
+    Float valueImpl(const Float qSqr) const {
+        if (qSqr >= sqr(radius())) {
+            return 0.f;
+        }
+        return normalization[D - 1] * exp(-qSqr);
+    }
+
+    Float gradImpl(const Float qSqr) const {
+        if (qSqr >= sqr(radius())) {
+            return 0.f;
+        }
+        if (qSqr == 0.f) {
+            return -2.f * normalization[D - 1];
+        }
+        const Float q = sqrt(qSqr);
+        return normalization[D - 1] / q * exp(-qSqr) * (-2.f * q);
+    }
+};
+
+/// \brief Triangular (piecewise linear) kernel.
+///
+/// Does not have continuous derivatives, mainly for testing purposes and non-SPH applications.
+template <Size D>
+class TriangleKernel : public Kernel<TriangleKernel<D>, D> {
+private:
+    const Float normalization[3] = { 1.f, 3.f / PI, 3.f / PI };
+
+public:
+    Float radius() const {
+        return 1.f;
+    }
+
+    Float valueImpl(const Float qSqr) const {
+        if (qSqr >= sqr(radius())) {
+            return 0.f;
+        }
+        const Float q = sqrt(qSqr);
+        return normalization[D - 1] * (1.f - q);
+    }
+
+    Float gradImpl(const Float qSqr) const {
+        if (qSqr >= sqr(radius())) {
+            return 0.f;
+        }
+        // unfortunately this gradient is nonzero at q->0, so grad/q diverges;
+        // let's return a reasonable value to avoid numerical problems
+        if (qSqr == 0.f) {
+            return -100.f;
+        }
+        const Float q = sqrt(qSqr);
+        return -normalization[D - 1] / q;
+    }
+};
+*/
+
+} // namespace Pvl
diff --git a/MarchingCubes.hpp b/MarchingCubes.hpp
new file mode 100644
index 0000000..a644e3c
--- /dev/null
+++ b/MarchingCubes.hpp
@@ -0,0 +1,445 @@
+#pragma once
+
+#include "TriangleMesh.hpp"
+#include <tbb/enumerable_thread_specific.h>
+
+namespace Pvl {
+
+template <typename Float>
+class Cell {
+private:
+    std::array<Vector<Float, 3>, 8> points_;
+    std::array<Float, 8> values_;
+
+public:
+    Float& value(const int idx) {
+        return values_[idx];
+    }
+
+    Vector<Float, 3>& node(const int idx) {
+        return points_[idx];
+    }
+};
+
+
+/*template <typename Volume, typename Float>
+void extractIsosurface(const Volume& volume, TriangleMesh& mesh, const float isovalue) {
+    typename Volume::ConstIterator iter;
+    typename Volume::ConstIterator end(volume.end());
+    for (iter = volume.begin(); iter != end; ++iter) {
+        Cell cell;
+        bool allBelow = true, allAbove = true;
+        int i = 0;
+        for (int z = 0; z <= 1; ++z) {
+            for (int y = 0; y <= 1; ++y) {
+                for (int x = 0; x <= 1; ++x) {
+                    // the mapping convention is (0, 0) - (1, 0) - (1, 1) - (0, 1)
+                    int actX = (x + y) % 2;
+                    cell.node(i) = v + dr * Vector(actX, y, z);
+                    cell.value(i) = cached.phi[mapping(idxs + Indices(actX, y, z))];
+                    if (cell.value(i) > surfaceLevel) {
+                        allBelow = false;
+                    } else if (cell.value(i) < surfaceLevel) {
+                        allAbove = false;
+                    }
+                    i++;
+                }
+            }
+        }
+
+        if (!allBelow && !allAbove) {
+            this->intersectCell(cell, tri.local());
+        }
+    }
+
+    for (Array<Triangle>& triTl : tri) {
+        triangles.pushAll(triTl);
+    }
+}
+
+
+template <typename TFunctor>
+bool MarchingCubes::iterateWithIndices(const Box& box, const Vector& step, TFunctor&& functor) {
+    MEASURE_SCOPE("MC - evaluating field");
+    ASSERT(box != Box::EMPTY());
+
+    Size reportCnt = max(Size(box.size()[Z] / step[Z]), 1u);
+    Size reportStep = max(reportCnt / 100, 1u);
+    std::atomic_int counter{ 0 };
+    std::atomic_bool shouldContinue{ true };
+    auto task = [this, &step, &box, &functor, reportStep, reportCnt, &counter, &shouldContinue](
+                    const Size k) {
+        const Float z = box.lower()[Z] + k * step[Z];
+        Size i = 0;
+        Size j = 0;
+        for (Float y = box.lower()[Y]; y <= box.upper()[Y]; y += step[Y], j++) {
+            i = 0;
+            for (Float x = box.lower()[X]; x <= box.upper()[X]; x += step[X], i++) {
+                functor(Indices(i, j, k), Vector(x, y, z));
+            }
+        }
+
+        if (progressCallback && (++counter % reportStep == 0)) {
+            shouldContinue = shouldContinue && progressCallback(Float(counter) / Float(reportCnt));
+        }
+    };
+    parallelFor(scheduler, 0, Size(box.size()[Z] / step[Z]) + 1, task);
+    return shouldContinue;
+}
+
+MarchingCubes::MarchingCubes(IScheduler& scheduler,
+    const Float surfaceLevel,
+    const SharedPtr<IScalarField>& field,
+    Function<bool(Float progress)> progressCallback)
+    : scheduler(scheduler)
+    , surfaceLevel(surfaceLevel)
+    , field(field)
+    , progressCallback(progressCallback) {}
+
+void MarchingCubes::addComponent(const Box& box, const Float gridResolution) {
+    MEASURE_SCOPE("MC addComponent");
+
+    const Vector dr = min(Vector(gridResolution), box.size() * (1._f - EPS));
+    cached.phi.clear();
+    // multiply by (1 + EPS) to handle case where box size is divisible by dr
+    Indices cnts((1._f + EPS) * box.size() / dr);
+    ASSERT(cnts[X] >= 1 && cnts[Y] >= 1 && cnts[Z] >= 1);
+
+    // find values of grid nodes
+    auto mapping = [&cnts](const Indices& idxs) {
+        ASSERT(idxs[X] >= 0 && idxs[X] <= cnts[X], idxs[X], cnts[X]);
+        ASSERT(idxs[Y] >= 0 && idxs[Y] <= cnts[Y], idxs[Y], cnts[Y]);
+        ASSERT(idxs[Z] >= 0 && idxs[Z] <= cnts[Z], idxs[Z], idxs[Z]);
+        return idxs[X] + (cnts[X] + 1) * idxs[Y] + (cnts[X] + 1) * (cnts[Y] + 1) * idxs[Z];
+    };
+    cached.phi.resize((cnts[X] + 1) * (cnts[Y] + 1) * (cnts[Z] + 1));
+    bool shouldContinue =
+        this->iterateWithIndices(box, dr, [this, &mapping](const Indices& idxs, const Vector& v) { //
+            cached.phi[mapping(idxs)] = (*field)(v);
+        });
+    if (!shouldContinue) {
+        return;
+    }
+
+    // for each non-empty grid, find all intersecting triangles
+    Box boxWithoutLast(box.lower(), box.upper() - dr);
+    ThreadLocal<Array<Triangle>> tri(scheduler);
+
+    auto intersect = [this, &dr, &mapping, &tri](const Indices& idxs, const Vector& v) {
+        Cell cell;
+        bool allBelow = true, allAbove = true;
+        Size i = 0;
+        for (Size z = 0; z <= 1; ++z) {
+            for (Size y = 0; y <= 1; ++y) {
+                for (Size x = 0; x <= 1; ++x) {
+                    Size actX = (x + y) % 2; // the mapping convention is (0, 0) - (1, 0) - (1, 1) - (0, 1)
+                    cell.node(i) = v + dr * Vector(actX, y, z);
+                    cell.value(i) = cached.phi[mapping(idxs + Indices(actX, y, z))];
+                    if (cell.value(i) > surfaceLevel) {
+                        allBelow = false;
+                    } else if (cell.value(i) < surfaceLevel) {
+                        allAbove = false;
+                    }
+                    i++;
+                }
+            }
+        }
+
+        if (!allBelow && !allAbove) {
+            this->intersectCell(cell, tri.local());
+        }
+    };
+    shouldContinue = this->iterateWithIndices(boxWithoutLast, dr, intersect);
+    if (!shouldContinue) {
+        return;
+    }
+
+    for (Array<Triangle>& triTl : tri) {
+        triangles.pushAll(triTl);
+    }
+}
+
+void MarchingCubes::intersectCell(Cell& cell, Array<Triangle>& tri) {
+    Size cubeIdx = 0;
+    for (Size i = 0; i < 8; ++i) {
+        if (cell.value(i) <= surfaceLevel) {
+            cubeIdx |= 1 << i;
+        }
+    }
+
+    if (MC_EDGES[cubeIdx] == 0) {
+        // cube is entirely in/out of the surface
+        return;
+    }
+
+    // find the vertices where the surface intersects the cube
+    StaticArray<Vector, 12> vertices;
+    for (Size i = 0; i < 12; ++i) {
+        if (MC_EDGES[cubeIdx] & (1 << i)) {
+            const Size k = IDXS1[i];
+            const Size l = IDXS2[i];
+            vertices[i] = this->interpolate(cell.node(k), cell.value(k), cell.node(l), cell.value(l));
+        } else {
+            vertices[i] = Vector(NAN);
+        }
+    }
+
+    for (Size i = 0; MC_TRIANGLES[cubeIdx][i] != -1; i += 3) {
+        Triangle t;
+        t[0] = vertices[MC_TRIANGLES[cubeIdx][i + 0]];
+        t[1] = vertices[MC_TRIANGLES[cubeIdx][i + 1]];
+        t[2] = vertices[MC_TRIANGLES[cubeIdx][i + 2]];
+        if (!t.isValid()) {
+            // skip degenerated triangles
+            continue;
+        }
+        tri.push(t);
+    }
+}
+
+INLINE Vector MarchingCubes::interpolate(const Vector& v1,
+    const Float p1,
+    const Vector& v2,
+    const Float p2) const {
+    if (almostEqual(p1, surfaceLevel)) {
+        return v1;
+    }
+    if (almostEqual(p2, surfaceLevel)) {
+        return v2;
+    }
+    if (almostEqual(p1, p2)) {
+        // small difference between values, just return the center to avoid instabilities
+        return 0.5_f * (v1 + v2);
+    }
+
+    Float mu = (p1 - surfaceLevel) / (p1 - p2);
+    ASSERT(mu >= 0._f && mu <= 1._f);
+    return v1 + mu * (v2 - v1);
+}
+
+namespace {
+
+    class ColorField : public IScalarField {
+    private:
+        LutKernel<3> kernel;
+        AutoPtr<IBasicFinder> finder;
+
+        ArrayView<const Vector> r;
+        ArrayView<const Float> m, rho;
+        ArrayView<const Size> flag;
+        ArrayView<const SymmetricTensor> G;
+        Float maxH = 0._f;
+
+        ThreadLocal<Array<NeighbourRecord>> neighs;
+
+    public:
+        /// \brief Creates the number density field.
+        ///
+        /// This implementation uses anisotropic kernel to reduce the perturbations of the boundary, see
+        /// https://www.cc.gatech.edu/~turk/my_papers/sph_surfaces.pdf.
+        /// \param storage Storage containing particle masses, densities and flags used to distinguish
+        /// different
+        ///                bodies (we don't want to blend together their surfaces)
+        /// \param scheduler Scheduler used for parallelization.
+        /// \param r Particle positions, generally different than the ones stored in the storage.
+        /// \param aniso Particle anisotropy matrix, for isotropic distribution equals to I/h
+        /// \param kernel SPH kernel used for particle smoothing
+        /// \param finder Neighbour finder
+        ColorField(const Storage& storage,
+            IScheduler& scheduler,
+            const ArrayView<const Vector> r,
+            const ArrayView<const SymmetricTensor> aniso,
+            const Float maxH,
+            LutKernel<3>&& kernel,
+            AutoPtr<IBasicFinder>&& finder)
+            : kernel(std::move(kernel))
+            , finder(std::move(finder))
+            , r(r)
+            , G(aniso)
+            , maxH(maxH)
+            , neighs(scheduler) {
+            tie(m, rho) = storage.getValues<Float>(QuantityId::MASS, QuantityId::DENSITY);
+            flag = storage.getValue<Size>(QuantityId::FLAG);
+
+            // we have to re-build the tree since we are using different positions (in general)
+            this->finder->build(scheduler, r);
+        }
+
+        virtual Float operator()(const Vector& pos) override {
+            ASSERT(maxH > 0._f);
+            Array<NeighbourRecord>& neighsTl = neighs.local();
+            /// \todo for now let's just search some random multiple of smoothing length, we should use the
+            /// largest singular value here
+            finder->findAll(pos, maxH * kernel.radius(), neighsTl);
+            Float phi = 0._f;
+
+            // find average h of neighbours and the flag of the closest particle
+            Size closestFlag = 0;
+            Float flagDistSqr = INFTY;
+            for (NeighbourRecord& n : neighsTl) {
+                const Size j = n.index;
+                if (n.distanceSqr < flagDistSqr) {
+                    closestFlag = flag[j];
+                    flagDistSqr = n.distanceSqr;
+                }
+            }
+
+            // interpolate values of neighbours
+            for (NeighbourRecord& n : neighsTl) {
+                const Size j = n.index;
+                if (flag[j] != closestFlag) {
+                    continue;
+                }
+                phi +=
+                    m[j] / rho[j] * G[j].determinant() * kernel.valueImpl(getSqrLength(G[j] * (pos - r[j])));
+            }
+            return phi;
+        }
+    };
+
+
+    class FallbackField : public IScalarField {
+    private:
+        LutKernel<3> kernel;
+        AutoPtr<IBasicFinder> finder;
+
+        ArrayView<const Vector> r;
+        ArrayView<const SymmetricTensor> G;
+        Float maxH = 0._f;
+
+        ThreadLocal<Array<NeighbourRecord>> neighs;
+
+    public:
+        FallbackField(IScheduler& scheduler,
+            const ArrayView<const Vector> r,
+            const ArrayView<const SymmetricTensor> aniso,
+            const Float maxH,
+            LutKernel<3>&& kernel,
+            AutoPtr<IBasicFinder>&& finder)
+            : kernel(std::move(kernel))
+            , finder(std::move(finder))
+            , r(r)
+            , G(aniso)
+            , maxH(maxH)
+            , neighs(scheduler) {
+
+            // we have to re-build the tree since we are using different positions (in general)
+            this->finder->build(scheduler, r);
+        }
+
+        virtual Float operator()(const Vector& pos) override {
+            ASSERT(maxH > 0._f);
+            Array<NeighbourRecord>& neighsTl = neighs.local();
+            /// \todo for now let's just search some random multiple of smoothing length, we should use the
+            /// largest singular value here
+            finder->findAll(pos, maxH * kernel.radius(), neighsTl);
+            Float phi = 0._f;
+
+            // interpolate values of neighbours
+            for (NeighbourRecord& n : neighsTl) {
+                const Size j = n.index;
+                phi += sphereVolume(0.5_f * r[j][H]) * G[j].determinant() *
+                       kernel.valueImpl(getSqrLength(G[j] * (pos - r[j])));
+            }
+            return phi;
+        }
+    };
+
+} // namespace
+
+INLINE Float weight(const Vector& r1, const Vector& r2) {
+    const Float lengthSqr = getSqrLength(r1 - r2);
+    // Eq. (11)
+    if (lengthSqr < sqr(2._f * r1[H])) {
+        return 1._f - pow<3>(sqrt(lengthSqr) / (2._f * r1[H]));
+    } else {
+        return 0._f;
+    }
+}
+
+Array<Triangle> getSurfaceMesh(IScheduler& scheduler, const Storage& storage, const McConfig& config) {
+    MEASURE_SCOPE("getSurfaceMesh");
+
+    // (according to http://www.cc.gatech.edu/~turk/my_papers/sph_surfaces.pdf)
+
+    ArrayView<const Vector> r = storage.getValue<Vector>(QuantityId::POSITION);
+    RunSettings settings;
+    LutKernel<3> kernel = Factory::getKernel<3>(settings);
+    AutoPtr<IBasicFinder> finder = Factory::getFinder(settings);
+
+    finder->build(scheduler, r);
+
+    Array<Vector> r_bar(r.size());
+    Array<SymmetricTensor> G(r.size()); // anisotropy matrix
+
+    ThreadLocal<Array<NeighbourRecord>> neighsData(scheduler);
+    parallelFor(scheduler, neighsData, 0, r.size(), [&](const Size i, Array<NeighbourRecord>& neighs) {
+        /// \todo point cloud denoising?
+        r_bar[i] = r[i];
+        r_bar[i][H] = r[i][H] * config.smoothingMult;
+
+        if (config.useAnisotropicKernels) {
+            Vector r_center = Vector(0._f);
+            finder->findAll(r_bar[i], 2 * r_bar[i][H], neighs);
+            for (const NeighbourRecord& n : neighs) {
+                r_center += r_bar[n.index];
+            }
+            r_center /= neighs.size();
+            SymmetricTensor C = SymmetricTensor::null();
+            for (const NeighbourRecord& n : neighs) {
+                C += symmetricOuter(r[n.index] - r_center, r[n.index] - r_center);
+            }
+
+            Svd svd = singularValueDecomposition(C);
+            const Float maxSigma = maxElement(svd.S);
+            for (Size i = 0; i < 3; ++i) {
+                svd.S[i] = 1._f / std::max(svd.S[i], 0.125_f * maxSigma);
+            }
+
+            AffineMatrix sigma = convert<AffineMatrix>(SymmetricTensor(svd.S, Vector(0._f)));
+            G[i] = convert<SymmetricTensor>(svd.V * sigma * svd.U.transpose());
+        } else {
+            G[i] = SymmetricTensor(Vector(1._f / r[i][H]), Vector(0._f));
+        }
+    });
+    // 5. find bounding box and maximum h (we need to search neighbours of arbitrary point in space)
+
+    Float maxH = 0._f;
+    for (Size i = 0; i < r_bar.size(); ++i) {
+        maxH = max(maxH, r_bar[i][H]);
+    }
+    SharedPtr<IScalarField> field;
+
+    if (storage.has(QuantityId::MASS) && storage.has(QuantityId::DENSITY) && storage.has(QuantityId::FLAG)) {
+        field =
+            makeShared<ColorField>(storage, scheduler, r_bar, G, maxH, std::move(kernel), std::move(finder));
+    } else {
+        field = makeShared<FallbackField>(scheduler, r_bar, G, maxH, std::move(kernel), std::move(finder));
+    }
+
+    MarchingCubes mc(scheduler, config.surfaceLevel, field, config.progressCallback);
+
+    Array<Size> components;
+    const Size numComponents = Post::findComponents(storage, 2._f, Post::ComponentFlag::OVERLAP, components);
+
+    // 6. find the surface using marching cubes for each component
+    Array<Box> boxes(numComponents);
+    Array<Size> counts(numComponents);
+    counts.fill(0);
+    for (Size j = 0; j < components.size(); ++j) {
+        const Vector padding(max(2._f * r_bar[j][H], 2._f * config.gridResolution));
+        boxes[components[j]].extend(r_bar[j] + padding);
+        boxes[components[j]].extend(r_bar[j] - padding);
+        counts[components[j]]++;
+    }
+    for (Size i = 0; i < numComponents; ++i) {
+        if (counts[i] > 10) {
+            mc.addComponent(boxes[i], config.gridResolution);
+        }
+    }
+
+    return std::move(mc.getTriangles());
+}
+
+*/
+} // namespace Pvl
diff --git a/Math.hpp b/Math.hpp
new file mode 100644
index 0000000..d2f7615
--- /dev/null
+++ b/Math.hpp
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+
+namespace Pvl {
+
+const float PI = M_PI;
+
+template <typename T>
+T sqr(const T& value) {
+    return value * value;
+}
+
+template <typename T, int N>
+struct Pow;
+
+template <typename T>
+struct Pow<T, 2> {
+    T operator()(const T& value) {
+        return value * value;
+    }
+};
+
+template <typename T>
+struct Pow<T, 3> {
+    T operator()(const T& value) {
+        return value * value * value;
+    }
+};
+
+template <int N, typename T>
+T pow(const T& value) {
+    return Pow<T, N>()(value);
+}
+
+template <typename T>
+T clamp(const T& value, const T& lower, const T& upper) {
+    return std::min(std::max(value, lower), upper);
+}
+
+} // namespace Pvl
diff --git a/Matrix.hpp b/Matrix.hpp
new file mode 100644
index 0000000..f1724fa
--- /dev/null
+++ b/Matrix.hpp
@@ -0,0 +1,196 @@
+#pragma once
+
+#include "Vector.hpp"
+
+namespace Pvl {
+
+template <typename T, int Cols, int Rows>
+class Matrix {
+    std::array<Vector<T, Cols>, Rows> rows_;
+
+public:
+    Matrix() = default;
+
+    Matrix(const Vector<T, Cols>& r1, const Vector<T, Cols>& r2, const Vector<T, Cols>& r3)
+        : rows_{ r1, r2, r3 } {}
+
+    Matrix(const Vector<T, Cols>& r1,
+        const Vector<T, Cols>& r2,
+        const Vector<T, Cols>& r3,
+        const Vector<T, Cols>& r4)
+        : rows_{ r1, r2, r3, r4 } {}
+
+    T& operator()(const int c, const int r) {
+        ASSERT(r < Rows);
+        ASSERT(c < Cols);
+        return rows_[r][c];
+    }
+
+    const T& operator()(const int c, const int r) const {
+        ASSERT(r < Rows);
+        ASSERT(c < Cols);
+        return rows_[r][c];
+    }
+
+
+    const Vector<T, Cols>& row(const int idx) const {
+        ASSERT(idx < Rows);
+        return rows_[idx];
+    }
+
+    Vector<T, Rows> column(const int idx) const {
+        ASSERT(idx < Cols);
+        Vector<T, Rows> col;
+        for (int i = 0; i < Rows; ++i) {
+            col[i] = row(i)[idx];
+        }
+        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 identity() {
+        Matrix m;
+        for (int j = 0; j < Rows; ++j) {
+            for (int i = 0; i < Cols; ++i) {
+                m(i, j) = int(i == j);
+            }
+        }
+        return m;
+    }
+
+    template <int N>
+    Matrix<T, Rows, N> operator*(const Matrix<T, N, Cols>& other) const {
+        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));
+            }
+        }
+        return result;
+    }
+};
+
+using Mat33f = Matrix<float, 3, 3>;
+using Mat44f = Matrix<float, 4, 4>;
+
+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)) +
+                 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
+
+    double invdet = 1 / det;
+
+    Mat33f minv; // inverse of matrix m
+    minv(0, 0) = (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) * invdet;
+    minv(0, 1) = (m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2)) * invdet;
+    minv(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * invdet;
+    minv(1, 0) = (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) * invdet;
+    minv(1, 1) = (m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0)) * invdet;
+    minv(1, 2) = (m(1, 0) * m(0, 2) - m(0, 0) * m(1, 2)) * invdet;
+    minv(2, 0) = (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)) * invdet;
+    minv(2, 1) = (m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1)) * invdet;
+    minv(2, 2) = (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) * invdet;
+    return minv;
+}
+
+inline Mat44f invert(const Mat44f& m) {
+    Mat44f minv;
+    minv(0, 0) = m(1, 1) * m(2, 2) * m(3, 3) - m(1, 1) * m(3, 2) * m(2, 3) - m(1, 2) * m(2, 1) * m(3, 3) +
+                 m(1, 2) * m(3, 1) * m(2, 3) + m(1, 3) * m(2, 1) * m(3, 2) - m(1, 3) * m(3, 1) * m(2, 2);
+
+    minv(0, 1) = -m(0, 1) * m(2, 2) * m(3, 3) + m(0, 1) * m(3, 2) * m(2, 3) + m(0, 2) * m(2, 1) * m(3, 3) -
+                 m(0, 2) * m(3, 1) * m(2, 3) - m(0, 3) * m(2, 1) * m(3, 2) + m(0, 3) * m(3, 1) * m(2, 2);
+
+    minv(0, 2) = m(0, 1) * m(1, 2) * m(3, 3) - m(0, 1) * m(3, 2) * m(1, 3) - m(0, 2) * m(1, 1) * m(3, 3) +
+                 m(0, 2) * m(3, 1) * m(1, 3) + m(0, 3) * m(1, 1) * m(3, 2) - m(0, 3) * m(3, 1) * m(1, 2);
+
+    minv(0, 3) = -m(0, 1) * m(1, 2) * m(2, 3) + m(0, 1) * m(2, 2) * m(1, 3) + m(0, 2) * m(1, 1) * m(2, 3) -
+                 m(0, 2) * m(2, 1) * m(1, 3) - m(0, 3) * m(1, 1) * m(2, 2) + m(0, 3) * m(2, 1) * m(1, 2);
+
+    minv(1, 0) = -m(1, 0) * m(2, 2) * m(3, 3) + m(1, 0) * m(3, 2) * m(2, 3) + m(1, 2) * m(2, 0) * m(3, 3) -
+                 m(1, 2) * m(3, 0) * m(2, 3) - m(1, 3) * m(2, 0) * m(3, 2) + m(1, 3) * m(3, 0) * m(2, 2);
+
+    minv(1, 1) = m(0, 0) * m(2, 2) * m(3, 3) - m(0, 0) * m(3, 2) * m(2, 3) - m(0, 2) * m(2, 0) * m(3, 3) +
+                 m(0, 2) * m(3, 0) * m(2, 3) + m(0, 3) * m(2, 0) * m(3, 2) - m(0, 3) * m(3, 0) * m(2, 2);
+
+    minv(1, 2) = -m(0, 0) * m(1, 2) * m(3, 3) + m(0, 0) * m(3, 2) * m(1, 3) + m(0, 2) * m(1, 0) * m(3, 3) -
+                 m(0, 2) * m(3, 0) * m(1, 3) - m(0, 3) * m(1, 0) * m(3, 2) + m(0, 3) * m(3, 0) * m(1, 2);
+
+    minv(1, 3) = m(0, 0) * m(1, 2) * m(2, 3) - m(0, 0) * m(2, 2) * m(1, 3) - m(0, 2) * m(1, 0) * m(2, 3) +
+                 m(0, 2) * m(2, 0) * m(1, 3) + m(0, 3) * m(1, 0) * m(2, 2) - m(0, 3) * m(2, 0) * m(1, 2);
+
+    minv(2, 0) = m(1, 0) * m(2, 1) * m(3, 3) - m(1, 0) * m(3, 1) * m(2, 3) - m(1, 1) * m(2, 0) * m(3, 3) +
+                 m(1, 1) * m(3, 0) * m(2, 3) + m(1, 3) * m(2, 0) * m(3, 1) - m(1, 3) * m(3, 0) * m(2, 1);
+
+    minv(2, 1) = -m(0, 0) * m(2, 1) * m(3, 3) + m(0, 0) * m(3, 1) * m(2, 3) + m(0, 1) * m(2, 0) * m(3, 3) -
+                 m(0, 1) * m(3, 0) * m(2, 3) - m(0, 3) * m(2, 0) * m(3, 1) + m(0, 3) * m(3, 0) * m(2, 1);
+
+    minv(2, 2) = m(0, 0) * m(1, 1) * m(3, 3) - m(0, 0) * m(3, 1) * m(1, 3) - m(0, 1) * m(1, 0) * m(3, 3) +
+                 m(0, 1) * m(3, 0) * m(1, 3) + m(0, 3) * m(1, 0) * m(3, 1) - m(0, 3) * m(3, 0) * m(1, 1);
+
+    minv(2, 3) = -m(0, 0) * m(1, 1) * m(2, 3) + m(0, 0) * m(2, 1) * m(1, 3) + m(0, 1) * m(1, 0) * m(2, 3) -
+                 m(0, 1) * m(2, 0) * m(1, 3) - m(0, 3) * m(1, 0) * m(2, 1) + m(0, 3) * m(2, 0) * m(1, 1);
+
+    minv(3, 0) = -m(1, 0) * m(2, 1) * m(3, 2) + m(1, 0) * m(3, 1) * m(2, 2) + m(1, 1) * m(2, 0) * m(3, 2) -
+                 m(1, 1) * m(3, 0) * m(2, 2) - m(1, 2) * m(2, 0) * m(3, 1) + m(1, 2) * m(3, 0) * m(2, 1);
+
+    minv(3, 1) = m(0, 0) * m(2, 1) * m(3, 2) - m(0, 0) * m(3, 1) * m(2, 2) - m(0, 1) * m(2, 0) * m(3, 2) +
+                 m(0, 1) * m(3, 0) * m(2, 2) + m(0, 2) * m(2, 0) * m(3, 1) - m(0, 2) * m(3, 0) * m(2, 1);
+
+    minv(3, 2) = -m(0, 0) * m(1, 1) * m(3, 2) + m(0, 0) * m(3, 1) * m(1, 2) + m(0, 1) * m(1, 0) * m(3, 2) -
+                 m(0, 1) * m(3, 0) * m(1, 2) - m(0, 2) * m(1, 0) * m(3, 1) + m(0, 2) * m(3, 0) * m(1, 1);
+
+    minv(3, 3) = m(0, 0) * m(1, 1) * m(2, 2) - m(0, 0) * m(2, 1) * m(1, 2) - m(0, 1) * m(1, 0) * m(2, 2) +
+                 m(0, 1) * m(2, 0) * m(1, 2) + m(0, 2) * m(1, 0) * m(2, 1) - m(0, 2) * m(2, 0) * m(1, 1);
+    const float det =
+        m(0, 0) * minv(0, 0) + m(1, 0) * minv(0, 1) + m(2, 0) * minv(0, 2) + m(3, 0) * minv(0, 3);
+    const float norm = 1.f / det;
+
+    for (int i = 0; i < 4; i++) {
+        for (int j = 0; j < 4; j++) {
+            minv(i, j) *= norm;
+        }
+    }
+
+    return minv;
+}
+
+inline Mat44f homogeneous(const Mat33f& m) {
+    Mat44f res = Mat44f::identity();
+    for (int i = 0; i < 3; ++i) {
+        for (int j = 0; j < 3; ++j) {
+            res(i, j) = m(i, j);
+        }
+    }
+    return res;
+}
+
+inline Mat44f getTranslationMatrix(const Vec3f& pos) {
+    Mat44f m = Mat44f::identity();
+    for (int i = 0; i < 3; ++i) {
+        m(3, i) = pos[i];
+    }
+    return m;
+}
+
+inline Mat33f getRotationMatrix(const Vec3f& axis, const float angle) {
+    const float u = axis[0];
+    const float v = axis[1];
+    const float w = axis[2];
+    const float s = sin(angle);
+    const float c = cos(angle);
+    return {
+        Vec3f(u * u + (v * v + w * w) * c, u * v * (1 - c) - w * s, u * w * (1 - c) + v * s),
+        Vec3f(u * v * (1 - c) + w * s, v * v + (u * u + w * w) * c, v * w * (1 - c) - u * s),
+        Vec3f(u * w * (1 - c) - v * s, v * w * (1 - c) + u * s, w * w + (u * u + v * v) * c),
+    };
+}
+
+} // namespace Pvl
diff --git a/MeshUtils.hpp b/MeshUtils.hpp
new file mode 100644
index 0000000..b85639c
--- /dev/null
+++ b/MeshUtils.hpp
@@ -0,0 +1,51 @@
+#pragma once
+
+#include "TriangleMesh.hpp"
+#include "Utils.hpp"
+#include <iostream>
+
+namespace Pvl {
+
+template <typename ConcurrencyTag = SequentialTag, typename Vec, typename Index>
+void laplacianSmoothing(TriangleMesh<Vec, Index>& mesh, bool preserveBoundary = true) {
+    std::vector<Vec> laplacian(mesh.numVertices(), Vec(0));
+    ParallelForEach<ConcurrencyTag>()(
+        mesh.vertexRange(), [&mesh, &laplacian, preserveBoundary](VertexHandle v1) {
+            if (preserveBoundary && mesh.boundary(v1)) {
+                return;
+            }
+            Vec3f delta(0, 0, 0);
+            std::size_t neighCnt = 0;
+            for (VertexHandle v2 : mesh.vertexRing(v1)) {
+                delta += mesh.points[v2];
+                ++neighCnt;
+            }
+            if (neighCnt > 0) {
+                laplacian[v1] = delta / neighCnt - mesh.points[v1];
+            }
+        });
+    std::vector<Vec> biharmonic(mesh.numVertices(), Vec(0));
+    ParallelForEach<ConcurrencyTag>()(
+        mesh.vertexRange(), [&mesh, &laplacian, &biharmonic, preserveBoundary](VertexHandle v1) {
+            if (preserveBoundary && mesh.boundary(v1)) {
+                return;
+            }
+            Vec3f delta(0, 0, 0);
+            // float weight = 0.;
+            std::size_t neighCnt = 0;
+            for (VertexHandle v2 : mesh.vertexRing(v1)) {
+                delta += laplacian[v2];
+                ++neighCnt;
+            }
+            if (neighCnt > 0) {
+                float norm = 1.; // (1. + )
+                biharmonic[v1] = -delta / neighCnt + laplacian[v1];
+            }
+        });
+
+    ParallelFor<ConcurrencyTag>()(Index(0), Index(mesh.numVertices()), [&mesh, &biharmonic](std::size_t i) {
+        mesh.points[i] += 0.5f * biharmonic[i];
+    });
+}
+
+} // namespace Pvl
diff --git a/OctreeGrid.hpp b/OctreeGrid.hpp
new file mode 100644
index 0000000..bc4333c
--- /dev/null
+++ b/OctreeGrid.hpp
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Vector.hpp"
+#include <memory>
+
+namespace Pvl {
+
+template <typename Point>
+class OctreeGrid {
+    class Node {
+        std::unique_ptr<std::array<Node, 8>> children_;
+    };
+
+    Node root_;
+};
+
+} // namespace Pvl
diff --git a/PlyReader.hpp b/PlyReader.hpp
new file mode 100644
index 0000000..1891d98
--- /dev/null
+++ b/PlyReader.hpp
@@ -0,0 +1,73 @@
+#pragma once
+
+#include "TriangleMesh.hpp"
+#include "Vector.hpp"
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <vector>
+
+namespace Pvl {
+
+class PlyReader {
+    std::istream& in_;
+
+public:
+    PlyReader(std::istream& in)
+        : in_(in) {}
+
+    std::vector<Vec3f> readCloud() {
+        std::string line;
+        // std::string countTag("element vertex ");
+        while (std::getline(in_, line)) {
+            /*   if (line.size() > countTag.size() && line.substr(0, countTag.size()) == countTag) {
+
+               }*/
+            if (line == "end_header") {
+                break;
+            }
+        }
+        std::vector<Vec3f> points;
+        while (std::getline(in_, line)) {
+            std::stringstream ss(line);
+            Vec3f p;
+            ss >> p[0] >> p[1] >> p[2];
+            points.push_back(p);
+        }
+        return points;
+    }
+
+    TriangleMesh<Vec3f, int> readMesh() {
+        std::string line;
+        std::size_t numVertices = 0;
+        std::size_t numFaces = 0;
+        while (std::getline(in_, line)) {
+            sscanf(line.c_str(), "element vertex %d", &numVertices);
+            sscanf(line.c_str(), "element face %d", &numFaces);
+            if (line == "end_header") {
+                break;
+            }
+        }
+        TriangleMesh<Vec3f, int> mesh;
+        std::cout << "Loading mesh with " << numVertices << " vertices and " << numFaces << " faces"
+                  << std::endl;
+        for (std::size_t i = 0; i < numVertices; ++i) {
+            std::getline(in_, line);
+            std::stringstream ss(line);
+            Vec3f p;
+            ss >> p[0] >> p[1] >> p[2];
+            mesh.addVertex();
+            mesh.points.push_back(p);
+        }
+        for (std::size_t i = 0; i < numFaces; ++i) {
+            std::getline(in_, line);
+            std::stringstream ss(line);
+            int dummy, a, b, c;
+            ss >> dummy >> a >> b >> c;
+            mesh.addFace(VertexHandle(a), VertexHandle(b), VertexHandle(c));
+        }
+        return mesh;
+    }
+};
+
+} // namespace Pvl
diff --git a/PlyWriter.hpp b/PlyWriter.hpp
new file mode 100644
index 0000000..e087df2
--- /dev/null
+++ b/PlyWriter.hpp
@@ -0,0 +1,83 @@
+#pragma once
+
+#include "TriangleMesh.hpp"
+#include "Vector.hpp"
+#include <fstream>
+
+namespace Pvl {
+class PlyWriter {
+    std::ostream& out_;
+    std::size_t vertexPos_;
+    std::size_t facePos_;
+
+    bool closed_ = false;
+    std::size_t vertexCnt_ = 0;
+    std::size_t faceCnt_ = 0;
+
+public:
+    PlyWriter(std::ostream& out)
+        : out_(out) {
+        out << "ply\n";
+        out << "format ascii 1.0\n";
+        out << "comment Created by PVL library\n";
+        out << "element vertex ";
+        vertexPos_ = out.tellp();
+        out << "              \n";
+        out << "property float x\n";
+        out << "property float y\n";
+        out << "property float z\n";
+        out << "element face ";
+        facePos_ = out.tellp();
+        out << "              \n";
+        out << "property list uchar int vertex_index\n";
+        out << "end_header\n";
+    }
+
+    template <typename T, int Dim>
+    PlyWriter& operator<<(const Vector<T, Dim>& p) {
+        for (int i = 0; i < Dim; ++i) {
+            out_ << p[i] << " ";
+        }
+        out_ << "\n";
+        ++vertexCnt_;
+        return *this;
+    }
+
+    template <typename T, int Dim>
+    PlyWriter& operator<<(const std::vector<Vector<T, Dim>>& cloud) {
+        for (auto& p : cloud) {
+            *this << p;
+        }
+        return *this;
+    }
+
+    template <typename Vec, typename Index>
+    PlyWriter& operator<<(const TriangleMesh<Vec, Index>& mesh) {
+        for (Index i = 0; i < mesh.numVertices(); ++i) {
+            const Vec& p = mesh.points[i];
+            out_ << p[0] << " " << p[1] << " " << p[2] << "\n";
+        }
+        for (Index i = 0; i < mesh.numFaces(); ++i) {
+            auto f = mesh.faceIndices(FaceHandle(i));
+            out_ << "3 " << f[0] << " " << f[1] << " " << f[2] << "\n";
+        }
+        vertexCnt_ += mesh.numVertices();
+        faceCnt_ += mesh.numFaces();
+        return *this;
+    }
+
+    void close() {
+        if (!closed_) {
+            out_.seekp(vertexPos_);
+            out_ << vertexCnt_;
+            out_.seekp(facePos_);
+            out_ << faceCnt_;
+        }
+        closed_ = true;
+    }
+
+    ~PlyWriter() {
+        close();
+    }
+};
+} // namespace Pvl
diff --git a/Range.hpp b/Range.hpp
new file mode 100644
index 0000000..102127d
--- /dev/null
+++ b/Range.hpp
@@ -0,0 +1,47 @@
+#pragma once
+
+namespace Pvl {
+
+template <typename T, typename TDerived>
+class IterBase {
+protected:
+    T iter_;
+
+public:
+    IterBase(const T& iter)
+        : iter_(iter) {}
+
+    TDerived& operator++() {
+        ++iter_;
+        return static_cast<TDerived&>(*this);
+    }
+
+    bool operator==(const TDerived& other) const {
+        return iter_ == other.iter_;
+    }
+
+    bool operator!=(const TDerived& other) const {
+        return iter_ != other.iter_;
+    }
+};
+
+template <typename TIter>
+class Range {
+    TIter begin_;
+    TIter end_;
+
+public:
+    Range(const TIter& begin, const TIter& end)
+        : begin_(begin)
+        , end_(end) {}
+
+    TIter begin() const {
+        return begin_;
+    }
+
+    TIter end() const {
+        return end_;
+    }
+};
+
+} // namespace Pvl
diff --git a/Svd.hpp b/Svd.hpp
new file mode 100644
index 0000000..00e89b8
--- /dev/null
+++ b/Svd.hpp
@@ -0,0 +1,293 @@
+#include "Matrix.hpp"
+
+namespace Pvl {
+
+template <typename T>
+struct Svd {
+    Matrix<T, 3, 3> U;
+    Vector<T, 3> S;
+    Matrix<T, 3, 3> V;
+};
+
+
+inline double SIGN(double a, double b) {
+    return b >= 0.0 ? fabs(a) : -fabs(a);
+}
+
+inline double PYTHAG(double a, double b) {
+    double at = fabs(a), bt = fabs(b), ct, result;
+
+    if (at > bt) {
+        ct = bt / at;
+        // ASSERT(isReal(ct));
+        result = at * sqrt(1.0 + ct * ct);
+    } else if (bt > 0.0) {
+        ct = at / bt;
+        // ASSERT(isReal(ct));
+        result = bt * sqrt(1.0 + ct * ct);
+    } else {
+        result = 0.0;
+    }
+    return result;
+}
+
+
+int dsvd(float (&a)[3][3], float* w, float (&v)[3][3]) {
+    const int m = 3, n = 3;
+    int flag, i, its, j, jj, k, l, nm;
+    double c, f, h, s, x, y, z;
+    double anorm = 0.0, g = 0.0, scale = 0.0;
+    double* rv1;
+
+    ASSERT(m >= n, "#rows must be > #cols");
+
+    rv1 = (double*)malloc((unsigned int)n * sizeof(double));
+
+    /* Householder reduction to bidiagonal form */
+    for (i = 0; i < n; i++) {
+        /* left-hand reduction */
+        l = i + 1;
+        rv1[i] = scale * g;
+        g = s = scale = 0.0;
+        if (i < m) {
+            for (k = i; k < m; k++)
+                scale += fabs((double)a[k][i]);
+            if (scale) {
+                for (k = i; k < m; k++) {
+                    a[k][i] = (float)((double)a[k][i] / scale);
+                    s += ((double)a[k][i] * (double)a[k][i]);
+                }
+                ASSERT(s >= 0., s);
+                f = (double)a[i][i];
+                g = -SIGN(sqrt(s), f);
+                h = f * g - s;
+                a[i][i] = (float)(f - g);
+                if (i != n - 1) {
+                    for (j = l; j < n; j++) {
+                        for (s = 0.0, k = i; k < m; k++)
+                            s += ((double)a[k][i] * (double)a[k][j]);
+                        f = s / h;
+                        for (k = i; k < m; k++)
+                            a[k][j] += (float)(f * (double)a[k][i]);
+                    }
+                }
+                for (k = i; k < m; k++)
+                    a[k][i] = (float)((double)a[k][i] * scale);
+            }
+        }
+        w[i] = (float)(scale * g);
+
+        /* right-hand reduction */
+        g = s = scale = 0.0;
+        if (i < m && i != n - 1) {
+            for (k = l; k < n; k++)
+                scale += fabs((double)a[i][k]);
+            if (scale) {
+                for (k = l; k < n; k++) {
+                    a[i][k] = (float)((double)a[i][k] / scale);
+                    s += ((double)a[i][k] * (double)a[i][k]);
+                }
+                ASSERT(s >= 0., s);
+                f = (double)a[i][l];
+                g = -SIGN(sqrt(s), f);
+                h = f * g - s;
+                a[i][l] = (float)(f - g);
+                for (k = l; k < n; k++)
+                    rv1[k] = (double)a[i][k] / h;
+                if (i != m - 1) {
+                    for (j = l; j < m; j++) {
+                        for (s = 0.0, k = l; k < n; k++)
+                            s += ((double)a[j][k] * (double)a[i][k]);
+                        for (k = l; k < n; k++)
+                            a[j][k] += (float)(s * rv1[k]);
+                    }
+                }
+                for (k = l; k < n; k++)
+                    a[i][k] = (float)((double)a[i][k] * scale);
+            }
+        }
+        anorm = std::max(anorm, (fabs((double)w[i]) + fabs(rv1[i])));
+    }
+
+    /* accumulate the right-hand transformation */
+    for (i = n - 1; i >= 0; i--) {
+        if (i < n - 1) {
+            if (g) {
+                for (j = l; j < n; j++)
+                    v[j][i] = (float)(((double)a[i][j] / (double)a[i][l]) / g);
+                /* double division to avoid underflow */
+                for (j = l; j < n; j++) {
+                    for (s = 0.0, k = l; k < n; k++)
+                        s += ((double)a[i][k] * (double)v[k][j]);
+                    for (k = l; k < n; k++)
+                        v[k][j] += (float)(s * (double)v[k][i]);
+                }
+            }
+            for (j = l; j < n; j++)
+                v[i][j] = v[j][i] = 0.0;
+        }
+        v[i][i] = 1.0;
+        g = rv1[i];
+        l = i;
+    }
+
+    /* accumulate the left-hand transformation */
+    for (i = n - 1; i >= 0; i--) {
+        l = i + 1;
+        g = (double)w[i];
+        if (i < n - 1)
+            for (j = l; j < n; j++)
+                a[i][j] = 0.0;
+        if (g) {
+            g = 1.0 / g;
+            if (i != n - 1) {
+                for (j = l; j < n; j++) {
+                    for (s = 0.0, k = l; k < m; k++)
+                        s += ((double)a[k][i] * (double)a[k][j]);
+                    f = (s / (double)a[i][i]) * g;
+                    for (k = i; k < m; k++)
+                        a[k][j] += (float)(f * (double)a[k][i]);
+                }
+            }
+            for (j = i; j < m; j++)
+                a[j][i] = (float)((double)a[j][i] * g);
+        } else {
+            for (j = i; j < m; j++)
+                a[j][i] = 0.0;
+        }
+        ++a[i][i];
+    }
+
+    /* diagonalize the bidiagonal form */
+    for (k = n - 1; k >= 0; k--) {       /* loop over singular values */
+        for (its = 0; its < 30; its++) { /* loop over allowed iterations */
+            flag = 1;
+            for (l = k; l >= 0; l--) { /* test for splitting */
+                nm = l - 1;
+                if (fabs(rv1[l]) + anorm == anorm) {
+                    flag = 0;
+                    break;
+                }
+                if (fabs((double)w[nm]) + anorm == anorm)
+                    break;
+            }
+            if (flag) {
+                c = 0.0;
+                s = 1.0;
+                for (i = l; i <= k; i++) {
+                    f = s * rv1[i];
+                    if (fabs(f) + anorm != anorm) {
+                        g = (double)w[i];
+                        h = PYTHAG(f, g);
+                        w[i] = (float)h;
+                        h = 1.0 / h;
+                        c = g * h;
+                        s = (-f * h);
+                        for (j = 0; j < m; j++) {
+                            y = (double)a[j][nm];
+                            z = (double)a[j][i];
+                            a[j][nm] = (float)(y * c + z * s);
+                            a[j][i] = (float)(z * c - y * s);
+                        }
+                    }
+                }
+            }
+            z = (double)w[k];
+            if (l == k) {      /* convergence */
+                if (z < 0.0) { /* make singular value nonnegative */
+                    w[k] = (float)(-z);
+                    for (j = 0; j < n; j++)
+                        v[j][k] = (-v[j][k]);
+                }
+                break;
+            }
+            ASSERT(its < 30, "No convergence after 30,000! iterations ");
+
+
+            /* shift from bottom 2 x 2 minor */
+            x = (double)w[l];
+            nm = k - 1;
+            y = (double)w[nm];
+            g = rv1[nm];
+            h = rv1[k];
+            f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
+            g = PYTHAG(f, 1.0);
+            f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
+
+            /* next QR transformation */
+            c = s = 1.0;
+            for (j = l; j <= nm; j++) {
+                i = j + 1;
+                g = rv1[i];
+                y = (double)w[i];
+                h = s * g;
+                g = c * g;
+                z = PYTHAG(f, h);
+                rv1[j] = z;
+                c = f / z;
+                s = h / z;
+                f = x * c + g * s;
+                g = g * c - x * s;
+                h = y * s;
+                y = y * c;
+                for (jj = 0; jj < n; jj++) {
+                    x = (double)v[jj][j];
+                    z = (double)v[jj][i];
+                    v[jj][j] = (float)(x * c + z * s);
+                    v[jj][i] = (float)(z * c - x * s);
+                }
+                z = PYTHAG(f, h);
+                w[j] = (float)z;
+                if (z) {
+                    z = 1.0 / z;
+                    c = f * z;
+                    s = h * z;
+                }
+                f = (c * g) + (s * y);
+                x = (c * y) - (s * g);
+                for (jj = 0; jj < m; jj++) {
+                    y = (double)a[jj][j];
+                    z = (double)a[jj][i];
+                    a[jj][j] = (float)(y * c + z * s);
+                    a[jj][i] = (float)(z * c - y * s);
+                }
+            }
+            rv1[l] = 0.0;
+            rv1[k] = f;
+            w[k] = (float)x;
+        }
+    }
+    free((void*)rv1);
+    return (1);
+}
+
+
+template <typename T>
+Svd<T> singularValueDecomposition(const Matrix<T, 3, 3>& t) {
+    Vector<T, 3> diag = t.diagonal();
+    Vector<T, 3> off = t.offDiagonal();
+    float V[3][3];
+    float S[3];
+
+    // clang-format off
+    float U[3][3] = {
+        { float(diag[0]), float(off[0]),  float(off[1]) },
+        { float(off[0]),  float(diag[1]), float(off[2]) },
+        { float(off[1]),  float(off[2]),  float(diag[2]) },
+    };
+
+    dsvd(U, S, V);
+
+    Svd<T> result;
+    result.S = Vector<T, 3>(S[0], S[1], S[2]);
+    result.U = Matrix<T, 3, 3>(Vector<T, 3>(U[0][0], U[0][1], U[0][2]),
+                               Vector<T, 3>(U[1][0], U[1][1], U[1][2]),
+                               Vector<T, 3>(U[2][0], U[2][1], U[2][2]));
+    result.V = Matrix<T, 3, 3>(Vector<T, 3>(V[0][0], V[0][1], V[0][2]),
+                               Vector<T, 3>(V[1][0], V[1][1], V[1][2]),
+                               Vector<T, 3>(V[2][0], V[2][1], V[2][2]));
+    // clang-format on
+    return result;
+}
+
+} // namespace Pvl
diff --git a/TriangleMesh.hpp b/TriangleMesh.hpp
new file mode 100644
index 0000000..64f9718
--- /dev/null
+++ b/TriangleMesh.hpp
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "Graph.hpp"
+#include "Vector.hpp"
+#include <vector>
+
+namespace Pvl {
+
+template <typename Vec, typename Index>
+class TriangleMesh : public Graph {
+public:
+    using Float = typename Vec::Type;
+    static constexpr int Dim = Vec::size();
+
+public:
+    TriangleMesh() = default;
+
+    std::vector<Vec> points;
+};
+
+} // namespace Pvl
diff --git a/UniformGrid.hpp b/UniformGrid.hpp
new file mode 100644
index 0000000..8b74563
--- /dev/null
+++ b/UniformGrid.hpp
@@ -0,0 +1,112 @@
+#pragma once
+#include "Vector.hpp"
+#include <vector>
+
+namespace Pvl {
+
+template <typename Object, int Dim>
+class GridIterator {
+public:
+    using Idxs = Vector<int, Dim>;
+
+private:
+    Object* ptr_;
+    std::size_t index_;
+    Idxs dims_;
+
+
+public:
+    GridIterator(Object* ptr, const std::size_t index, const Idxs& dims)
+        : ptr_(ptr)
+        , index_(index)
+        , dims_(dims) {}
+
+    GridIterator& operator++() {
+        ptr_++;
+        index_++;
+        return *this;
+    }
+    Object& operator*() const {
+        return *ptr_;
+    }
+    bool operator==(const GridIterator& other) const {
+        return ptr_ == other.ptr_;
+    }
+    bool operator!=(const GridIterator& other) const {
+        return ptr_ != other.ptr_;
+    }
+
+    Idxs idxs() const {
+        Idxs c;
+        std::size_t k = index_;
+        for (int i = 0; i < Dim; ++i) {
+            c[i] = k % dims_[i];
+            k /= dims_[i];
+        }
+    }
+};
+
+template <typename Object, int Dim>
+class UniformGrid {
+public:
+    // using Point = Point;
+    using Idxs = Vector<int, Dim>;
+
+public:
+    std::vector<Object> data_;
+    Idxs dims_;
+
+public:
+    UniformGrid() = default;
+
+    UniformGrid(const Idxs& dims)
+        : dims_(dims) {
+        data_.resize(voxelCount());
+    }
+
+    Object& operator()(const Idxs& idxs) {
+        return data_[map(idxs)];
+    }
+
+    const Object& operator()(const Idxs& idxs) const {
+        return data_[map(idxs)];
+    }
+
+    std::size_t voxelCount() const {
+        std::size_t cnt = 1;
+        for (int i = 0; i < Dim; ++i) {
+            cnt *= dims_[i];
+        }
+        return cnt;
+    }
+
+    GridIterator<Object, Dim> begin() {
+        return { data_.data(), 0, dims_ };
+    }
+
+    GridIterator<Object, Dim> end() {
+        return { data_.data() + data_.size(), 0, dims_ };
+    }
+
+    GridIterator<const Object, Dim> begin() const {
+        return { data_.data(), 0, dims_ };
+    }
+
+    GridIterator<const Object, Dim> end() const {
+        return { data_.data() + data_.size(), 0, dims_ };
+    }
+
+private:
+    std::size_t map(const Idxs& idxs) const {
+        std::size_t linear = 0;
+        std::size_t stride = 1;
+        for (int i = 0; i < Dim; ++i) {
+            linear += idxs[i] * stride;
+            stride *= dims_[i];
+        }
+        return linear;
+    }
+};
+
+
+} // namespace Pvl
diff --git a/Utils.hpp b/Utils.hpp
new file mode 100644
index 0000000..d1893a0
--- /dev/null
+++ b/Utils.hpp
@@ -0,0 +1,70 @@
+#pragma once
+#include <tbb/tbb.h>
+
+namespace Pvl {
+
+struct Noncopyable {
+    Noncopyable() = default;
+    Noncopyable(const Noncopyable&) = delete;
+    Noncopyable(Noncopyable&&) = delete;
+    Noncopyable& operator=(const Noncopyable&) = delete;
+    Noncopyable& operator=(Noncopyable&&) = delete;
+};
+
+struct ParallelTag {};
+struct SequentialTag {};
+
+template <typename Tag>
+struct ParallelFor;
+
+template <>
+struct ParallelFor<SequentialTag> {
+    template <typename Index, typename Func>
+    void operator()(Index n1, Index n2, const Func& func) {
+        for (Index n = n1; n < n2; ++n) {
+            func(n);
+        }
+    }
+};
+
+template <>
+struct ParallelFor<ParallelTag> {
+    template <typename Index, typename Func>
+    void operator()(Index n1, Index n2, const Func& func) {
+        tbb::parallel_for(n1, n2, func);
+    }
+};
+
+
+template <typename Tag>
+struct ParallelForEach;
+
+template <>
+struct ParallelForEach<SequentialTag> {
+    template <typename Iter, typename Func>
+    void operator()(Iter from, Iter to, const Func& func) {
+        for (Iter iter = from; iter != to; ++iter) {
+            func(*iter);
+        }
+    }
+    template <typename Range, typename Func>
+    void operator()(const Range& range, const Func& func) {
+        for (auto&& value : range) {
+            func(value);
+        }
+    }
+};
+
+template <>
+struct ParallelForEach<ParallelTag> {
+    template <typename Iter, typename Func>
+    void operator()(Iter from, Iter to, const Func& func) {
+        tbb::parallel_for_each(from, to, func);
+    }
+    template <typename Range, typename Func>
+    void operator()(const Range& range, const Func& func) {
+        tbb::parallel_for_each(range, func);
+    }
+};
+
+} // namespace Pvl
diff --git a/Vector.hpp b/Vector.hpp
new file mode 100644
index 0000000..54d22a3
--- /dev/null
+++ b/Vector.hpp
@@ -0,0 +1,248 @@
+#pragma once
+
+#include "Assert.hpp"
+#include "Math.hpp"
+#include <array>
+
+namespace Pvl {
+
+template <typename T, int Dim>
+class Vector {
+    std::array<T, Dim> values_;
+
+public:
+    using Type = T;
+
+    Vector() = default;
+
+    Vector(const T value) {
+        for (int i = 0; i < Dim; ++i) {
+            values_[i] = value;
+        }
+    }
+
+    Vector(const T x, const T y) /// \todo
+        : values_{ x, y } {}
+
+    Vector(const T x, const T y, const T z)
+        : values_{ x, y, z } {}
+
+    Vector(const T x, const T y, const T z, const T w)
+        : values_{ x, y, z, w } {}
+
+    T& operator[](const int idx) {
+        ASSERT(unsigned(idx) < unsigned(Dim), idx, Dim);
+        return values_[idx];
+    }
+
+    const T& operator[](const int idx) const {
+        ASSERT(unsigned(idx) < unsigned(Dim), idx, Dim);
+        return values_[idx];
+    }
+
+    Vector operator-() const {
+        Vector res;
+        for (int i = 0; i < Dim; ++i) {
+            res[i] = -values_[i];
+        }
+        return res;
+    }
+
+    bool operator==(const Vector& other) const {
+        for (int i = 0; i < Dim; ++i) {
+            if (values_[i] != other[i]) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    bool operator!=(const Vector& other) const {
+        return !(*this == other);
+    }
+
+    Vector& operator+=(const Vector& other) {
+        *this = *this + other;
+        return *this;
+    }
+
+    Vector& operator-=(const Vector& other) {
+        *this = *this - other;
+        return *this;
+    }
+
+    Vector operator+(const Vector& other) const {
+        Vector res;
+        for (int i = 0; i < Dim; ++i) {
+            res[i] = values_[i] + other[i];
+        }
+        return res;
+    }
+
+    Vector operator-(const Vector& other) const {
+        Vector res;
+        for (int i = 0; i < Dim; ++i) {
+            res[i] = values_[i] - other[i];
+        }
+        return res;
+    }
+
+    Vector operator*(const T f) const {
+        Vector res;
+        for (int i = 0; i < Dim; ++i) {
+            res[i] = values_[i] * f;
+        }
+        return res;
+    }
+
+    Vector operator/(const T f) const {
+        Vector res;
+        for (int i = 0; i < Dim; ++i) {
+            res[i] = values_[i] / f;
+        }
+        return res;
+    }
+
+    friend Vector operator*(const T f, const Vector& v) {
+        return v * f;
+    }
+
+    static constexpr int size() {
+        return Dim;
+    }
+
+    T* begin() {
+        return values_.begin();
+    }
+
+    T* end() {
+        return values_.end();
+    }
+
+    const T* begin() const {
+        return values_.begin();
+    }
+
+    const T* end() const {
+        return values_.end();
+    }
+
+    const T* data() const {
+        return values_.data();
+    }
+}; // namespace Universe
+
+using Vec2f = Vector<float, 2>;
+using Vec3f = Vector<float, 3>;
+using Vec4f = Vector<float, 4>;
+
+using Vec2i = Vector<int, 2>;
+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 res = 0.f;
+    for (int i = 0; i < Dim; ++i) {
+        res += v1[i] * v2[i];
+    }
+    return res;
+}
+
+template <typename T, int Dim>
+T normSqr(const Vector<T, Dim>& v) {
+    return dot(v, v);
+}
+
+template <typename T, int Dim>
+T norm(const Vector<T, Dim>& v) {
+    return sqrt(normSqr(v));
+}
+
+template <typename T, int Dim>
+T normL1(const Vector<T, Dim>& v) {
+    T sum = T(0);
+    for (int i = 0; i < Dim; ++i) {
+        sum += std::abs(v[i]);
+    }
+    return sum;
+}
+
+template <typename T, int Dim>
+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]);
+}
+
+template <typename T1, typename T2, int Dim>
+Vector<T1, Dim> vectorCast(const Vector<T2, Dim>& v) {
+    Vector<T1, Dim> res;
+    for (int i = 0; i < Dim; ++i) {
+        res[i] = static_cast<T1>(v[i]);
+    }
+    return res;
+}
+
+template <typename T, int Dim>
+Vector<T, Dim> max(const Vector<T, Dim>& v1, const Vector<T, Dim>& v2) {
+    Vector<T, Dim> res;
+    for (int i = 0; i < Dim; ++i) {
+        res[i] = std::max(v1[i], v2[i]);
+    }
+    return res;
+}
+
+template <typename T, int Dim>
+Vector<T, Dim> min(const Vector<T, Dim>& v1, const Vector<T, Dim>& v2) {
+    Vector<T, Dim> res;
+    for (int i = 0; i < Dim; ++i) {
+        res[i] = std::min(v1[i], v2[i]);
+    }
+    return res;
+}
+
+template <typename T, int Dim>
+int argMax(const Vector<T, Dim>& v) {
+    return int(std::max_element(v.begin(), v.end()) - v.begin());
+}
+
+template <typename T, int Dim>
+int argMin(const Vector<T, Dim>& v) {
+    return int(std::min_element(v.begin(), v.end()) - v.begin());
+}
+
+template <typename T, int Dim>
+Vector<T, Dim> floor(const Vector<T, Dim>& v) {
+    Vector<T, Dim> res;
+    for (int i = 0; i < Dim; ++i) {
+        res[i] = std::floor(v[i]);
+    }
+    return res;
+}
+
+template <typename T, int Dim>
+Vector<T, Dim> round(const Vector<T, Dim>& v) {
+    Vector<T, Dim> res;
+    for (int i = 0; i < Dim; ++i) {
+        res[i] = std::round(v[i]);
+    }
+    return res;
+}
+
+template <typename T, int Dim>
+Vector<T, Dim> sqr(const Vector<T, Dim>& v) {
+    Vector<T, Dim> res;
+    for (int i = 0; i < Dim; ++i) {
+        res[i] = Pvl::sqr(v[i]);
+    }
+    return res;
+}
+
+inline Vec4f homogeneous(const Vec3f& v) {
+    return Vec4f(v[0], v[1], v[2], 1.f);
+}
+
+} // namespace Pvl