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