From f079a267ba6333efa699b45eb22a26839f696beb Mon Sep 17 00:00:00 2001 From: Ilya Glushchenko Date: Thu, 1 Nov 2018 19:49:37 +0300 Subject: [PATCH] [#27] QHull DOD refactoring --- Epona/CMakeLists.txt | 1 - Epona/include/Epona/Analysis.hpp | 14 + Epona/include/Epona/HalfEdgeDataStructure.hpp | 151 ++++++-- Epona/include/Epona/JacobiEigenvalue.hpp | 233 ++++++++---- Epona/include/Epona/QuickhullConvexHull.hpp | 351 +++++++++++++++++- Epona/sources/Analysis.cpp | 16 +- Epona/sources/HalfEdgeDataStructure.cpp | 12 +- Epona/sources/JacobiEigenvalue.cpp | 96 ----- demo/Main.cpp | 7 +- 9 files changed, 670 insertions(+), 211 deletions(-) delete mode 100644 Epona/sources/JacobiEigenvalue.cpp diff --git a/Epona/CMakeLists.txt b/Epona/CMakeLists.txt index a9f611e..2afbc6d 100644 --- a/Epona/CMakeLists.txt +++ b/Epona/CMakeLists.txt @@ -37,7 +37,6 @@ set(EPONA_SOURCES sources/Analysis.cpp sources/HalfEdgeDataStructure.cpp sources/HyperPlane.cpp - sources/JacobiEigenvalue.cpp ) if (MSVC) diff --git a/Epona/include/Epona/Analysis.hpp b/Epona/include/Epona/Analysis.hpp index d1a4f64..a0b41d1 100644 --- a/Epona/include/Epona/Analysis.hpp +++ b/Epona/include/Epona/Analysis.hpp @@ -6,6 +6,7 @@ #ifndef EPONA_HPP #define EPONA_HPP +#include #include #define GLM_ENABLE_EXPERIMENTAL @@ -373,6 +374,19 @@ float LineSegmentPointDistance( * @return barycentric coordinates */ glm::vec3 CalculateBarycentricCoordinates(glm::vec3 p, glm::vec3 a, glm::vec3 b, glm::vec3 c); + +/* + * @brief Calculate area of the triangle and returns true if area is equal to zero + * + * @param a triangle's point + * @param b triangle's point + * @param c triangle's point + * @return if points are on the same line + */ +inline bool OneLine(glm::vec3 a, glm::vec3 b, glm::vec3 c) +{ + return epona::fp::IsZero((a.x * (b.y - c.y) + b.x * (c.y - a.y) + c.x * (a.y - b.y)) / 2.f); +} } // namespace epona #endif // EPONA_HPP diff --git a/Epona/include/Epona/HalfEdgeDataStructure.hpp b/Epona/include/Epona/HalfEdgeDataStructure.hpp index 60f1a0a..2edec70 100644 --- a/Epona/include/Epona/HalfEdgeDataStructure.hpp +++ b/Epona/include/Epona/HalfEdgeDataStructure.hpp @@ -1,11 +1,12 @@ /* -* Copyright (C) 2017 by Godlike +* Copyright (C) 2018 by Godlike * This code is licensed under the MIT license (MIT) * (http://opensource.org/licenses/MIT) */ #ifndef EPONA_HEDS_HPP #define EPONA_HEDS_HPP +#include #include #include #include @@ -21,10 +22,38 @@ class HalfEdgeDataStructure struct HalfEdge; class Face; - using Faces = std::list; - using HalfEdges = std::list; - using face_iterator = Faces::iterator; - using const_face_iterator = Faces::const_iterator; + using face_iterator = std::list::iterator; + using const_face_iterator = std::list::const_iterator; + + HalfEdgeDataStructure() = default; + + HalfEdgeDataStructure(HalfEdgeDataStructure const&) = delete; + + HalfEdgeDataStructure& operator=(HalfEdgeDataStructure const& heds) = delete; + + HalfEdgeDataStructure(HalfEdgeDataStructure&& heds) + { + m_halfEdgeList = std::move(heds.m_halfEdgeList); + m_halfEdgePointerIteratorMap = std::move(heds.m_halfEdgePointerIteratorMap); + m_halfEdgeVerticesIteratorMap = std::move(heds.m_halfEdgeVerticesIteratorMap); + + m_facesList = std::move(heds.m_facesList); + m_faceIteratorMap = std::move(heds.m_faceIteratorMap); + m_faceVerticesIteratorMap = std::move(heds.m_faceVerticesIteratorMap); + } + + HalfEdgeDataStructure& operator=(HalfEdgeDataStructure&& heds) + { + m_halfEdgeList = std::move(heds.m_halfEdgeList); + m_halfEdgePointerIteratorMap = std::move(heds.m_halfEdgePointerIteratorMap); + m_halfEdgeVerticesIteratorMap = std::move(heds.m_halfEdgeVerticesIteratorMap); + + m_facesList = std::move(heds.m_facesList); + m_faceIteratorMap = std::move(heds.m_faceIteratorMap); + m_faceVerticesIteratorMap = std::move(heds.m_faceVerticesIteratorMap); + + return *this; + } /** * @brief HEDS Face data container @@ -51,7 +80,7 @@ class HalfEdgeDataStructure * Under the hood iterates along the inner half-edges of the face. * @tparam HalfEdgeType half-edge type */ - template + template < typename HalfEdgeType = HalfEdge > class HalfEdgeCirculator : public std::iterator { public: @@ -60,7 +89,7 @@ class HalfEdgeDataStructure * @param halfEdge */ explicit HalfEdgeCirculator(HalfEdgeType& halfEdge) - : m_current(&halfEdge) + : m_currentHalfEdge(&halfEdge) { } @@ -70,7 +99,7 @@ class HalfEdgeDataStructure */ HalfEdgeType& operator*() const { - return *m_current; + return *m_currentHalfEdge; } /** @@ -79,7 +108,7 @@ class HalfEdgeDataStructure */ HalfEdgeType* operator->() const { - return m_current; + return m_currentHalfEdge; } /** @@ -88,7 +117,7 @@ class HalfEdgeDataStructure */ HalfEdgeCirculator& operator++() { - m_current = m_current->next; + m_currentHalfEdge = m_currentHalfEdge->next; return *this; } @@ -109,7 +138,7 @@ class HalfEdgeDataStructure */ HalfEdgeCirculator& operator--() { - m_current = m_current->prev; + m_currentHalfEdge = m_currentHalfEdge->prev; return *this; } @@ -131,7 +160,7 @@ class HalfEdgeDataStructure */ bool operator==(HalfEdgeCirculator const& other) const { - return m_current == other.m_current; + return m_currentHalfEdge == other.m_currentHalfEdge; } /** @@ -151,7 +180,7 @@ class HalfEdgeDataStructure */ friend void swap(HalfEdgeCirculator& lhs, HalfEdgeCirculator& rhs) noexcept { - std::swap(lhs.m_current, rhs.m_current); + std::swap(lhs.m_currentHalfEdge, rhs.m_currentHalfEdge); } /** @@ -163,7 +192,7 @@ class HalfEdgeDataStructure } private: - HalfEdgeType* m_current; + HalfEdgeType* m_currentHalfEdge; }; /** @@ -176,7 +205,7 @@ class HalfEdgeDataStructure * Access to adjacent faces is performed via twin half-edge. * @tparam FaceType face type */ - template + template < typename FaceType = Face> class AdjacentFaceCirculator : public std::iterator { public: @@ -185,7 +214,7 @@ class HalfEdgeDataStructure * @param[in] halfEdge a half-edge object */ explicit AdjacentFaceCirculator(HalfEdge& halfEdge) - : m_current(&halfEdge) + : m_currentHalfEdge(&halfEdge) { } @@ -195,7 +224,7 @@ class HalfEdgeDataStructure */ FaceType& operator*() const { - return *m_current->twin->face; + return *m_currentHalfEdge->twin->face; } /** @@ -204,7 +233,7 @@ class HalfEdgeDataStructure */ FaceType* operator->() const { - return m_current->twin->face; + return m_currentHalfEdge->twin->face; } /** @@ -213,7 +242,7 @@ class HalfEdgeDataStructure */ AdjacentFaceCirculator& operator++() { - m_current = m_current->next; + m_currentHalfEdge = m_currentHalfEdge->next; return *this; } @@ -234,7 +263,7 @@ class HalfEdgeDataStructure */ AdjacentFaceCirculator& operator--() { - m_current = m_current->prev; + m_currentHalfEdge = m_currentHalfEdge->prev; return *this; } @@ -256,7 +285,7 @@ class HalfEdgeDataStructure */ bool operator==(AdjacentFaceCirculator const& other) const { - return m_current == other.m_current; + return m_currentHalfEdge == other.m_currentHalfEdge; } /** @@ -276,7 +305,7 @@ class HalfEdgeDataStructure */ friend void swap(AdjacentFaceCirculator& lhs, AdjacentFaceCirculator& rhs) noexcept { - std::swap(lhs.m_current, lhs.m_current); + std::swap(lhs.m_currentHalfEdge, lhs.m_currentHalfEdge); } /** @@ -286,9 +315,14 @@ class HalfEdgeDataStructure { return AdjacentFaceCirculator(*this); } + + operator HalfEdge*() const + { + return m_currentHalfEdge; + } private: - HalfEdge* m_current; + HalfEdge* m_currentHalfEdge; }; /** @@ -321,6 +355,12 @@ class HalfEdgeDataStructure */ const_face_iterator GetAdjacentFaceIterator() const; + //! Stores face hyperplane + HyperPlane hyperPlane; + + //! Stores face index + uint32_t index; + private: HalfEdge* m_halfEdge; }; @@ -360,13 +400,48 @@ class HalfEdgeDataStructure uint64_t vertexIndex; }; + /** + * @brief Inserts face into the current data structure + * @param[in] a vertex index + * @param[in] b vertex index + * @param[in] c vertex index + */ + void MakeFace(uint64_t a, uint64_t b, uint64_t c) + { + MakeFace(a, b, c, {}, 0); + } + /** * @brief Inserts face into the current data structure - * @param[in] a vertex index - * @param[in] b vertex index - * @param[in] c vertex index + * @param a vertex index + * @param b vertex index + * @param c vertex index + * @param hp hyperplane containing input vertices + * @param index outside face index */ - void MakeFace(uint64_t a, uint64_t b, uint64_t c); + void MakeFace(uint64_t a, uint64_t b, uint64_t c, HyperPlane hp, uint32_t index); + + /** + * @brief Inserts face into the current data structure + * @type T array-like type with overaloded operator[] + * @param index face indices array of size 3 + */ + template < typename T > + decltype(auto) GetFace(T index) + { + return GetFace(index[0], index[1], index[2]); + } + + /** + * @brief Inserts face into the current data structure + * @type T array-like type with overaloded operator[] + * @param index face indices array of size 3 + */ + template < typename T > + decltype(auto) GetFace(T index) const + { + return GetFace(index[0], index[1], index[2]); + } /** * @brief Returns a face iterator @@ -429,20 +504,20 @@ class HalfEdgeDataStructure bool operator==(HalfEdgeVertices const& other) const; }; - HalfEdges m_halfEdgeList; + std::list m_halfEdgeList; std::unordered_map< - HalfEdge const*, HalfEdges::iterator + HalfEdge const*, std::list::iterator > m_halfEdgePointerIteratorMap; std::unordered_map< - HalfEdgeVertices, HalfEdges::iterator, HalfEdgeVertices::Hasher + HalfEdgeVertices, std::list::iterator, HalfEdgeVertices::Hasher > m_halfEdgeVerticesIteratorMap; - Faces m_facesList; + std::list m_facesList; std::unordered_map< - Face const*, Faces::iterator + Face const*, std::list::iterator > m_faceIteratorMap; std::unordered_map< - FaceVertices, Faces::iterator, FaceVertices::Hasher + FaceVertices, std::list::iterator, FaceVertices::Hasher > m_faceVerticesIteratorMap; /** @@ -455,12 +530,20 @@ class HalfEdgeDataStructure * @param[in] vertexIndexTo end edge vertex index */ void IntializeHalfEdge( - HalfEdges::iterator he, + std::list::iterator he, HalfEdge* next, HalfEdge* prev, Face* face, uint64_t vertexIndexFrom, uint64_t vertexIndexTo ); }; + +template < typename T > +std::tuple GetRidge(HalfEdgeDataStructure::face_iterator it) +{ + + + return {}; +} } // namespace epona #endif // EPONA_HEDS_HPP diff --git a/Epona/include/Epona/JacobiEigenvalue.hpp b/Epona/include/Epona/JacobiEigenvalue.hpp index af240bb..fbfc3e7 100644 --- a/Epona/include/Epona/JacobiEigenvalue.hpp +++ b/Epona/include/Epona/JacobiEigenvalue.hpp @@ -1,5 +1,5 @@ /* -* Copyright (C) 2017 by Godlike +* Copyright (C) 2018 by Godlike * This code is licensed under the MIT license (MIT) * (http://opensource.org/licenses/MIT) */ @@ -7,77 +7,178 @@ #define EPONA_JACOBI_EIGENVALUE_HPP #include +#define GLM_ENABLE_EXPERIMENTAL +#include #include +#include + +namespace +{ +/** + * @brief Finds maximal absolute value off diagonal matrix element and sets its indices + * + * @param[in] mat matrix to search + * @param[out] i max element row index + * @param[out] j max element column index + */ +inline std::tuple FindMaxNormOffDiagonal(glm::mat3 mat) +{ + uint8_t i = 0; + uint8_t j = 1; + + for (uint8_t p = 0; p < 3; ++p) + { + for (uint8_t q = 0; q < 3; ++q) + { + if (p != q) + { + if (glm::abs(mat[i][j]) < glm::abs(mat[p][q])) + { + i = p; + j = q; + } + } + } + } + + return { i, j }; +} + +/** + * @brief Calculates rotation angle for a given matrix and its element + * + * @param[in] mat matrix to rotate + * @param[in] i element row index + * @param[in] j element column index + * + * @return angle in radians + */ +inline float CalculateRotationAngle(glm::mat3 mat, uint8_t i, uint8_t j, float coverageThreshold) +{ + if (glm::abs(mat[i][i] - mat[j][j]) < coverageThreshold) + { + return (glm::pi() / 4.0f) * (mat[i][j] > 0 ? 1.0f : -1.0f); + } + + return 0.5f * glm::atan(2.0f * mat[i][j] / (mat[i][i] - mat[j][j])); +} + +/** + * @brief Makes Givens rotation matrix from the angle and indices + * + * @param[in] theta rotation angle in radians + * @param[in] i row index + * @param[in] j column index + * + * @return Givens rotation matrix + */ +inline glm::mat3 MakeGivensRotationMatrix(float theta, uint8_t i, uint8_t j) +{ + glm::mat3 g(1); + + g[i][i] = glm::cos(theta); + g[i][j] = glm::sin(theta); + g[j][i] = -glm::sin(theta); + g[j][j] = glm::cos(theta); + + return g; +} +} // namespace :: namespace epona { + +/** + * @brief Calculates eigenvalues and eigenvectors + * + * @param[in] symmetricMatrix target symmetric matrix + * @param[in] coverageThreshold coverage precision threshold + * @param[in] maxIterations maximum amount of iteration + */ +inline std::tuple CalculateJacobiEigenvectorsEigenvalue( + glm::mat3 symmetricMatrix, float coverageThreshold = epona::fp::g_floatingPointThreshold, uint32_t maxIterations = 100 +) +{ + glm::mat3 eigenvectors; + glm::mat3 rotationMatrix; + uint8_t i; + uint8_t j; + uint16_t iterations = 0; + + do + { + std::tie(i, j) = ::FindMaxNormOffDiagonal(symmetricMatrix); + + rotationMatrix = ::MakeGivensRotationMatrix(::CalculateRotationAngle(symmetricMatrix, i, j, coverageThreshold), i, j); + eigenvectors = eigenvectors * rotationMatrix; + symmetricMatrix = (glm::transpose(rotationMatrix) * symmetricMatrix) * rotationMatrix; + + std::tie(i, j) = ::FindMaxNormOffDiagonal(symmetricMatrix); + } + while ((++iterations < maxIterations) && (glm::abs(symmetricMatrix[i][j]) > coverageThreshold)); + + return { eigenvectors, { symmetricMatrix[0][0], symmetricMatrix[1][1], symmetricMatrix[2][2] } }; +} + /** - * @brief Jacobi eigenvalue calculation algorithm + * @brief Calculates eigenvalues + * + * @param[in] symmetricMatrix target symmetric matrix + * @param[in] coverageThreshold coverage precision threshold + * @param[in] maxIterations maximum amount of iteration */ -class JacobiEigenvalue +inline glm::vec3 CalculateJacobiEigenvalue( + glm::mat3 symmetricMatrix, float coverageThreshold = epona::fp::g_floatingPointThreshold, uint32_t maxIterations = 100 +) { -public: - /** - * @brief Constructs and calculates eigenvalues and eigenvectors of a given symmetric matrix - * @param[in] symmetricMatrix target symmetric matrix - * @param[in] coverageThreshold coverage precision threshold - * @param[in] maxIterations maximum amount of iteration - */ - explicit JacobiEigenvalue( - glm::mat3 const& symmetricMatrix, - float coverageThreshold = epona::fp::g_floatingPointThreshold, - uint32_t maxIterations = 100 - ); - - /** - * @brief Returns eigenvectors - * @return eigenvectos matrix - */ - glm::mat3 const& GetEigenvectors() const; - - /** - * @brief Returns eigenvalues - * @return eigenvalue vector - */ - glm::vec3 const& GetEigenvalues() const; - -private: - glm::mat3 const& m_symmetricMatrix; - float const m_coverageThreshold; - uint32_t const m_maxIterations; - glm::mat3 m_eigenvectors; - glm::vec3 m_eigenvalues; - - /** - * @brief Finds maximal absolute value off diagonal matrix element and sets its indices - * @param[in] mat matrix to search - * @param[out] i max element row index - * @param[out] j max element column index - */ - static void FindMaxNormOffDiagonal(glm::mat3 const& mat, uint8_t& i, uint8_t& j); - - /** - * @brief Calculates rotation angle for a given matrix and its element - * @param[in] mat matrix to rotate - * @param[in] i element row index - * @param[in] j element column index - * @return angle in radians - */ - float CalculateRotationAngle(glm::mat3 const& mat, uint8_t i, uint8_t j) const; - - /** - * @brief Makes Givens rotation matrix from the angle and indices - * @param[in] theta rotation angle in radians - * @param[in] i row index - * @param[in] j column index - * @return Givens rotation matrix - */ - static glm::mat3 MakeGivensRotationMatrix(float theta, uint8_t i, uint8_t j); - - /** - * @brief Calculates eigenvalues and eigenvectors - */ - void Calculate(); -}; + glm::mat3 rotationMatrix; + uint8_t i; + uint8_t j; + uint16_t iterations = 0; + + do + { + std::tie(i, j) = ::FindMaxNormOffDiagonal(symmetricMatrix); + + rotationMatrix = ::MakeGivensRotationMatrix(::CalculateRotationAngle(symmetricMatrix, i, j, coverageThreshold), i, j); + symmetricMatrix = (glm::transpose(rotationMatrix) * symmetricMatrix) * rotationMatrix; + + std::tie(i, j) = ::FindMaxNormOffDiagonal(symmetricMatrix); + } while ((++iterations < maxIterations) && (glm::abs(symmetricMatrix[i][j]) > coverageThreshold)); + + return { symmetricMatrix[0][0], symmetricMatrix[1][1], symmetricMatrix[2][2] }; +} + +/** + * @brief Calculates eigenvectors + * + * @param[in] symmetricMatrix target symmetric matrix + * @param[in] coverageThreshold coverage precision threshold + * @param[in] maxIterations maximum amount of iteration + */ +inline glm::mat3 CalculateJacobiEigenvectors( + glm::mat3 symmetricMatrix, float coverageThreshold = epona::fp::g_floatingPointThreshold, uint32_t maxIterations = 100 +) +{ + glm::mat3 eigenvectors(1); + glm::mat3 rotationMatrix(1); + uint8_t i = 0; + uint8_t j = 0; + uint16_t iterations = 0; + + do + { + std::tie(i, j) = ::FindMaxNormOffDiagonal(symmetricMatrix); + + rotationMatrix = ::MakeGivensRotationMatrix(::CalculateRotationAngle(symmetricMatrix, i, j, coverageThreshold), i, j); + eigenvectors = eigenvectors * rotationMatrix; + symmetricMatrix = (glm::transpose(rotationMatrix) * symmetricMatrix) * rotationMatrix; + + std::tie(i, j) = ::FindMaxNormOffDiagonal(symmetricMatrix); + } while ((++iterations < maxIterations) && (glm::abs(symmetricMatrix[i][j]) > coverageThreshold)); + + return eigenvectors; +} + } // namespace epona #endif // EPONA_JACOBI_EIGENVALUE_HPP diff --git a/Epona/include/Epona/QuickhullConvexHull.hpp b/Epona/include/Epona/QuickhullConvexHull.hpp index 7e3f1fa..1102f2f 100644 --- a/Epona/include/Epona/QuickhullConvexHull.hpp +++ b/Epona/include/Epona/QuickhullConvexHull.hpp @@ -1,5 +1,5 @@ /* -* Copyright (C) 2017 by Godlike +* Copyright (C) 2018 by Godlike * This code is licensed under the MIT license (MIT) * (http://opensource.org/licenses/MIT) */ @@ -25,6 +25,351 @@ namespace epona { + +struct ConvexHull +{ + std::vector indices; + std::vector faces; + HalfEdgeDataStructure heds; +}; + +} // namespace epona + + +namespace +{ + +struct FaceOutliers { + std::vector> outliers; + std::vector extremeIndices; + std::vector extremeSignedDistances; +}; + +struct Ridge { + uint32_t aVertex; + uint32_t bVertex; + + bool operator==(Ridge other) const { + return (aVertex == other.aVertex && bVertex == other.bVertex) + || (aVertex == other.bVertex && bVertex == other.aVertex); + } + + bool operator==(uint32_t index) const { + return aVertex == index || bVertex == index; + } +}; + +/** + * @note vertices size must be >= 4 + */ +epona::ConvexHull CalculateConvexHullInitialTetrahedron(std::vector const& vertices) +{ + //Find directions of the maximum spread for the calculation of the frist approximation of the convex hull + glm::mat3 basis = epona::CalculateJacobiEigenvectors( + epona::CalculateCovarianceMatrix( + vertices.begin(), vertices.end(), epona::CalculateExpectedValue(vertices.begin(), vertices.end()))); + basis = { glm::normalize(basis[0]), glm::normalize(basis[1]), glm::normalize(basis[2]) }; + + float distances[4] = {}; + uint32_t indices[4] = {}; + + //Find the furthest point in the direction of the first eigenvector of the covariance matrix + for (uint32_t i = 0; i < vertices.size(); ++i) + { + float const distance = glm::abs(glm::dot(basis[0], vertices[i])); + + if (distance > distances[0]) + { + distances[0] = distance; + indices[0] = i; + } + } + + //Find the furthest point in the direction of the second eigenvector of the covariance matrix + for (uint32_t i = 0; i < vertices.size(); ++i) + { + float const distance = glm::abs(glm::dot(basis[1], vertices[i])); + + if (distance > distances[1] && i != indices[0]) + { + distances[1] = distance; + indices[1] = i; + } + } + + //Find the furthest point in the direction of the third eigenvector of the covariance matrix + for (uint32_t i = 0; i < vertices.size(); ++i) + { + float const distance = glm::abs(glm::dot(basis[2], vertices[i])); + + if (distance > distances[2] && i != indices[0] && i != indices[1]) + { + distances[2] = distance; + indices[2] = i; + } + } + + //Find the furthest point in the direction of the normal of the hyperplane formed by the extreme points + epona::HyperPlane const hyperPlane(vertices[indices[0]], vertices[indices[1]], vertices[indices[3]]); + for (uint32_t i = 0; i < vertices.size(); ++i) + { + float const distance = hyperPlane.Distance(vertices[i]); + + if (distance > distances[3] && indices[0] != i && indices[1] != i && indices[2] != i) + { + distances[3] = distance; + indices[3] = i; + } + } + + glm::vec3 mean(0); + for (uint32_t index : indices) + mean += vertices[index]; + mean /= 4; + + assert( !epona::OneLine(vertices[indices[0]], vertices[indices[1]], vertices[indices[2]]) ); + assert( !epona::OneLine(vertices[indices[0]], vertices[indices[1]], vertices[indices[3]]) ); + assert( !epona::OneLine(vertices[indices[0]], vertices[indices[2]], vertices[indices[3]]) ); + assert( !epona::OneLine(vertices[indices[1]], vertices[indices[2]], vertices[indices[3]]) ); + + epona::HalfEdgeDataStructure heds; + heds.MakeFace(indices[0], indices[1], indices[2], + epona::HyperPlane{vertices[indices[0]], vertices[indices[1]], vertices[indices[2]], &mean }, 0 ); + heds.MakeFace(indices[0], indices[1], indices[3], + epona::HyperPlane{vertices[indices[0]], vertices[indices[1]], vertices[indices[3]], &mean }, 1 ); + heds.MakeFace(indices[0], indices[2], indices[3], + epona::HyperPlane{vertices[indices[0]], vertices[indices[2]], vertices[indices[3]], &mean }, 2 ); + heds.MakeFace(indices[1], indices[2], indices[3], + epona::HyperPlane{vertices[indices[1]], vertices[indices[2]], vertices[indices[3]], &mean }, 3 ); + + //Create initial convex hull tetrahedron + return {{ indices[0], indices[1], indices[2], indices[3] }, + { glm::u32vec3{indices[0], indices[1], indices[2]}, + glm::u32vec3{indices[0], indices[1], indices[3]}, + glm::u32vec3{indices[0], indices[2], indices[3]}, + glm::u32vec3{indices[1], indices[2], indices[3]} }, + std::move(heds) + }; +} + +} // namespace :: + +namespace epona +{ + +inline bool RecalculateConvexHull(ConvexHull& hull, std::vector const& vertices, uint32_t maxIterations = 100) +{ + bool changed = false; + + std::vector indices; + indices.reserve(vertices.size()); + for (uint32_t v = 0; v < vertices.size(); ++v) + if (std::find(hull.indices.begin(), hull.indices.end(), v) == hull.indices.end()) + indices.push_back(v); + std::sort(hull.indices.begin(), hull.indices.end(), std::greater()); + + uint32_t totalOutliersCount = 0; + ::FaceOutliers faceOutliers; + std::vector visibleFaces; + std::vector<::Ridge> boundary; + std::vector temporaryOutliers; + + do { + glm::vec3 mean(0); + for (auto i : hull.indices) + mean += vertices[i]; + mean /= hull.indices.size(); + + //Find outliers and extreme vertices + uint32_t newOutliersCount = 0; + faceOutliers.outliers.resize(hull.faces.size()); + faceOutliers.extremeIndices.resize(hull.faces.size(), UINT32_MAX); + faceOutliers.extremeSignedDistances.resize(hull.faces.size(), -FLT_MAX); + for (uint32_t v : indices) + { + for (uint32_t f = 0; f < hull.faces.size(); ++f) + { + float const signedDistance = hull.heds.GetFace(hull.faces[f])->hyperPlane.SignedDistance(vertices[v]); + if (fp::IsGreater(signedDistance, 0.0f)) { + faceOutliers.outliers[f].push_back(v); + newOutliersCount++; + if (signedDistance > faceOutliers.extremeSignedDistances[f]) { + faceOutliers.extremeSignedDistances[f] = signedDistance; + faceOutliers.extremeIndices[f] = v; + } + break; + } + } + } + + //Find visible faces from the first extreme outlier + uint32_t extremeOutlierIndex = UINT32_MAX; + visibleFaces.clear(); + visibleFaces.reserve(hull.faces.size() / 2); + for (uint32_t f = 0; f < hull.faces.size(); ++f) + { + if (!faceOutliers.outliers[f].empty()) + { + visibleFaces.push_back(f); + std::vector visitedFaces(1, f); + visitedFaces.reserve(hull.faces.size() / 2); + glm::vec3 const extremeVertex = vertices[faceOutliers.extremeIndices[f]]; + + for (uint32_t vf = 0; vf < visibleFaces.size(); ++vf) + { + auto adjFaceIt = hull.heds.GetFace(hull.faces[visibleFaces[vf]])->GetAdjacentFaceIterator(); + + for (uint8_t i = 0; i < 3; ++i, ++adjFaceIt) { + //NOTE: will work faster then open address map till ~ visitedFaces.size() < 100 + if (std::find(visitedFaces.begin(), visitedFaces.end(), adjFaceIt->index) == visitedFaces.end()) + { + visitedFaces.push_back(adjFaceIt->index); + float const signedDistance = adjFaceIt->hyperPlane.SignedDistance(extremeVertex); + if (fp::IsGreater(signedDistance, 0.0f)) + visibleFaces.push_back(adjFaceIt->index); + } + } + } + extremeOutlierIndex = faceOutliers.extremeIndices[f]; + break; + } + } + std::sort(visibleFaces.begin(), visibleFaces.end()); + + if (visibleFaces.empty()) + { + indices.clear(); + } + else + { + changed = true; + assert(extremeOutlierIndex != UINT32_MAX); + + //Find boundary ridges + boundary.clear(); + boundary.reserve(visibleFaces.size()); + + for (uint32_t vf : visibleFaces) + { + auto adjFaceIt = hull.heds.GetFace(hull.faces[vf])->GetAdjacentFaceIterator(); + for (uint8_t i = 0; i < 3; ++i, ++adjFaceIt) + { + if (std::find(visibleFaces.begin(), visibleFaces.end(), adjFaceIt->index) == visibleFaces.end()) + { + auto halfEdge = static_cast(adjFaceIt); + ::Ridge const ridge{ static_cast(halfEdge->vertexIndex), static_cast(halfEdge->prev->vertexIndex) }; + //NOTE: will work faster then open address map till ~ boundary.size() < 100 + if (std::find(boundary.begin(), boundary.end(), ridge) == boundary.end()) + { + boundary.push_back(ridge); + } + } + } + } + + //Collect inliers as outliers and remove them from hull indices + temporaryOutliers.clear(); + temporaryOutliers.reserve(visibleFaces.size() * 3 + newOutliersCount); + for (uint32_t vf : visibleFaces) { + auto face = hull.faces[vf]; + for (uint8_t i = 0; i < 3; ++i) { + //NOTE: will work faster then open address map till ~ (boundary/outliers).size() < 100 + if (std::find(boundary.begin(), boundary.end(), face[i]) == boundary.end() + && std::find(temporaryOutliers.begin(), temporaryOutliers.end(), face[i]) == temporaryOutliers.end()) + { + temporaryOutliers.push_back(face[i]); + //NOTE: will work faster then open address map till ~ hull.indieces.size() < 100 + auto it = std::find(hull.indices.begin(), hull.indices.end(), face[i]); + if (it != hull.indices.end()) + hull.indices.erase(it); + } + } + } + + //Remove visible faces and collect unassigned outliers + for (uint32_t vf : visibleFaces) + { + hull.heds.RemoveFace(hull.heds.GetFace(hull.faces[vf])); + temporaryOutliers.insert(temporaryOutliers.end(), + std::make_move_iterator(faceOutliers.outliers[vf].begin()), + std::make_move_iterator(faceOutliers.outliers[vf].end()) + ); + } + indices.swap(temporaryOutliers); + temporaryOutliers.clear(); + + //Create new faces and assign outliers + for (::Ridge ridge : boundary) + { + uint32_t faceIndex = static_cast(hull.faces.size()); + if (!visibleFaces.empty()) + { + faceIndex = visibleFaces.back(); + hull.faces[faceIndex] = { extremeOutlierIndex, ridge.aVertex, ridge.bVertex }; + visibleFaces.pop_back(); + + faceOutliers.extremeIndices[faceIndex] = UINT32_MAX; + faceOutliers.extremeSignedDistances[faceIndex] = -FLT_MAX; + faceOutliers.outliers[faceIndex].clear(); + } + else + { + hull.faces.emplace_back(extremeOutlierIndex, ridge.aVertex, ridge.bVertex); + } + hull.heds.MakeFace(extremeOutlierIndex, ridge.aVertex, ridge.bVertex, + HyperPlane{ vertices[extremeOutlierIndex], vertices[ridge.aVertex], vertices[ridge.bVertex], &mean }, + faceIndex + ); + //NOTE: will work faster then open address map till ~ hull.indieces.size() < 100 + if (std::find(hull.indices.begin(), hull.indices.end(), extremeOutlierIndex) == hull.indices.end()) + hull.indices.push_back(extremeOutlierIndex); + } + + //Restore structure + if (!visibleFaces.empty()) + { + std::sort(visibleFaces.begin(), visibleFaces.end()); + std::vector faces; + FaceOutliers tmpFaceOutliers; + tmpFaceOutliers.extremeIndices.reserve(faceOutliers.extremeIndices.size()); + tmpFaceOutliers.extremeSignedDistances.reserve(faceOutliers.extremeSignedDistances.size()); + tmpFaceOutliers.outliers.reserve(faceOutliers.outliers.size()); + for (uint32_t i = 0, j = 0; i < hull.faces.size(); ++i) + { + if (j == visibleFaces.size() || i != visibleFaces[j]) + { + faces.push_back(hull.faces[i]); + hull.heds.GetFace(hull.faces[i])->index = static_cast(faces.size() - 1); + tmpFaceOutliers.extremeIndices.push_back(faceOutliers.extremeIndices[i]); + tmpFaceOutliers.extremeSignedDistances.push_back(faceOutliers.extremeSignedDistances[i]); + tmpFaceOutliers.outliers.push_back(std::move(faceOutliers.outliers[i])); + } + else { ++j; } + } + faceOutliers.extremeIndices.swap(tmpFaceOutliers.extremeIndices); + faceOutliers.extremeSignedDistances.swap(tmpFaceOutliers.extremeSignedDistances); + faceOutliers.outliers.swap(tmpFaceOutliers.outliers); + hull.faces.swap(faces); + } + } + + totalOutliersCount = newOutliersCount + static_cast(indices.size()); + for (auto& outliers : faceOutliers.outliers) + totalOutliersCount += static_cast(outliers.size()); + } while (totalOutliersCount && --maxIterations); + + return changed; +} + +inline ConvexHull CalculateConvexHull(std::vector const& vertices, uint32_t maxIterations = 100) +{ + ConvexHull hull = ::CalculateConvexHullInitialTetrahedron(vertices); + + RecalculateConvexHull(hull, vertices, maxIterations); + + return hull; +} + /** * @brief Quickhull convex hull calculation algorithm * @tparam VerticesType STL compatible random access glm::vec3 container @@ -408,8 +753,8 @@ class QuickhullConvexHull { //Calculate covariance matrix and its eigenvectors m_mean = CalculateExpectedValue(m_vertexBuffer.begin(), m_vertexBuffer.end()); - JacobiEigenvalue const eigenvalue(CalculateCovarianceMatrix(m_vertexBuffer.begin(), m_vertexBuffer.end(), m_mean)); - glm::mat3 eigenvectorsNormalized = eigenvalue.GetEigenvectors(); + glm::mat3 eigenvectorsNormalized = CalculateJacobiEigenvectors( + CalculateCovarianceMatrix(m_vertexBuffer.begin(), m_vertexBuffer.end(), m_mean)); eigenvectorsNormalized = { glm::normalize(eigenvectorsNormalized[0]), glm::normalize(eigenvectorsNormalized[1]), diff --git a/Epona/sources/Analysis.cpp b/Epona/sources/Analysis.cpp index 91abcd2..3914b7f 100644 --- a/Epona/sources/Analysis.cpp +++ b/Epona/sources/Analysis.cpp @@ -3,6 +3,7 @@ * This code is licensed under the MIT license (MIT) * (http://opensource.org/licenses/MIT) */ +#include #include #include @@ -29,10 +30,17 @@ glm::vec3 epona::CalculateBarycentricCoordinates(glm::vec3 p, glm::vec3 a, glm:: float const d21 = glm::dot(v2, v1); float const denominator = d00 * d11 - d01 * d01; - glm::vec3 coordinates; - coordinates.y = (d11 * d20 - d01 * d21) / denominator; - coordinates.z = (d00 * d21 - d01 * d20) / denominator; - coordinates.x = 1.0f - coordinates.y - coordinates.z; + glm::vec3 coordinates{ 1, 0, 0 }; + + if (!fp::IsZero(denominator)) + { + coordinates.y = (d11 * d20 - d01 * d21) / denominator; + coordinates.z = (d00 * d21 - d01 * d20) / denominator; + coordinates.x = 1.0f - coordinates.y - coordinates.z; + } + + assert(!isnan(coordinates.x) && !isnan(coordinates.y) && !isnan(coordinates.z)); + assert(!isinf(coordinates.x) && !isinf(coordinates.y) && !isinf(coordinates.z)); return coordinates; } diff --git a/Epona/sources/HalfEdgeDataStructure.cpp b/Epona/sources/HalfEdgeDataStructure.cpp index e160004..2742dd8 100644 --- a/Epona/sources/HalfEdgeDataStructure.cpp +++ b/Epona/sources/HalfEdgeDataStructure.cpp @@ -53,22 +53,24 @@ size_t HalfEdgeDataStructure::FaceVertices::Hasher::operator()(FaceVertices cons ^ std::hash{}(face.c); } -void HalfEdgeDataStructure::MakeFace(uint64_t a, uint64_t b, uint64_t c) +void HalfEdgeDataStructure::MakeFace(uint64_t a, uint64_t b, uint64_t c, HyperPlane hp, uint32_t index) { FaceVertices const faceVerticesKey{a, b, c}; if (m_faceVerticesIteratorMap.find(faceVerticesKey) == m_faceVerticesIteratorMap.end()) { //Allocate half edges - std::array newHalfEdges = {{ + std::array::iterator, 3> newHalfEdges = {{ m_halfEdgeList.emplace(m_halfEdgeList.end()), m_halfEdgeList.emplace(m_halfEdgeList.end()), m_halfEdgeList.emplace(m_halfEdgeList.end()) }}; - //Allocate face + //Allocate face and set HyperPlane auto const backFaceIterator = m_facesList.emplace(m_facesList.end(), *newHalfEdges.back()); m_faceIteratorMap[&*backFaceIterator] = backFaceIterator; m_faceVerticesIteratorMap[faceVerticesKey] = backFaceIterator; + backFaceIterator->hyperPlane = hp; + backFaceIterator->index = index; //Initialize half edges IntializeHalfEdge(newHalfEdges[0], &*newHalfEdges[1], &*newHalfEdges[2], &*backFaceIterator, a, b); @@ -115,7 +117,7 @@ void HalfEdgeDataStructure::RemoveFace(uint64_t a, uint64_t b, uint64_t c) void HalfEdgeDataStructure::RemoveFace(face_iterator faceIterator) { auto heIterator = faceIterator->GetHalfEdgeIterator(); - std::array markedHalfEdgeIterators; + std::array::iterator, 3> markedHalfEdgeIterators; markedHalfEdgeIterators[0] = m_halfEdgePointerIteratorMap[&*heIterator++]; markedHalfEdgeIterators[1] = m_halfEdgePointerIteratorMap[&*heIterator++]; markedHalfEdgeIterators[2] = m_halfEdgePointerIteratorMap[&*heIterator]; @@ -176,7 +178,7 @@ size_t HalfEdgeDataStructure::HalfEdgeVertices::Hasher::operator()(HalfEdgeVerti } void HalfEdgeDataStructure::IntializeHalfEdge( - HalfEdges::iterator he, HalfEdge* next, HalfEdge* prev, Face* face, + std::list::iterator he, HalfEdge* next, HalfEdge* prev, Face* face, uint64_t vertexIndexFrom, uint64_t vertexIndexTo ) { diff --git a/Epona/sources/JacobiEigenvalue.cpp b/Epona/sources/JacobiEigenvalue.cpp deleted file mode 100644 index 95c9a3e..0000000 --- a/Epona/sources/JacobiEigenvalue.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/* -* Copyright (C) 2017 by Godlike -* This code is licensed under the MIT license (MIT) -* (http://opensource.org/licenses/MIT) -*/ -#include - -#define GLM_ENABLE_EXPERIMENTAL -#include -#include - -using namespace epona; - -JacobiEigenvalue::JacobiEigenvalue(glm::mat3 const& symmetricMatrix, float coverageThreshold, uint32_t maxIterations) - : m_symmetricMatrix{symmetricMatrix} - , m_coverageThreshold{glm::abs(coverageThreshold)} - , m_maxIterations{maxIterations} -{ - Calculate(); -} - -glm::mat3 const& JacobiEigenvalue::GetEigenvectors() const -{ - return m_eigenvectors; -} - -glm::vec3 const& JacobiEigenvalue::GetEigenvalues() const -{ - return m_eigenvalues; -} - -void JacobiEigenvalue::FindMaxNormOffDiagonal(glm::mat3 const& mat, uint8_t& i, uint8_t& j) -{ - i = 0; - j = 1; - - for (uint8_t p = 0; p < 3; ++p) - { - for (uint8_t q = 0; q < 3; ++q) - { - if (p != q) - { - if (glm::abs(mat[i][j]) < glm::abs(mat[p][q])) - { - i = p; - j = q; - } - } - } - } -} - -float JacobiEigenvalue::CalculateRotationAngle(glm::mat3 const& mat, uint8_t i, uint8_t j) const -{ - if (glm::abs(mat[i][i] - mat[j][j]) < m_coverageThreshold) - { - return (glm::pi() / 4.0f) * (mat[i][j] > 0 ? 1.0f : -1.0f); - } - - return 0.5f * glm::atan(2.0f * mat[i][j] / (mat[i][i] - mat[j][j])); -} - -glm::mat3 JacobiEigenvalue::MakeGivensRotationMatrix(float theta, uint8_t i, uint8_t j) -{ - glm::mat3 g; - g[i][i] = glm::cos(theta); - g[i][j] = glm::sin(theta); - g[j][i] = -glm::sin(theta); - g[j][j] = glm::cos(theta); - - return g; -} - -void JacobiEigenvalue::Calculate() -{ - glm::mat3 D(m_symmetricMatrix); - glm::mat3 S; - - uint16_t iterations = 0; - bool iterate = true; - while (iterate) - { - uint8_t i, j; - FindMaxNormOffDiagonal(D, i, j); - - glm::mat3 S1 = MakeGivensRotationMatrix(CalculateRotationAngle(D, i, j), i, j); - S = S * S1; - D = (glm::transpose(S1) * D) * S1; - - FindMaxNormOffDiagonal(D, i, j); - iterate = (++iterations < m_maxIterations) && (glm::abs(D[i][j]) > m_coverageThreshold); - } - - m_eigenvalues = {D[0][0], D[1][1], D[2][2]}; - m_eigenvectors = S; -} diff --git a/demo/Main.cpp b/demo/Main.cpp index c7273eb..df85a2c 100644 --- a/demo/Main.cpp +++ b/demo/Main.cpp @@ -1,7 +1,10 @@ -#include +/* +* Copyright (C) 2018 by Godlike +* This code is licensed under the MIT license (MIT) +* (http://opensource.org/licenses/MIT) +*/ int main() { - epona::JacobiEigenvalue eigenvalue(glm::dmat3(1)); return 0; }