From e3495cb4df1a566395ffda1fbaf6f061f89d3bca Mon Sep 17 00:00:00 2001 From: pavel Date: Wed, 24 Jun 2020 22:08:07 +0200 Subject: [PATCH] simplification --- Assert.hpp | 2 +- Box.hpp | 56 +-- CMakeLists.txt | 14 +- Cloud.hpp | 60 +++- CloudUtils.hpp | 217 +++++++++++ Graph.hpp | 504 +++++++++++++++++++++++--- Graph.inl.hpp | 29 +- KdTree.hpp | 614 ++++++++++++++++++++++++++++++++ KdTree.inl.hpp | 520 +++++++++++++++++++++++++++ Kernels.hpp | 24 +- MarchingCubes.cpp | 293 +++++++++++++++ Math.hpp | 10 + Matrix.hpp | 56 ++- PlyWriter.hpp | 29 +- MeshUtils.hpp => Refinement.hpp | 45 +-- Simplification.hpp | 212 +++++++++++ Svd.hpp | 18 +- TriangleMesh.hpp | 8 +- Vector.hpp | 18 +- main.cpp | 435 ++++++++++++++++++++++ 20 files changed, 3029 insertions(+), 135 deletions(-) create mode 100644 CloudUtils.hpp create mode 100644 KdTree.hpp create mode 100644 KdTree.inl.hpp create mode 100644 MarchingCubes.cpp rename MeshUtils.hpp => Refinement.hpp (50%) create mode 100644 Simplification.hpp create mode 100644 main.cpp diff --git a/Assert.hpp b/Assert.hpp index ca5f5ce..75b2df8 100644 --- a/Assert.hpp +++ b/Assert.hpp @@ -2,4 +2,4 @@ #include -#define ASSERT(x, ...) assert(x) +#define PVL_ASSERT(x, ...) assert(x) diff --git a/Box.hpp b/Box.hpp index 413f2ce..56cc34d 100644 --- a/Box.hpp +++ b/Box.hpp @@ -4,47 +4,49 @@ namespace Pvl { -template +template class BoundingBox { - Vector lower_; - Vector upper_; + Vec lower_; + Vec upper_; public: + using Vector = Vec; + using Float = typename Vec::Float; + BoundingBox() - : lower_(std::numeric_limits::max()) - , upper_(std::numeric_limits::lowest()) {} + : lower_(std::numeric_limits::max()) + , upper_(std::numeric_limits::lowest()) {} - BoundingBox(const Vector& lower, const Vector& upper) + BoundingBox(const Vec& lower, const Vec& upper) : lower_(lower) , upper_(upper) {} - Vector& lower() { + Vec& lower() { return lower_; } - const Vector& lower() const { + const Vec& lower() const { return lower_; } - Vector& upper() { + Vec& upper() { return upper_; } - const Vector& upper() const { + const Vec& upper() const { return upper_; } - Vector size() const { + Vec size() const { return upper_ - lower_; } - Vector center() const { - return T(0.5) * (upper_ + lower_); + Vec center() const { + return Float(0.5) * (upper_ + lower_); } - - bool contains(const Vector& p) const { - for (int i = 0; i < Dim; ++i) { + bool contains(const Vec& p) const { + for (int i = 0; i < Vec::size(); ++i) { if (p[i] < lower_[i] || p[i] > upper_[i]) { return false; } @@ -52,26 +54,26 @@ class BoundingBox { return true; } - void extend(const Vector& p) { + void extend(const Vec& p) { lower_ = min(lower_, p); upper_ = max(upper_, p); } }; +using Box3f = BoundingBox; + /// \brief Splits the box along given coordinate. /// /// The splitting plane must pass through the box. -template -std::pair, BoundingBox> splitBox(const BoundingBox& box, - const int dim, - const T x) { +template +std::pair splitBox(const Box& box, const int dim, const T x) { /*ASSERT(isValid());*/ - ASSERT(dim < Dim); - ASSERT(x >= box.lower()[dim] && x <= box.upper()[dim]); - BoundingBox b1 = box; - BoundingBox b2 = box; - b1.lower()[dim] = x; - b2.upper()[dim] = x; + PVL_ASSERT(dim < Box::Vector::size()); + PVL_ASSERT(x >= box.lower()[dim] && x <= box.upper()[dim]); + Box b1 = box; + Box b2 = box; + b1.upper()[dim] = x; + b2.lower()[dim] = x; return std::make_pair(b1, b2); } diff --git a/CMakeLists.txt b/CMakeLists.txt index e5dc210..f95739d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,8 +5,16 @@ project(pvl LANGUAGES CXX) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") + find_package(TBB REQUIRED tbb) +find_package(Catch2 REQUIRED catch2) + +#find_package( Boost 1.40 COMPONENTS bgl REQUIRED ) +#include_directories( ${Boost_INCLUDE_DIR} ) +include_directories("/usr/include/catch2/") + add_executable(pvl main.cpp Utils.hpp @@ -16,12 +24,14 @@ add_executable(pvl Matrix.hpp Svd.hpp Box.hpp - KdTree.h KdTree.inl.h + KdTree.hpp KdTree.inl.hpp PlyWriter.hpp PlyReader.hpp Cloud.hpp + CloudUtils.hpp Kernels.hpp TriangleMesh.hpp - MeshUtils.hpp + Refinement.hpp + Simplification.hpp Range.hpp Graph.hpp Graph.inl.hpp UniformGrid.hpp diff --git a/Cloud.hpp b/Cloud.hpp index 0abadad..dfc7a1b 100644 --- a/Cloud.hpp +++ b/Cloud.hpp @@ -1,3 +1,61 @@ #pragma once -namespace Pvl {} +#include "Box.hpp" +#include + +namespace Pvl { + +template +class CloudProperty { +private: + std::vector values_; + +public: +}; + + +template +class PointInsideBoxPredicate { + BoundingBox box_; + +public: + bool operator()(const Vec& p) const { + return box_.contains(p); + } +}; + +template +class SubsetIterator { + Iterator iter_; + Predicate predicate_; + +public: + SubsetIterator(const Predicate& predicate) + : predicate_(predicate) {} + + SubsetIterator& operator++() { + do { + ++iter_; + } while (!predicate_()); + return *this; + } + + bool operator!=(const SubsetIterator& other) const { + return iter_ != other.iter_; + } +}; + +template +class CloudSubset { +public: + /*SubsetIterator begin() {} + + SubsetIterator end() {}*/ +}; + +template +class Cloud : public Properties... {}; + +// using MyCloud = Cloud; + +} // namespace Pvl diff --git a/CloudUtils.hpp b/CloudUtils.hpp new file mode 100644 index 0000000..cb7bcff --- /dev/null +++ b/CloudUtils.hpp @@ -0,0 +1,217 @@ +#pragma once + +#include "KdTree.hpp" +#include "Matrix.hpp" +#include "PlyWriter.hpp" +#include "Svd.hpp" +#include + +namespace Pvl { + +template +Box3f boundingBox(const Cloud& cloud) { + Box3f box; + for (const auto& p : cloud) { + box.extend(p); + } + return box; +} + +template +Vec3f centroid(const Cloud& cloud) { + Vec3f pos(0.); + std::size_t count = 0; + for (const auto& p : cloud) { + pos += p; + count++; + } + return pos / count; +} + +template +std::vector estimateNormals(Cloud& cloud) { + KdTree tree; + tree.build(cloud); + std::vector normals(cloud.size()); + + for (std::size_t i = 0; i < cloud.size(); ++i) { + const Vec3f& p = cloud[i]; + float radius = 0.01; + std::vector neighs; + do { + neighs.clear(); + tree.rangeQuery(p, radius, std::back_inserter(neighs)); + radius *= 2.f; + } while (neighs.size() < 20); + + Vec3f centroid(0.); + for (int j : neighs) { + centroid += cloud[j]; + } + centroid /= neighs.size(); + Mat33f cov = Mat33f::null(); + for (int j : neighs) { + Vec3f diff = cloud[j] - centroid; + cov += outerProd(diff, diff); + } + Svd svd = singularValueDecomposition(cov); + + // 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])); + } + return normals; +} + +Vector join(Vec3f v, Vec3f n) { + Vector w; + w[0] = v[0]; + w[1] = v[1]; + w[2] = v[2]; + w[3] = n[0]; + w[4] = n[1]; + w[5] = n[2]; + return w; +} + +template +void orientNormals(Cloud& cloud, Normals& normals) { + // get consistently oriented patches + + const std::size_t unassigned = std::size_t(-1); + std::vector indices(cloud.size(), unassigned); + std::size_t componentIdx = 0; + + std::vector stack; + std::vector neighs; + + KdTree tree; + tree.build(cloud); + + for (std::size_t i = 0; i < cloud.size(); ++i) { + if (indices[i] == unassigned) { + indices[i] = componentIdx; + stack.push_back(i); + // find new neigbours recursively until we find all particles in the component + while (!stack.empty()) { + const std::size_t n1 = stack.back(); + stack.pop_back(); + + const Vec3f& p = cloud[n1]; + float radius = 0.001; + std::vector neighs; + do { + neighs.clear(); + tree.rangeQuery(p, radius, std::back_inserter(neighs)); + radius *= 1.5f; + } while (neighs.size() < 5); + + for (std::size_t n2 : neighs) { + if (n1 == n2) { + 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) { + // do not count as neighbours + continue; + } + if (indices[n2] == unassigned) { + indices[n2] = componentIdx; + stack.push_back(n2); + } + } + } + componentIdx++; + } + } + + // declare component 0 as correct + for (int c = 1; c < componentIdx; ++c) { + int votes = 0; + for (std::size_t i = 0; i < cloud.size(); ++i) { + if (indices[i] != c) { + continue; + } + const Vec3f& p = cloud[i]; + float radius = 0.001; + std::vector neighs; + do { + neighs.clear(); + tree.rangeQuery(p, radius, std::back_inserter(neighs)); + radius *= 1.5f; + } while (neighs.size() < 20); + 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)); + } + } + } + if (votes >= 0) { + continue; + } + for (std::size_t i = 0; i < cloud.size(); ++i) { + if (indices[i] != c) { + continue; + } + normals[i] *= -1; + } + } + +#if 0 + KdTree tree; + tree.build(cloud); + + std::set visited; + std::queue queue; + queue.push(0); + while (!queue.empty()) { + std::cout << "queue size: " << queue.size() << std::endl; + std::cout << "visited size: " << visited.size() << std::endl; + std::size_t i = queue.front(); + queue.pop(); + + const Vec3f& p = cloud[i]; + float radius = 0.003; + std::vector neighs; + do { + neighs.clear(); + tree.rangeQuery(p, radius, std::back_inserter(neighs)); + radius *= 2.f; + } while (neighs.size() < 20); + + int votes = 0; + for (int j : neighs) { + votes += sign(dot(normals[i], normals[j])); + if (visited.find(j) == visited.end()) { + queue.push(j); + visited.insert(j); + } + } + normals[i] *= sign(votes); + } +#endif + +#if 0 + Vec3f center = centroid(cloud); + std::vector cameras{ + center + Vec3f(-100., 0, 0), + center + Vec3f(100., 0, 0), + center + Vec3f(0., -100, 0), + center + Vec3f(0., 100, 0), + center + Vec3f(0., 0., -100), + center + Vec3f(0., 0., 100), + }; + for (std::size_t i = 0; i < cloud.size(); ++i) { + const Vec3f camera = + *std::min_element(cameras.begin(), cameras.end(), [&](const Vec3f& cam1, const Vec3f& cam2) { + return normSqr(cam1 - cloud[i]) < normSqr(cam2 - cloud[i]); + }); + normals[i] *= sign(dot(normals[i], camera - cloud[i])); + } +#endif +} + +} // namespace Pvl diff --git a/Graph.hpp b/Graph.hpp index 238b2ab..904896a 100644 --- a/Graph.hpp +++ b/Graph.hpp @@ -26,6 +26,11 @@ class Handle { explicit Handle(const Index idx) : idx_(idx) {} + Handle& operator++() { + ++idx_; + return *this; + } + Index index() const { return idx_; } @@ -38,11 +43,13 @@ class Handle { class HalfEdge; class Vertex; class Face; +class Edge; class Graph; using HalfEdgeHandle = Handle; using VertexHandle = Handle; using FaceHandle = Handle; +using EdgeHandle = Handle; struct HalfEdge { using Handle = HalfEdgeHandle; @@ -53,19 +60,26 @@ struct HalfEdge { HalfEdgeHandle next; HalfEdgeHandle prev; - HalfEdge(VertexHandle to) - : to(to) {} + : to(to) { + left = FaceHandle(-1); + } bool boundary() const { return opposite < 0; } + + /* bool removed() const { + return left < 0; + }*/ }; struct Vertex { using Handle = VertexHandle; - HalfEdgeHandle edge; // emanating vertex + // boundary emanating edge for boundary vertices, any emanating vertex for internal + // vertices + HalfEdgeHandle edge; Vertex() { edge = HalfEdgeHandle(-1); @@ -74,9 +88,9 @@ struct Vertex { Vertex(HalfEdgeHandle eh) : edge(eh) {} - bool valid() const { + /*bool valid() const { return edge >= 0; - } + }*/ struct EndTag {}; @@ -105,6 +119,12 @@ struct Vertex { public: using IteratorBase::IteratorBase; + using iterator_category = std::forward_iterator_tag; + using value_type = HalfEdgeHandle; + using difference_type = int; + using reference = HalfEdgeHandle&; + using pointer = HalfEdgeHandle*; + HalfEdgeHandle operator*() const; }; @@ -112,6 +132,12 @@ struct Vertex { public: using IteratorBase::IteratorBase; + using iterator_category = std::forward_iterator_tag; + using value_type = VertexHandle; + using difference_type = int; + using reference = VertexHandle&; + using pointer = VertexHandle*; + VertexHandle operator*() const; }; @@ -119,11 +145,33 @@ struct Vertex { public: using IteratorBase::IteratorBase; + using iterator_category = std::forward_iterator_tag; + using value_type = FaceHandle; + using difference_type = int; + using reference = FaceHandle&; + using pointer = FaceHandle*; + FaceHandle operator*() const; }; + class EdgeIterator : public IteratorBase { + public: + using IteratorBase::IteratorBase; + + using iterator_category = std::forward_iterator_tag; + using value_type = EdgeHandle; + using difference_type = int; + using reference = EdgeHandle&; + using pointer = EdgeHandle*; + + EdgeIterator& operator++(); + + EdgeHandle operator*() const; + }; + using HalfEdgeRange = Range; + using EdgeRange = Range; using VertexRange = Range; using FaceRange = Range; }; @@ -133,11 +181,16 @@ static_assert(sizeof(Vertex) == sizeof(Handle), "error"); struct Face { HalfEdgeHandle edge; - Face() = default; + Face() + : edge(-1) {} Face(HalfEdgeHandle eh) : edge(eh) {} + /* bool valid() const { + return edge >= 0; + }*/ + struct EndTag {}; class IteratorBase { @@ -160,6 +213,12 @@ struct Face { public: using IteratorBase::IteratorBase; + using iterator_category = std::forward_iterator_tag; + using value_type = HalfEdgeHandle; + using difference_type = int; + using reference = HalfEdgeHandle&; + using pointer = HalfEdgeHandle*; + HalfEdgeHandle operator*() const; }; @@ -167,11 +226,23 @@ struct Face { public: using IteratorBase::IteratorBase; + using iterator_category = std::forward_iterator_tag; + using value_type = VertexHandle; + using difference_type = int; + using reference = VertexHandle&; + using pointer = VertexHandle*; + VertexHandle operator*() const; }; class FaceIterator : public IteratorBase { public: + using iterator_category = std::forward_iterator_tag; + using value_type = FaceHandle; + using difference_type = int; + using reference = FaceHandle&; + using pointer = FaceHandle*; + FaceIterator(const Graph& graph, FaceHandle fh); FaceIterator(const Graph& graph, FaceHandle fh, EndTag); @@ -187,54 +258,134 @@ struct Face { using FaceRange = Range; }; +struct Edge {}; + class Graph { friend class Vertex; friend class Face; std::vector vertices_; std::vector faces_; - std::vector edges_; + std::vector halfEdges_; // maps vertex to the set of emanating boundary edges std::map> boundaryEdges_; - public: HalfEdgeHandle next(HalfEdgeHandle eh) const { - return edges_[eh].next; + PVL_ASSERT(valid(eh)); + return halfEdges_[eh].next; } HalfEdgeHandle prev(HalfEdgeHandle eh) const { - return edges_[eh].prev; + PVL_ASSERT(valid(eh)); + return halfEdges_[eh].prev; } HalfEdgeHandle opposite(HalfEdgeHandle eh) const { - ASSERT(!boundary(eh)); - return edges_[eh].opposite; + PVL_ASSERT(valid(eh)); + PVL_ASSERT(!boundary(eh)); + return halfEdges_[eh].opposite; } bool boundary(HalfEdgeHandle eh) const { - return edges_[eh].boundary(); + PVL_ASSERT(valid(eh)); + return halfEdges_[eh].boundary(); + } + bool boundary(EdgeHandle eh) const { + PVL_ASSERT(valid(eh)); + return boundary(halfEdge(eh)); } bool boundary(VertexHandle vh) const { + PVL_ASSERT(valid(vh)); return boundary(vertices_[vh].edge); } VertexHandle to(HalfEdgeHandle eh) const { - return edges_[eh].to; + if (!valid(eh)) { + std::cout << "Invalid he handle " << halfEdges_[halfEdges_[eh].prev].to << "-" + << halfEdges_[eh].to << std::endl; + } + PVL_ASSERT(valid(eh)); + return halfEdges_[eh].to; } VertexHandle from(HalfEdgeHandle eh) const { - return edges_[prev(eh)].to; + PVL_ASSERT(valid(eh)); + return halfEdges_[prev(eh)].to; } FaceHandle left(HalfEdgeHandle eh) const { - return edges_[eh].left; + PVL_ASSERT(valid(eh)); + return halfEdges_[eh].left; } FaceHandle right(HalfEdgeHandle eh) const { - ASSERT(!edges_[eh].boundary()); - return edges_[opposite(eh)].left; + PVL_ASSERT(!boundary(eh)); + return halfEdges_[opposite(eh)].left; } 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)); + for (HalfEdgeHandle eh : halfEdgeRing(vh1)) { + if (to(eh) == vh2) { + return eh; + } + } + return HalfEdgeHandle(-1); + } + // returns any half edge in a face + HalfEdgeHandle halfEdge(FaceHandle fh) const { + PVL_ASSERT(valid(fh)); + return faces_[fh].edge; + } + // returns any half edge for given edge + HalfEdgeHandle halfEdge(EdgeHandle eh) const { + PVL_ASSERT(valid(eh)); + return HalfEdgeHandle(eh.index()); + } + EdgeHandle edge(HalfEdgeHandle heh) const { + PVL_ASSERT(valid(heh)); + EdgeHandle eh; + if (boundary(heh) || from(heh) < to(heh)) { + eh = EdgeHandle(heh.index()); + } else { + eh = EdgeHandle(opposite(heh).index()); + } + PVL_ASSERT(valid(eh)); + 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)); + } + return edge(eh); + } + bool valid(FaceHandle fh) const { + PVL_ASSERT(fh < int(faces_.size())); + return fh != FaceHandle(-1) && faces_[fh].edge != HalfEdgeHandle(-1); + } + bool valid(HalfEdgeHandle heh) const { + PVL_ASSERT(heh < int(halfEdges_.size())); + return heh != HalfEdgeHandle(-1) && halfEdges_[heh].left != FaceHandle(-1); + } + bool valid(VertexHandle vh) const { + PVL_ASSERT(vh < int(vertices_.size())); + return vh != VertexHandle(-1) && vertices_[vh].edge != HalfEdgeHandle(-1); + } + bool valid(EdgeHandle eh) const { + PVL_ASSERT(eh < int(halfEdges_.size())); + HalfEdgeHandle heh(eh.index()); + return valid(heh) && (boundary(heh) || from(heh) < to(heh)); + } template class HandleIterator { @@ -255,7 +406,7 @@ class Graph { } HandleIterator& operator++() { - handle_ = THandle(handle_ + 1); + ++handle_; return *this; } @@ -268,6 +419,59 @@ class Graph { } }; + + class EdgeIterator { + const Graph& graph_; + HalfEdgeHandle eh_; + + public: + using iterator_category = std::forward_iterator_tag; + using value_type = HalfEdgeHandle; + using difference_type = int; + using reference = HalfEdgeHandle&; + using pointer = HalfEdgeHandle*; + + EdgeIterator(const Graph& graph, HalfEdgeHandle eh) + : graph_(graph) + , eh_(eh) { + while (!end() && !dereferencable()) { + ++eh_; + } + } + + EdgeHandle operator*() const { + return EdgeHandle(eh_); + } + + EdgeIterator& operator++() { + do { + ++eh_; + } while (!end() && !dereferencable()); + return *this; + } + + bool operator==(const EdgeIterator& other) const { + return eh_ == other.eh_; + } + + bool operator!=(const EdgeIterator& other) const { + return eh_ != other.eh_; + } + + private: + bool end() const { + return eh_ >= int(graph_.halfEdges_.size()); + } + + bool dereferencable() const { + if (!graph_.valid(eh_)) { + // other iterators visit removed simplices, so let's be consistent + return true; + } + return graph_.boundary(eh_) || graph_.from(eh_) < graph_.to(eh_); + } + }; + using HalfEdgeIterator = HandleIterator; using VertexIterator = HandleIterator; using FaceIterator = HandleIterator; @@ -275,10 +479,16 @@ class Graph { using HalfEdgeRange = Range; using VertexRange = Range; using FaceRange = Range; + using EdgeRange = Range; /// \todo rename to Iterators and Circulators HalfEdgeRange halfEdgeRange() const { - return { HalfEdgeHandle(0), HalfEdgeHandle(edges_.size()) }; + return { HalfEdgeHandle(0), HalfEdgeHandle(halfEdges_.size()) }; + } + + EdgeRange edgeRange() const { + return { EdgeIterator(*this, HalfEdgeHandle(0)), + EdgeIterator(*this, HalfEdgeHandle(halfEdges_.size())) }; } VertexRange vertexRange() const { @@ -290,30 +500,44 @@ class Graph { } Vertex::HalfEdgeRange halfEdgeRing(VertexHandle vh) const { - return { Vertex::HalfEdgeIterator(*this, vh), Vertex::HalfEdgeIterator(*this, vh, Vertex::EndTag{}) }; + return { Vertex::HalfEdgeIterator(*this, vh), + Vertex::HalfEdgeIterator(*this, vh, Vertex::EndTag{}) }; + } + + Vertex::EdgeRange edgeRing(VertexHandle vh) const { + return { Vertex::EdgeIterator(*this, vh), + Vertex::EdgeIterator(*this, vh, Vertex::EndTag{}) }; } Vertex::VertexRange vertexRing(VertexHandle vh) const { - return { Vertex::VertexIterator(*this, vh), Vertex::VertexIterator(*this, vh, Vertex::EndTag{}) }; + 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{}) }; + 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{}) }; + 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{}) }; + 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{}) }; + return { Face::FaceIterator(*this, fh), + Face::FaceIterator(*this, fh, Face::EndTag{}) }; } std::array faceIndices(FaceHandle fh) const { + if (!valid(fh)) { + return { 0, 0, 0 }; + } std::array indices; int i = 0; for (VertexHandle vh : vertexRing(fh)) { @@ -334,14 +558,14 @@ class Graph { FaceHandle fh = FaceHandle(faces_.size() - 1); Face& f = faces_[fh]; - edges_.emplace_back(vh2); - edges_.emplace_back(vh3); - edges_.emplace_back(vh1); + halfEdges_.emplace_back(vh2); + halfEdges_.emplace_back(vh3); + halfEdges_.emplace_back(vh1); std::array eh = { - HalfEdgeHandle(edges_.size() - 3), - HalfEdgeHandle(edges_.size() - 2), - HalfEdgeHandle(edges_.size() - 1), + HalfEdgeHandle(halfEdges_.size() - 3), + HalfEdgeHandle(halfEdges_.size() - 2), + HalfEdgeHandle(halfEdges_.size() - 1), }; std::array vh = { vh1, vh2, vh3 }; @@ -350,12 +574,12 @@ class Graph { for (int i = 0; i < 3; ++i) { // set emanating edge - if (!vertices_[vh[i]].valid()) { + if (!valid(vh[i])) { vertices_[vh[i]].edge = eh[i]; } // set next and previous edge - HalfEdge& e = edges_[eh[i]]; + HalfEdge& e = halfEdges_[eh[i]]; e.left = fh; e.next = eh[(i + 1) % 3]; e.prev = eh[(i + 2) % 3]; @@ -366,26 +590,26 @@ class Graph { for (HalfEdgeHandle neh : halfEdgeRing(to(eh[i]))) { // std::cout << "Visiting " << neh << "\n"; if (to(neh) == from(eh[i])) { - if (!edges_[neh].boundary()) { + if (!halfEdges_[neh].boundary()) { std::cout << "Complex edge!\n"; break; } - edges_[neh].opposite = eh[i]; - edges_[eh[i]].opposite = neh; + halfEdges_[neh].opposite = eh[i]; + halfEdges_[eh[i]].opposite = neh; break; } } - if (edges_[eh[i]].boundary()) { + if (halfEdges_[eh[i]].boundary()) { // opposite not found by circulation std::set removed; for (HalfEdgeHandle neh : boundaryEdges_[to(eh[i])]) { if (to(neh) == from(eh[i])) { - if (!edges_[neh].boundary()) { + if (!halfEdges_[neh].boundary()) { std::cout << "Complex edge!\n"; } - edges_[neh].opposite = eh[i]; - edges_[eh[i]].opposite = neh; + halfEdges_[neh].opposite = eh[i]; + halfEdges_[eh[i]].opposite = neh; removed.insert(neh); break; } @@ -399,7 +623,7 @@ class Graph { } } for (int i = 0; i < 3; ++i) { - if (edges_[eh[i]].boundary()) { + if (halfEdges_[eh[i]].boundary()) { boundaryEdges_[from(eh[i])].insert(eh[i]); } @@ -430,6 +654,154 @@ class Graph { return fh; } + struct CollapseContext { + HalfEdgeHandle edge; // halfedge from "remaining" to "removed" + VertexHandle removed; + VertexHandle remaining; + FaceHandle left; // left of the halfedge + FaceHandle right; // right of the halfedge + + CollapseContext(const Graph& graph, const HalfEdgeHandle heh) + : edge(heh) { + removed = graph.to(edge); + remaining = graph.from(edge); + left = graph.left(edge); + right = graph.right(edge); + } + }; + + + // collapse to to from + void collapse(HalfEdgeHandle heh) { + PVL_ASSERT(collapseAllowed(heh)); + // check(); + CollapseContext context(*this, heh); + HalfEdgeHandle oheh = opposite(heh); + + /*if (vertices_[context.remaining].edge == heh) { + HalfEdgeHandle l = opposite(prev(heh)); + std::cout << "Moving emanating edge for vertex " << context.remaining << " to " + << from(l) << "-" << to(l) << std::endl; + 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? + /// \todo fix boundary case + vertices_[v0].edge = opposite(prev(heh)); + vertices_[vl].edge = next(opposite(prev(heh))); + vertices_[vr].edge = opposite(next(oheh)); + + /*for (HalfEdgeHandle neh : halfEdgeRing(to(heh))) { + if (to(neh) == from(heh)) { + continue; + } + halfEdges_[prev(neh)].to = from(heh); + }*/ + HalfEdgeHandle neh = heh; + do { + neh = opposite(next(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); + } 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))); + + vertices_[context.removed].edge = HalfEdgeHandle(-1); + remove(context.left); + remove(context.right); + + // check(); + /*remove(opposite(next(heh))); + remove(prev(opposite(heh)));*/ + // remove(heh); + // remove(to(eh)); + } + + bool collapseAllowed(HalfEdgeHandle eh) { + PVL_ASSERT(valid(eh)); + 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; + } + std::set ring1; + for (VertexHandle vh : vertexRing(to(eh))) { + // std::cout << "circ " << vh << std::endl; + if (vh != from(eh)) { + ring1.insert(vh); + } + for (HalfEdgeHandle neh : halfEdgeRing(vh)) { + if (boundary(neh)) { + return false; + } + } + } + std::set ring2; + for (VertexHandle vh : vertexRing(from(eh))) { + if (vh != to(eh)) { + ring2.insert(vh); + } + for (HalfEdgeHandle neh : halfEdgeRing(vh)) { + if (boundary(neh)) { + return false; + } + } + } + std::vector is; + std::set_intersection( + ring1.begin(), ring1.end(), ring2.begin(), ring2.end(), std::back_inserter(is)); + if (is.size() != 2) { + return false; + } + VertexHandle vl = to(next(eh)); + VertexHandle vr = to(next(opposite(eh))); + return ((is[0] == vl && is[1] == vr) || (is[1] == vl && is[0] == vr)); + } + + void remove(FaceHandle fh) { + PVL_ASSERT(valid(fh)); + HalfEdgeHandle eh = halfEdge(fh); + HalfEdgeHandle eh0 = eh; + do { + // careful not to call next with invalid handle + HalfEdgeHandle eh1 = eh; + eh = next(eh); + halfEdges_[eh1].left = FaceHandle(-1); + } while (eh != eh0); + faces_[fh].edge = HalfEdgeHandle(-1); + + /// \todo can create non-manifold graph, re-add boundary edges + } + + /*void remove(HalfEdgeHandle heh) { + PVL_ASSERT(valid(heh)); + if (!boundary(heh)) { + halfEdges_[opposite(heh)].left = FaceHandle(-1); + } + halfEdges_[heh].left = FaceHandle(-1); + }*/ + + bool removed(HalfEdgeHandle heh) { + return halfEdges_[heh].left == -1; + } + + bool removed(EdgeHandle heh) { + return halfEdges_[heh].left == -1; + } + + void collectGarbage() {} std::size_t numVertices() const { return vertices_.size(); @@ -438,7 +810,53 @@ class Graph { std::size_t numFaces() const { return faces_.size(); } -}; // namespace Nevim + + // warning: O(n) complexity + std::size_t numEdges() const { + EdgeRange edges = edgeRange(); + return std::distance(edges.begin(), edges.end()); + } + +private: + void connect(HalfEdgeHandle eh1, HalfEdgeHandle eh2) { + halfEdges_[eh1].opposite = eh2; + halfEdges_[eh2].opposite = eh1; + } + + void check() const { + // consistency checks + for (VertexHandle vh : vertexRange()) { + if (!valid(vh)) { + continue; // removed or isolated + } + + for (VertexHandle nvh : vertexRing(vh)) { + if (!valid(nvh)) { + std::cout << "vertex " << vh << " connected to invalid vertex " << nvh + << std::endl; + } + } + for (HalfEdgeHandle heh : halfEdgeRing(vh)) { + if (!valid(heh)) { + std::cout << "vertex " << vh << " connected to invalid halfedge " << heh + << std::endl; + } + } + for (FaceHandle fh : faceRing(vh)) { + if (!valid(fh)) { + std::cout << "vertex " << vh << " connected to invalid face " << fh + << std::endl; + } + } + for (EdgeHandle eh : edgeRing(vh)) { + if (!valid(eh)) { + std::cout << "vertex " << vh << " connected to invalid edge " << eh + << std::endl; + } + } + } + } +}; } // namespace Pvl diff --git a/Graph.inl.hpp b/Graph.inl.hpp index 93ae296..242ca1f 100644 --- a/Graph.inl.hpp +++ b/Graph.inl.hpp @@ -28,16 +28,13 @@ inline Vertex::IteratorBase& Vertex::IteratorBase::operator++() { } else { status_ = Status::END; } - ASSERT(eh_ != HalfEdgeHandle(-1)); - /* std::cout << "++ - " << eh_ << ", status = " - << (status_ == Status::BEGIN ? "begin" : (status_ == Status::ITERS ? "iters" : "end")) - << std::endl; - */ + PVL_ASSERT(eh_ != HalfEdgeHandle(-1)); return *this; } inline bool Vertex::IteratorBase::operator!=(const IteratorBase& other) const { /// \todo proper + // std::cout << "Comparing " << eh_ << " and " << other.eh_ << std::endl; return (status_ != Status::END || other.status_ != Status::END) && (eh_ != other.eh_ || status_ == Status::BEGIN); } @@ -54,6 +51,28 @@ inline FaceHandle Vertex::FaceIterator::operator*() const { return graph_.left(eh_); } +inline Vertex::EdgeIterator& Vertex::EdgeIterator::operator++() { + if (status_ == Status::ITERS && graph_.boundary(eh_)) { + // already reached the end + status_ = Status::END; + } else { + const HalfEdgeHandle prev = graph_.prev(eh_); + if (!graph_.boundary(prev)) { + eh_ = graph_.opposite(prev); + } else { + eh_ = prev; + } + status_ = Status::ITERS; + } + PVL_ASSERT(eh_ != HalfEdgeHandle(-1)); + return *this; +} + +inline EdgeHandle Vertex::EdgeIterator::operator*() const { + return graph_.edge(eh_); +} + + inline Face::IteratorBase::IteratorBase(const Graph& graph, FaceHandle fh) : graph_(graph) { eh_ = graph_.faces_[fh].edge; diff --git a/KdTree.hpp b/KdTree.hpp new file mode 100644 index 0000000..d898ff7 --- /dev/null +++ b/KdTree.hpp @@ -0,0 +1,614 @@ +#pragma once + +#include "Box.hpp" +#include "Utils.hpp" +#include +#include +#include +#include +#include +#include +#include + + +namespace Pvl { + +/// \brief Base class for nodes of K-d tree. +/// +/// Can be derived to include additional user data for each node. +template +struct KdNode { + /// Here X, Y, Z must be 0, 1, 2 + int8_t type; + + /// Bounding box of particles in the node + BoundingBox> box; + + struct LeafTag {}; + + KdNode(const int8_t type) + : type(type) {} + + KdNode(const LeafTag) + : type(Dim) {} + + bool isLeaf() const { + return type == Dim; + } +}; + +/// \brief Inner node of K-d tree +template +struct KdInnerNode : public KdNode { + /// Position where the selected dimension is split + Float splitPosition; + + /// Index of left child node + Index left; + + /// Index of right child node + Index right; + + KdInnerNode() + : KdNode(-1) {} + + KdInnerNode(const int8_t type) + : KdNode(type) {} +}; + +/// \brief Leaf (bucket) node of K-d tree +template +struct KdLeafNode : public KdNode { + /// First index of particlse belonging to the leaf + Index from; + + /// One-past-last index of particles belonging to the leaf + Index to; + + /// Unused, used so that LeafNode and InnerNode have the same size + Index padding; + + KdLeafNode() + : KdNode(typename KdNode::LeafTag{}) {} + + void setLeaf() { + this->type = Dim; + } + + /// Returns the number of points in the leaf. Can be zero. + Index size() const { + return to - from; + } +}; + +// static_assert(sizeof(Size) == sizeof(float), "Sizes must match to keep this layout"); + +/// \brief Index iterator with given mapping (index permutation). +/// +/// Returns value mapping[index] when dereferenced, +/*class LeafIndexIterator : public IndexIterator { +private: + ArrayView mapping; + +public: + LeafIndexIterator(const Size idx, ArrayView mapping) + : IndexIterator(idx) + , mapping(mapping) {} + + Size operator*() const { + return mapping[idx]; + } +}; + +/// \brief Helper index sequence to iterate over particle indices of a leaf node. +class LeafIndexSequence : public IndexSequence { +private: + ArrayView mapping; + +public: + LeafIndexSequence(const Size from, const Size to, ArrayView mapping) + : IndexSequence(from, to) + , mapping(mapping) { + ASSERT(to <= mapping.size()); + } + + LeafIndexIterator begin() const { + return LeafIndexIterator(from, mapping); + } + + LeafIndexIterator end() const { + return LeafIndexIterator(to, mapping); + } +}; +*/ + +struct EuclideanMetric { + template + T lengthSqr(const Vector& v) const { + return normSqr(v); + } +}; + + +enum class KdChild { + LEFT = 0, + RIGHT = 1, +}; + +template +struct Neighbor { + Index index; + Float distSqr; + + Neighbor() = default; + Neighbor(Index index, Float distSqr) + : index(index) + , distSqr(distSqr) {} + + operator Index() const { + return index; + } +}; + +/// \brief K-d tree, used for hierarchical clustering of particles and accelerated Kn queries. +/// +/// Allows storing arbitrary data at each node of the tree. +/// +/// https://www.cs.umd.edu/~mount/Papers/cgc99-smpack.pdf +/// \tparam TNode Nodes of the tree, should always derive from KdNode and should be POD structs. +/// \tparam TMetric Functor returning the squared distance of two vectors. +template +class KdTree : public Noncopyable { +private: + struct { + /// Maximal number of particles in the leaf node + int leafSize; + + /// Maximal depth for which the build is parallelized + int maxParallelDepth; + } config_; + + using Float = typename Vec::Type; + static constexpr int Dim = Vec::size(); + using Box = BoundingBox>; + using InnerNode = KdInnerNode; + using LeafNode = KdLeafNode; + using IndexDiff = std::make_signed_t; + + /// Holds all nodes, either \ref InnerNode or \ref LeafNode (depending on the value of \ref type). + Box entireBox_; + /// \todo optionally weak copy? + std::vector values_; + std::vector idxs_; + std::vector nodes_; + std::atomic_int nodeCounter_; + // std::shared_timed_mutex nodesMutex_; + + static constexpr Index ROOT_PARENT_NODE = Index(-1); + +public: + explicit KdTree(const int leafSize = 25, const int maxParallelDepth = 50) { + PVL_ASSERT(leafSize >= 1); + config_.leafSize = leafSize; + config_.maxParallelDepth = maxParallelDepth; + } + + template + void build(const TContainer& points) { + static_assert(sizeof(LeafNode) == sizeof(InnerNode), "Sizes of nodes must match"); + + // clean the current tree + const Index currentCnt = nodes_.size(); + this->init(); + + Index index = 0; + for (const Vec& p : points) { + entireBox_.extend(p); + values_.push_back(p); + idxs_.push_back(index); + index++; + } + + if (points.empty()) { + return; + } + + const Index nodeCnt = std::max(2 * points.size() / config_.leafSize + 1, currentCnt); + nodes_.resize(nodeCnt); + + // tbb::task_group tg; + buildTree(ROOT_PARENT_NODE, KdChild(-1), 0, points.size(), entireBox_, 0, 0); + // tg.wait(); + + // shrink nodes to only the constructed ones + // nodes.resize(nodeCounter); + + // ASSERT(this->sanityCheck(), this->sanityCheck().error()); + } + + template + Index rangeQuery(const Vec& pos, const Float radius, TOutIter neighs) const { + struct TraversalNode { + Index idx; + Vec sizeSqr; + Float distanceSqr; + }; + static thread_local std::vector nodeStack; + + const Float radiusSqr = sqr(radius); + const Vec maxDistSqr = sqr(max(Vec(0), max(entireBox_.lower() - pos, pos - entireBox_.upper()))); + + // L1 norm + const Float l1 = normL1(maxDistSqr); + TraversalNode node{ 0, maxDistSqr, l1 }; + + PVL_ASSERT(nodeStack.empty()); // not sure if there can be some nodes from previous search ... + + Index neighCnt = 0; + Metric metric; + while (node.distanceSqr < radiusSqr) { + if (nodes_[node.idx].isLeaf()) { + // for leaf just add all + const LeafNode& leaf = (const LeafNode&)nodes_[node.idx]; + if (leaf.size() > 0) { + const Float leafDistSqr = + metric.lengthSqr(max(Vec(0), max(leaf.box.lower() - pos, pos - leaf.box.upper()))); + if (leafDistSqr < radiusSqr) { + // leaf intersects the sphere + for (Index i = leaf.from; i < leaf.to; ++i) { + const Index actIndex = idxs_[i]; + const Float distSqr = metric.lengthSqr(values_[actIndex] - pos); + if (distSqr < radiusSqr) { + *neighs++ = Neighbor{ actIndex, distSqr }; + neighCnt++; + } + } + } + } + if (nodeStack.empty()) { + break; + } + node = nodeStack.back(); + nodeStack.pop_back(); + } else { + // inner node + const InnerNode& inner = (InnerNode&)nodes_[node.idx]; + const Index splitDimension = Index(inner.type); + PVL_ASSERT(splitDimension < Dim); + const Float splitPosition = inner.splitPosition; + if (pos[splitDimension] < splitPosition) { + // process left subtree, put right on stack + TraversalNode right = node; + node.idx = inner.left; + + const Float dx = splitPosition - pos[splitDimension]; + right.distanceSqr += sqr(dx) - right.sizeSqr[splitDimension]; + right.sizeSqr[splitDimension] = sqr(dx); + if (right.distanceSqr < radiusSqr) { + const InnerNode& next = (const InnerNode&)nodes_[right.idx]; + right.idx = next.right; + nodeStack.push_back(right); + } + } else { + // process right subtree, put left on stack + TraversalNode left = node; + node.idx = inner.right; + const Float dx = splitPosition - pos[splitDimension]; + left.distanceSqr += sqr(dx) - left.sizeSqr[splitDimension]; + left.sizeSqr[splitDimension] = sqr(dx); + if (left.distanceSqr < radiusSqr) { + const InnerNode& next = (const InnerNode&)nodes_[left.idx]; + left.idx = next.left; + nodeStack.push_back(left); + } + } + } + } + + return neighCnt; + } + + /// \brief Returns the node with given index + /*TNode& getNode(const Size nodeIdx) { + return nodes[nodeIdx]; + } + + /// \brief Returns the node with given index + const TNode& getNode(const Size nodeIdx) const { + return nodes[nodeIdx]; + } + + /// \brief Returns the number of nodes in the tree + Size getNodeCnt() const { + return nodes.size(); + } + + /// \brief Returns the sequence of particles indices belonging to given leaf. + LeafIndexSequence getLeafIndices(const LeafNode& leaf) const { + return LeafIndexSequence(leaf.from, leaf.to, idxs); + }*/ + + bool sanityCheck() const; + +private: + void init() { + entireBox_ = Box(); + values_.clear(); + idxs_.clear(); + nodes_.clear(); + nodeCounter_ = 0; + } + + void buildTree(const Index parent, + const KdChild child, + const Index from, + const Index to, + const Box& box, + const Index slidingCnt, + const Index depth) { + Box box1, box2; + Vec boxSize = box.size(); + + // split by the dimension of largest extent + Index splitIdx = argMax(boxSize); + + bool slidingMidpoint = false; + bool degeneratedBox = false; + + if (to - from <= config_.leafSize) { + // enough points to fit inside one leaf + addLeaf(parent, child, from, to); + return; + } else { + // check for singularity of dimensions + for (int dim = 0; dim < Dim; ++dim) { + if (isSingular(from, to, splitIdx)) { + boxSize[splitIdx] = 0.f; + // find new largest dimension + splitIdx = argMax(boxSize); + + if (boxSize == Vec(0)) { + // too many overlapping points, just split until they fit within a leaf, + // the code can handle this case, but it smells with an error ... + PVL_ASSERT(false, "Too many overlapping points, something is probably wrong ..."); + degeneratedBox = true; + break; + } + } else { + break; + } + } + + // split around center of the box + Float splitPosition = box.center()[splitIdx]; + IndexDiff n1 = from, n2 = to - 1; + + if (slidingCnt <= 5 && !degeneratedBox) { + for (;; std::swap(idxs_[n1], idxs_[n2])) { + for (; n1 < IndexDiff(to) && values_[idxs_[n1]][splitIdx] <= splitPosition; ++n1) + ; + for (; n2 >= IndexDiff(from) && values_[idxs_[n2]][splitIdx] >= splitPosition; --n2) + ; + if (n1 >= n2) { + break; + } + } + + if (n1 == IndexDiff(from)) { + Index idx = from; + splitPosition = values_[idxs_[from]][splitIdx]; + for (Index i = from + 1; i < to; ++i) { + const Float x1 = values_[idxs_[i]][splitIdx]; + if (x1 < splitPosition) { + idx = i; + splitPosition = x1; + } + } + std::swap(idxs_[from], idxs_[idx]); + n1++; + slidingMidpoint = true; + } else if (n1 == IndexDiff(to)) { + Index idx = from; + splitPosition = values_[idxs_[from]][splitIdx]; + for (Index i = from + 1; i < to; ++i) { + const Float x2 = values_[idxs_[i]][splitIdx]; + if (x2 > splitPosition) { + idx = i; + splitPosition = x2; + } + } + std::swap(idxs_[to - 1], idxs_[idx]); + n1--; + slidingMidpoint = true; + } + + std::tie(box1, box2) = splitBox(box, splitIdx, splitPosition); + } else { + n1 = (from + to) >> 1; + // do quick select to sort elements around the midpoint + typename std::vector::iterator iter = idxs_.begin(); + if (!degeneratedBox) { + std::nth_element(iter + from, iter + n1, iter + to, [this, splitIdx](Index i1, Index i2) { + return values_[i1][splitIdx] < values_[i2][splitIdx]; + }); + } + + std::tie(box1, box2) = splitBox(box, splitIdx, values_[idxs_[n1]][splitIdx]); + } + + // sanity check + PVL_ASSERT(checkBoxes(from, to, n1, box1, box2)); + + // add inner node and connect it to the parent + const Index index = addInner(parent, child, splitPosition, splitIdx); + + // recurse to left and right subtree + const Index nextSlidingCnt = slidingMidpoint ? slidingCnt + 1 : 0; + // auto processRightSubTree = [this, &scheduler, index, to, n1, box2, nextSlidingCnt, depth] { + buildTree(index, KdChild::RIGHT, n1, to, box2, nextSlidingCnt, depth + 1); + //}; + /*if (depth < config.maxParallelDepth) { + // ad hoc decision - split the build only for few topmost nodes, there is no point in + // splitting the work for child node in the bottom, it would only overburden the + ThreadPool. scheduler.submit(processRightSubTree); } else { + // otherwise simply process both subtrees in the same thread + processRightSubTree(); + }*/ + buildTree(index, KdChild::LEFT, from, n1, box1, nextSlidingCnt, depth + 1); + } + } + + void addLeaf(const Index parent, const KdChild child, const Index from, const Index to) { + const Index index = nodeCounter_++; + if (index >= nodes_.size()) { + // needs more nodes than estimated; allocate up to 2x more than necessary to avoid frequent + // reallocations + // nodesMutex.lock(); + nodes_.resize(std::max(2 * index, nodes_.size())); + // nodesMutex.unlock(); + } + + // nodesMutex.lock_shared(); + // auto releaseLock = finally([this] { nodesMutex.unlock_shared(); }); + + LeafNode& node = (LeafNode&)nodes_[index]; + node.setLeaf(); + PVL_ASSERT(node.isLeaf()); + + node.from = node.to = -1; + + node.from = from; + node.to = to; + + // find the bounding box of the leaf + Box box; + for (Index i = from; i < to; ++i) { + box.extend(values_[idxs_[i]]); + } + node.box = box; + + if (parent == ROOT_PARENT_NODE) { + return; + } + InnerNode& parentNode = (InnerNode&)nodes_[parent]; + PVL_ASSERT(!parentNode.isLeaf()); + if (child == KdChild::LEFT) { + // left child + parentNode.left = index; + } else { + PVL_ASSERT(child == KdChild::RIGHT); + // right child + parentNode.right = index; + } + } + + Index addInner(const Index parent, const KdChild child, const Float splitPosition, const Index splitIdx) { + /*static_assert(int(KdNode::Type::X) == 0 && int(KdNode::Type::Y) == 1 && int(KdNode::Type::Z) == 2, + "Invalid values of KdNode::Type enum");*/ + + const Index index = nodeCounter_++; + if (index >= nodes_.size()) { + // needs more nodes than estimated; allocate up to 2x more than necessary to avoid frequent + // reallocations + // nodesMutex.lock(); + nodes_.resize(std::max(2 * index, nodes_.size())); + // nodesMutex.unlock(); + } + + // nodesMutex.lock_shared(); + // auto releaseLock = finally([this] { nodesMutex.unlock_shared(); }); + InnerNode& node = (InnerNode&)nodes_[index]; + node.type = splitIdx; + PVL_ASSERT(!node.isLeaf()); + + node.left = node.right = -1; + node.box = Box(); // will be computed later + + node.splitPosition = splitPosition; + + if (parent == ROOT_PARENT_NODE) { + // no need to set up parents + return index; + } + InnerNode& parentNode = (InnerNode&)nodes_[parent]; + if (child == KdChild::LEFT) { + // left child + PVL_ASSERT(parentNode.left == Index(-1)); + parentNode.left = index; + } else { + PVL_ASSERT(child == KdChild::RIGHT); + // right child + PVL_ASSERT(parentNode.right == Index(-1)); + parentNode.right = index; + } + + return index; + } + + bool isSingular(const Index from, const Index to, const Index splitIdx) const { + for (Index i = from; i < to; ++i) { + if (values_[idxs_[i]][splitIdx] != values_[idxs_[to - 1]][splitIdx]) { + return false; + } + } + return true; + } + + + bool checkBoxes(const Index from, + const Index to, + const Index mid, + const Box& box1, + const Box& box2) const { + for (Index i = from; i < to; ++i) { + if (i < mid && !box1.contains(values_[idxs_[i]])) { + return false; + } + if (i >= mid && !box2.contains(values_[idxs_[i]])) { + return false; + } + } + return true; + } + + /* bool checkBoxes(const Size from, + const Size to, + const Size mid, + const BoundingBox& box1, + const BoundingBox& box2) const;*/ +}; + + +enum class IterateDirection { + TOP_DOWN, ///< From root to leaves + BOTTOM_UP, ///< From leaves to root +}; + +/// \brief Calls a functor for every node of a K-d tree tree in specified direction. +/// +/// The functor is called with the node as a parameter. For top-down direction, functor may return false +/// to skip all children nodes from processing, otherwise the iteration proceedes through the tree into +/// leaf nodes. +/// \param tree KdTree to iterate. +/// \param scheduler Scheduler used for sequential or parallelized task execution +/// \param functor Functor executed for every node +/// \param nodeIdx Index of the first processed node; use 0 for root node +/// \param depthLimit Maximal depth processed in parallel. +/*template +void iterateTree(KdTree& tree, + IScheduler& scheduler, + const TFunctor& functor, + const Size nodeIdx = 0, + const Size depthLimit = Size(-1)); + +/// \copydoc iterateTree +template +void iterateTree(const KdTree& tree, + IScheduler& scheduler, + const TFunctor& functor, + const Size nodeIdx = 0, + const Size depthLimit = Size(-1)); +*/ +} // namespace Pvl + +#include "KdTree.inl.hpp" diff --git a/KdTree.inl.hpp b/KdTree.inl.hpp new file mode 100644 index 0000000..f81f610 --- /dev/null +++ b/KdTree.inl.hpp @@ -0,0 +1,520 @@ +namespace Tvl { + +/*enum class KdChild { + LEFT = 0, + RIGHT = 1, +}; + +template +void KdTree::buildImpl(IScheduler& scheduler, ArrayView points) { + VERBOSE_LOG + + static_assert(sizeof(LeafNode) == sizeof(InnerNode), "Sizes of nodes must match"); + + // clean the current tree + const Size currentCnt = nodes.size(); + this->init(); + + for (const auto& i : iterateWithIndex(points)) { + entireBox.extend(i.value()); + idxs.push(i.index()); + } + + if (SPH_UNLIKELY(points.empty())) { + return; + } + + const Size nodeCnt = max(2 * points.size() / config.leafSize + 1, currentCnt); + nodes.resize(nodeCnt); + + SharedPtr rootTask = scheduler.submit([this, &scheduler, points] { + this->buildTree(scheduler, ROOT_PARENT_NODE, KdChild(-1), 0, points.size(), entireBox, 0, 0); + }); + rootTask->wait(); + + // shrink nodes to only the constructed ones + nodes.resize(nodeCounter); + + ASSERT(this->sanityCheck(), this->sanityCheck().error()); +} + +template +void KdTree::buildTree(IScheduler& scheduler, + const Size parent, + const KdChild child, + const Size from, + const Size to, + const Box& box, + const Size slidingCnt, + const Size depth) { + + Box box1, box2; + Vector boxSize = box.size(); + + // split by the dimension of largest extent + Size splitIdx = argMax(boxSize); + + bool slidingMidpoint = false; + bool degeneratedBox = false; + + if (to - from <= config.leafSize) { + // enough points to fit inside one leaf + this->addLeaf(parent, child, from, to); + return; + } else { + // check for singularity of dimensions + for (Size dim = 0; dim < 3; ++dim) { + if (this->isSingular(from, to, splitIdx)) { + boxSize[splitIdx] = 0.f; + // find new largest dimension + splitIdx = argMax(boxSize); + + if (boxSize == Vector(0._f)) { + // too many overlapping points, just split until they fit within a leaf, + // the code can handle this case, but it smells with an error ... + ASSERT(false, "Too many overlapping points, something is probably wrong ..."); + degeneratedBox = true; + break; + } + } else { + break; + } + } + + // split around center of the box + Float splitPosition = box.center()[splitIdx]; + std::make_signed_t n1 = from, n2 = to - 1; // use ints for easier for loop ending with 0 + + if (slidingCnt <= 5 && !degeneratedBox) { + for (;; std::swap(idxs[n1], idxs[n2])) { + for (; n1 < int(to) && this->values[idxs[n1]][splitIdx] <= splitPosition; ++n1) + ; + for (; n2 >= int(from) && this->values[idxs[n2]][splitIdx] >= splitPosition; --n2) + ; + if (n1 >= n2) { + break; + } + } + + if (n1 == int(from)) { + Size idx = from; + splitPosition = this->values[idxs[from]][splitIdx]; + for (Size i = from + 1; i < to; ++i) { + const Float x1 = this->values[idxs[i]][splitIdx]; + if (x1 < splitPosition) { + idx = i; + splitPosition = x1; + } + } + std::swap(idxs[from], idxs[idx]); + n1++; + slidingMidpoint = true; + } else if (n1 == int(to)) { + Size idx = from; + splitPosition = this->values[idxs[from]][splitIdx]; + for (Size i = from + 1; i < to; ++i) { + const Float x2 = this->values[idxs[i]][splitIdx]; + if (x2 > splitPosition) { + idx = i; + splitPosition = x2; + } + } + std::swap(idxs[to - 1], idxs[idx]); + n1--; + slidingMidpoint = true; + } + + tie(box1, box2) = box.split(splitIdx, splitPosition); + } else { + n1 = (from + to) >> 1; + // do quick select to sort elements around the midpoint + Iterator iter = idxs.begin(); + if (!degeneratedBox) { + std::nth_element(iter + from, iter + n1, iter + to, [this, splitIdx](Size i1, Size i2) { + return this->values[i1][splitIdx] < this->values[i2][splitIdx]; + }); + } + + tie(box1, box2) = box.split(splitIdx, this->values[idxs[n1]][splitIdx]); + } + + // sanity check + ASSERT(this->checkBoxes(from, to, n1, box1, box2)); + + // add inner node and connect it to the parent + const Size index = this->addInner(parent, child, splitPosition, splitIdx); + + // recurse to left and right subtree + const Size nextSlidingCnt = slidingMidpoint ? slidingCnt + 1 : 0; + auto processRightSubTree = [this, &scheduler, index, to, n1, box2, nextSlidingCnt, depth] { + this->buildTree(scheduler, index, KdChild::RIGHT, n1, to, box2, nextSlidingCnt, depth + 1); + }; + if (depth < config.maxParallelDepth) { + // ad hoc decision - split the build only for few topmost nodes, there is no point in splitting + // the work for child node in the bottom, it would only overburden the ThreadPool. + scheduler.submit(processRightSubTree); + } else { + // otherwise simply process both subtrees in the same thread + processRightSubTree(); + } + this->buildTree(scheduler, index, KdChild::LEFT, from, n1, box1, nextSlidingCnt, depth + 1); + } +} + +template +void KdTree::addLeaf(const Size parent, const KdChild child, const Size from, const Size to) { + const Size index = nodeCounter++; + if (index >= nodes.size()) { + // needs more nodes than estimated; allocate up to 2x more than necessary to avoid frequent + // reallocations + nodesMutex.lock(); + nodes.resize(max(2 * index, nodes.size())); + nodesMutex.unlock(); + } + + nodesMutex.lock_shared(); + auto releaseLock = finally([this] { nodesMutex.unlock_shared(); }); + + LeafNode& node = (LeafNode&)nodes[index]; + node.type = KdNode::Type::LEAF; + ASSERT(node.isLeaf()); + +#ifdef SPH_DEBUG + node.from = node.to = -1; +#endif + + node.from = from; + node.to = to; + + // find the bounding box of the leaf + Box box; + for (Size i = from; i < to; ++i) { + box.extend(this->values[idxs[i]]); + } + node.box = box; + + if (parent == ROOT_PARENT_NODE) { + return; + } + InnerNode& parentNode = (InnerNode&)nodes[parent]; + ASSERT(!parentNode.isLeaf()); + if (child == KdChild::LEFT) { + // left child + parentNode.left = index; + } else { + ASSERT(child == KdChild::RIGHT); + // right child + parentNode.right = index; + } +} + +template +Size KdTree::addInner(const Size parent, + const KdChild child, + const Float splitPosition, + const Size splitIdx) { + static_assert(int(KdNode::Type::X) == 0 && int(KdNode::Type::Y) == 1 && int(KdNode::Type::Z) == 2, + "Invalid values of KdNode::Type enum"); + + const Size index = nodeCounter++; + if (index >= nodes.size()) { + // needs more nodes than estimated; allocate up to 2x more than necessary to avoid frequent + // reallocations + nodesMutex.lock(); + nodes.resize(max(2 * index, nodes.size())); + nodesMutex.unlock(); + } + + nodesMutex.lock_shared(); + auto releaseLock = finally([this] { nodesMutex.unlock_shared(); }); + InnerNode& node = (InnerNode&)nodes[index]; + node.type = KdNode::Type(splitIdx); + ASSERT(!node.isLeaf()); + +#ifdef SPH_DEBUG + node.left = node.right = -1; + node.box = Box(); // will be computed later +#endif + + node.splitPosition = float(splitPosition); + + if (parent == ROOT_PARENT_NODE) { + // no need to set up parents + return index; + } + InnerNode& parentNode = (InnerNode&)nodes[parent]; + if (child == KdChild::LEFT) { + // left child + ASSERT(parentNode.left == Size(-1)); + parentNode.left = index; + } else { + ASSERT(child == KdChild::RIGHT); + // right child + ASSERT(parentNode.right == Size(-1)); + parentNode.right = index; + } + + return index; +} + +template +void KdTree::init() { + entireBox = Box(); + idxs.clear(); + nodes.clear(); + nodeCounter = 0; +} + +template +bool KdTree::isSingular(const Size from, const Size to, const Size splitIdx) const { + for (Size i = from; i < to; ++i) { + if (this->values[idxs[i]][splitIdx] != this->values[idxs[to - 1]][splitIdx]) { + return false; + } + } + return true; +} + +template +bool KdTree::checkBoxes(const Size from, + const Size to, + const Size mid, + const Box& box1, + const Box& box2) const { + for (Size i = from; i < to; ++i) { + if (i < mid && !box1.contains(this->values[idxs[i]])) { + return false; + } + if (i >= mid && !box2.contains(this->values[idxs[i]])) { + return false; + } + } + return true; +} + +/// \brief Object used during traversal. +/// +/// Holds an index of the node and squared distance of the bounding box. +struct ProcessedNode { + /// Index into the nodeStack array. We cannot use pointers because the array might get reallocated. + Size idx; + + Vector sizeSqr; + + Float distanceSqr; +}; + +/// \brief Cached stack to avoid reallocation +/// +/// It is thread_local to allow using KdTree from multiple threads +extern thread_local Array nodeStack; + +template +template +Size KdTree::find(const Vector& r0, + const Size index, + const Float radius, + Array& neighbours) const { + + ASSERT(neighbours.empty()); + const Float radiusSqr = sqr(radius); + const Vector maxDistSqr = sqr(max(Vector(0._f), entireBox.lower() - r0, r0 - entireBox.upper())); + + // L1 norm + const Float l1 = l1Norm(maxDistSqr); + ProcessedNode node{ 0, maxDistSqr, l1 }; + + ASSERT(nodeStack.empty()); // not sure if there can be some nodes from previous search ... + + TMetric metric; + while (node.distanceSqr < radiusSqr) { + if (nodes[node.idx].isLeaf()) { + // for leaf just add all + const LeafNode& leaf = (const LeafNode&)nodes[node.idx]; + if (leaf.size() > 0) { + const Float leafDistSqr = + metric(max(Vector(0._f), leaf.box.lower() - r0, r0 - leaf.box.upper())); + if (leafDistSqr < radiusSqr) { + // leaf intersects the sphere + for (Size i = leaf.from; i < leaf.to; ++i) { + const Size actIndex = idxs[i]; + const Float distSqr = metric(this->values[actIndex] - r0); + if (distSqr < radiusSqr && (FindAll || this->rank[actIndex] < this->rank[index])) { + /// \todo order part + neighbours.push(NeighbourRecord{ actIndex, distSqr }); + } + } + } + } + if (nodeStack.empty()) { + break; + } + node = nodeStack.pop(); + } else { + // inner node + const InnerNode& inner = (InnerNode&)nodes[node.idx]; + const Size splitDimension = Size(inner.type); + ASSERT(splitDimension < 3); + const Float splitPosition = inner.splitPosition; + if (r0[splitDimension] < splitPosition) { + // process left subtree, put right on stack + ProcessedNode right = node; + node.idx = inner.left; + + const Float dx = splitPosition - r0[splitDimension]; + right.distanceSqr += sqr(dx) - right.sizeSqr[splitDimension]; + right.sizeSqr[splitDimension] = sqr(dx); + if (right.distanceSqr < radiusSqr) { + const InnerNode& next = (const InnerNode&)nodes[right.idx]; + right.idx = next.right; + nodeStack.push(right); + } + } else { + // process right subtree, put left on stack + ProcessedNode left = node; + node.idx = inner.right; + const Float dx = splitPosition - r0[splitDimension]; + left.distanceSqr += sqr(dx) - left.sizeSqr[splitDimension]; + left.sizeSqr[splitDimension] = sqr(dx); + if (left.distanceSqr < radiusSqr) { + const InnerNode& next = (const InnerNode&)nodes[left.idx]; + left.idx = next.left; + nodeStack.push(left); + } + } + } + } + + return neighbours.size(); +} + +template +Outcome KdTree::sanityCheck() const { + if (this->values.size() != idxs.size()) { + return makeFailed("Number of values does not match the number of indices"); + } + + // check bounding box + for (const Vector& v : this->values) { + if (!entireBox.contains(v)) { + return makeFailed("Points are not strictly within the bounding box"); + } + } + + // check node connectivity + Size counter = 0; + std::set indices; + + Function countNodes = [this, &indices, &counter, &countNodes]( + const Size idx) -> Outcome { + // count this + counter++; + + // check index validity + if (idx >= nodes.size()) { + return makeFailed("Invalid index found: ", idx, " (", nodes.size(), ")"); + } + + // if inner node, count children + if (!nodes[idx].isLeaf()) { + const InnerNode& inner = (const InnerNode&)nodes[idx]; + return countNodes(inner.left) && countNodes(inner.right); + } else { + // check that all points fit inside the bounding box of the leaf + const LeafNode& leaf = (const LeafNode&)nodes[idx]; + if (leaf.to == leaf.from) { + // empty leaf? + return makeFailed("Empty leaf: ", leaf.to); + } + for (Size i = leaf.from; i < leaf.to; ++i) { + if (!leaf.box.contains(this->values[idxs[i]])) { + return makeFailed("Leaf points do not fit inside the bounding box"); + } + if (indices.find(i) != indices.end()) { + // child referenced twice? + return makeFailed("Index repeated: ", i); + } + indices.insert(i); + } + } + return SUCCESS; + }; + const Outcome result = countNodes(0); + if (!result) { + return result; + } + // we should count exactly nodes.size() + if (counter != nodes.size()) { + return makeFailed("Unexpected number of nodes: ", counter, " == ", nodes.size()); + } + // each index should have been inserted exactly once + Size i = 0; + for (Size idx : indices) { + // std::set is sorted, so we can check sequentially + if (idx != i) { + return makeFailed("Invalid index: ", idx, " == ", i); + } + ++i; + } + return SUCCESS; +} + +template +void iterateTree(KdTree& tree, + IScheduler& scheduler, + const TFunctor& functor, + const Size nodeIdx, + const Size depthLimit) { + TNode& node = tree.getNode(nodeIdx); + if (Dir == IterateDirection::TOP_DOWN) { + if (node.isLeaf()) { + functor(node, nullptr, nullptr); + } else { + InnerNode& inner = reinterpret_cast&>(node); + if (!functor(inner, &tree.getNode(inner.left), &tree.getNode(inner.right))) { + return; + } + } + } + SharedPtr task; + if (!node.isLeaf()) { + InnerNode& inner = reinterpret_cast&>(node); + + const Size newDepth = depthLimit == 0 ? 0 : depthLimit - 1; + auto iterateRightSubtree = [&tree, &scheduler, &functor, &inner, newDepth] { + iterateTree(tree, scheduler, functor, inner.right, newDepth); + }; + if (newDepth > 0) { + task = scheduler.submit(iterateRightSubtree); + } else { + iterateRightSubtree(); + } + iterateTree(tree, scheduler, functor, inner.left, newDepth); + } + if (task) { + task->wait(); + } + if (Dir == IterateDirection::BOTTOM_UP) { + if (node.isLeaf()) { + functor(node, nullptr, nullptr); + } else { + InnerNode& inner = reinterpret_cast&>(node); + functor(inner, &tree.getNode(inner.left), &tree.getNode(inner.right)); + } + } +} + +/// \copydoc iterateTree +template +void iterateTree(const KdTree& tree, + IScheduler& scheduler, + const TFunctor& functor, + const Size nodeIdx, + const Size depthLimit) { + // use non-const overload using const_cast, but call the functor with const reference + auto actFunctor = [&functor](TNode& node, TNode* left, TNode* right) + INL { return functor(asConst(node), left, right); }; + iterateTree(const_cast&>(tree), scheduler, actFunctor, nodeIdx, depthLimit); +} +*/ +} // namespace Tvl diff --git a/Kernels.hpp b/Kernels.hpp index ec978bd..158479f 100644 --- a/Kernels.hpp +++ b/Kernels.hpp @@ -22,13 +22,13 @@ class Kernel : public Noncopyable { /// 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); + PVL_ASSERT(h > 0.f); const Float hInv = 1.f / h; return pow(hInv) * impl().valueImpl(normSqr(r) * sqr(hInv)); } Vec grad(const Vec& r, const Float h) const noexcept { - ASSERT(h > 0.f); + PVL_ASSERT(h > 0.f); const Float hInv = 1.f / h; return r * pow(hInv) * impl().gradImpl(normSqr(r) * sqr(hInv)); } @@ -89,7 +89,7 @@ class LutKernel : public Kernel, Float, Dim> { LutKernel(TKernel&& source) { rad = source.radius(); - ASSERT(rad > 0.f); + PVL_ASSERT(rad > 0.f); const Float radInvSqr = 1.f / (rad * rad); qSqrToIdx = Float(NEntries) * radInvSqr; for (int i = 0; i < NEntries; ++i) { @@ -109,8 +109,8 @@ class LutKernel : public Kernel, Float, Dim> { } Float valueImpl(const Float qSqr) const noexcept { - ASSERT(qSqr >= 0.f); - ASSERT(initialized()); + PVL_ASSERT(qSqr >= 0.f); + PVL_ASSERT(initialized()); if (qSqr >= sqr(rad)) { // outside of kernel support return 0.f; @@ -118,27 +118,27 @@ class LutKernel : public Kernel, Float, Dim> { // linear interpolation of stored values const Float floatIdx = qSqrToIdx * qSqr; const int idx1 = Size(floatIdx); - ASSERT(idx1 < NEntries); + PVL_ASSERT(idx1 < NEntries); const int idx2 = idx1 + 1; const Float ratio = floatIdx - Float(idx1); - ASSERT(ratio >= 0.f && ratio < 1.f); + PVL_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()); + PVL_ASSERT(qSqr >= 0.f); + PVL_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)); + PVL_ASSERT(unsigned(idx1) < unsigned(NEntries)); const int idx2 = idx1 + 1; const Float ratio = floatIdx - Float(idx1); - ASSERT(ratio >= 0.f && ratio < 1.f); + PVL_ASSERT(ratio >= 0.f && ratio < 1.f); return grads[idx1] * (1.f - ratio) + (int(idx2 < NEntries) * grads[idx2]) * ratio; } @@ -159,7 +159,7 @@ class CubicSpline : public Kernel, Float, Dim> { // template Float valueImpl(const Float qSqr) const { const Float q = sqrt(qSqr); - ASSERT(q >= 0); + PVL_ASSERT(q >= 0); if (q < 1.f) { return normalization[Dim - 1] * (0.25f * pow<3>(2.f - q) - pow<3>(1.f - q)); } diff --git a/MarchingCubes.cpp b/MarchingCubes.cpp new file mode 100644 index 0000000..4f3f78a --- /dev/null +++ b/MarchingCubes.cpp @@ -0,0 +1,293 @@ +#include "MarchingCubes.hpp" + +namespace Pvl { + +/// Cached edge data and triangle indices +/// see http://paulbourke.net/geometry/polygonise/ + +// clang-format off +const int MC_EDGES[256]= { + 0x000, 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c, 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00, + 0x190, 0x099, 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c, 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90, + 0x230, 0x339, 0x033, 0x13a, 0x636, 0x73f, 0x435, 0x53c, 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30, + 0x3a0, 0x2a9, 0x1a3, 0x0aa, 0x7a6, 0x6af, 0x5a5, 0x4ac, 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0, + 0x460, 0x569, 0x663, 0x76a, 0x066, 0x16f, 0x265, 0x36c, 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60, + 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0x0ff, 0x3f5, 0x2fc, 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0, + 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x055, 0x15c, 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950, + 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0x0cc, 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0, + 0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc, 0x0cc, 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0, + 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c, 0x15c, 0x055, 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650, + 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc, 0x2fc, 0x3f5, 0x0ff, 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0, + 0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c, 0x36c, 0x265, 0x16f, 0x066, 0x76a, 0x663, 0x569, 0x460, + 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac, 0x4ac, 0x5a5, 0x6af, 0x7a6, 0x0aa, 0x1a3, 0x2a9, 0x3a0, + 0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c, 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x033, 0x339, 0x230, + 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c, 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x099, 0x190, + 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c, 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x000 +}; + +const int MC_TRIANGLES[256][16] = +{{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1}, +{3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1}, +{3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1}, +{3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1}, +{9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1}, +{9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1}, +{2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1}, +{8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1}, +{9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1}, +{4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1}, +{3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1}, +{1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1}, +{4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1}, +{4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1}, +{9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1}, +{5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1}, +{2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1}, +{9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1}, +{0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1}, +{2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1}, +{10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1}, +{4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1}, +{5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1}, +{5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1}, +{9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1}, +{0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1}, +{1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1}, +{10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1}, +{8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1}, +{2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1}, +{7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1}, +{9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1}, +{2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1}, +{11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1}, +{9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1}, +{5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1}, +{11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1}, +{11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1}, +{1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1}, +{9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1}, +{5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1}, +{2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1}, +{0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1}, +{5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1}, +{6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1}, +{3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1}, +{6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1}, +{5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1}, +{1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1}, +{10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1}, +{6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1}, +{8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1}, +{7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1}, +{3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1}, +{5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1}, +{0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1}, +{9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1}, +{8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1}, +{5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1}, +{0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1}, +{6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1}, +{10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1}, +{10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1}, +{8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1}, +{1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1}, +{3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1}, +{0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1}, +{10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1}, +{3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1}, +{6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1}, +{9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1}, +{8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1}, +{3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1}, +{6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1}, +{0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1}, +{10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1}, +{10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1}, +{2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1}, +{7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1}, +{7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1}, +{2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1}, +{1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1}, +{11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1}, +{8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1}, +{0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1}, +{7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1}, +{10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1}, +{2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1}, +{6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1}, +{7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1}, +{2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1}, +{1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1}, +{10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1}, +{10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1}, +{0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1}, +{7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1}, +{6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1}, +{8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1}, +{9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1}, +{6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1}, +{4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1}, +{10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1}, +{8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1}, +{0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1}, +{1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1}, +{8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1}, +{10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1}, +{4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1}, +{10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1}, +{5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1}, +{11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1}, +{9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1}, +{6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1}, +{7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1}, +{3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1}, +{7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1}, +{9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1}, +{3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1}, +{6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1}, +{9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1}, +{1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1}, +{4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1}, +{7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1}, +{6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1}, +{3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1}, +{0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1}, +{6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1}, +{0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1}, +{11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1}, +{6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1}, +{5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1}, +{9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1}, +{1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1}, +{1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1}, +{10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1}, +{0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1}, +{5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1}, +{10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1}, +{11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1}, +{9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1}, +{7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1}, +{2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1}, +{8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1}, +{9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1}, +{9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1}, +{1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1}, +{9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1}, +{9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1}, +{5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1}, +{0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1}, +{10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1}, +{2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1}, +{0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1}, +{0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1}, +{9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1}, +{5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1}, +{3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1}, +{5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1}, +{8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1}, +{0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1}, +{9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1}, +{0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1}, +{1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1}, +{3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1}, +{4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1}, +{9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1}, +{11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1}, +{11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1}, +{2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1}, +{9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1}, +{3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1}, +{1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1}, +{4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1}, +{4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1}, +{0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1}, +{3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1}, +{3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1}, +{0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1}, +{9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1}, +{1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, +{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}; + +// clang-format on + + +const int IDXS1[12] = { 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3 }; +const int IDXS2[12] = { 1, 2, 3, 0, 5, 6, 7, 4, 4, 5, 6, 7 }; + + +} // namespace Pvl diff --git a/Math.hpp b/Math.hpp index d2f7615..382a813 100644 --- a/Math.hpp +++ b/Math.hpp @@ -11,6 +11,16 @@ template T sqr(const T& value) { return value * value; } +template +int sign(const T& value) { + if (value > 0) { + return 1; + } else if (value < 0) { + return -1; + } else { + return 0; + } +} template struct Pow; diff --git a/Matrix.hpp b/Matrix.hpp index f1724fa..90d04f1 100644 --- a/Matrix.hpp +++ b/Matrix.hpp @@ -21,25 +21,25 @@ class Matrix { : rows_{ r1, r2, r3, r4 } {} T& operator()(const int c, const int r) { - ASSERT(r < Rows); - ASSERT(c < Cols); + PVL_ASSERT(r < Rows); + PVL_ASSERT(c < Cols); return rows_[r][c]; } const T& operator()(const int c, const int r) const { - ASSERT(r < Rows); - ASSERT(c < Cols); + PVL_ASSERT(r < Rows); + PVL_ASSERT(c < Cols); return rows_[r][c]; } const Vector& row(const int idx) const { - ASSERT(idx < Rows); + PVL_ASSERT(idx < Rows); return rows_[idx]; } Vector column(const int idx) const { - ASSERT(idx < Cols); + PVL_ASSERT(idx < Cols); Vector col; for (int i = 0; i < Rows; ++i) { col[i] = row(i)[idx]; @@ -55,6 +55,10 @@ class Matrix { return res; } + static Matrix null() { + return Matrix(Vector(0), Vector(0), Vector(0)); + } + static Matrix identity() { Matrix m; for (int j = 0; j < Rows; ++j) { @@ -65,6 +69,36 @@ class Matrix { return m; } + Matrix operator+(const Matrix& other) const { + Matrix m; + for (int j = 0; j < Rows; ++j) { + for (int i = 0; i < Cols; ++i) { + m(i, j) = (*this)(i, j) + other(i, j); + } + } + return m; + } + + Matrix operator-(const Matrix& other) const { + Matrix m; + for (int j = 0; j < Rows; ++j) { + for (int i = 0; i < Cols; ++i) { + m(i, j) = (*this)(i, j) - other(i, j); + } + } + return m; + } + + Matrix& operator+=(const Matrix& other) { + *this = *this + other; + return *this; + } + + Matrix& operator-=(const Matrix& other) { + *this = *this - other; + return *this; + } + template Matrix operator*(const Matrix& other) const { Matrix result; @@ -80,6 +114,16 @@ class Matrix { using Mat33f = Matrix; using Mat44f = Matrix; +inline Mat33f outerProd(const Vec3f& v1, const Vec3f& v2) { + Mat33f res; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + res(i, j) = v1[i] * v2[j]; + } + } + return res; +} + 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/PlyWriter.hpp b/PlyWriter.hpp index e087df2..9496bf0 100644 --- a/PlyWriter.hpp +++ b/PlyWriter.hpp @@ -26,6 +26,9 @@ class PlyWriter { out << "property float x\n"; out << "property float y\n"; out << "property float z\n"; + out << "property float nx\n"; + out << "property float ny\n"; + out << "property float nz\n"; /// \todo out << "element face "; facePos_ = out.tellp(); out << " \n"; @@ -38,7 +41,9 @@ class PlyWriter { for (int i = 0; i < Dim; ++i) { out_ << p[i] << " "; } - out_ << "\n"; + if (Dim == 3) { + out_ << "0 0 0\n"; + } ++vertexCnt_; return *this; } @@ -51,18 +56,36 @@ class PlyWriter { return *this; } + template + PlyWriter& write(const std::vector>& cloud, + const std::vector>& normals) { + for (std::size_t i = 0; i < cloud.size(); ++i) { + const auto& p = cloud[i]; + const auto& n = normals[i]; + out_ << p[0] << " " << p[1] << " " << p[2] << " " << n[0] << " " << n[1] << " " + << n[2] << "\n"; + } + vertexCnt_ += cloud.size(); + return *this; + } + template PlyWriter& operator<<(const TriangleMesh& mesh) { for (Index i = 0; i < mesh.numVertices(); ++i) { const Vec& p = mesh.points[i]; - out_ << p[0] << " " << p[1] << " " << p[2] << "\n"; + out_ << p[0] << " " << p[1] << " " << p[2] << " 0 0 0\n"; } + std::size_t validCnt = 0; for (Index i = 0; i < mesh.numFaces(); ++i) { + if (!mesh.valid(FaceHandle(i))) { + continue; + } auto f = mesh.faceIndices(FaceHandle(i)); out_ << "3 " << f[0] << " " << f[1] << " " << f[2] << "\n"; + validCnt++; } vertexCnt_ += mesh.numVertices(); - faceCnt_ += mesh.numFaces(); + faceCnt_ += validCnt; return *this; } diff --git a/MeshUtils.hpp b/Refinement.hpp similarity index 50% rename from MeshUtils.hpp rename to Refinement.hpp index b85639c..c014407 100644 --- a/MeshUtils.hpp +++ b/Refinement.hpp @@ -7,7 +7,7 @@ namespace Pvl { template -void laplacianSmoothing(TriangleMesh& mesh, bool preserveBoundary = true) { +void laplacianSmoothing(TriangleMesh& mesh, bool preserveBoundary = true, float rer = 1.f) { std::vector laplacian(mesh.numVertices(), Vec(0)); ParallelForEach()( mesh.vertexRange(), [&mesh, &laplacian, preserveBoundary](VertexHandle v1) { @@ -25,27 +25,30 @@ void laplacianSmoothing(TriangleMesh& mesh, bool preserveBoundary = } }); std::vector biharmonic(mesh.numVertices(), Vec(0)); - ParallelForEach()( - 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]; - } - }); + if (rer > 0.f) { + ParallelForEach()( + 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()(Index(0), Index(mesh.numVertices()), [&mesh, &biharmonic](std::size_t i) { - mesh.points[i] += 0.5f * biharmonic[i]; - }); + ParallelFor()( + Index(0), Index(mesh.numVertices()), [&mesh, &laplacian, &biharmonic, &rer](std::size_t i) { + mesh.points[i] += 0.5 * (rer * biharmonic[i] + (1.f - rer) * laplacian[i]); + }); } } // namespace Pvl diff --git a/Simplification.hpp b/Simplification.hpp new file mode 100644 index 0000000..1913ac3 --- /dev/null +++ b/Simplification.hpp @@ -0,0 +1,212 @@ +#pragma once + +#include "PlyWriter.hpp" +#include "TriangleMesh.hpp" +#include + +namespace Pvl { + +template +class MidpointPlacement { +public: + Vec operator()(const Vec& v1, const Vec& v2) const { + return 0.5 * (v1 + v2); + } +}; + +template +class EdgeLengthCost { +public: + typename Vec::Float operator()(const Vec& v1, const Vec& v2) const { + return norm(v1 - v2); + } +}; + +class EdgeCountStop { + std::size_t count_; + +public: + EdgeCountStop(std::size_t count) + : count_(count) {} + + bool operator()(std::size_t collapsed) const { + return collapsed > count_; + } +}; + +class CollapseQueue { + using EdgeCost = std::pair; + std::map edges_; + std::set queue_; + +public: + void insert(HalfEdgeHandle eh, const float cost) { + EdgeCost ec{ cost, eh }; + PVL_ASSERT(edges_.find(eh) == edges_.end()); + queue_.insert(ec); + edges_[eh] = ec; + } + + void update(HalfEdgeHandle eh, const float cost) { + /// \todo fix PVL_ASSERT(edges_.find(eh) != edges_.end()); + if (edges_.find(eh) == edges_.end()) { + insert(eh, cost); + } else { + EdgeCost oldEc = edges_[eh]; + queue_.erase(oldEc); + + EdgeCost newEc{ cost, eh }; + queue_.insert(newEc); + } + } + + std::pair pop() { + EdgeCost ec = *queue_.begin(); + queue_.erase(ec); + edges_.erase(ec.second); + return std::make_pair(ec.second, ec.first); + } + + void remove(HalfEdgeHandle eh) { + PVL_ASSERT(edges_.find(eh) != edges_.end()); + EdgeCost ec = edges_[eh]; + queue_.erase(ec); + edges_.erase(ec.second); + } + + bool empty() const { + return queue_.empty(); + } +}; + +template +void savePatch(const TriangleMesh& mesh, HalfEdgeHandle eh) { + TriangleMesh patch; + std::set faces; + for (FaceHandle fh : mesh.faceRing(mesh.from(eh))) { + faces.insert(fh); + } + for (FaceHandle fh : mesh.faceRing(mesh.to(eh))) { + faces.insert(fh); + } + + std::set vertices; + for (FaceHandle fh : faces) { + VertexHandle v1 = patch.addVertex(); + VertexHandle v2 = patch.addVertex(); + VertexHandle v3 = patch.addVertex(); + patch.addFace(v1, v2, v3); + + for (VertexHandle vh : mesh.vertexRing(fh)) { + vertices.insert(vh); + } + + std::array is = mesh.faceIndices(fh); + patch.points.push_back(mesh.points[is[0]]); + patch.points.push_back(mesh.points[is[1]]); + patch.points.push_back(mesh.points[is[2]]); + } + std::ofstream ofs("patch-faces.ply"); + PlyWriter writer(ofs); + writer << patch; + + std::ofstream out("patch-points.ply"); + out << "ply\n"; + out << "format ascii 1.0\n"; + out << "comment Created by PVL library\n"; + out << "element vertex " << vertices.size() << "\n"; + out << "property float x\n"; + out << "property float y\n"; + out << "property float z\n"; + out << "property uchar red\n"; + out << "property uchar green\n"; + out << "property uchar blue\n"; + out << "element face 0\n"; + out << "property list uchar int vertex_index\n"; + out << "end_header\n"; + for (VertexHandle vh : vertices) { + Vec p = mesh.points[vh]; + Vec c; + if (vh == mesh.from(eh)) { + c = Vec(0, 0, 255); + } else if (vh == mesh.to(eh)) { + c = Vec(255, 0, 0); + } else { + c = Vec(128, 128, 128); + } + out << p[0] << " " << p[1] << " " << p[2] << " " << c[0] << " " << c[1] << " " << c[2] + << "\n"; + } +} + +template +void simplify(TriangleMesh& mesh, + const Cost& cost, + const Placement& placement, + const Stop& stop) { + + CollapseQueue queue; + for (EdgeHandle eh : mesh.edgeRange()) { + HalfEdgeHandle heh = mesh.halfEdge(eh); + float c = cost(mesh.points[mesh.from(heh)], mesh.points[mesh.to(heh)]); + queue.insert(heh, c); + } + + /*for (HalfEdgeHandle heh : mesh.halfEdgeRange()) { + EdgeHandle eh = mesh.edge(heh); + HalfEdgeHandle h = mesh.halfEdge(eh); + float c = cost(mesh.points[mesh.from(h)], mesh.points[mesh.to(h)]); + queue.update(h, c); + }*/ + + std::size_t cnt = 0; + HalfEdgeHandle collapsedEdge; + float c; + while (!queue.empty()) { + std::tie(collapsedEdge, c) = queue.pop(); + if (mesh.removed(collapsedEdge)) { + continue; + } + if (mesh.collapseAllowed(collapsedEdge)) { + Vec p = placement( + mesh.points[mesh.from(collapsedEdge)], mesh.points[mesh.to(collapsedEdge)]); + std::vector ring; + for (HalfEdgeHandle eh : mesh.halfEdgeRing(mesh.from(collapsedEdge))) { + ring.push_back(eh); + } + for (HalfEdgeHandle eh : mesh.halfEdgeRing(mesh.to(collapsedEdge))) { + ring.push_back(eh); + } + std::cout << "# " << cnt << " Collapsing " << mesh.to(collapsedEdge) << " into " + << mesh.from(collapsedEdge) << ", cost = " << c << std::endl; + /*std::cout << "ring:\n"; + for (HalfEdgeHandle eh : ring) { + std::cout << mesh.from(eh) << "-" << mesh.to(eh) << "\n"; + } + std::cout << "faces:\n"; + for (HalfEdgeHandle eh : ring) { + std::array is = mesh.faceIndices(mesh.left(eh)); + std::cout << is[0] << " " << is[1] << " " << is[2] << std::endl; + } + std::cout << std::endl; + savePatch(mesh, collapsedEdge);*/ + mesh.collapse(collapsedEdge, p); + for (HalfEdgeHandle heh : ring) { + /// \todo fix this + if (!mesh.valid(heh)) { + continue; + } + HalfEdgeHandle heh1 = mesh.halfEdge(mesh.edge(heh)); + if (!mesh.removed(heh1)) { + float c = cost(mesh.points[mesh.from(heh1)], mesh.points[mesh.to(heh1)]); + queue.update(heh1, c); + } + } + if (stop(cnt++)) { + return; + } + } + } +} + +} // namespace Pvl diff --git a/Svd.hpp b/Svd.hpp index 00e89b8..6add7fc 100644 --- a/Svd.hpp +++ b/Svd.hpp @@ -1,3 +1,5 @@ +#pragma once + #include "Matrix.hpp" namespace Pvl { @@ -39,7 +41,7 @@ int dsvd(float (&a)[3][3], float* w, float (&v)[3][3]) { double anorm = 0.0, g = 0.0, scale = 0.0; double* rv1; - ASSERT(m >= n, "#rows must be > #cols"); + PVL_ASSERT(m >= n, "#rows must be > #cols"); rv1 = (double*)malloc((unsigned int)n * sizeof(double)); @@ -57,7 +59,7 @@ int dsvd(float (&a)[3][3], float* w, float (&v)[3][3]) { a[k][i] = (float)((double)a[k][i] / scale); s += ((double)a[k][i] * (double)a[k][i]); } - ASSERT(s >= 0., s); + PVL_ASSERT(s >= 0., s); f = (double)a[i][i]; g = -SIGN(sqrt(s), f); h = f * g - s; @@ -87,7 +89,7 @@ int dsvd(float (&a)[3][3], float* w, float (&v)[3][3]) { a[i][k] = (float)((double)a[i][k] / scale); s += ((double)a[i][k] * (double)a[i][k]); } - ASSERT(s >= 0., s); + PVL_ASSERT(s >= 0., s); f = (double)a[i][l]; g = -SIGN(sqrt(s), f); h = f * g - s; @@ -201,7 +203,7 @@ int dsvd(float (&a)[3][3], float* w, float (&v)[3][3]) { } break; } - ASSERT(its < 30, "No convergence after 30,000! iterations "); + PVL_ASSERT(its < 30, "No convergence after 30,000! iterations "); /* shift from bottom 2 x 2 minor */ @@ -264,16 +266,14 @@ int dsvd(float (&a)[3][3], float* w, float (&v)[3][3]) { template Svd singularValueDecomposition(const Matrix& t) { - Vector diag = t.diagonal(); - Vector 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]) }, + { float(t(0, 0)), float(t(1, 0)), float(t(2, 0)) }, + { float(t(0, 1)), float(t(1, 1)), float(t(2, 1)) }, + { float(t(0, 2)), float(t(1, 2)), float(t(2, 2)) }, }; dsvd(U, S, V); diff --git a/TriangleMesh.hpp b/TriangleMesh.hpp index 64f9718..6802f5b 100644 --- a/TriangleMesh.hpp +++ b/TriangleMesh.hpp @@ -9,13 +9,19 @@ namespace Pvl { template class TriangleMesh : public Graph { public: - using Float = typename Vec::Type; + using Float = typename Vec::Float; static constexpr int Dim = Vec::size(); public: TriangleMesh() = default; std::vector points; + + // to -> from + void collapse(HalfEdgeHandle eh, const Vec& placement) { + points[to(eh)] = points[from(eh)] = placement; + Graph::collapse(eh); + } }; } // namespace Pvl diff --git a/Vector.hpp b/Vector.hpp index 54d22a3..8e3ba93 100644 --- a/Vector.hpp +++ b/Vector.hpp @@ -11,11 +11,11 @@ class Vector { std::array values_; public: - using Type = T; + using Float = T; Vector() = default; - Vector(const T value) { + explicit Vector(const T value) { for (int i = 0; i < Dim; ++i) { values_[i] = value; } @@ -31,12 +31,12 @@ class Vector { : values_{ x, y, z, w } {} T& operator[](const int idx) { - ASSERT(unsigned(idx) < unsigned(Dim), idx, Dim); + PVL_ASSERT(unsigned(idx) < unsigned(Dim), idx, Dim); return values_[idx]; } const T& operator[](const int idx) const { - ASSERT(unsigned(idx) < unsigned(Dim), idx, Dim); + PVL_ASSERT(unsigned(idx) < unsigned(Dim), idx, Dim); return values_[idx]; } @@ -71,6 +71,16 @@ class Vector { return *this; } + Vector& operator*=(const T value) { + *this = *this * value; + return *this; + } + + Vector& operator/=(const T value) { + *this = *this / value; + return *this; + } + Vector operator+(const Vector& other) const { Vector res; for (int i = 0; i < Dim; ++i) { diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..72a7c23 --- /dev/null +++ b/main.cpp @@ -0,0 +1,435 @@ +#include "Cloud.hpp" +#include "Graph.hpp" +#include "KdTree.hpp" +#include "Kernels.hpp" +#include "PlyReader.hpp" +#include "PlyWriter.hpp" +#include "Refinement.hpp" +#include "Simplification.hpp" +#include "Svd.hpp" +#include "UniformGrid.hpp" +#include +#include + +#define CATCH_CONFIG_MAIN // This tells Catch to provide a main() - only do this in one cpp + // file +#include "catch.hpp" + +using namespace Pvl; +/*std::cout << "Test" << std::endl; +Pvl::KdTree tree; +std::vector points = { + { 0.f, 0.f, 0.f }, + { 0.f, 0.f, 4.f }, + { 1.f, 0.f, 0.f }, +}; +tree.build(points); + +// std::vector neighs; +std::array neighs; +int n = tree.rangeQuery(Pvl::Vec3f(0.f, 0.f, 0.f), 1.5f, neighs.begin()); + +std::cout << "Found " << n << " neighs" << std::endl; +for (auto& n : neighs) { + std::cout << " - " << n << std::endl; +} + +std::stringstream ss; +Pvl::PlyWriter ply(ss); +for (auto& p : points) { + ply << p; +} +ply.close(); +std::cout << ss.str() << "\n"; + +std::stringstream iss(ss.str()); +Pvl::PlyReader rd(iss); +std::vector ps = rd.read(); +std::cout << "Read points \n"; +for (auto& p : ps) { + std::cout << p[0] << "," << p[1] << "," << p[2] << "\n"; +} + +std::cout << "Grid\n"; +Pvl::UniformGrid grid(Pvl::Vec3i(2, 3, 2)); +int cntr = 0; +for (auto& f : grid) { + std::cout << cntr++ << "-" << f << "\n"; +}*/ + +/* Pvl::Graph graph; + Pvl::VertexHandle a = graph.addVertex(); + Pvl::VertexHandle b = graph.addVertex(); + Pvl::VertexHandle c = graph.addVertex(); + Pvl::VertexHandle d = graph.addVertex(); + Pvl::VertexHandle e = graph.addVertex(); + Pvl::VertexHandle f = graph.addVertex(); + + Pvl::FaceHandle f1 = graph.addFace(a, b, c); + Pvl::FaceHandle f2 = graph.addFace(e, f, a); + Pvl::FaceHandle f3 = graph.addFace(d, e, a); + Pvl::FaceHandle f4 = graph.addFace(a, c, d); + + std::cout << "Faces = " << graph.numFaces() << std::endl; + std::cout << "Vertices = " << graph.numVertices() << std::endl; + + for (Pvl::VertexHandle vh : graph.vertexRing(a)) { + std::cout << " - " << vh.index() << std::endl; + }*/ + +TEST_CASE("edge circulator inner", "[circulators]") { + Graph graph; + VertexHandle center = graph.addVertex(); + VertexHandle v0 = graph.addVertex(); + VertexHandle v1 = graph.addVertex(); + VertexHandle v2 = graph.addVertex(); + VertexHandle v3 = graph.addVertex(); + graph.addFace(v0, v1, center); + graph.addFace(v1, v2, center); + graph.addFace(v2, v3, center); + graph.addFace(v3, v0, center); + + Vertex::EdgeRange edgeRing = graph.edgeRing(center); + for (EdgeHandle eh : edgeRing) { + std::cout << graph.from(graph.halfEdge(eh)) << "-" << graph.to(graph.halfEdge(eh)) + << std::endl; + } +} + +TEST_CASE("edge circulator boundary", "[circulators]") { + Graph graph; + VertexHandle center = graph.addVertex(); + VertexHandle v0 = graph.addVertex(); + VertexHandle v1 = graph.addVertex(); + VertexHandle v2 = graph.addVertex(); + VertexHandle v3 = graph.addVertex(); + graph.addFace(center, v0, v1); + graph.addFace(center, v1, v2); + graph.addFace(center, v2, v3); + + Vertex::EdgeRange edgeRing = graph.edgeRing(center); + for (EdgeHandle eh : edgeRing) { + std::cout << graph.from(graph.halfEdge(eh)) << "-" << graph.to(graph.halfEdge(eh)) + << std::endl; + } +} + +TEST_CASE("collapse allowed", "[simplify]") { + Graph graph; + VertexHandle vA = graph.addVertex(); + VertexHandle vB = graph.addVertex(); + VertexHandle v0 = graph.addVertex(); + VertexHandle v1 = graph.addVertex(); + + graph.addFace(vA, vB, v0); + graph.addFace(vA, v0, v1); + graph.addFace(vB, v1, v0); + + HalfEdgeHandle eh = graph.halfEdge(vA, vB); + REQUIRE_FALSE(graph.removed(eh)); + REQUIRE_FALSE(graph.collapseAllowed(eh)); +} + +VertexHandle operator"" _vh(unsigned long long int i) { + return VertexHandle(i); +} + +/* Creates a following graph + * + * 1 - 2 - 3 + * / \ / \ / \ + * 0 - A - B - 4 + * \ / \ / \ / + * 7 - 6 - 5 + */ +struct SimpleGraphFixture { + Graph graph; + VertexHandle vA; + VertexHandle vB; + std::array vs; + + SimpleGraphFixture() { + for (int i = 0; i < 8; ++i) { + vs[i] = graph.addVertex(); + } + vA = graph.addVertex(); + vB = graph.addVertex(); + + graph.addFace(vs[0], vA, vs[1]); + graph.addFace(vA, vs[2], vs[1]); + graph.addFace(vA, vB, vs[2]); + graph.addFace(vB, vs[3], vs[2]); + graph.addFace(vB, vs[4], vs[3]); + graph.addFace(vs[5], vs[4], vB); + graph.addFace(vs[6], vs[5], vB); + graph.addFace(vs[7], vs[6], vA); + graph.addFace(vs[6], vB, vA); + graph.addFace(vs[0], vs[7], vA); + } +}; + +TEST_CASE_METHOD(SimpleGraphFixture, "halfedge", "[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, "edge range", "[graph]") { + Graph::EdgeRange edges = graph.edgeRange(); + REQUIRE(std::distance(edges.begin(), edges.end()) == 19); + REQUIRE(std::all_of( + edges.begin(), edges.end(), [&](EdgeHandle eh) { return graph.valid(eh); })); + + HalfEdgeHandle heh = graph.halfEdge(vA, vB); + int visited = std::count_if(edges.begin(), edges.end(), [this, heh](EdgeHandle eh) { + return heh == graph.halfEdge(eh); + }); + REQUIRE(visited == 1); +} + +TEST_CASE_METHOD(SimpleGraphFixture, "collapse simple", "[simplify]") { + Graph::EdgeRange edges = graph.edgeRange(); + HalfEdgeHandle collapsible = graph.halfEdge(vA, vB); + + REQUIRE(std::all_of(edges.begin(), edges.end(), [&](EdgeHandle eh) { + HalfEdgeHandle heh = graph.halfEdge(eh); + if (heh == collapsible) { + return graph.collapseAllowed(heh); + } else { + return !graph.collapseAllowed(heh); + } + })); + + 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); + } + })); +} + +TEST_CASE("collapse simple 2", "[simplify]") { + Graph graph; + std::array vs; + for (int i = 0; i < 8; ++i) { + vs[i] = graph.addVertex(); + } + Pvl::VertexHandle vA = graph.addVertex(); + Pvl::VertexHandle vB = graph.addVertex(); + + graph.addFace(vs[0], vA, vs[1]); + graph.addFace(vA, vs[2], vs[1]); + graph.addFace(vA, vB, vs[2]); + graph.addFace(vB, vs[3], vs[2]); + graph.addFace(vB, vs[4], vs[3]); + graph.addFace(vs[5], vs[4], vB); + graph.addFace(vs[6], vs[5], vB); + graph.addFace(vs[7], vs[6], vA); + graph.addFace(vs[6], vB, vA); + graph.addFace(vs[0], vs[7], vA); + + std::cout << "precollapse" << std::endl; + for (FaceHandle fh : graph.faceRange()) { + std::cout << "Face " << fh << " - " << std::flush; + for (VertexHandle vh : graph.vertexRing(fh)) { + std::cout << vh << " "; + } + std::cout << std::endl; + } + + HalfEdgeHandle eh = graph.halfEdge(vA, vB); + graph.collapse(eh); + + REQUIRE(graph.valid(vA)); + REQUIRE_FALSE(graph.valid(vB)); + auto range = graph.vertexRing(vA); + std::set ring(range.begin(), range.end()); + REQUIRE( + ring == std::set({ 0_vh, 1_vh, 2_vh, 3_vh, 4_vh, 5_vh, 6_vh, 7_vh })); + std::cout << "postcollapse " << std::endl; + for (FaceHandle fh : graph.faceRange()) { + std::cout << "Face " << fh << " - " << std::flush; + if (!graph.valid(fh)) { + std::cout << " removed!" << std::endl; + continue; + } + for (VertexHandle vh : graph.vertexRing(fh)) { + REQUIRE(graph.valid(vh)); + std::cout << vh << " "; + } + for (HalfEdgeHandle heh : graph.halfEdgeRing(fh)) { + REQUIRE(graph.valid(heh)); + } + for (FaceHandle f : graph.faceRing(fh)) { + REQUIRE(graph.valid(f)); + } + std::cout << std::endl; + } +} + +TEST_CASE("simplify simple", "[simplify]") { + Pvl::TriangleMesh mesh; + std::array vs; + for (int i = 0; i < 8; ++i) { + vs[i] = mesh.addVertex(); + } + Pvl::VertexHandle vA = mesh.addVertex(); + Pvl::VertexHandle vB = mesh.addVertex(); + mesh.points.resize(mesh.numVertices()); + mesh.points[vA] = Pvl::Vec3f(0, 0, 0); + mesh.points[vB] = Pvl::Vec3f(1, 0, 0); + mesh.points[vs[0]] = Pvl::Vec3f(-0.5, 0, 0); + mesh.points[vs[1]] = Pvl::Vec3f(-0.1, 0.5, 0); + mesh.points[vs[2]] = Pvl::Vec3f(0.5, 0.6, 0); + mesh.points[vs[3]] = Pvl::Vec3f(1.3, 0.3, 0); + mesh.points[vs[4]] = Pvl::Vec3f(1.6, 0, 0); + mesh.points[vs[5]] = Pvl::Vec3f(1.3, -0.3, 0); + mesh.points[vs[6]] = Pvl::Vec3f(1, -0.8, 0); + mesh.points[vs[7]] = Pvl::Vec3f(0, -0.9, 0); + + mesh.addFace(vs[0], vA, vs[1]); + mesh.addFace(vA, vs[2], vs[1]); + mesh.addFace(vA, vB, vs[2]); + mesh.addFace(vB, vs[3], vs[2]); + mesh.addFace(vB, vs[4], vs[3]); + mesh.addFace(vs[5], vs[4], vB); + mesh.addFace(vs[6], vs[5], vB); + mesh.addFace(vs[7], vs[6], vA); + mesh.addFace(vs[6], vB, vA); + mesh.addFace(vs[0], vs[7], vA); + + Pvl::HalfEdgeHandle eh = mesh.halfEdge(vA, vB); + std::cout << "A-B edge = " << eh << std::endl; + { + std::ofstream ofs("base.ply"); + Pvl::PlyWriter writer(ofs); + writer << mesh; + } + std::cout << "A ring:" << std::endl; + for (Pvl::VertexHandle vh : mesh.vertexRing(vA)) { + std::cout << vh << ","; + } + std::cout << std::endl; + std::cout << "B ring:" << std::endl; + for (Pvl::VertexHandle vh : mesh.vertexRing(vB)) { + std::cout << vh << ","; + } + std::cout << std::endl; + + mesh.collapse(eh, Pvl::Vec3f(0.5, 0., 0.)); + std::cout << "After collapse A ring:" << std::endl; + std::cout << "A ring:" << std::endl; + for (Pvl::VertexHandle vh : mesh.vertexRing(vA)) { + std::cout << vh << ","; + } + std::cout << std::endl; + + { + std::ofstream ofs("simplified.ply"); + Pvl::PlyWriter writer(ofs); + writer << mesh; + } +} + +TEST_CASE("Test bunny", "[mesh]") { + std::ifstream ifs("/home/pavel/projects/pvl/data/bunny-fixed.ply"); + Pvl::PlyReader reader(ifs); + auto bunny = reader.readMesh(); + + auto vertices = bunny.vertexRange(); + auto faces = bunny.faceRange(); + auto halfEdges = bunny.halfEdgeRange(); + auto edges = bunny.edgeRange(); + + auto valid = [&](auto h) { return bunny.valid(h); }; + REQUIRE(std::all_of(vertices.begin(), vertices.end(), valid)); + REQUIRE(std::all_of(faces.begin(), faces.end(), valid)); + REQUIRE(std::all_of(halfEdges.begin(), halfEdges.end(), valid)); + REQUIRE(std::all_of(edges.begin(), edges.end(), valid)); +} + +TEST_CASE("simplify bunny", "[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::simplify(bunny, + Pvl::EdgeLengthCost{}, + Pvl::MidpointPlacement{}, + Pvl::EdgeCountStop{ 33000 }); + { + std::ofstream ofs("simplified.ply"); + Pvl::PlyWriter writer(ofs); + writer << bunny; + } +} +/* std::ifstream ifs("/home/pavel/projects/pvl/data/pc.ply"); + Pvl::PlyReader reader(ifs); + std::vector points; + { + auto all = reader.readCloud(); + for (std::size_t i = 0; i < all.size(); i += 10) { + points.push_back(all[i]); + } + } + std::cout << "Loaded cloud with " << points.size() << " points " << std::endl; + std::cout << "Estimating normals" << std::endl; + std::vector normals = Pvl::estimateNormals(points); + + { + std::ofstream ofs("pc-raw.ply"); + Pvl::PlyWriter writer(ofs); + writer.write(points, normals); + } + + std::cout << "Orienting normals" << std::endl; + Pvl::orientNormals(points, normals); + { + std::ofstream ofs("pc-oriented.ply"); + Pvl::PlyWriter writer(ofs); + writer.write(points, normals); + } +*/ +/*std::ifstream +ifs("/home/pavel/projects/random/mesh/data/bunny/reconstruction/bun_zipper.ply"); +Pvl::PlyReader reader(ifs); +auto mesh = reader.readMesh();*/ + +/* for (int i = 0; i < 200; ++i) { + std::cout << "iter = " << i << std::endl; + Pvl::laplacianSmoothing(mesh); + } + std::ofstream ofs("bunny.ply"); + Pvl::PlyWriter writer(ofs); + writer << mesh; + + std::ofstream ofs2("boundary.ply"); + Pvl::PlyWriter boun(ofs2); + for (auto vh : mesh.vertexRange()) { + if (mesh.boundary(vh)) { + boun << mesh.points[vh]; + } + }*/ + +/*std::vector normals = Pvl::estimateNormals(mesh.points); + +{ + std::ofstream ofs("bunny-raw.ply"); + Pvl::PlyWriter writer(ofs); + writer.write(mesh.points, normals); +} + +Pvl::orientNormals(mesh.points, normals); +{ + std::ofstream ofs("bunny-oriented.ply"); + Pvl::PlyWriter writer(ofs); + writer.write(mesh.points, normals); +*/