From 93f0118c669ddfe0b8c75a4b5634d608fd1aa7ab Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 24 Jul 2024 19:14:00 +0000 Subject: [PATCH] Replace GTS with CDT Signed-off-by: Michael Carroll --- CMakeLists.txt | 7 - .../gz/common/DelaunayTriangulation.hh | 45 + graphics/include/gz/common/GTSMeshUtils.hh | 64 - graphics/include/gz/common/MeshCSG.hh | 70 - graphics/include/gz/common/MeshManager.hh | 10 - graphics/src/CDT/CDT.h | 448 ++++ graphics/src/CDT/CDT.hpp | 107 + graphics/src/CDT/CDTUtils.h | 508 ++++ graphics/src/CDT/CDTUtils.hpp | 318 +++ graphics/src/CDT/KDTree.h | 412 ++++ graphics/src/CDT/LICENSE | 373 +++ graphics/src/CDT/LocatorKDTree.h | 81 + graphics/src/CDT/Triangulation.h | 945 ++++++++ graphics/src/CDT/Triangulation.hpp | 2087 +++++++++++++++++ graphics/src/CDT/portable_nth_element.hpp | 313 +++ graphics/src/CDT/predicates.h | 915 ++++++++ graphics/src/CDT/remove_at.hpp | 55 + graphics/src/CMakeLists.txt | 4 +- graphics/src/DelaunayTriangulation.cc | 79 + ..._TEST.cc => DelaunayTriangulation_TEST.cc} | 22 +- graphics/src/GTSMeshUtils.cc | 248 -- graphics/src/MeshCSG.cc | 357 --- graphics/src/MeshManager.cc | 23 +- 23 files changed, 6699 insertions(+), 792 deletions(-) create mode 100644 graphics/include/gz/common/DelaunayTriangulation.hh delete mode 100644 graphics/include/gz/common/GTSMeshUtils.hh delete mode 100644 graphics/include/gz/common/MeshCSG.hh create mode 100644 graphics/src/CDT/CDT.h create mode 100644 graphics/src/CDT/CDT.hpp create mode 100644 graphics/src/CDT/CDTUtils.h create mode 100644 graphics/src/CDT/CDTUtils.hpp create mode 100644 graphics/src/CDT/KDTree.h create mode 100644 graphics/src/CDT/LICENSE create mode 100644 graphics/src/CDT/LocatorKDTree.h create mode 100644 graphics/src/CDT/Triangulation.h create mode 100644 graphics/src/CDT/Triangulation.hpp create mode 100644 graphics/src/CDT/portable_nth_element.hpp create mode 100644 graphics/src/CDT/predicates.h create mode 100644 graphics/src/CDT/remove_at.hpp create mode 100644 graphics/src/DelaunayTriangulation.cc rename graphics/src/{GTSMeshUtils_TEST.cc => DelaunayTriangulation_TEST.cc} (79%) delete mode 100644 graphics/src/GTSMeshUtils.cc delete mode 100644 graphics/src/MeshCSG.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 67db83b5d..3033ff68d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,15 +67,8 @@ endif() gz_find_package(FreeImage VERSION 3.9 REQUIRED_BY graphics PRIVATE_FOR graphics) - #------------------------------------ -# Find GNU Triangulation Surface Library -gz_find_package( - GTS PRETTY gts PURPOSE "GNU Triangulation Surface library" - REQUIRED_BY graphics - PRIVATE_FOR graphics) -#------------------------------------ # Find GDAL gz_find_package(GDAL VERSION 3.0 PKGCONFIG gdal diff --git a/graphics/include/gz/common/DelaunayTriangulation.hh b/graphics/include/gz/common/DelaunayTriangulation.hh new file mode 100644 index 000000000..91f46a600 --- /dev/null +++ b/graphics/include/gz/common/DelaunayTriangulation.hh @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef GZ_COMMON_DELAUNAYTRIANGULATION_HH_ +#define GZ_COMMON_DELAUNAYTRIANGULATION_HH_ + +#include + +#include + +#include + + +namespace gz::common +{ +class SubMesh; + +/// \brief Perform delaunay triangulation on input vertices. +/// \param[in] _vertices A list of all vertices +/// \param[in] _edges A list of edges. Each edge is made of 2 vertex +/// indices from _vertices +/// \param[out] _submesh A submesh that will be populated with the +/// resulting triangles. +/// \return True on success. +bool GZ_COMMON_GRAPHICS_VISIBLE DelaunayTriangulation( + const std::vector &_vertices, + const std::vector &_edges, + gz::common::SubMesh *_submesh); + +} // namespace gz::common +#endif // GZ_COMMON_DELAUNAYTRIANGULATION_HH_ diff --git a/graphics/include/gz/common/GTSMeshUtils.hh b/graphics/include/gz/common/GTSMeshUtils.hh deleted file mode 100644 index fedab7a29..000000000 --- a/graphics/include/gz/common/GTSMeshUtils.hh +++ /dev/null @@ -1,64 +0,0 @@ -/* - * Copyright (C) 2016 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef GZ_COMMON_GTSMESHUTILS_HH_ -#define GZ_COMMON_GTSMESHUTILS_HH_ - -#include - -#include - -#include - - -struct _GtsSurface; -typedef _GtsSurface GtsSurface; - -namespace gz -{ - namespace common - { - class SubMesh; - - /// \class GTSMeshUtils GTSMeshUtils.hh gz/common/GTSMeshUtils.hh - /// \brief Creates GTS utilities for meshes - class GZ_COMMON_GRAPHICS_VISIBLE GTSMeshUtils - { - /// \brief Perform delaunay triangulation on input vertices. - /// \param[in] _vertices A list of all vertices - /// \param[in] _edges A list of edges. Each edge is made of 2 vertex - /// indices from _vertices - /// \param[out] _submesh A submesh that will be populated with the - /// resulting triangles. - /// \return True on success. - public: static bool DelaunayTriangulation( - const std::vector &_vertices, - const std::vector &_edges, - SubMesh *_submesh); - - /// \brief Perform delaunay triangulation on input vertices. - /// \param[in] _vertices A list of all vertices - /// \param[in] _edges A list of edges. Each edge is made of 2 vertex - /// indices from _vertices - /// \return Triangulated GTS surface. - private: static GtsSurface *DelaunayTriangulation( - const std::vector &_vertices, - const std::vector &_edges); - }; - } -} -#endif diff --git a/graphics/include/gz/common/MeshCSG.hh b/graphics/include/gz/common/MeshCSG.hh deleted file mode 100644 index fe2d64723..000000000 --- a/graphics/include/gz/common/MeshCSG.hh +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright (C) 2016 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#ifndef GZ_COMMON_MESHCSG_HH_ -#define GZ_COMMON_MESHCSG_HH_ - -#include - -#include - -struct _GtsSurface; -typedef _GtsSurface GtsSurface; -struct _GPtrArray; -typedef _GPtrArray GPtrArray; - -namespace gz -{ - namespace common - { - class Mesh; - - /// \class MeshCSG MeshCSG.hh gz/common/MeshCSG.hh - /// \brief Creates CSG meshes - class GZ_COMMON_GRAPHICS_VISIBLE MeshCSG - { - /// \brief An enumeration of the boolean operations - public: enum BooleanOperation {UNION, INTERSECTION, DIFFERENCE}; - - /// \brief Constructor - public: MeshCSG(); - - /// \brief Destructor. - public: virtual ~MeshCSG(); - - /// \brief Create a boolean mesh from two meshes - /// \param[in] _m1 the parent mesh in the boolean operation - /// \param[in] _m2 the child mesh in the boolean operation - /// \param[in] _operation the boolean operation applied to the two meshes - /// \param[in] _offset _m2's pose offset from _m1 - /// \return a pointer to the created mesh - public: Mesh *CreateBoolean(const Mesh *_m1, const Mesh *_m2, - const int _operation, - const gz::math::Pose3d &_offset = gz::math::Pose3d::Zero); - - /// \brief Helper method for converting Mesh to GTS Surface - private: void ConvertMeshToGTS(const Mesh *mesh, GtsSurface *surface); - - /// \brief Helper method for merging neighborhood vertices whose positions - // are within epsilon - /// \param[in] _vertices Array of GTS vertices. - /// \param[in] _epsilon Epsilon - private: void MergeVertices(GPtrArray * _vertices, double _epsilon); - }; - } -} - -#endif diff --git a/graphics/include/gz/common/MeshManager.hh b/graphics/include/gz/common/MeshManager.hh index 4b7873f86..da2194bcc 100644 --- a/graphics/include/gz/common/MeshManager.hh +++ b/graphics/include/gz/common/MeshManager.hh @@ -270,16 +270,6 @@ namespace gz /// \param[in] _scale scaling factor for the camera public: void CreateCamera(const std::string &_name, const float _scale); - /// \brief Create a boolean mesh from two meshes - /// \param[in] _name the name of the new mesh - /// \param[in] _m1 the parent mesh in the boolean operation - /// \param[in] _m2 the child mesh in the boolean operation - /// \param[in] _operation the boolean operation applied to the two meshes - /// \param[in] _offset _m2's pose offset from _m1 - public: void CreateBoolean(const std::string &_name, const Mesh *_m1, - const Mesh *_m2, const int _operation, - const gz::math::Pose3d &_offset = gz::math::Pose3d::Zero); - /// \brief Perform convex decomposition on a submesh. /// The submesh is decomposed into multiple convex submeshes. The output /// submeshes contain vertices and indices but texture coordinates diff --git a/graphics/src/CDT/CDT.h b/graphics/src/CDT/CDT.h new file mode 100644 index 000000000..a8055a386 --- /dev/null +++ b/graphics/src/CDT/CDT.h @@ -0,0 +1,448 @@ +/* This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + +/** + * @file + * Public API + */ + +#ifndef CDT_lNrmUayWQaIR5fxnsg9B +#define CDT_lNrmUayWQaIR5fxnsg9B + +#include "CDTUtils.h" +#include "Triangulation.h" + +#include "remove_at.hpp" + +#include +#include +#include +#include +#include +#include +#include + +/// Namespace containing triangulation functionality +namespace CDT +{ + +/** @defgroup API Public API + * Contains API for constrained and conforming Delaunay triangulations + */ +/// @{ + +/** + * Type used for storing layer depths for triangles + * @note LayerDepth should support 60K+ layers, which could be to much or + * too little for some use cases. Feel free to re-define this typedef. + */ +typedef unsigned short LayerDepth; +typedef LayerDepth BoundaryOverlapCount; + +/// Triangles by vertex index +typedef std::vector VerticesTriangles; + +/** @defgroup helpers Helpers + * Helpers for working with CDT::Triangulation. + */ +/// @{ + +/** + * Calculate triangles adjacent to vertices (triangles by vertex index) + * @param triangles triangulation + * @param verticesSize total number of vertices to pre-allocate the output + * @return triangles by vertex index + */ +CDT_EXPORT VerticesTriangles +calculateTrianglesByVertex(const TriangleVec& triangles, VertInd verticesSize); + +/** + * Information about removed duplicated vertices. + * + * Contains mapping information and removed duplicates indices. + * @note vertices {0,1,2,3,4} where 0 and 3 are the same will produce mapping + * {0,1,2,0,3} (to new vertices {0,1,2,3}) and duplicates {3} + */ +struct CDT_EXPORT DuplicatesInfo +{ + std::vector mapping; ///< vertex index mapping + std::vector duplicates; ///< duplicates' indices +}; + +/** + * Find duplicates in given custom point-type range + * @note duplicates are points with exactly same X and Y coordinates + * @tparam TVertexIter iterator that dereferences to custom point type + * @tparam TGetVertexCoordX function object getting x coordinate from vertex. + * Getter signature: const TVertexIter::value_type& -> T + * @tparam TGetVertexCoordY function object getting y coordinate from vertex. + * Getter signature: const TVertexIter::value_type& -> T + * @param first beginning of the range of vertices + * @param last end of the range of vertices + * @param getX getter of X-coordinate + * @param getY getter of Y-coordinate + * @returns information about vertex duplicates + */ +template < + typename T, + typename TVertexIter, + typename TGetVertexCoordX, + typename TGetVertexCoordY> +DuplicatesInfo FindDuplicates( + TVertexIter first, + TVertexIter last, + TGetVertexCoordX getX, + TGetVertexCoordY getY); + +/** + * Remove duplicates in-place from vector of custom points + * @tparam TVertex vertex type + * @tparam TAllocator allocator used by input vector of vertices + * @param vertices vertices to remove duplicates from + * @param duplicates information about duplicates + */ +template +void RemoveDuplicates( + std::vector& vertices, + const std::vector& duplicates); + +/** + * Remove duplicated points in-place + * + * @tparam T type of vertex coordinates (e.g., float, double) + * @param[in, out] vertices collection of vertices to remove duplicates from + * @returns information about duplicated vertices that were removed. + */ +template +CDT_EXPORT DuplicatesInfo RemoveDuplicates(std::vector >& vertices); + +/** + * Remap vertex indices in edges (in-place) using given vertex-index mapping. + * @tparam TEdgeIter iterator that dereferences to custom edge type + * @tparam TGetEdgeVertexStart function object getting start vertex index + * from an edge. + * Getter signature: const TEdgeIter::value_type& -> CDT::VertInd + * @tparam TGetEdgeVertexEnd function object getting end vertex index from + * an edge. Getter signature: const TEdgeIter::value_type& -> CDT::VertInd + * @tparam TMakeEdgeFromStartAndEnd function object that makes new edge from + * start and end vertices + * @param first beginning of the range of edges + * @param last end of the range of edges + * @param mapping vertex-index mapping + * @param getStart getter of edge start vertex index + * @param getEnd getter of edge end vertex index + * @param makeEdge factory for making edge from vetices + */ +template < + typename TEdgeIter, + typename TGetEdgeVertexStart, + typename TGetEdgeVertexEnd, + typename TMakeEdgeFromStartAndEnd> +CDT_EXPORT void RemapEdges( + TEdgeIter first, + TEdgeIter last, + const std::vector& mapping, + TGetEdgeVertexStart getStart, + TGetEdgeVertexEnd getEnd, + TMakeEdgeFromStartAndEnd makeEdge); + +/** + * Remap vertex indices in edges (in-place) using given vertex-index mapping. + * + * @note Mapping can be a result of RemoveDuplicates function + * @param[in,out] edges collection of edges to remap + * @param mapping vertex-index mapping + */ +CDT_EXPORT void +RemapEdges(std::vector& edges, const std::vector& mapping); + +/** + * Find point duplicates, remove them from vector (in-place) and remap edges + * (in-place) + * @note Same as a chained call of CDT::FindDuplicates, CDT::RemoveDuplicates, + * and CDT::RemapEdges + * @tparam T type of vertex coordinates (e.g., float, double) + * @tparam TVertex type of vertex + * @tparam TGetVertexCoordX function object getting x coordinate from vertex. + * Getter signature: const TVertexIter::value_type& -> T + * @tparam TGetVertexCoordY function object getting y coordinate from vertex. + * Getter signature: const TVertexIter::value_type& -> T + * @tparam TEdgeIter iterator that dereferences to custom edge type + * @tparam TGetEdgeVertexStart function object getting start vertex index + * from an edge. + * Getter signature: const TEdgeIter::value_type& -> CDT::VertInd + * @tparam TGetEdgeVertexEnd function object getting end vertex index from + * an edge. Getter signature: const TEdgeIter::value_type& -> CDT::VertInd + * @tparam TMakeEdgeFromStartAndEnd function object that makes new edge from + * start and end vertices + * @param[in, out] vertices vertices to remove duplicates from + * @param[in, out] edges collection of edges connecting vertices + * @param getX getter of X-coordinate + * @param getY getter of Y-coordinate + * @param edgesFirst beginning of the range of edges + * @param edgesLast end of the range of edges + * @param getStart getter of edge start vertex index + * @param getEnd getter of edge end vertex index + * @param makeEdge factory for making edge from vetices + * @returns information about vertex duplicates + */ +template < + typename T, + typename TVertex, + typename TGetVertexCoordX, + typename TGetVertexCoordY, + typename TVertexAllocator, + typename TEdgeIter, + typename TGetEdgeVertexStart, + typename TGetEdgeVertexEnd, + typename TMakeEdgeFromStartAndEnd> +DuplicatesInfo RemoveDuplicatesAndRemapEdges( + std::vector& vertices, + TGetVertexCoordX getX, + TGetVertexCoordY getY, + TEdgeIter edgesFirst, + TEdgeIter edgesLast, + TGetEdgeVertexStart getStart, + TGetEdgeVertexEnd getEnd, + TMakeEdgeFromStartAndEnd makeEdge); + +/** + * Same as a chained call of CDT::RemoveDuplicates + CDT::RemapEdges + * + * @tparam T type of vertex coordinates (e.g., float, double) + * @param[in, out] vertices collection of vertices to remove duplicates from + * @param[in,out] edges collection of edges to remap + */ +template +CDT_EXPORT DuplicatesInfo RemoveDuplicatesAndRemapEdges( + std::vector >& vertices, + std::vector& edges); + +/** + * Extract all edges of triangles + * + * @param triangles triangles used to extract edges + * @return an unordered set of all edges of triangulation + */ +CDT_EXPORT EdgeUSet extractEdgesFromTriangles(const TriangleVec& triangles); + +/*! + * Converts piece->original_edges mapping to original_edge->pieces + * @param pieceToOriginals maps pieces to original edges + * @return mapping of original edges to pieces + */ +CDT_EXPORT unordered_map +EdgeToPiecesMapping(const unordered_map& pieceToOriginals); + +/*! + * Convert edge-to-pieces mapping into edge-to-split-vertices mapping + * @tparam T type of vertex coordinates (e.g., float, double) + * @param edgeToPieces edge-to-pieces mapping + * @param vertices vertex buffer + * @return mapping of edge-to-split-points. + * Split points are sorted from edge's start (v1) to end (v2) + */ +template +CDT_EXPORT unordered_map > EdgeToSplitVertices( + const unordered_map& edgeToPieces, + const std::vector >& vertices); + +/// @} + +/// @} + +} // namespace CDT + +//***************************************************************************** +// Implementations of template functionlity +//***************************************************************************** +// hash for CDT::V2d +#ifdef CDT_CXX11_IS_SUPPORTED +namespace std +#else +namespace boost +#endif +{ +template +struct hash > +{ + size_t operator()(const CDT::V2d& xy) const + { +#ifdef CDT_CXX11_IS_SUPPORTED + typedef std::hash Hasher; +#else + typedef boost::hash Hasher; +#endif + return Hasher()(xy.x) ^ Hasher()(xy.y); + } +}; +} // namespace std + +namespace CDT +{ + +//----- +// API +//----- +template < + typename T, + typename TVertexIter, + typename TGetVertexCoordX, + typename TGetVertexCoordY> +DuplicatesInfo FindDuplicates( + TVertexIter first, + TVertexIter last, + TGetVertexCoordX getX, + TGetVertexCoordY getY) +{ + typedef unordered_map, std::size_t> PosToIndex; + PosToIndex uniqueVerts; + const std::size_t verticesSize = std::distance(first, last); + DuplicatesInfo di = { + std::vector(verticesSize), std::vector()}; + for(std::size_t iIn = 0, iOut = iIn; iIn < verticesSize; ++iIn, ++first) + { + typename PosToIndex::const_iterator it; + bool isUnique; + tie(it, isUnique) = uniqueVerts.insert( + std::make_pair(V2d::make(getX(*first), getY(*first)), iOut)); + if(isUnique) + { + di.mapping[iIn] = iOut++; + continue; + } + di.mapping[iIn] = it->second; // found a duplicate + di.duplicates.push_back(iIn); + } + return di; +} + +template +void RemoveDuplicates( + std::vector& vertices, + const std::vector& duplicates) +{ + vertices.erase( + remove_at( + vertices.begin(), + vertices.end(), + duplicates.begin(), + duplicates.end()), + vertices.end()); +} + +template < + typename TEdgeIter, + typename TGetEdgeVertexStart, + typename TGetEdgeVertexEnd, + typename TMakeEdgeFromStartAndEnd> +void RemapEdges( + TEdgeIter first, + const TEdgeIter last, + const std::vector& mapping, + TGetEdgeVertexStart getStart, + TGetEdgeVertexEnd getEnd, + TMakeEdgeFromStartAndEnd makeEdge) +{ + for(; first != last; ++first) + { + *first = makeEdge( + static_cast(mapping[getStart(*first)]), + static_cast(mapping[getEnd(*first)])); + } +} + +template < + typename T, + typename TVertex, + typename TGetVertexCoordX, + typename TGetVertexCoordY, + typename TVertexAllocator, + typename TEdgeIter, + typename TGetEdgeVertexStart, + typename TGetEdgeVertexEnd, + typename TMakeEdgeFromStartAndEnd> +DuplicatesInfo RemoveDuplicatesAndRemapEdges( + std::vector& vertices, + TGetVertexCoordX getX, + TGetVertexCoordY getY, + const TEdgeIter edgesFirst, + const TEdgeIter edgesLast, + TGetEdgeVertexStart getStart, + TGetEdgeVertexEnd getEnd, + TMakeEdgeFromStartAndEnd makeEdge) +{ + const DuplicatesInfo di = + FindDuplicates(vertices.begin(), vertices.end(), getX, getY); + RemoveDuplicates(vertices, di.duplicates); + RemapEdges(edgesFirst, edgesLast, di.mapping, getStart, getEnd, makeEdge); + return di; +} + +template +unordered_map > EdgeToSplitVertices( + const unordered_map& edgeToPieces, + const std::vector >& vertices) +{ + typedef std::pair VertCoordPair; + struct ComparePred + { + bool operator()(const VertCoordPair& a, const VertCoordPair& b) const + { + return a.second < b.second; + } + } comparePred; + + unordered_map > edgeToSplitVerts; + typedef unordered_map::const_iterator It; + for(It e2pIt = edgeToPieces.begin(); e2pIt != edgeToPieces.end(); ++e2pIt) + { + const Edge& e = e2pIt->first; + const T dX = vertices[e.v2()].x - vertices[e.v1()].x; + const T dY = vertices[e.v2()].y - vertices[e.v1()].y; + const bool isX = std::abs(dX) >= std::abs(dY); // X-coord longer + const bool isAscending = + isX ? dX >= 0 : dY >= 0; // Longer coordinate ascends + const EdgeVec& pieces = e2pIt->second; + std::vector splitVerts; + // size is: 2[ends] + (pieces - 1)[split vertices] = pieces + 1 + splitVerts.reserve(pieces.size() + 1); + typedef EdgeVec::const_iterator EIt; + for(EIt pieceIt = pieces.begin(); pieceIt != pieces.end(); ++pieceIt) + { + const array vv = {pieceIt->v1(), pieceIt->v2()}; + typedef array::const_iterator VIt; + for(VIt v = vv.begin(); v != vv.end(); ++v) + { + const T c = isX ? vertices[*v].x : vertices[*v].y; + splitVerts.push_back(std::make_pair(*v, isAscending ? c : -c)); + } + } + // sort by longest coordinate + std::sort(splitVerts.begin(), splitVerts.end(), comparePred); + // remove duplicates + splitVerts.erase( + std::unique(splitVerts.begin(), splitVerts.end()), + splitVerts.end()); + assert(splitVerts.size() > 2); // 2 end points with split vertices + std::pair > val = + std::make_pair(e, std::vector()); + val.second.reserve(splitVerts.size()); + typedef typename std::vector::const_iterator SEIt; + for(SEIt it = splitVerts.begin() + 1; it != splitVerts.end() - 1; ++it) + { + val.second.push_back(it->first); + } + edgeToSplitVerts.insert(val); + } + return edgeToSplitVerts; +} + +} // namespace CDT + +#ifndef CDT_USE_AS_COMPILED_LIBRARY +#include "CDT.hpp" +#endif + +#endif // header-guard diff --git a/graphics/src/CDT/CDT.hpp b/graphics/src/CDT/CDT.hpp new file mode 100644 index 000000000..66f9f26c1 --- /dev/null +++ b/graphics/src/CDT/CDT.hpp @@ -0,0 +1,107 @@ +/* This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + +/** + * @file + * Public API - implementation + */ + +#include "CDT.h" + +#include +#include +#include +#include + +namespace CDT +{ + +CDT_INLINE_IF_HEADER_ONLY VerticesTriangles calculateTrianglesByVertex( + const TriangleVec& triangles, + const VertInd verticesSize) +{ + VerticesTriangles vertTris(verticesSize); + for(TriInd iT(0); iT < triangles.size(); ++iT) + { + const VerticesArr3& vv = triangles[iT].vertices; + for(VerticesArr3::const_iterator v = vv.begin(); v != vv.end(); ++v) + { + vertTris[*v].push_back(iT); + } + } + return vertTris; +} + +template +DuplicatesInfo RemoveDuplicates(std::vector >& vertices) +{ + const DuplicatesInfo di = FindDuplicates( + vertices.begin(), vertices.end(), getX_V2d, getY_V2d); + RemoveDuplicates(vertices, di.duplicates); + return di; +} + +CDT_INLINE_IF_HEADER_ONLY void +RemapEdges(std::vector& edges, const std::vector& mapping) +{ + RemapEdges( + edges.begin(), + edges.end(), + mapping, + edge_get_v1, + edge_get_v2, + edge_make); +} + +template +DuplicatesInfo RemoveDuplicatesAndRemapEdges( + std::vector >& vertices, + std::vector& edges) +{ + return RemoveDuplicatesAndRemapEdges( + vertices, + getX_V2d, + getY_V2d, + edges.begin(), + edges.end(), + edge_get_v1, + edge_get_v2, + edge_make); +} + +CDT_INLINE_IF_HEADER_ONLY EdgeUSet +extractEdgesFromTriangles(const TriangleVec& triangles) +{ + EdgeUSet edges; + typedef TriangleVec::const_iterator CIt; + for(CIt t = triangles.begin(); t != triangles.end(); ++t) + { + edges.insert(Edge(VertInd(t->vertices[0]), VertInd(t->vertices[1]))); + edges.insert(Edge(VertInd(t->vertices[1]), VertInd(t->vertices[2]))); + edges.insert(Edge(VertInd(t->vertices[2]), VertInd(t->vertices[0]))); + } + return edges; +} + +CDT_INLINE_IF_HEADER_ONLY unordered_map +EdgeToPiecesMapping(const unordered_map& pieceToOriginals) +{ + unordered_map originalToPieces; + typedef unordered_map::const_iterator Cit; + for(Cit ptoIt = pieceToOriginals.begin(); ptoIt != pieceToOriginals.end(); + ++ptoIt) + { + const Edge piece = ptoIt->first; + const EdgeVec& originals = ptoIt->second; + for(EdgeVec::const_iterator origIt = originals.begin(); + origIt != originals.end(); + ++origIt) + { + originalToPieces[*origIt].push_back(piece); + } + } + return originalToPieces; +} + +} // namespace CDT diff --git a/graphics/src/CDT/CDTUtils.h b/graphics/src/CDT/CDTUtils.h new file mode 100644 index 000000000..1b9a0ff3e --- /dev/null +++ b/graphics/src/CDT/CDTUtils.h @@ -0,0 +1,508 @@ +/* This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + +/** + * @file + * Utilities and helpers + */ + +#ifndef CDT_obwOaxOTdAWcLNTlNnaq +#define CDT_obwOaxOTdAWcLNTlNnaq + +#ifdef CDT_DONT_USE_BOOST_RTREE +// CDT_DONT_USE_BOOST_RTREE was replaced with CDT_USE_BOOST +typedef char CDT_DONT_USE_BOOST_RTREE__was__replaced__with__CDT_USE_BOOST[-1]; +#endif + +// #define CDT_USE_STRONG_TYPING // strong type checks on indices + +// check if c++11 is supported +#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900) +#define CDT_CXX11_IS_SUPPORTED +#elif !defined(__cplusplus) && !defined(_MSC_VER) +typedef char couldnt_parse_cxx_standard[-1]; ///< Error: couldn't parse standard +#endif + +// Functions defined outside the class need to be 'inline' +// if CDT is configured to be used as header-only library: +// single-definition rule is violated otherwise +#ifdef CDT_USE_AS_COMPILED_LIBRARY +#define CDT_INLINE_IF_HEADER_ONLY +#include "cdt_export.h" // automatically generated by CMake +#else +/** + * Macro for inlining non-template functions when in header-only mode to + * avoid multiple declaration errors. + */ +#define CDT_INLINE_IF_HEADER_ONLY inline +/// Export not needed in header-only mode +#define CDT_EXPORT +#endif + +#include +#include +#include +#include +#include + +#ifdef CDT_USE_STRONG_TYPING +#include +#endif + +// use fall-backs for c++11 features +#ifdef CDT_CXX11_IS_SUPPORTED + +#include +#include +#include +#include +#include +namespace CDT +{ +using std::array; +using std::get; +using std::make_tuple; +using std::tie; +using std::tuple; +using std::unordered_map; +using std::unordered_set; +} // namespace CDT + +#else +#include +#include +#include +#include +#include +namespace CDT +{ +using boost::array; +using boost::get; +using boost::make_tuple; +using boost::tie; +using boost::tuple; +using boost::unordered_map; +using boost::unordered_set; +} // namespace CDT +#endif + +namespace CDT +{ + +/// 2D vector +template +struct CDT_EXPORT V2d +{ + T x; ///< X-coordinate + T y; ///< Y-coordinate + + /// Create vector from X and Y coordinates + static V2d make(T x, T y); +}; + +/// X- coordinate getter for V2d +template +const T& getX_V2d(const V2d& v) +{ + return v.x; +} + +/// Y-coordinate getter for V2d +template +const T& getY_V2d(const V2d& v) +{ + return v.y; +} + +/// If two 2D vectors are exactly equal +template +bool operator==(const CDT::V2d& lhs, const CDT::V2d& rhs) +{ + return lhs.x == rhs.x && lhs.y == rhs.y; +} + +#ifdef CDT_USE_64_BIT_INDEX_TYPE +typedef unsigned long long IndexSizeType; +#else +typedef unsigned int IndexSizeType; +#endif + +#ifdef CDT_USE_STRONG_TYPING +/// Index in triangle +BOOST_STRONG_TYPEDEF(unsigned char, Index); +/// Vertex index +BOOST_STRONG_TYPEDEF(IndexSizeType, VertInd); +/// Triangle index +BOOST_STRONG_TYPEDEF(IndexSizeType, TriInd); +#else +/// Index in triangle +typedef unsigned char Index; +/// Vertex index +typedef IndexSizeType VertInd; +/// Triangle index +typedef IndexSizeType TriInd; +#endif + +/// Constant representing no valid value for index +const static IndexSizeType + invalidIndex(std::numeric_limits::max()); +/// Constant representing no valid neighbor for a triangle +const static TriInd noNeighbor(invalidIndex); +/// Constant representing no valid vertex for a triangle +const static VertInd noVertex(invalidIndex); + +typedef std::vector TriIndVec; ///< Vector of triangle indices +typedef array VerticesArr3; ///< array of three vertex indices +typedef array NeighborsArr3; ///< array of three neighbors + +/// 2D bounding box +template +struct CDT_EXPORT Box2d +{ + V2d min; ///< min box corner + V2d max; ///< max box corner + + /// Envelop box around a point + void envelopPoint(const V2d& p) + { + envelopPoint(p.x, p.y); + } + /// Envelop box around a point with given coordinates + void envelopPoint(const T x, const T y) + { + min.x = std::min(x, min.x); + max.x = std::max(x, max.x); + min.y = std::min(y, min.y); + max.y = std::max(y, max.y); + } +}; + +/// Bounding box of a collection of custom 2D points given coordinate getters +template < + typename T, + typename TVertexIter, + typename TGetVertexCoordX, + typename TGetVertexCoordY> +Box2d envelopBox( + TVertexIter first, + TVertexIter last, + TGetVertexCoordX getX, + TGetVertexCoordY getY) +{ + const T max = std::numeric_limits::max(); + Box2d box = {{max, max}, {-max, -max}}; + for(; first != last; ++first) + { + box.envelopPoint(getX(*first), getY(*first)); + } + return box; +} + +/// Bounding box of a collection of 2D points +template +CDT_EXPORT Box2d envelopBox(const std::vector >& vertices); + +/// Edge connecting two vertices: vertex with smaller index is always first +/// \note: hash Edge is specialized at the bottom +struct CDT_EXPORT Edge +{ + /// Constructor + Edge(VertInd iV1, VertInd iV2); + /// Equals operator + bool operator==(const Edge& other) const; + /// Not-equals operator + bool operator!=(const Edge& other) const; + /// V1 getter + VertInd v1() const; + /// V2 getter + VertInd v2() const; + /// Edges' vertices + const std::pair& verts() const; + +private: + std::pair m_vertices; +}; + +/// Get edge first vertex +inline VertInd edge_get_v1(const Edge& e) +{ + return e.v1(); +} + +/// Get edge second vertex +inline VertInd edge_get_v2(const Edge& e) +{ + return e.v2(); +} + +/// Get edge second vertex +inline Edge edge_make(VertInd iV1, VertInd iV2) +{ + return Edge(iV1, iV2); +} + +typedef std::vector EdgeVec; ///< Vector of edges +typedef unordered_set EdgeUSet; ///< Hash table of edges +typedef unordered_set TriIndUSet; ///< Hash table of triangles +typedef unordered_map TriIndUMap; ///< Triangle hash map + +/// Triangulation triangle (counter-clockwise winding) +/* + * v3 + * /\ + * n3/ \n2 + * /____\ + * v1 n1 v2 + */ +struct CDT_EXPORT Triangle +{ + VerticesArr3 vertices; ///< triangle's three vertices + NeighborsArr3 neighbors; ///< triangle's three neighbors + + /** + * Factory method + * @note needed for c++03 compatibility (no uniform initialization + * available) + */ + static Triangle + make(const array& vertices, const array& neighbors) + { + Triangle t = {vertices, neighbors}; + return t; + } + + /// Next triangle adjacent to a vertex (clockwise) + /// @returns pair of next triangle and the other vertex of a common edge + std::pair next(const VertInd i) const + { + assert(vertices[0] == i || vertices[1] == i || vertices[2] == i); + if(vertices[0] == i) + { + return std::make_pair(neighbors[0], vertices[1]); + } + if(vertices[1] == i) + { + return std::make_pair(neighbors[1], vertices[2]); + } + return std::make_pair(neighbors[2], vertices[0]); + } + /// Previous triangle adjacent to a vertex (counter-clockwise) + /// @returns pair of previous triangle and the other vertex of a common edge + std::pair prev(const VertInd i) const + { + assert(vertices[0] == i || vertices[1] == i || vertices[2] == i); + if(vertices[0] == i) + return std::make_pair(neighbors[2], vertices[2]); + if(vertices[1] == i) + return std::make_pair(neighbors[0], vertices[0]); + return std::make_pair(neighbors[1], vertices[1]); + } + + bool containsVertex(const VertInd i) const + { + return std::find(vertices.begin(), vertices.end(), i) != vertices.end(); + } +}; + +typedef std::vector TriangleVec; ///< Vector of triangles + +/// Advance vertex or neighbor index counter-clockwise +CDT_EXPORT Index ccw(Index i); + +/// Advance vertex or neighbor index clockwise +CDT_EXPORT Index cw(Index i); + +/// Location of point on a triangle +struct CDT_EXPORT PtTriLocation +{ + /// Enum + enum Enum + { + Inside, + Outside, + OnEdge1, + OnEdge2, + OnEdge3, + OnVertex, + }; +}; + +/// Check if location is classified as on any of three edges +CDT_EXPORT bool isOnEdge(PtTriLocation::Enum location); + +/// Neighbor index from a on-edge location +/// \note Call only if located on the edge! +CDT_EXPORT Index edgeNeighbor(PtTriLocation::Enum location); + +/// Relative location of point to a line +struct CDT_EXPORT PtLineLocation +{ + /// Enum + enum Enum + { + Left, + Right, + OnLine, + }; +}; + +/// Orient p against line v1-v2 2D: robust geometric predicate +template +CDT_EXPORT T orient2D(const V2d& p, const V2d& v1, const V2d& v2); + +/// Check if point lies to the left of, to the right of, or on a line +template +CDT_EXPORT PtLineLocation::Enum locatePointLine( + const V2d& p, + const V2d& v1, + const V2d& v2, + T orientationTolerance = T(0)); + +/// Classify value of orient2d predicate +template +CDT_EXPORT PtLineLocation::Enum +classifyOrientation(T orientation, T orientationTolerance = T(0)); + +/// Check if point a lies inside of, outside of, or on an edge of a triangle +template +CDT_EXPORT PtTriLocation::Enum locatePointTriangle( + const V2d& p, + const V2d& v1, + const V2d& v2, + const V2d& v3); + +/// Opposed neighbor index from vertex index +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY Index opoNbr(Index vertIndex); + +/// Opposed vertex index from neighbor index +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY Index opoVrt(Index neighborIndex); + +/// Index of triangle's neighbor opposed to a vertex +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY Index +opposedTriangleInd(const VerticesArr3& vv, VertInd iVert); + +/// Index of triangle's neighbor opposed to an edge +CDT_INLINE_IF_HEADER_ONLY Index +edgeNeighborInd(const VerticesArr3& vv, VertInd iVedge1, VertInd iVedge2); + +/// Index of triangle's vertex opposed to a triangle +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY Index +opposedVertexInd(const NeighborsArr3& nn, TriInd iTopo); + +/// If triangle has a given vertex return vertex-index +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY Index +vertexInd(const VerticesArr3& vv, VertInd iV); + +/// Given triangle and a vertex find opposed triangle +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY TriInd +opposedTriangle(const Triangle& tri, VertInd iVert); + +/// Given triangle and an edge find neighbor sharing the edge +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY TriInd +edgeNeighbor(const Triangle& tri, VertInd iVedge1, VertInd iVedge2); + +/// Given two triangles, return vertex of first triangle opposed to the second +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY VertInd +opposedVertex(const Triangle& tri, TriInd iTopo); + +/// Test if point lies in a circumscribed circle of a triangle +template +CDT_EXPORT bool isInCircumcircle( + const V2d& p, + const V2d& v1, + const V2d& v2, + const V2d& v3); + +/// Test if two vertices share at least one common triangle +CDT_EXPORT CDT_INLINE_IF_HEADER_ONLY bool +verticesShareEdge(const TriIndVec& aTris, const TriIndVec& bTris); + +/// Distance between two 2D points +template +CDT_EXPORT T distance(const V2d& a, const V2d& b); + +/// Squared distance between two 2D points +template +CDT_EXPORT T distanceSquared(const V2d& a, const V2d& b); + +/// Check if any of triangle's vertices belongs to a super-triangle +CDT_INLINE_IF_HEADER_ONLY bool touchesSuperTriangle(const Triangle& t); + +} // namespace CDT + +#ifndef CDT_USE_AS_COMPILED_LIBRARY +#include "CDTUtils.hpp" +#endif + +//***************************************************************************** +// Specialize hash functions +//***************************************************************************** +#ifdef CDT_CXX11_IS_SUPPORTED +namespace std +#else +namespace boost +#endif +{ + +#ifdef CDT_USE_STRONG_TYPING + +/// Vertex index hasher +template <> +struct hash +{ + /// Hash operator + std::size_t operator()(const CDT::VertInd& vi) const + { + return std::hash()(vi.t); + } +}; + +/// Triangle index hasher +template <> +struct hash +{ + /// Hash operator + std::size_t operator()(const CDT::TriInd& vi) const + { + return std::hash()(vi.t); + } +}; + +#endif // CDT_USE_STRONG_TYPING + +/// Edge hasher +template <> +struct hash +{ + /// Hash operator + std::size_t operator()(const CDT::Edge& e) const + { + return hashEdge(e); + } + +private: + static void hashCombine(std::size_t& seed, const CDT::VertInd& key) + { +#ifdef CDT_CXX11_IS_SUPPORTED + typedef std::hash Hasher; +#else + typedef boost::hash Hasher; +#endif + seed ^= Hasher()(key) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } + static std::size_t hashEdge(const CDT::Edge& e) + { + const std::pair& vv = e.verts(); + std::size_t seed1(0); + hashCombine(seed1, vv.first); + hashCombine(seed1, vv.second); + std::size_t seed2(0); + hashCombine(seed2, vv.second); + hashCombine(seed2, vv.first); + return std::min(seed1, seed2); + } +}; +} // namespace std/boost + +#endif // header guard diff --git a/graphics/src/CDT/CDTUtils.hpp b/graphics/src/CDT/CDTUtils.hpp new file mode 100644 index 000000000..001098aed --- /dev/null +++ b/graphics/src/CDT/CDTUtils.hpp @@ -0,0 +1,318 @@ +/* This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + +/** + * @file + * Utilities and helpers - implementation + */ + +#include "CDTUtils.h" + +#include "predicates.h" // robust predicates: orient, in-circle + +#include + +namespace CDT +{ + +//***************************************************************************** +// V2d +//***************************************************************************** +template +V2d V2d::make(const T x, const T y) +{ + V2d out = {x, y}; + return out; +} + +//***************************************************************************** +// Box2d +//***************************************************************************** +template +Box2d envelopBox(const std::vector >& vertices) +{ + return envelopBox( + vertices.begin(), vertices.end(), getX_V2d, getY_V2d); +} + +//***************************************************************************** +// Edge +//***************************************************************************** +CDT_INLINE_IF_HEADER_ONLY Edge::Edge(VertInd iV1, VertInd iV2) + : m_vertices( + iV1 < iV2 ? std::make_pair(iV1, iV2) : std::make_pair(iV2, iV1)) +{} + +CDT_INLINE_IF_HEADER_ONLY bool Edge::operator==(const Edge& other) const +{ + return m_vertices == other.m_vertices; +} + +CDT_INLINE_IF_HEADER_ONLY bool Edge::operator!=(const Edge& other) const +{ + return !(this->operator==(other)); +} + +CDT_INLINE_IF_HEADER_ONLY VertInd Edge::v1() const +{ + return m_vertices.first; +} + +CDT_INLINE_IF_HEADER_ONLY VertInd Edge::v2() const +{ + return m_vertices.second; +} + +CDT_INLINE_IF_HEADER_ONLY const std::pair& Edge::verts() const +{ + return m_vertices; +} + +//***************************************************************************** +// Utility functions +//***************************************************************************** +CDT_INLINE_IF_HEADER_ONLY Index ccw(Index i) +{ + return Index((i + 1) % 3); +} + +CDT_INLINE_IF_HEADER_ONLY Index cw(Index i) +{ + return Index((i + 2) % 3); +} + +CDT_INLINE_IF_HEADER_ONLY bool isOnEdge(const PtTriLocation::Enum location) +{ + return location == PtTriLocation::OnEdge1 || + location == PtTriLocation::OnEdge2 || + location == PtTriLocation::OnEdge3; +} + +CDT_INLINE_IF_HEADER_ONLY Index edgeNeighbor(const PtTriLocation::Enum location) +{ + assert(isOnEdge(location)); + return static_cast(location - PtTriLocation::OnEdge1); +} + +template +T orient2D(const V2d& p, const V2d& v1, const V2d& v2) +{ + return predicates::adaptive::orient2d(v1.x, v1.y, v2.x, v2.y, p.x, p.y); +} + +template +PtLineLocation::Enum locatePointLine( + const V2d& p, + const V2d& v1, + const V2d& v2, + const T orientationTolerance) +{ + return classifyOrientation(orient2D(p, v1, v2), orientationTolerance); +} + +template +PtLineLocation::Enum +classifyOrientation(const T orientation, const T orientationTolerance) +{ + if(orientation < -orientationTolerance) + return PtLineLocation::Right; + if(orientation > orientationTolerance) + return PtLineLocation::Left; + return PtLineLocation::OnLine; +} + +template +PtTriLocation::Enum locatePointTriangle( + const V2d& p, + const V2d& v1, + const V2d& v2, + const V2d& v3) +{ + using namespace predicates::adaptive; + PtTriLocation::Enum result = PtTriLocation::Inside; + PtLineLocation::Enum edgeCheck = locatePointLine(p, v1, v2); + if(edgeCheck == PtLineLocation::Right) + return PtTriLocation::Outside; + if(edgeCheck == PtLineLocation::OnLine) + result = PtTriLocation::OnEdge1; + edgeCheck = locatePointLine(p, v2, v3); + if(edgeCheck == PtLineLocation::Right) + return PtTriLocation::Outside; + if(edgeCheck == PtLineLocation::OnLine) + { + result = (result == PtTriLocation::Inside) ? PtTriLocation::OnEdge2 + : PtTriLocation::OnVertex; + } + edgeCheck = locatePointLine(p, v3, v1); + if(edgeCheck == PtLineLocation::Right) + return PtTriLocation::Outside; + if(edgeCheck == PtLineLocation::OnLine) + { + result = (result == PtTriLocation::Inside) ? PtTriLocation::OnEdge3 + : PtTriLocation::OnVertex; + } + return result; +} + +CDT_INLINE_IF_HEADER_ONLY Index opoNbr(const Index vertIndex) +{ + if(vertIndex == Index(0)) + return Index(1); + if(vertIndex == Index(1)) + return Index(2); + if(vertIndex == Index(2)) + return Index(0); + assert(false && "Invalid vertex index"); + throw std::runtime_error("Invalid vertex index"); +} + +CDT_INLINE_IF_HEADER_ONLY Index opoVrt(const Index neighborIndex) +{ + if(neighborIndex == Index(0)) + return Index(2); + if(neighborIndex == Index(1)) + return Index(0); + if(neighborIndex == Index(2)) + return Index(1); + assert(false && "Invalid neighbor index"); + throw std::runtime_error("Invalid neighbor index"); +} + +CDT_INLINE_IF_HEADER_ONLY Index +opposedTriangleInd(const VerticesArr3& vv, const VertInd iVert) +{ + assert(vv[0] == iVert || vv[1] == iVert || vv[2] == iVert); + if(vv[0] == iVert) + return Index(1); + if(vv[1] == iVert) + return Index(2); + return Index(0); +} + +CDT_INLINE_IF_HEADER_ONLY Index edgeNeighborInd( + const VerticesArr3& vv, + const VertInd iVedge1, + const VertInd iVedge2) +{ + assert(vv[0] == iVedge1 || vv[1] == iVedge1 || vv[2] == iVedge1); + assert(vv[0] == iVedge2 || vv[1] == iVedge2 || vv[2] == iVedge2); + assert( + (vv[0] != iVedge1 && vv[0] != iVedge2) || + (vv[1] != iVedge1 && vv[1] != iVedge2) || + (vv[2] != iVedge1 && vv[2] != iVedge2)); + /* + * vv[2] + * /\ + * n[2]/ \n[1] + * /____\ + * vv[0] n[0] vv[1] + */ + if(vv[0] == iVedge1) + { + if(vv[1] == iVedge2) + return Index(0); + return Index(2); + } + if(vv[0] == iVedge2) + { + if(vv[1] == iVedge1) + return Index(0); + return Index(2); + } + return Index(1); +} + +CDT_INLINE_IF_HEADER_ONLY Index +opposedVertexInd(const NeighborsArr3& nn, const TriInd iTopo) +{ + assert(nn[0] == iTopo || nn[1] == iTopo || nn[2] == iTopo); + if(nn[0] == iTopo) + return Index(2); + if(nn[1] == iTopo) + return Index(0); + return Index(1); +} + +CDT_INLINE_IF_HEADER_ONLY Index +vertexInd(const VerticesArr3& vv, const VertInd iV) +{ + assert(vv[0] == iV || vv[1] == iV || vv[2] == iV); + if(vv[0] == iV) + return Index(0); + if(vv[1] == iV) + return Index(1); + return Index(2); +} + +CDT_INLINE_IF_HEADER_ONLY TriInd +opposedTriangle(const Triangle& tri, const VertInd iVert) +{ + return tri.neighbors[opposedTriangleInd(tri.vertices, iVert)]; +} + +CDT_INLINE_IF_HEADER_ONLY VertInd +opposedVertex(const Triangle& tri, const TriInd iTopo) +{ + return tri.vertices[opposedVertexInd(tri.neighbors, iTopo)]; +} + +/// Given triangle and an edge find neighbor sharing the edge +CDT_INLINE_IF_HEADER_ONLY TriInd +edgeNeighbor(const Triangle& tri, VertInd iVedge1, VertInd iVedge2) +{ + return tri.neighbors[edgeNeighborInd(tri.vertices, iVedge1, iVedge2)]; +} + +template +bool isInCircumcircle( + const V2d& p, + const V2d& v1, + const V2d& v2, + const V2d& v3) +{ + using namespace predicates::adaptive; + return incircle(v1.x, v1.y, v2.x, v2.y, v3.x, v3.y, p.x, p.y) > T(0); +} + +CDT_INLINE_IF_HEADER_ONLY +bool verticesShareEdge(const TriIndVec& aTris, const TriIndVec& bTris) +{ + for(TriIndVec::const_iterator it = aTris.begin(); it != aTris.end(); ++it) + if(std::find(bTris.begin(), bTris.end(), *it) != bTris.end()) + return true; + return false; +} + +template +T distanceSquared(const T ax, const T ay, const T bx, const T by) +{ + const T dx = bx - ax; + const T dy = by - ay; + return dx * dx + dy * dy; +} + +template +T distance(const T ax, const T ay, const T bx, const T by) +{ + return std::sqrt(distanceSquared(ax, ay, bx, by)); +} + +template +T distance(const V2d& a, const V2d& b) +{ + return distance(a.x, a.y, b.x, b.y); +} + +template +T distanceSquared(const V2d& a, const V2d& b) +{ + return distanceSquared(a.x, a.y, b.x, b.y); +} + +bool touchesSuperTriangle(const Triangle& t) +{ + return t.vertices[0] < 3 || t.vertices[1] < 3 || t.vertices[2] < 3; +} + +} // namespace CDT diff --git a/graphics/src/CDT/KDTree.h b/graphics/src/CDT/KDTree.h new file mode 100644 index 000000000..2aa4eb24e --- /dev/null +++ b/graphics/src/CDT/KDTree.h @@ -0,0 +1,412 @@ +/// This Source Code Form is subject to the terms of the Mozilla Public +/// License, v. 2.0. If a copy of the MPL was not distributed with this +/// file, You can obtain one at https://mozilla.org/MPL/2.0/. +/// Contribution of original implementation: +/// Andre Fecteau + +#ifndef KDTREE_KDTREE_H +#define KDTREE_KDTREE_H + +#include "CDTUtils.h" + +#include +#include + +namespace CDT +{ +namespace KDTree +{ + +struct NodeSplitDirection +{ + enum Enum + { + X, + Y, + }; +}; + +/// Simple tree structure with alternating half splitting nodes +/// @details Simple tree structure +/// - Tree to incrementally add points to the structure. +/// - Get the nearest point to a given input. +/// - Does not check for duplicates, expect unique points. +/// @tparam TCoordType type used for storing point coordinate. +/// @tparam NumVerticesInLeaf The number of points per leaf. +/// @tparam InitialStackDepth initial size of stack depth for nearest query. +/// Should be at least 1. +/// @tparam StackDepthIncrement increment of stack depth for nearest query when +/// stack depth is reached. +template < + typename TCoordType, + size_t NumVerticesInLeaf, + size_t InitialStackDepth, + size_t StackDepthIncrement> +class KDTree +{ +public: + typedef TCoordType coord_type; + typedef CDT::V2d point_type; + typedef CDT::VertInd point_index; + typedef std::pair value_type; + typedef std::vector point_data_vec; + typedef point_data_vec::const_iterator pd_cit; + typedef CDT::VertInd node_index; + typedef CDT::array children_type; + + /// Stores kd-tree node data + struct Node + { + children_type children; ///< two children if not leaf; {0,0} if leaf + point_data_vec data; ///< points' data if leaf + /// Create empty leaf + Node() + { + setChildren(node_index(0), node_index(0)); + data.reserve(NumVerticesInLeaf); + } + /// Children setter for convenience + void setChildren(const node_index c1, const node_index c2) + { + children[0] = c1; + children[1] = c2; + } + /// Check if node is a leaf (has no valid children) + bool isLeaf() const + { + return children[0] == children[1]; + } + }; + + /// Default constructor + KDTree() + : m_rootDir(NodeSplitDirection::X) + , m_min(point_type::make( + -std::numeric_limits::max(), + -std::numeric_limits::max())) + , m_max(point_type::make( + std::numeric_limits::max(), + std::numeric_limits::max())) + , m_size(0) + , m_isRootBoxInitialized(false) + , m_tasksStack(InitialStackDepth, NearestTask()) + { + m_root = addNewNode(); + } + + /// Constructor with bounding box known in advance + KDTree(const point_type& min, const point_type& max) + : m_rootDir(NodeSplitDirection::X) + , m_min(min) + , m_max(max) + , m_size(0) + , m_isRootBoxInitialized(true) + , m_tasksStack(InitialStackDepth, NearestTask()) + { + m_root = addNewNode(); + } + + CDT::VertInd size() const + { + return m_size; + } + + /// Insert a point into kd-tree + /// @note external point-buffer is used to reduce kd-tree's memory footprint + /// @param iPoint index of point in external point-buffer + /// @param points external point-buffer + void + insert(const point_index& iPoint, const std::vector& points) + { + ++m_size; + // if point is outside root, extend tree by adding new roots + const point_type& pos = points[iPoint]; + while(!isInsideBox(pos, m_min, m_max)) + { + extendTree(pos); + } + // now insert the point into the tree + node_index node = m_root; + point_type min = m_min; + point_type max = m_max; + NodeSplitDirection::Enum dir = m_rootDir; + + // below: initialized only to suppress warnings + NodeSplitDirection::Enum newDir(NodeSplitDirection::X); + coord_type mid(0); + point_type newMin, newMax; + while(true) + { + if(m_nodes[node].isLeaf()) + { + // add point if capacity is not reached + point_data_vec& pd = m_nodes[node].data; + if(pd.size() < NumVerticesInLeaf) + { + pd.push_back(iPoint); + return; + } + // initialize bbox first time the root capacity is reached + if(!m_isRootBoxInitialized) + { + initializeRootBox(points); + min = m_min; + max = m_max; + } + // split a full leaf node + calcSplitInfo(min, max, dir, mid, newDir, newMin, newMax); + const node_index c1 = addNewNode(), c2 = addNewNode(); + Node& n = m_nodes[node]; + n.setChildren(c1, c2); + point_data_vec& c1data = m_nodes[c1].data; + point_data_vec& c2data = m_nodes[c2].data; + // move node's points to children + for(pd_cit it = n.data.begin(); it != n.data.end(); ++it) + { + whichChild(points[*it], mid, dir) == 0 + ? c1data.push_back(*it) + : c2data.push_back(*it); + } + n.data = point_data_vec(); + } + else + { + calcSplitInfo(min, max, dir, mid, newDir, newMin, newMax); + } + // add the point to a child + const std::size_t iChild = whichChild(points[iPoint], mid, dir); + iChild == 0 ? max = newMax : min = newMin; + node = m_nodes[node].children[iChild]; + dir = newDir; + } + } + + /// Query kd-tree for a nearest neighbor point + /// @note external point-buffer is used to reduce kd-tree's memory footprint + /// @param point query point position + /// @param points external point-buffer + value_type nearest( + const point_type& point, + const std::vector& points) const + { + value_type out; + int iTask = -1; + coord_type minDistSq = std::numeric_limits::max(); + m_tasksStack[++iTask] = + NearestTask(m_root, m_min, m_max, m_rootDir, minDistSq); + while(iTask != -1) + { + const NearestTask t = m_tasksStack[iTask--]; + if(t.distSq > minDistSq) + continue; + const Node& n = m_nodes[t.node]; + if(n.isLeaf()) + { + for(pd_cit it = n.data.begin(); it != n.data.end(); ++it) + { + const point_type& p = points[*it]; + const coord_type distSq = CDT::distanceSquared(point, p); + if(distSq < minDistSq) + { + minDistSq = distSq; + out.first = p; + out.second = *it; + } + } + } + else + { + coord_type mid(0); + NodeSplitDirection::Enum newDir; + point_type newMin, newMax; + calcSplitInfo(t.min, t.max, t.dir, mid, newDir, newMin, newMax); + + const coord_type distToMid = t.dir == NodeSplitDirection::X + ? (point.x - mid) + : (point.y - mid); + const coord_type toMidSq = distToMid * distToMid; + + const std::size_t iChild = whichChild(point, mid, t.dir); + if(iTask + 2 >= static_cast(m_tasksStack.size())) + { + m_tasksStack.resize( + m_tasksStack.size() + StackDepthIncrement); + } + // node containing point should end up on top of the stack + if(iChild == 0) + { + m_tasksStack[++iTask] = NearestTask( + n.children[1], newMin, t.max, newDir, toMidSq); + m_tasksStack[++iTask] = NearestTask( + n.children[0], t.min, newMax, newDir, toMidSq); + } + else + { + m_tasksStack[++iTask] = NearestTask( + n.children[0], t.min, newMax, newDir, toMidSq); + m_tasksStack[++iTask] = NearestTask( + n.children[1], newMin, t.max, newDir, toMidSq); + } + } + } + return out; + } + +private: + /// Add a new node and return it's index in nodes buffer + node_index addNewNode() + { + const node_index newNodeIndex = static_cast(m_nodes.size()); + m_nodes.push_back(Node()); + return newNodeIndex; + } + + /// Test which child point belongs to after the split + /// @returns 0 if first child, 1 if second child + std::size_t whichChild( + const point_type& point, + const coord_type& split, + const NodeSplitDirection::Enum dir) const + { + return static_cast( + dir == NodeSplitDirection::X ? point.x > split : point.y > split); + } + + /// Calculate split location, direction, and children boxes + static void calcSplitInfo( + const point_type& min, + const point_type& max, + const NodeSplitDirection::Enum dir, + coord_type& midOut, + NodeSplitDirection::Enum& newDirOut, + point_type& newMinOut, + point_type& newMaxOut) + { + newMaxOut = max; + newMinOut = min; + switch(dir) + { + case NodeSplitDirection::X: + midOut = (min.x + max.x) / coord_type(2); + newDirOut = NodeSplitDirection::Y; + newMinOut.x = midOut; + newMaxOut.x = midOut; + return; + case NodeSplitDirection::Y: + midOut = (min.y + max.y) / coord_type(2); + newDirOut = NodeSplitDirection::X; + newMinOut.y = midOut; + newMaxOut.y = midOut; + return; + } + } + + /// Test if point is inside a box + static bool isInsideBox( + const point_type& p, + const point_type& min, + const point_type& max) + { + return p.x >= min.x && p.x <= max.x && p.y >= min.y && p.y <= max.y; + } + + /// Extend a tree by creating new root with old root and a new node as + /// children + void extendTree(const point_type& point) + { + const node_index newRoot = addNewNode(); + const node_index newLeaf = addNewNode(); + switch(m_rootDir) + { + case NodeSplitDirection::X: + m_rootDir = NodeSplitDirection::Y; + point.y < m_min.y ? m_nodes[newRoot].setChildren(newLeaf, m_root) + : m_nodes[newRoot].setChildren(m_root, newLeaf); + if(point.y < m_min.y) + m_min.y -= m_max.y - m_min.y; + else if(point.y > m_max.y) + m_max.y += m_max.y - m_min.y; + break; + case NodeSplitDirection::Y: + m_rootDir = NodeSplitDirection::X; + point.x < m_min.x ? m_nodes[newRoot].setChildren(newLeaf, m_root) + : m_nodes[newRoot].setChildren(m_root, newLeaf); + if(point.x < m_min.x) + m_min.x -= m_max.x - m_min.x; + else if(point.x > m_max.x) + m_max.x += m_max.x - m_min.x; + break; + } + m_root = newRoot; + } + + /// Calculate root's box enclosing all root points + void initializeRootBox(const std::vector& points) + { + const point_data_vec& data = m_nodes[m_root].data; + m_min = points[data.front()]; + m_max = m_min; + for(pd_cit it = data.begin(); it != data.end(); ++it) + { + const point_type& p = points[*it]; + m_min = point_type::make( + std::min(m_min.x, p.x), std::min(m_min.y, p.y)); + m_max = point_type::make( + std::max(m_max.x, p.x), std::max(m_max.y, p.y)); + } + // Make sure bounding box does not have a zero size by adding padding: + // zero-size bounding box cannot be extended properly + const TCoordType padding(1); + if(m_min.x == m_max.x) + { + m_min.x -= padding; + m_max.x += padding; + } + if(m_min.y == m_max.y) + { + m_min.y -= padding; + m_max.y += padding; + } + m_isRootBoxInitialized = true; + } + +private: + node_index m_root; + std::vector m_nodes; + NodeSplitDirection::Enum m_rootDir; + point_type m_min; + point_type m_max; + CDT::VertInd m_size; + + bool m_isRootBoxInitialized; + + // used for nearest query + struct NearestTask + { + node_index node; + point_type min, max; + NodeSplitDirection::Enum dir; + coord_type distSq; + NearestTask() + {} + NearestTask( + const node_index node_, + const point_type& min_, + const point_type& max_, + const NodeSplitDirection::Enum dir_, + const coord_type distSq_) + : node(node_) + , min(min_) + , max(max_) + , dir(dir_) + , distSq(distSq_) + {} + }; + // allocated in class (not in the 'nearest' method) for better performance + mutable std::vector m_tasksStack; +}; + +} // namespace KDTree +} // namespace CDT + +#endif // header guard diff --git a/graphics/src/CDT/LICENSE b/graphics/src/CDT/LICENSE new file mode 100644 index 000000000..a612ad981 --- /dev/null +++ b/graphics/src/CDT/LICENSE @@ -0,0 +1,373 @@ +Mozilla Public License Version 2.0 +================================== + +1. Definitions +-------------- + +1.1. "Contributor" + means each individual or legal entity that creates, contributes to + the creation of, or owns Covered Software. + +1.2. "Contributor Version" + means the combination of the Contributions of others (if any) used + by a Contributor and that particular Contributor's Contribution. + +1.3. "Contribution" + means Covered Software of a particular Contributor. + +1.4. "Covered Software" + means Source Code Form to which the initial Contributor has attached + the notice in Exhibit A, the Executable Form of such Source Code + Form, and Modifications of such Source Code Form, in each case + including portions thereof. + +1.5. "Incompatible With Secondary Licenses" + means + + (a) that the initial Contributor has attached the notice described + in Exhibit B to the Covered Software; or + + (b) that the Covered Software was made available under the terms of + version 1.1 or earlier of the License, but not also under the + terms of a Secondary License. + +1.6. "Executable Form" + means any form of the work other than Source Code Form. + +1.7. "Larger Work" + means a work that combines Covered Software with other material, in + a separate file or files, that is not Covered Software. + +1.8. "License" + means this document. + +1.9. "Licensable" + means having the right to grant, to the maximum extent possible, + whether at the time of the initial grant or subsequently, any and + all of the rights conveyed by this License. + +1.10. "Modifications" + means any of the following: + + (a) any file in Source Code Form that results from an addition to, + deletion from, or modification of the contents of Covered + Software; or + + (b) any new file in Source Code Form that contains any Covered + Software. + +1.11. "Patent Claims" of a Contributor + means any patent claim(s), including without limitation, method, + process, and apparatus claims, in any patent Licensable by such + Contributor that would be infringed, but for the grant of the + License, by the making, using, selling, offering for sale, having + made, import, or transfer of either its Contributions or its + Contributor Version. + +1.12. "Secondary License" + means either the GNU General Public License, Version 2.0, the GNU + Lesser General Public License, Version 2.1, the GNU Affero General + Public License, Version 3.0, or any later versions of those + licenses. + +1.13. "Source Code Form" + means the form of the work preferred for making modifications. + +1.14. "You" (or "Your") + means an individual or a legal entity exercising rights under this + License. For legal entities, "You" includes any entity that + controls, is controlled by, or is under common control with You. For + purposes of this definition, "control" means (a) the power, direct + or indirect, to cause the direction or management of such entity, + whether by contract or otherwise, or (b) ownership of more than + fifty percent (50%) of the outstanding shares or beneficial + ownership of such entity. + +2. License Grants and Conditions +-------------------------------- + +2.1. Grants + +Each Contributor hereby grants You a world-wide, royalty-free, +non-exclusive license: + +(a) under intellectual property rights (other than patent or trademark) + Licensable by such Contributor to use, reproduce, make available, + modify, display, perform, distribute, and otherwise exploit its + Contributions, either on an unmodified basis, with Modifications, or + as part of a Larger Work; and + +(b) under Patent Claims of such Contributor to make, use, sell, offer + for sale, have made, import, and otherwise transfer either its + Contributions or its Contributor Version. + +2.2. Effective Date + +The licenses granted in Section 2.1 with respect to any Contribution +become effective for each Contribution on the date the Contributor first +distributes such Contribution. + +2.3. Limitations on Grant Scope + +The licenses granted in this Section 2 are the only rights granted under +this License. No additional rights or licenses will be implied from the +distribution or licensing of Covered Software under this License. +Notwithstanding Section 2.1(b) above, no patent license is granted by a +Contributor: + +(a) for any code that a Contributor has removed from Covered Software; + or + +(b) for infringements caused by: (i) Your and any other third party's + modifications of Covered Software, or (ii) the combination of its + Contributions with other software (except as part of its Contributor + Version); or + +(c) under Patent Claims infringed by Covered Software in the absence of + its Contributions. + +This License does not grant any rights in the trademarks, service marks, +or logos of any Contributor (except as may be necessary to comply with +the notice requirements in Section 3.4). + +2.4. Subsequent Licenses + +No Contributor makes additional grants as a result of Your choice to +distribute the Covered Software under a subsequent version of this +License (see Section 10.2) or under the terms of a Secondary License (if +permitted under the terms of Section 3.3). + +2.5. Representation + +Each Contributor represents that the Contributor believes its +Contributions are its original creation(s) or it has sufficient rights +to grant the rights to its Contributions conveyed by this License. + +2.6. Fair Use + +This License is not intended to limit any rights You have under +applicable copyright doctrines of fair use, fair dealing, or other +equivalents. + +2.7. Conditions + +Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted +in Section 2.1. + +3. Responsibilities +------------------- + +3.1. Distribution of Source Form + +All distribution of Covered Software in Source Code Form, including any +Modifications that You create or to which You contribute, must be under +the terms of this License. You must inform recipients that the Source +Code Form of the Covered Software is governed by the terms of this +License, and how they can obtain a copy of this License. You may not +attempt to alter or restrict the recipients' rights in the Source Code +Form. + +3.2. Distribution of Executable Form + +If You distribute Covered Software in Executable Form then: + +(a) such Covered Software must also be made available in Source Code + Form, as described in Section 3.1, and You must inform recipients of + the Executable Form how they can obtain a copy of such Source Code + Form by reasonable means in a timely manner, at a charge no more + than the cost of distribution to the recipient; and + +(b) You may distribute such Executable Form under the terms of this + License, or sublicense it under different terms, provided that the + license for the Executable Form does not attempt to limit or alter + the recipients' rights in the Source Code Form under this License. + +3.3. Distribution of a Larger Work + +You may create and distribute a Larger Work under terms of Your choice, +provided that You also comply with the requirements of this License for +the Covered Software. If the Larger Work is a combination of Covered +Software with a work governed by one or more Secondary Licenses, and the +Covered Software is not Incompatible With Secondary Licenses, this +License permits You to additionally distribute such Covered Software +under the terms of such Secondary License(s), so that the recipient of +the Larger Work may, at their option, further distribute the Covered +Software under the terms of either this License or such Secondary +License(s). + +3.4. Notices + +You may not remove or alter the substance of any license notices +(including copyright notices, patent notices, disclaimers of warranty, +or limitations of liability) contained within the Source Code Form of +the Covered Software, except that You may alter any license notices to +the extent required to remedy known factual inaccuracies. + +3.5. Application of Additional Terms + +You may choose to offer, and to charge a fee for, warranty, support, +indemnity or liability obligations to one or more recipients of Covered +Software. However, You may do so only on Your own behalf, and not on +behalf of any Contributor. You must make it absolutely clear that any +such warranty, support, indemnity, or liability obligation is offered by +You alone, and You hereby agree to indemnify every Contributor for any +liability incurred by such Contributor as a result of warranty, support, +indemnity or liability terms You offer. You may include additional +disclaimers of warranty and limitations of liability specific to any +jurisdiction. + +4. Inability to Comply Due to Statute or Regulation +--------------------------------------------------- + +If it is impossible for You to comply with any of the terms of this +License with respect to some or all of the Covered Software due to +statute, judicial order, or regulation then You must: (a) comply with +the terms of this License to the maximum extent possible; and (b) +describe the limitations and the code they affect. Such description must +be placed in a text file included with all distributions of the Covered +Software under this License. Except to the extent prohibited by statute +or regulation, such description must be sufficiently detailed for a +recipient of ordinary skill to be able to understand it. + +5. Termination +-------------- + +5.1. The rights granted under this License will terminate automatically +if You fail to comply with any of its terms. However, if You become +compliant, then the rights granted under this License from a particular +Contributor are reinstated (a) provisionally, unless and until such +Contributor explicitly and finally terminates Your grants, and (b) on an +ongoing basis, if such Contributor fails to notify You of the +non-compliance by some reasonable means prior to 60 days after You have +come back into compliance. Moreover, Your grants from a particular +Contributor are reinstated on an ongoing basis if such Contributor +notifies You of the non-compliance by some reasonable means, this is the +first time You have received notice of non-compliance with this License +from such Contributor, and You become compliant prior to 30 days after +Your receipt of the notice. + +5.2. If You initiate litigation against any entity by asserting a patent +infringement claim (excluding declaratory judgment actions, +counter-claims, and cross-claims) alleging that a Contributor Version +directly or indirectly infringes any patent, then the rights granted to +You by any and all Contributors for the Covered Software under Section +2.1 of this License shall terminate. + +5.3. In the event of termination under Sections 5.1 or 5.2 above, all +end user license agreements (excluding distributors and resellers) which +have been validly granted by You or Your distributors under this License +prior to termination shall survive termination. + +************************************************************************ +* * +* 6. Disclaimer of Warranty * +* ------------------------- * +* * +* Covered Software is provided under this License on an "as is" * +* basis, without warranty of any kind, either expressed, implied, or * +* statutory, including, without limitation, warranties that the * +* Covered Software is free of defects, merchantable, fit for a * +* particular purpose or non-infringing. The entire risk as to the * +* quality and performance of the Covered Software is with You. * +* Should any Covered Software prove defective in any respect, You * +* (not any Contributor) assume the cost of any necessary servicing, * +* repair, or correction. This disclaimer of warranty constitutes an * +* essential part of this License. No use of any Covered Software is * +* authorized under this License except under this disclaimer. * +* * +************************************************************************ + +************************************************************************ +* * +* 7. Limitation of Liability * +* -------------------------- * +* * +* Under no circumstances and under no legal theory, whether tort * +* (including negligence), contract, or otherwise, shall any * +* Contributor, or anyone who distributes Covered Software as * +* permitted above, be liable to You for any direct, indirect, * +* special, incidental, or consequential damages of any character * +* including, without limitation, damages for lost profits, loss of * +* goodwill, work stoppage, computer failure or malfunction, or any * +* and all other commercial damages or losses, even if such party * +* shall have been informed of the possibility of such damages. This * +* limitation of liability shall not apply to liability for death or * +* personal injury resulting from such party's negligence to the * +* extent applicable law prohibits such limitation. Some * +* jurisdictions do not allow the exclusion or limitation of * +* incidental or consequential damages, so this exclusion and * +* limitation may not apply to You. * +* * +************************************************************************ + +8. Litigation +------------- + +Any litigation relating to this License may be brought only in the +courts of a jurisdiction where the defendant maintains its principal +place of business and such litigation shall be governed by laws of that +jurisdiction, without reference to its conflict-of-law provisions. +Nothing in this Section shall prevent a party's ability to bring +cross-claims or counter-claims. + +9. Miscellaneous +---------------- + +This License represents the complete agreement concerning the subject +matter hereof. If any provision of this License is held to be +unenforceable, such provision shall be reformed only to the extent +necessary to make it enforceable. Any law or regulation which provides +that the language of a contract shall be construed against the drafter +shall not be used to construe this License against a Contributor. + +10. Versions of the License +--------------------------- + +10.1. New Versions + +Mozilla Foundation is the license steward. Except as provided in Section +10.3, no one other than the license steward has the right to modify or +publish new versions of this License. Each version will be given a +distinguishing version number. + +10.2. Effect of New Versions + +You may distribute the Covered Software under the terms of the version +of the License under which You originally received the Covered Software, +or under the terms of any subsequent version published by the license +steward. + +10.3. Modified Versions + +If you create software not governed by this License, and you want to +create a new license for such software, you may create and use a +modified version of this License if you rename the license and remove +any references to the name of the license steward (except to note that +such modified license differs from this License). + +10.4. Distributing Source Code Form that is Incompatible With Secondary +Licenses + +If You choose to distribute Source Code Form that is Incompatible With +Secondary Licenses under the terms of this version of the License, the +notice described in Exhibit B of this License must be attached. + +Exhibit A - Source Code Form License Notice +------------------------------------------- + + This Source Code Form is subject to the terms of the Mozilla Public + License, v. 2.0. If a copy of the MPL was not distributed with this + file, You can obtain one at http://mozilla.org/MPL/2.0/. + +If it is not possible or desirable to put the notice in a particular +file, then You may include the notice in a location (such as a LICENSE +file in a relevant directory) where a recipient would be likely to look +for such a notice. + +You may add additional accurate notices of copyright ownership. + +Exhibit B - "Incompatible With Secondary Licenses" Notice +--------------------------------------------------------- + + This Source Code Form is "Incompatible With Secondary Licenses", as + defined by the Mozilla Public License, v. 2.0. diff --git a/graphics/src/CDT/LocatorKDTree.h b/graphics/src/CDT/LocatorKDTree.h new file mode 100644 index 000000000..bb3684e24 --- /dev/null +++ b/graphics/src/CDT/LocatorKDTree.h @@ -0,0 +1,81 @@ +/* This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + +/** + * @file + * Adapter between for KDTree and CDT + */ + +#ifndef CDT_POINTKDTREE_H +#define CDT_POINTKDTREE_H + +#include "CDTUtils.h" +#include "KDTree.h" + +namespace CDT +{ + +/// KD-tree holding points +template < + typename TCoordType, + size_t NumVerticesInLeaf = 32, + size_t InitialStackDepth = 32, + size_t StackDepthIncrement = 32> +class LocatorKDTree +{ +public: + /// Initialize KD-tree with points + void initialize(const std::vector >& points) + { + typedef V2d V2d_t; + V2d_t min = points.front(); + V2d_t max = min; + typedef typename std::vector::const_iterator Cit; + for(Cit it = points.begin(); it != points.end(); ++it) + { + min = V2d_t::make(std::min(min.x, it->x), std::min(min.y, it->y)); + max = V2d_t::make(std::max(max.x, it->x), std::max(max.y, it->y)); + } + m_kdTree = KDTree_t(min, max); + for(VertInd i(0); i < points.size(); ++i) + { + m_kdTree.insert(i, points); + } + } + /// Add point to KD-tree + void addPoint(const VertInd i, const std::vector >& points) + { + m_kdTree.insert(i, points); + } + /// Find nearest point using R-tree + VertInd nearPoint( + const V2d& pos, + const std::vector >& points) const + { + return m_kdTree.nearest(pos, points).second; + } + + CDT::VertInd size() const + { + return m_kdTree.size(); + } + + bool empty() const + { + return !size(); + } + +private: + typedef KDTree::KDTree< + TCoordType, + NumVerticesInLeaf, + InitialStackDepth, + StackDepthIncrement> + KDTree_t; + KDTree_t m_kdTree; +}; + +} // namespace CDT + +#endif diff --git a/graphics/src/CDT/Triangulation.h b/graphics/src/CDT/Triangulation.h new file mode 100644 index 000000000..7ca3a3d9e --- /dev/null +++ b/graphics/src/CDT/Triangulation.h @@ -0,0 +1,945 @@ +/* This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + +/** + * @file + * Triangulation class + */ + +#ifndef CDT_vW1vZ0lO8rS4gY4uI4fB +#define CDT_vW1vZ0lO8rS4gY4uI4fB + +#include "CDTUtils.h" +#include "LocatorKDTree.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +/// Namespace containing triangulation functionality +namespace CDT +{ + +/// @addtogroup API +/// @{ + +/** + * Enum of strategies specifying order in which a range of vertices is inserted + * @note VertexInsertionOrder::Randomized will only randomize order of + * inserting in triangulation, vertex indices will be preserved as they were + * specified in the final triangulation + */ +struct CDT_EXPORT VertexInsertionOrder +{ + /** + * The Enum itself + * @note needed to pre c++11 compilers that don't support 'class enum' + */ + enum Enum + { + /** + * Automatic insertion order optimized for better performance + * @details breadth-first traversal of a Kd-tree for initial bulk-load, + * randomized for subsequent insertions + */ + Auto, + /// insert vertices in same order they are provided + AsProvided, + }; +}; + +/// Enum of what type of geometry used to embed triangulation into +struct CDT_EXPORT SuperGeometryType +{ + /** + * The Enum itself + * @note needed to pre c++11 compilers that don't support 'class enum' + */ + enum Enum + { + SuperTriangle, ///< conventional super-triangle + Custom, ///< user-specified custom geometry (e.g., grid) + }; +}; + +/** + * Enum of strategies for treating intersecting constraint edges + */ +struct CDT_EXPORT IntersectingConstraintEdges +{ + /** + * The Enum itself + * @note needed to pre c++11 compilers that don't support 'class enum' + */ + enum Enum + { + NotAllowed, ///< constraint edge intersections are not allowed + TryResolve, ///< attempt to resolve constraint edge intersections + /** + * No checks: slightly faster but less safe. + * User must provide a valid input without intersecting constraints. + */ + DontCheck, + }; +}; + +/** + * Type used for storing layer depths for triangles + * @note LayerDepth should support 60K+ layers, which could be to much or + * too little for some use cases. Feel free to re-define this typedef. + */ +typedef unsigned short LayerDepth; +typedef LayerDepth BoundaryOverlapCount; + +/** + * Contains source location info: file, function, line + */ +class SourceLocation +{ +public: + SourceLocation(const std::string& file, const std::string& func, int line) + : m_file(file) + , m_func(func) + , m_line(line) + {} + const std::string& file() const + { + return m_file; + } + const std::string& func() const + { + return m_func; + } + int line() const + { + return m_line; + } + +private: + std::string m_file; + std::string m_func; + int m_line; +}; + +/// Macro for getting source location +#define CDT_SOURCE_LOCATION \ + SourceLocation(std::string(__FILE__), std::string(__func__), __LINE__) + +/** + * Base class for errors. Contains error description and source location: file, + * function, line + */ +class Error : public std::runtime_error +{ +public: + /// Constructor + Error(const std::string& description, const SourceLocation& srcLoc) + : std::runtime_error( + description + "\nin '" + srcLoc.func() + "' at " + srcLoc.file() + + ":" + std::to_string(srcLoc.line())) + , m_description(description) + , m_srcLoc(srcLoc) + {} + /// Get error description + const std::string& description() const + { + return m_description; + } + /// Get source location from where the error was thrown + const SourceLocation& sourceLocation() const + { + return m_srcLoc; + } + +private: + std::string m_description; + SourceLocation m_srcLoc; +}; + +/** + * Error thrown when triangulation modification is attempted after it was + * finalized + */ +class FinalizedError : public Error +{ +public: + FinalizedError(const SourceLocation& srcLoc) + : Error( + "Triangulation was finalized with 'erase...' method. Further " + "modification is not possible.", + srcLoc) + {} +}; + +/** + * Error thrown when duplicate vertex is detected during vertex insertion + */ +class DuplicateVertexError : public Error +{ +public: + DuplicateVertexError( + const VertInd v1, + const VertInd v2, + const SourceLocation& srcLoc) + : Error( + "Duplicate vertex detected: #" + std::to_string(v1) + + " is a duplicate of #" + std::to_string(v2), + srcLoc) + , m_v1(v1) + , m_v2(v2) + {} + VertInd v1() const + { + return m_v1; + } + VertInd v2() const + { + return m_v2; + } + +private: + VertInd m_v1, m_v2; +}; + +/** + * Error thrown when intersecting constraint edges are detected, but + * triangulation is not configured to attempt to resolve them + */ +class IntersectingConstraintsError : public Error +{ +public: + IntersectingConstraintsError( + const Edge& e1, + const Edge& e2, + const SourceLocation& srcLoc) + : Error( + "Intersecting constraint edges detected: (" + + std::to_string(e1.v1()) + ", " + std::to_string(e1.v2()) + + ") intersects (" + std::to_string(e2.v1()) + ", " + + std::to_string(e2.v2()) + ")", + srcLoc) + , m_e1(e1) + , m_e2(e2) + {} + const Edge& e1() const + { + return m_e1; + } + const Edge& e2() const + { + return m_e2; + } + +private: + Edge m_e1, m_e2; +}; + +/** + * @defgroup Triangulation Triangulation Class + * Class performing triangulations. + */ +/// @{ + +/** + * Data structure representing a 2D constrained Delaunay triangulation + * + * @tparam T type of vertex coordinates (e.g., float, double) + * @tparam TNearPointLocator class providing locating near point for efficiently + * inserting new points. Provides methods: 'addPoint(vPos, iV)' and + * 'nearPoint(vPos) -> iV' + */ +template > +class CDT_EXPORT Triangulation +{ +public: + typedef std::vector > V2dVec; ///< Vertices vector + V2dVec vertices; ///< triangulation's vertices + TriangleVec triangles; ///< triangulation's triangles + EdgeUSet fixedEdges; ///< triangulation's constraints (fixed edges) + + /** Stores count of overlapping boundaries for a fixed edge. If no entry is + * present for an edge: no boundaries overlap. + * @note map only has entries for fixed for edges that represent overlapping + * boundaries + * @note needed for handling depth calculations and hole-removel in case of + * overlapping boundaries + */ + unordered_map overlapCount; + + /** Stores list of original edges represented by a given fixed edge + * @note map only has entries for edges where multiple original fixed edges + * overlap or where a fixed edge is a part of original edge created by + * conforming Delaunay triangulation vertex insertion + */ + unordered_map pieceToOriginals; + + /*____ API _____*/ + /// Default constructor + Triangulation(); + /** + * Constructor + * @param vertexInsertionOrder strategy used for ordering vertex insertions + */ + explicit Triangulation(VertexInsertionOrder::Enum vertexInsertionOrder); + /** + * Constructor + * @param vertexInsertionOrder strategy used for ordering vertex insertions + * @param intersectingEdgesStrategy strategy for treating intersecting + * constraint edges + * @param minDistToConstraintEdge distance within which point is considered + * to be lying on a constraint edge. Used when adding constraints to the + * triangulation. + */ + Triangulation( + VertexInsertionOrder::Enum vertexInsertionOrder, + IntersectingConstraintEdges::Enum intersectingEdgesStrategy, + T minDistToConstraintEdge); + /** + * Constructor + * @param vertexInsertionOrder strategy used for ordering vertex insertions + * @param nearPtLocator class providing locating near point for efficiently + * inserting new points + * @param intersectingEdgesStrategy strategy for treating intersecting + * constraint edges + * @param minDistToConstraintEdge distance within which point is considered + * to be lying on a constraint edge. Used when adding constraints to the + * triangulation. + */ + Triangulation( + VertexInsertionOrder::Enum vertexInsertionOrder, + const TNearPointLocator& nearPtLocator, + IntersectingConstraintEdges::Enum intersectingEdgesStrategy, + T minDistToConstraintEdge); + /** + * Insert custom point-types specified by iterator range and X/Y-getters + * @tparam TVertexIter iterator that dereferences to custom point type + * @tparam TGetVertexCoordX function object getting x coordinate from + * vertex. Getter signature: const TVertexIter::value_type& -> T + * @tparam TGetVertexCoordY function object getting y coordinate from + * vertex. Getter signature: const TVertexIter::value_type& -> T + * @param first beginning of the range of vertices to add + * @param last end of the range of vertices to add + * @param getX getter of X-coordinate + * @param getY getter of Y-coordinate + */ + template < + typename TVertexIter, + typename TGetVertexCoordX, + typename TGetVertexCoordY> + void insertVertices( + TVertexIter first, + TVertexIter last, + TGetVertexCoordX getX, + TGetVertexCoordY getY); + /** + * Insert vertices into triangulation + * @param vertices vector of vertices to insert + */ + void insertVertices(const std::vector >& vertices); + /** + * Insert constraint edges into triangulation for Constrained Delaunay + * Triangulation (for example see figure below). + * + * Uses only original vertices: no new verties are added + * + * CDT show-case: constrained and + * conforming triangulations, convex hulls, automatically removing holes + * + * @note Each fixed edge is inserted by deleting the triangles it crosses, + * followed by the triangulation of the polygons on each side of the edge. + * No new vertices are inserted. + * @note If some edge appears more than once in the input this means that + * multiple boundaries overlap at the edge and impacts how hole detection + * algorithm of Triangulation::eraseOuterTrianglesAndHoles works. + * Make sure there are no erroneous duplicates. + * @tparam TEdgeIter iterator that dereferences to custom edge type + * @tparam TGetEdgeVertexStart function object getting start vertex index + * from an edge. + * Getter signature: const TEdgeIter::value_type& -> CDT::VertInd + * @tparam TGetEdgeVertexEnd function object getting end vertex index from + * an edge. Getter signature: const TEdgeIter::value_type& -> CDT::VertInd + * @param first beginning of the range of edges to add + * @param last end of the range of edges to add + * @param getStart getter of edge start vertex index + * @param getEnd getter of edge end vertex index + */ + template < + typename TEdgeIter, + typename TGetEdgeVertexStart, + typename TGetEdgeVertexEnd> + void insertEdges( + TEdgeIter first, + TEdgeIter last, + TGetEdgeVertexStart getStart, + TGetEdgeVertexEnd getEnd); + /** + * Insert constraint edges into triangulation for Constrained Delaunay + * Triangulation (for example see figure below). + * + * Uses only original vertices: no new verties are added + * + * CDT show-case: constrained and + * conforming triangulations, convex hulls, automatically removing holes + * + * @note Each fixed edge is inserted by deleting the triangles it crosses, + * followed by the triangulation of the polygons on each side of the edge. + * No new vertices are inserted. + * @note If some edge appears more than once in the input this means that + * multiple boundaries overlap at the edge and impacts how hole detection + * algorithm of Triangulation::eraseOuterTrianglesAndHoles works. + * Make sure there are no erroneous duplicates. + * @tparam edges constraint edges + */ + void insertEdges(const std::vector& edges); + /** + * Insert constraint edges into triangulation for Conforming Delaunay + * Triangulation (for example see figure below). + * + * May add new vertices. + * + * CDT show-case: constrained and + * conforming triangulations, convex hulls, automatically removing holes + * + * @note For each fixed edge that is not present in the triangulation its + * midpoint is recursively added until the original edge is represented by a + * sequence of its pieces. New vertices are inserted. + * @note If some edge appears more than once the input this + * means that multiple boundaries overlap at the edge and impacts how hole + * detection algorithm of Triangulation::eraseOuterTrianglesAndHoles works. + * Make sure there are no erroneous duplicates. + * @tparam TEdgeIter iterator that dereferences to custom edge type + * @tparam TGetEdgeVertexStart function object getting start vertex index + * from an edge. + * Getter signature: const TEdgeIter::value_type& -> CDT::VertInd + * @tparam TGetEdgeVertexEnd function object getting end vertex index from + * an edge. Getter signature: const TEdgeIter::value_type& -> CDT::VertInd + * @param first beginning of the range of edges to add + * @param last end of the range of edges to add + * @param getStart getter of edge start vertex index + * @param getEnd getter of edge end vertex index + */ + template < + typename TEdgeIter, + typename TGetEdgeVertexStart, + typename TGetEdgeVertexEnd> + void conformToEdges( + TEdgeIter first, + TEdgeIter last, + TGetEdgeVertexStart getStart, + TGetEdgeVertexEnd getEnd); + /** + * Insert constraint edges into triangulation for Conforming Delaunay + * Triangulation (for example see figure below). + * + * May add new vertices. + * + * CDT show-case: constrained and + * conforming triangulations, convex hulls, automatically removing holes + * + * @note For each fixed edge that is not present in the triangulation its + * midpoint is recursively added until the original edge is represented by a + * sequence of its pieces. New vertices are inserted. + * @note If some edge appears more than once the input this + * means that multiple boundaries overlap at the edge and impacts how hole + * detection algorithm of Triangulation::eraseOuterTrianglesAndHoles works. + * Make sure there are no erroneous duplicates. + * @tparam edges edges to conform to + */ + void conformToEdges(const std::vector& edges); + /** + * Erase triangles adjacent to super triangle + * + * @note does nothing if custom geometry is used + */ + void eraseSuperTriangle(); + /// Erase triangles outside of constrained boundary using growing + void eraseOuterTriangles(); + /** + * Erase triangles outside of constrained boundary and auto-detected holes + * + * @note detecting holes relies on layer peeling based on layer depth + * @note supports overlapping or touching boundaries + */ + void eraseOuterTrianglesAndHoles(); + /** + * Call this method after directly setting custom super-geometry via + * vertices and triangles members + */ + void initializedWithCustomSuperGeometry(); + + /** + * Check if the triangulation was finalized with `erase...` method and + * super-triangle was removed. + * @return true if triangulation is finalized, false otherwise + */ + bool isFinalized() const; + + /** + * Calculate depth of each triangle in constraint triangulation. Supports + * overlapping boundaries. + * + * Perform depth peeling from super triangle to outermost boundary, + * then to next boundary and so on until all triangles are traversed.@n + * For example depth is: + * - 0 for triangles outside outermost boundary + * - 1 for triangles inside boundary but outside hole + * - 2 for triangles in hole + * - 3 for triangles in island and so on... + * @return vector where element at index i stores depth of i-th triangle + */ + std::vector calculateTriangleDepths() const; + + /** + * @defgroup Advanced Advanced Triangulation Methods + * Advanced methods for manually modifying the triangulation from + * outside. Please only use them when you know what you are doing. + */ + /// @{ + + /** + * Flip an edge between two triangle. + * @note Advanced method for manually modifying the triangulation from + * outside. Please call it when you know what you are doing. + * @param iT first triangle + * @param iTopo second triangle + */ + void flipEdge(TriInd iT, TriInd iTopo); + + void flipEdge( + TriInd iT, + TriInd iTopo, + VertInd v1, + VertInd v2, + VertInd v3, + VertInd v4, + TriInd n1, + TriInd n2, + TriInd n3, + TriInd n4); + + /** + * Remove triangles with specified indices. + * Adjust internal triangulation state accordingly. + * @param removedTriangles indices of triangles to remove + */ + void removeTriangles(const TriIndUSet& removedTriangles); + + /// Access internal vertex adjacent triangles + TriIndVec& VertTrisInternal(); + /// Access internal vertex adjacent triangles + const TriIndVec& VertTrisInternal() const; + /// @} + +private: + /*____ Detail __*/ + void addSuperTriangle(const Box2d& box); + void addNewVertex(const V2d& pos, TriInd iT); + void insertVertex(VertInd iVert); + void insertVertex(VertInd iVert, VertInd walkStart); + void ensureDelaunayByEdgeFlips(VertInd iV1, std::stack& triStack); + /// Flip fixed edges and return a list of flipped fixed edges + std::vector insertVertex_FlipFixedEdges(VertInd iV1); + + /// State for an iteration of triangulate pseudo-polygon + typedef tuple + TriangulatePseudoPolygonTask; + + /** + * Insert an edge into constraint Delaunay triangulation + * @param edge edge to insert + * @param originalEdge original edge inserted edge is part of + * @param[in,out] remaining parts of the edge that still need to + * be inserted + * @param[in,out] tppIterations stack to be used for storing iterations of + * triangulating pseudo-polygon + * @note in-out state (@param remaining @param tppIterations) is shared + * between different runs for performance gains (reducing memory + * allocations) + */ + void insertEdge( + Edge edge, + Edge originalEdge, + EdgeVec& remaining, + std::vector& tppIterations); + + /** + * Insert an edge or its part into constraint Delaunay triangulation + * @param edge edge to insert + * @param originalEdge original edge inserted edge is part of + * @param[in,out] remainingStack parts of the edge that still need to + * be inserted + * @param[in,out] tppIterations stack to be used for storing iterations of + * triangulating pseudo-polygon + * @note in-out state (@param remaining @param tppIterations) is shared + * between different runs for performance gains (reducing memory + * allocations) + */ + void insertEdgeIteration( + Edge edge, + Edge originalEdge, + EdgeVec& remaining, + std::vector& tppIterations); + + /// State for iteration of conforming to edge + typedef tuple ConformToEdgeTask; + + /** + * Conform Delaunay triangulation to a fixed edge by recursively inserting + * mid point of the edge and then conforming to its halves + * @param edge fixed edge to conform to + * @param originals original edges that new edge is piece of + * @param overlaps count of overlapping boundaries at the edge. Only used + * when re-introducing edge with overlaps > 0 + * @param[in,out] remaining remaining edge parts to be conformed to + * @note in-out state (@param remaining @param reintroduce) is shared + * between different runs for performance gains (reducing memory + * allocations) + */ + void conformToEdge( + Edge edge, + EdgeVec originals, + BoundaryOverlapCount overlaps, + std::vector& remaining); + + /** + * Iteration of conform to fixed edge. + * @param edge fixed edge to conform to + * @param originals original edges that new edge is piece of + * @param overlaps count of overlapping boundaries at the edge. Only used + * when re-introducing edge with overlaps > 0 + * @param[in,out] remaining remaining edge parts + * @note in-out state (@param remaining @param reintroduce) is shared + * between different runs for performance gains (reducing memory + * allocations) + */ + void conformToEdgeIteration( + Edge edge, + const EdgeVec& originals, + BoundaryOverlapCount overlaps, + std::vector& remaining); + + tuple intersectedTriangle( + VertInd iA, + const V2d& a, + const V2d& b, + T orientationTolerance = T(0)) const; + /// Returns indices of three resulting triangles + std::stack insertVertexInsideTriangle(VertInd v, TriInd iT); + /// Returns indices of four resulting triangles + std::stack insertVertexOnEdge(VertInd v, TriInd iT1, TriInd iT2); + array trianglesAt(const V2d& pos) const; + array + walkingSearchTrianglesAt(VertInd iV, VertInd startVertex) const; + TriInd walkTriangles(VertInd startVertex, const V2d& pos) const; + /// Given triangle and its vertex find opposite triangle and the other three + /// vertices and surrounding neighbors + void edgeFlipInfo( + TriInd iT, + VertInd iV1, + TriInd& iTopo, + VertInd& iV2, + VertInd& iV3, + VertInd& iV4, + TriInd& n1, + TriInd& n2, + TriInd& n3, + TriInd& n4); + bool isFlipNeeded(VertInd iV1, VertInd iV2, VertInd iV3, VertInd iV4) const; + void changeNeighbor(TriInd iT, TriInd oldNeighbor, TriInd newNeighbor); + void changeNeighbor( + TriInd iT, + VertInd iVedge1, + VertInd iVedge2, + TriInd newNeighbor); + void triangulatePseudoPolygon( + const std::vector& poly, + unordered_map& outerTris, + TriInd iT, + TriInd iN, + std::vector& iterations); + void triangulatePseudoPolygonIteration( + const std::vector& poly, + unordered_map& outerTris, + std::vector& iterations); + IndexSizeType findDelaunayPoint( + const std::vector& poly, + IndexSizeType iA, + IndexSizeType iB) const; + TriInd addTriangle(const Triangle& t); // note: invalidates iterators! + TriInd addTriangle(); // note: invalidates triangle iterators! + /** + * Remove super-triangle (if used) and triangles with specified indices. + * Adjust internal triangulation state accordingly. + * @removedTriangles indices of triangles to remove + */ + void finalizeTriangulation(const TriIndUSet& removedTriangles); + TriIndUSet growToBoundary(std::stack seeds) const; + void fixEdge(const Edge& edge); + void fixEdge(const Edge& edge, const Edge& originalEdge); + /** + * Split existing constraint (fixed) edge + * @param edge fixed edge to split + * @param iSplitVert index of the vertex to be used as a split vertex + */ + void splitFixedEdge(const Edge& edge, const VertInd iSplitVert); + /** + * Add a vertex that splits an edge into the triangulation + * @param splitVert position of split vertex + * @param iT index of a first triangle adjacent to the split edge + * @param iTopo index of a second triangle adjacent to the split edge + * (opposed to the first triangle) + * @return index of a newly added split vertex + */ + VertInd addSplitEdgeVertex( + const V2d& splitVert, + const TriInd iT, + const TriInd iTopo); + /** + * Split fixed edge and add a split vertex into the triangulation + * @param edge fixed edge to split + * @param splitVert position of split vertex + * @param iT index of a first triangle adjacent to the split edge + * @param iTopo index of a second triangle adjacent to the split edge + * (opposed to the first triangle) + * @return index of a newly added split vertex + */ + VertInd splitFixedEdgeAt( + const Edge& edge, + const V2d& splitVert, + const TriInd iT, + const TriInd iTopo); + /** + * Flag triangle as dummy + * @note Advanced method for manually modifying the triangulation from + * outside. Please call it when you know what you are doing. + * @param iT index of a triangle to flag + */ + void makeDummy(TriInd iT); + /** + * Erase all dummy triangles + * @note Advanced method for manually modifying the triangulation from + * outside. Please call it when you know what you are doing. + */ + void eraseDummies(); + /** + * Depth-peel a layer in triangulation, used when calculating triangle + * depths + * + * It takes starting seed triangles, traverses neighboring triangles, and + * assigns given layer depth to the traversed triangles. Traversal is + * blocked by constraint edges. Triangles behind constraint edges are + * recorded as seeds of next layer and returned from the function. + * + * @param seeds indices of seed triangles + * @param layerDepth current layer's depth to mark triangles with + * @param[in, out] triDepths depths of triangles + * @return triangles of the deeper layers that are adjacent to the peeled + * layer. To be used as seeds when peeling deeper layers. + */ + unordered_map peelLayer( + std::stack seeds, + LayerDepth layerDepth, + std::vector& triDepths) const; + + void insertVertices_AsProvided(VertInd superGeomVertCount); + void insertVertices_Randomized(VertInd superGeomVertCount); + void insertVertices_KDTreeBFS( + VertInd superGeomVertCount, + V2d boxMin, + V2d boxMax); + std::pair edgeTriangles(VertInd a, VertInd b) const; + bool hasEdge(VertInd a, VertInd b) const; + void setAdjacentTriangle(const VertInd v, const TriInd t); + void pivotVertexTriangleCW(VertInd v); + /// Add vertex to nearest-point locator if locator is initialized + void tryAddVertexToLocator(const VertInd v); + /// Perform lazy initialization of nearest-point locator after the Kd-tree + /// BFS bulk load if necessary + void tryInitNearestPointLocator(); + + std::vector m_dummyTris; + TNearPointLocator m_nearPtLocator; + IndexSizeType m_nTargetVerts; + SuperGeometryType::Enum m_superGeomType; + VertexInsertionOrder::Enum m_vertexInsertionOrder; + IntersectingConstraintEdges::Enum m_intersectingEdgesStrategy; + T m_minDistToConstraintEdge; + TriIndVec m_vertTris; /// one triangle adjacent to each vertex +}; + +/// @} +/// @} + +namespace detail +{ + +/// SplitMix64 pseudo-random number generator +struct SplitMix64RandGen +{ + typedef unsigned long long uint64; + uint64 m_state; + explicit SplitMix64RandGen(uint64 state) + : m_state(state) + {} + explicit SplitMix64RandGen() + : m_state(0) + {} + uint64 operator()() + { + uint64 z = (m_state += 0x9e3779b97f4a7c15); + z = (z ^ (z >> 30)) * 0xbf58476d1ce4e5b9; + z = (z ^ (z >> 27)) * 0x94d049bb133111eb; + return z ^ (z >> 31); + } +}; + +template +void random_shuffle(RandomIt first, RandomIt last) +{ + detail::SplitMix64RandGen prng; + typename std::iterator_traits::difference_type i, n; + n = last - first; + for(i = n - 1; i > 0; --i) + { + std::swap(first[i], first[prng() % (i + 1)]); + } +} + +// backport from c++11 +template +void iota(ForwardIt first, ForwardIt last, T value) +{ + while(first != last) + { + *first++ = value; + ++value; + } +} + +} // namespace detail + +//----------------------- +// Triangulation methods +//----------------------- +template +template < + typename TVertexIter, + typename TGetVertexCoordX, + typename TGetVertexCoordY> +void Triangulation::insertVertices( + const TVertexIter first, + const TVertexIter last, + TGetVertexCoordX getX, + TGetVertexCoordY getY) +{ + if(isFinalized()) + throw FinalizedError(CDT_SOURCE_LOCATION); + + const bool isFirstTime = vertices.empty(); + const T max = std::numeric_limits::max(); + Box2d box = {{max, max}, {-max, -max}}; + if(vertices.empty()) // called first time + { + box = envelopBox(first, last, getX, getY); + addSuperTriangle(box); + } + tryInitNearestPointLocator(); + + const VertInd nExistingVerts = static_cast(vertices.size()); + const VertInd nVerts = + static_cast(nExistingVerts + std::distance(first, last)); + // optimization, try to pre-allocate tris + triangles.reserve(triangles.size() + 2 * nVerts); + vertices.reserve(nVerts); + m_vertTris.reserve(nVerts); + for(TVertexIter it = first; it != last; ++it) + addNewVertex(V2d::make(getX(*it), getY(*it)), noNeighbor); + + switch(m_vertexInsertionOrder) + { + case VertexInsertionOrder::AsProvided: + insertVertices_AsProvided(nExistingVerts); + break; + case VertexInsertionOrder::Auto: + isFirstTime ? insertVertices_KDTreeBFS(nExistingVerts, box.min, box.max) + : insertVertices_Randomized(nExistingVerts); + break; + } +} + +template +template < + typename TEdgeIter, + typename TGetEdgeVertexStart, + typename TGetEdgeVertexEnd> +void Triangulation::insertEdges( + TEdgeIter first, + const TEdgeIter last, + TGetEdgeVertexStart getStart, + TGetEdgeVertexEnd getEnd) +{ + if(isFinalized()) + throw FinalizedError(CDT_SOURCE_LOCATION); + + std::vector tppIterations; + EdgeVec remaining; + for(; first != last; ++first) + { + // +3 to account for super-triangle vertices + const Edge edge( + VertInd(getStart(*first) + m_nTargetVerts), + VertInd(getEnd(*first) + m_nTargetVerts)); + insertEdge(edge, edge, remaining, tppIterations); + } + eraseDummies(); +} + +template +template < + typename TEdgeIter, + typename TGetEdgeVertexStart, + typename TGetEdgeVertexEnd> +void Triangulation::conformToEdges( + TEdgeIter first, + const TEdgeIter last, + TGetEdgeVertexStart getStart, + TGetEdgeVertexEnd getEnd) +{ + if(isFinalized()) + throw FinalizedError(CDT_SOURCE_LOCATION); + + tryInitNearestPointLocator(); + // state shared between different runs for performance gains + std::vector remaining; + for(; first != last; ++first) + { + // +3 to account for super-triangle vertices + const Edge e( + VertInd(getStart(*first) + m_nTargetVerts), + VertInd(getEnd(*first) + m_nTargetVerts)); + conformToEdge(e, EdgeVec(1, e), 0, remaining); + } + eraseDummies(); +} + +} // namespace CDT + +#ifndef CDT_USE_AS_COMPILED_LIBRARY +#include "Triangulation.hpp" +#endif + +#endif // header-guard diff --git a/graphics/src/CDT/Triangulation.hpp b/graphics/src/CDT/Triangulation.hpp new file mode 100644 index 000000000..70c13cf17 --- /dev/null +++ b/graphics/src/CDT/Triangulation.hpp @@ -0,0 +1,2087 @@ +/* This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + +/** + * @file + * Triangulation class - implementation + */ + +#include "Triangulation.h" +#include "portable_nth_element.hpp" + +#include +#include +#include +#include +#include + +namespace CDT +{ + +typedef std::deque TriDeque; + +namespace detail +{ + +/// Needed for c++03 compatibility (no uniform initialization available) +template +array arr3(const T& v0, const T& v1, const T& v2) +{ + const array out = {v0, v1, v2}; + return out; +} + +namespace defaults +{ + +const std::size_t nTargetVerts = 0; +const SuperGeometryType::Enum superGeomType = SuperGeometryType::SuperTriangle; +const VertexInsertionOrder::Enum vertexInsertionOrder = + VertexInsertionOrder::Auto; +const IntersectingConstraintEdges::Enum intersectingEdgesStrategy = + IntersectingConstraintEdges::NotAllowed; +const float minDistToConstraintEdge(0); + +} // namespace defaults + +} // namespace detail + +template +Triangulation::Triangulation() + : m_nTargetVerts(detail::defaults::nTargetVerts) + , m_superGeomType(detail::defaults::superGeomType) + , m_vertexInsertionOrder(detail::defaults::vertexInsertionOrder) + , m_intersectingEdgesStrategy(detail::defaults::intersectingEdgesStrategy) + , m_minDistToConstraintEdge(detail::defaults::minDistToConstraintEdge) +{} + +template +Triangulation::Triangulation( + const VertexInsertionOrder::Enum vertexInsertionOrder) + : m_nTargetVerts(detail::defaults::nTargetVerts) + , m_superGeomType(detail::defaults::superGeomType) + , m_vertexInsertionOrder(vertexInsertionOrder) + , m_intersectingEdgesStrategy(detail::defaults::intersectingEdgesStrategy) + , m_minDistToConstraintEdge(detail::defaults::minDistToConstraintEdge) +{} + +template +Triangulation::Triangulation( + const VertexInsertionOrder::Enum vertexInsertionOrder, + const IntersectingConstraintEdges::Enum intersectingEdgesStrategy, + const T minDistToConstraintEdge) + : m_nTargetVerts(detail::defaults::nTargetVerts) + , m_superGeomType(detail::defaults::superGeomType) + , m_vertexInsertionOrder(vertexInsertionOrder) + , m_intersectingEdgesStrategy(intersectingEdgesStrategy) + , m_minDistToConstraintEdge(minDistToConstraintEdge) +{} + +template +Triangulation::Triangulation( + const VertexInsertionOrder::Enum vertexInsertionOrder, + const TNearPointLocator& nearPtLocator, + const IntersectingConstraintEdges::Enum intersectingEdgesStrategy, + const T minDistToConstraintEdge) + : m_nearPtLocator(nearPtLocator) + , m_nTargetVerts(detail::defaults::nTargetVerts) + , m_superGeomType(detail::defaults::superGeomType) + , m_vertexInsertionOrder(vertexInsertionOrder) + , m_intersectingEdgesStrategy(intersectingEdgesStrategy) + , m_minDistToConstraintEdge(minDistToConstraintEdge) +{} + +template +void Triangulation::eraseDummies() +{ + if(m_dummyTris.empty()) + return; + const TriIndUSet dummySet(m_dummyTris.begin(), m_dummyTris.end()); + TriIndUMap triIndMap; + triIndMap[noNeighbor] = noNeighbor; + for(TriInd iT(0), iTnew(0); iT < TriInd(triangles.size()); ++iT) + { + if(dummySet.count(iT)) + continue; + triIndMap[iT] = iTnew; + triangles[iTnew] = triangles[iT]; + iTnew++; + } + triangles.erase(triangles.end() - dummySet.size(), triangles.end()); + + // remap adjacent triangle indices for vertices + for(TriIndVec::iterator iT = m_vertTris.begin(); iT != m_vertTris.end(); + ++iT) + { + *iT = triIndMap[*iT]; + } + // remap neighbor indices for triangles + for(TriangleVec::iterator t = triangles.begin(); t != triangles.end(); ++t) + { + NeighborsArr3& nn = t->neighbors; + for(NeighborsArr3::iterator iN = nn.begin(); iN != nn.end(); ++iN) + *iN = triIndMap[*iN]; + } + // clear dummy triangles + m_dummyTris = std::vector(); +} + +template +void Triangulation::eraseSuperTriangle() +{ + if(m_superGeomType != SuperGeometryType::SuperTriangle) + return; + // find triangles adjacent to super-triangle's vertices + TriIndUSet toErase; + for(TriInd iT(0); iT < TriInd(triangles.size()); ++iT) + { + if(touchesSuperTriangle(triangles[iT])) + toErase.insert(iT); + } + finalizeTriangulation(toErase); +} + +template +void Triangulation::eraseOuterTriangles() +{ + // make dummy triangles adjacent to super-triangle's vertices + assert(m_vertTris[0] != noNeighbor); + const std::stack seed(std::deque(1, m_vertTris[0])); + const TriIndUSet toErase = growToBoundary(seed); + finalizeTriangulation(toErase); +} + +template +void Triangulation::eraseOuterTrianglesAndHoles() +{ + const std::vector triDepths = calculateTriangleDepths(); + TriIndUSet toErase; + toErase.reserve(triangles.size()); + for(std::size_t iT = 0; iT != triangles.size(); ++iT) + { + if(triDepths[iT] % 2 == 0) + toErase.insert(static_cast(iT)); + } + finalizeTriangulation(toErase); +} + +/// Remap removing super-triangle: subtract 3 from vertices +inline Edge RemapNoSuperTriangle(const Edge& e) +{ + return Edge(VertInd(e.v1() - 3), VertInd(e.v2() - 3)); +} + +template +void Triangulation::removeTriangles( + const TriIndUSet& removedTriangles) +{ + if(removedTriangles.empty()) + return; + // remove triangles and calculate triangle index mapping + TriIndUMap triIndMap; + for(TriInd iT(0), iTnew(0); iT < TriInd(triangles.size()); ++iT) + { + if(removedTriangles.count(iT)) + continue; + triIndMap[iT] = iTnew; + triangles[iTnew] = triangles[iT]; + iTnew++; + } + triangles.erase(triangles.end() - removedTriangles.size(), triangles.end()); + // adjust triangles' neighbors + for(TriInd iT(0); iT < triangles.size(); ++iT) + { + Triangle& t = triangles[iT]; + // update neighbors to account for removed triangles + NeighborsArr3& nn = t.neighbors; + for(NeighborsArr3::iterator n = nn.begin(); n != nn.end(); ++n) + { + if(removedTriangles.count(*n)) + { + *n = noNeighbor; + } + else if(*n != noNeighbor) + { + *n = triIndMap[*n]; + } + } + } +} + +template +TriIndVec& Triangulation::VertTrisInternal() +{ + return m_vertTris; +} + +template +const TriIndVec& Triangulation::VertTrisInternal() const +{ + return m_vertTris; +} + +template +void Triangulation::finalizeTriangulation( + const TriIndUSet& removedTriangles) +{ + eraseDummies(); + m_vertTris = TriIndVec(); + // remove super-triangle + if(m_superGeomType == SuperGeometryType::SuperTriangle) + { + vertices.erase(vertices.begin(), vertices.begin() + 3); + // Edge re-mapping + { // fixed edges + EdgeUSet updatedFixedEdges; + typedef CDT::EdgeUSet::const_iterator It; + for(It e = fixedEdges.begin(); e != fixedEdges.end(); ++e) + { + updatedFixedEdges.insert(RemapNoSuperTriangle(*e)); + } + fixedEdges = updatedFixedEdges; + } + { // overlap count + unordered_map updatedOverlapCount; + typedef unordered_map::const_iterator + It; + for(It it = overlapCount.begin(); it != overlapCount.end(); ++it) + { + updatedOverlapCount.insert(std::make_pair( + RemapNoSuperTriangle(it->first), it->second)); + } + overlapCount = updatedOverlapCount; + } + { // split edges mapping + unordered_map updatedPieceToOriginals; + typedef unordered_map::const_iterator It; + for(It it = pieceToOriginals.begin(); it != pieceToOriginals.end(); + ++it) + { + EdgeVec ee = it->second; + for(EdgeVec::iterator eeIt = ee.begin(); eeIt != ee.end(); + ++eeIt) + { + *eeIt = RemapNoSuperTriangle(*eeIt); + } + updatedPieceToOriginals.insert( + std::make_pair(RemapNoSuperTriangle(it->first), ee)); + } + pieceToOriginals = updatedPieceToOriginals; + } + } + // remove other triangles + removeTriangles(removedTriangles); + // adjust triangle vertices: account for removed super-triangle + if(m_superGeomType == SuperGeometryType::SuperTriangle) + { + for(TriangleVec::iterator t = triangles.begin(); t != triangles.end(); + ++t) + { + VerticesArr3& vv = t->vertices; + for(VerticesArr3::iterator v = vv.begin(); v != vv.end(); ++v) + { + *v -= 3; + } + } + } +} + +template +void Triangulation::initializedWithCustomSuperGeometry() +{ + m_nearPtLocator.initialize(vertices); + m_nTargetVerts = static_cast(vertices.size()); + m_superGeomType = SuperGeometryType::Custom; +} + +template +TriIndUSet Triangulation::growToBoundary( + std::stack seeds) const +{ + TriIndUSet traversed; + while(!seeds.empty()) + { + const TriInd iT = seeds.top(); + seeds.pop(); + traversed.insert(iT); + const Triangle& t = triangles[iT]; + for(Index i(0); i < Index(3); ++i) + { + const Edge opEdge(t.vertices[ccw(i)], t.vertices[cw(i)]); + if(fixedEdges.count(opEdge)) + continue; + const TriInd iN = t.neighbors[opoNbr(i)]; + if(iN != noNeighbor && traversed.count(iN) == 0) + seeds.push(iN); + } + } + return traversed; +} + +template +void Triangulation::makeDummy(const TriInd iT) +{ + m_dummyTris.push_back(iT); +} + +template +TriInd Triangulation::addTriangle(const Triangle& t) +{ + if(m_dummyTris.empty()) + { + triangles.push_back(t); + return TriInd(triangles.size() - 1); + } + const TriInd nxtDummy = m_dummyTris.back(); + m_dummyTris.pop_back(); + triangles[nxtDummy] = t; + return nxtDummy; +} + +template +TriInd Triangulation::addTriangle() +{ + if(m_dummyTris.empty()) + { + const Triangle dummy = { + {noVertex, noVertex, noVertex}, + {noNeighbor, noNeighbor, noNeighbor}}; + triangles.push_back(dummy); + return TriInd(triangles.size() - 1); + } + const TriInd nxtDummy = m_dummyTris.back(); + m_dummyTris.pop_back(); + return nxtDummy; +} + +template +void Triangulation::insertEdges( + const std::vector& edges) +{ + insertEdges(edges.begin(), edges.end(), edge_get_v1, edge_get_v2); +} + +template +void Triangulation::conformToEdges( + const std::vector& edges) +{ + conformToEdges(edges.begin(), edges.end(), edge_get_v1, edge_get_v2); +} + +template +void Triangulation::fixEdge(const Edge& edge) +{ + if(!fixedEdges.insert(edge).second) + { + ++overlapCount[edge]; // if edge is already fixed increment the counter + } +} + +namespace detail +{ + +// add element to 'to' if not already in 'to' +template +void insert_unique(std::vector& to, const T& elem) +{ + if(std::find(to.begin(), to.end(), elem) == to.end()) + { + to.push_back(elem); + } +} + +// add elements of 'from' that are not present in 'to' to 'to' +template +void insert_unique( + std::vector& to, + const std::vector& from) +{ + typedef typename std::vector::const_iterator Cit; + to.reserve(to.size() + from.size()); + for(Cit cit = from.begin(); cit != from.end(); ++cit) + { + insert_unique(to, *cit); + } +} + +} // namespace detail + +template +void Triangulation::splitFixedEdge( + const Edge& edge, + const VertInd iSplitVert) +{ + // split constraint (fixed) edge that already exists in triangulation + const Edge half1(edge.v1(), iSplitVert); + const Edge half2(iSplitVert, edge.v2()); + // remove the edge that and add its halves + fixedEdges.erase(edge); + fixEdge(half1); + fixEdge(half2); + // maintain overlaps + typedef unordered_map::const_iterator It; + const It overlapIt = overlapCount.find(edge); + if(overlapIt != overlapCount.end()) + { + overlapCount[half1] += overlapIt->second; + overlapCount[half2] += overlapIt->second; + overlapCount.erase(overlapIt); + } + // maintain piece-to-original mapping + EdgeVec newOriginals(1, edge); + const unordered_map::const_iterator originalsIt = + pieceToOriginals.find(edge); + if(originalsIt != pieceToOriginals.end()) + { // edge being split was split before: pass-through originals + newOriginals = originalsIt->second; + pieceToOriginals.erase(originalsIt); + } + detail::insert_unique(pieceToOriginals[half1], newOriginals); + detail::insert_unique(pieceToOriginals[half2], newOriginals); +} + +template +VertInd Triangulation::addSplitEdgeVertex( + const V2d& splitVert, + const TriInd iT, + const TriInd iTopo) +{ + // add a new point on the edge that splits an edge in two + const VertInd iSplitVert = static_cast(vertices.size()); + addNewVertex(splitVert, noNeighbor); + std::stack triStack = insertVertexOnEdge(iSplitVert, iT, iTopo); + tryAddVertexToLocator(iSplitVert); + ensureDelaunayByEdgeFlips(iSplitVert, triStack); + return iSplitVert; +} + +template +VertInd Triangulation::splitFixedEdgeAt( + const Edge& edge, + const V2d& splitVert, + const TriInd iT, + const TriInd iTopo) +{ + const VertInd iSplitVert = addSplitEdgeVertex(splitVert, iT, iTopo); + splitFixedEdge(edge, iSplitVert); + return iSplitVert; +} + +template +void Triangulation::fixEdge( + const Edge& edge, + const Edge& originalEdge) +{ + fixEdge(edge); + if(edge != originalEdge) + detail::insert_unique(pieceToOriginals[edge], originalEdge); +} + +namespace detail +{ + +template +T lerp(const T& a, const T& b, const T t) +{ + return (T(1) - t) * a + t * b; +} + +// Precondition: ab and cd intersect normally +template +V2d intersectionPosition( + const V2d& a, + const V2d& b, + const V2d& c, + const V2d& d) +{ + using namespace predicates::adaptive; + + // note: for better accuracy we interpolate x and y separately + // on a segment with the shortest x/y-projection correspondingly + const T a_cd = orient2d(c.x, c.y, d.x, d.y, a.x, a.y); + const T b_cd = orient2d(c.x, c.y, d.x, d.y, b.x, b.y); + const T t_ab = a_cd / (a_cd - b_cd); + + const T c_ab = orient2d(a.x, a.y, b.x, b.y, c.x, c.y); + const T d_ab = orient2d(a.x, a.y, b.x, b.y, d.x, d.y); + const T t_cd = c_ab / (c_ab - d_ab); + + return V2d::make( + std::fabs(a.x - b.x) < std::fabs(c.x - d.x) ? lerp(a.x, b.x, t_ab) + : lerp(c.x, d.x, t_cd), + std::fabs(a.y - b.y) < std::fabs(c.y - d.y) ? lerp(a.y, b.y, t_ab) + : lerp(c.y, d.y, t_cd)); +} + +} // namespace detail + +template +void Triangulation::insertEdgeIteration( + const Edge edge, + const Edge originalEdge, + EdgeVec& remaining, + std::vector& tppIterations) +{ + const VertInd iA = edge.v1(); + VertInd iB = edge.v2(); + if(iA == iB) // edge connects a vertex to itself + return; + + if(hasEdge(iA, iB)) + { + fixEdge(edge, originalEdge); + return; + } + + const V2d& a = vertices[iA]; + const V2d& b = vertices[iB]; + const T distanceTolerance = + m_minDistToConstraintEdge == T(0) + ? T(0) + : m_minDistToConstraintEdge * distance(a, b); + + TriInd iT; + // Note: 'L' is left and 'R' is right of the inserted constraint edge + VertInd iVL, iVR; + tie(iT, iVL, iVR) = intersectedTriangle(iA, a, b, distanceTolerance); + // if one of the triangle vertices is on the edge, move edge start + if(iT == noNeighbor) + { + const Edge edgePart(iA, iVL); + fixEdge(edgePart, originalEdge); + remaining.push_back(Edge(iVL, iB)); + return; + } + Triangle t = triangles[iT]; + std::vector intersected(1, iT); + std::vector polyL, polyR; + polyL.reserve(2); + polyL.push_back(iA); + polyL.push_back(iVL); + polyR.reserve(2); + polyR.push_back(iA); + polyR.push_back(iVR); + unordered_map outerTris; + outerTris[Edge(iA, iVL)] = edgeNeighbor(t, iA, iVL); + outerTris[Edge(iA, iVR)] = edgeNeighbor(t, iA, iVR); + VertInd iV = iA; + + while(!t.containsVertex(iB)) + { + const TriInd iTopo = opposedTriangle(t, iV); + const Triangle& tOpo = triangles[iTopo]; + const VertInd iVopo = opposedVertex(tOpo, iT); + + switch(m_intersectingEdgesStrategy) + { + case IntersectingConstraintEdges::NotAllowed: + if(fixedEdges.count(Edge(iVL, iVR))) + { + // make sure to report original input edges in the exception + Edge e1 = originalEdge; + Edge e2 = Edge(iVL, iVR); + e2 = pieceToOriginals.count(e2) + ? pieceToOriginals.at(e2).front() + : e2; + // don't count super-triangle vertices + e1 = Edge(e1.v1() - m_nTargetVerts, e1.v2() - m_nTargetVerts); + e2 = Edge(e2.v1() - m_nTargetVerts, e2.v2() - m_nTargetVerts); + throw IntersectingConstraintsError( + e1, + pieceToOriginals.count(e2) ? pieceToOriginals.at(e2).front() + : e2, + CDT_SOURCE_LOCATION); + } + break; + case IntersectingConstraintEdges::TryResolve: { + if(!fixedEdges.count(Edge(iVL, iVR))) + break; + // split edge at the intersection of two constraint edges + const V2d newV = detail::intersectionPosition( + vertices[iA], vertices[iB], vertices[iVL], vertices[iVR]); + const VertInd iNewVert = + splitFixedEdgeAt(Edge(iVL, iVR), newV, iT, iTopo); + // TODO: is it's possible to re-use pseudo-polygons + // for inserting [iA, iNewVert] edge half? + remaining.push_back(Edge(iA, iNewVert)); + remaining.push_back(Edge(iNewVert, iB)); + return; + } + case IntersectingConstraintEdges::DontCheck: + assert(!fixedEdges.count(Edge(iVL, iVR))); + break; + } + + const PtLineLocation::Enum loc = + locatePointLine(vertices[iVopo], a, b, distanceTolerance); + if(loc == PtLineLocation::Left) + { + const Edge e(polyL.back(), iVopo); + const TriInd outer = edgeNeighbor(tOpo, e.v1(), e.v2()); + if(!outerTris.insert(std::make_pair(e, outer)).second) + outerTris.at(e) = noNeighbor; // hanging edge detected + polyL.push_back(iVopo); + iV = iVL; + iVL = iVopo; + } + else if(loc == PtLineLocation::Right) + { + const Edge e(polyR.back(), iVopo); + const TriInd outer = edgeNeighbor(tOpo, e.v1(), e.v2()); + if(!outerTris.insert(std::make_pair(e, outer)).second) + outerTris.at(e) = noNeighbor; // hanging edge detected + polyR.push_back(iVopo); + iV = iVR; + iVR = iVopo; + } + else // encountered point on the edge + iB = iVopo; + + intersected.push_back(iTopo); + iT = iTopo; + t = triangles[iT]; + } + outerTris[Edge(polyL.back(), iB)] = edgeNeighbor(t, polyL.back(), iB); + outerTris[Edge(polyR.back(), iB)] = edgeNeighbor(t, polyR.back(), iB); + polyL.push_back(iB); + polyR.push_back(iB); + + assert(!intersected.empty()); + // make sure start/end vertices have a valid adjacent triangle + // that is not intersected by an edge + if(m_vertTris[iA] == intersected.front()) + pivotVertexTriangleCW(iA); + if(m_vertTris[iB] == intersected.back()) + pivotVertexTriangleCW(iB); + // Remove intersected triangles + typedef std::vector::const_iterator TriIndCit; + for(TriIndCit it = intersected.begin(); it != intersected.end(); ++it) + makeDummy(*it); + { // Triangulate pseudo-polygons on both sides + std::reverse(polyR.begin(), polyR.end()); + const TriInd iTL = addTriangle(); + const TriInd iTR = addTriangle(); + triangulatePseudoPolygon(polyL, outerTris, iTL, iTR, tppIterations); + triangulatePseudoPolygon(polyR, outerTris, iTR, iTL, tppIterations); + } + + if(iB != edge.v2()) // encountered point on the edge + { + // fix edge part + const Edge edgePart(iA, iB); + fixEdge(edgePart, originalEdge); + remaining.push_back(Edge(iB, edge.v2())); + return; + } + else + { + fixEdge(edge, originalEdge); + } +} + +template +void Triangulation::insertEdge( + Edge edge, + const Edge originalEdge, + EdgeVec& remaining, + std::vector& tppIterations) +{ + // use iteration over recursion to avoid stack overflows + remaining.clear(); + remaining.push_back(edge); + while(!remaining.empty()) + { + edge = remaining.back(); + remaining.pop_back(); + insertEdgeIteration(edge, originalEdge, remaining, tppIterations); + } +} + +template +void Triangulation::conformToEdgeIteration( + Edge edge, + const EdgeVec& originals, + BoundaryOverlapCount overlaps, + std::vector& remaining) +{ + const VertInd iA = edge.v1(); + VertInd iB = edge.v2(); + if(iA == iB) // edge connects a vertex to itself + return; + + if(hasEdge(iA, iB)) + { + fixEdge(edge); + if(overlaps > 0) + overlapCount[edge] = overlaps; + // avoid marking edge as a part of itself + if(!originals.empty() && edge != originals.front()) + { + detail::insert_unique(pieceToOriginals[edge], originals); + } + return; + } + + const V2d& a = vertices[iA]; + const V2d& b = vertices[iB]; + const T distanceTolerance = + m_minDistToConstraintEdge == T(0) + ? T(0) + : m_minDistToConstraintEdge * distance(a, b); + TriInd iT; + VertInd iVleft, iVright; + tie(iT, iVleft, iVright) = intersectedTriangle(iA, a, b, distanceTolerance); + // if one of the triangle vertices is on the edge, move edge start + if(iT == noNeighbor) + { + const Edge edgePart(iA, iVleft); + fixEdge(edgePart); + if(overlaps > 0) + overlapCount[edgePart] = overlaps; + detail::insert_unique(pieceToOriginals[edgePart], originals); +#ifdef CDT_CXX11_IS_SUPPORTED + remaining.emplace_back(Edge(iVleft, iB), originals, overlaps); +#else + remaining.push_back(make_tuple(Edge(iVleft, iB), originals, overlaps)); +#endif + return; + } + + VertInd iV = iA; + Triangle t = triangles[iT]; + while(std::find(t.vertices.begin(), t.vertices.end(), iB) == + t.vertices.end()) + { + const TriInd iTopo = opposedTriangle(t, iV); + const Triangle& tOpo = triangles[iTopo]; + const VertInd iVopo = opposedVertex(tOpo, iT); + const V2d vOpo = vertices[iVopo]; + + switch(m_intersectingEdgesStrategy) + { + case IntersectingConstraintEdges::NotAllowed: + if(fixedEdges.count(Edge(iVleft, iVright))) + { + // make sure to report original input edges in the exception + Edge e1 = pieceToOriginals.count(edge) + ? pieceToOriginals.at(edge).front() + : edge; + Edge e2(iVleft, iVright); + e2 = pieceToOriginals.count(e2) + ? pieceToOriginals.at(e2).front() + : e2; + // don't count super-triangle vertices + e1 = Edge(e1.v1() - m_nTargetVerts, e1.v2() - m_nTargetVerts); + e2 = Edge(e2.v1() - m_nTargetVerts, e2.v2() - m_nTargetVerts); + throw IntersectingConstraintsError(e1, e2, CDT_SOURCE_LOCATION); + } + break; + case IntersectingConstraintEdges::TryResolve: { + if(!fixedEdges.count(Edge(iVleft, iVright))) + break; + // split edge at the intersection of two constraint edges + const V2d newV = detail::intersectionPosition( + vertices[iA], + vertices[iB], + vertices[iVleft], + vertices[iVright]); + const VertInd iNewVert = + splitFixedEdgeAt(Edge(iVleft, iVright), newV, iT, iTopo); +#ifdef CDT_CXX11_IS_SUPPORTED + remaining.emplace_back(Edge(iNewVert, iB), originals, overlaps); + remaining.emplace_back(Edge(iA, iNewVert), originals, overlaps); +#else + remaining.push_back( + make_tuple(Edge(iNewVert, iB), originals, overlaps)); + remaining.push_back( + make_tuple(Edge(iA, iNewVert), originals, overlaps)); +#endif + return; + } + case IntersectingConstraintEdges::DontCheck: + assert(!fixedEdges.count(Edge(iVleft, iVright))); + break; + } + + iT = iTopo; + t = triangles[iT]; + + const PtLineLocation::Enum loc = + locatePointLine(vOpo, a, b, distanceTolerance); + if(loc == PtLineLocation::Left) + { + iV = iVleft; + iVleft = iVopo; + } + else if(loc == PtLineLocation::Right) + { + iV = iVright; + iVright = iVopo; + } + else // encountered point on the edge + iB = iVopo; + } + + // encountered one or more points on the edge: add remaining edge part + if(iB != edge.v2()) + { +#ifdef CDT_CXX11_IS_SUPPORTED + remaining.emplace_back(Edge(iB, edge.v2()), originals, overlaps); +#else + remaining.push_back( + make_tuple(Edge(iB, edge.v2()), originals, overlaps)); +#endif + } + + // add mid-point to triangulation + const VertInd iMid = static_cast(vertices.size()); + const V2d& start = vertices[iA]; + const V2d& end = vertices[iB]; + addNewVertex( + V2d::make((start.x + end.x) / T(2), (start.y + end.y) / T(2)), + noNeighbor); + const std::vector flippedFixedEdges = + insertVertex_FlipFixedEdges(iMid); + +#ifdef CDT_CXX11_IS_SUPPORTED + remaining.emplace_back(Edge(iMid, iB), originals, overlaps); + remaining.emplace_back(Edge(iA, iMid), originals, overlaps); +#else + remaining.push_back(make_tuple(Edge(iMid, iB), originals, overlaps)); + remaining.push_back(make_tuple(Edge(iA, iMid), originals, overlaps)); +#endif + + // re-introduce fixed edges that were flipped + // and make sure overlap count is preserved + for(std::vector::const_iterator it = flippedFixedEdges.begin(); + it != flippedFixedEdges.end(); + ++it) + { + const Edge& flippedFixedEdge = *it; + fixedEdges.erase(flippedFixedEdge); + + BoundaryOverlapCount prevOverlaps = 0; + const unordered_map::const_iterator + overlapsIt = overlapCount.find(flippedFixedEdge); + if(overlapsIt != overlapCount.end()) + { + prevOverlaps = overlapsIt->second; + overlapCount.erase(overlapsIt); + } + // override overlapping boundaries count when re-inserting an edge + EdgeVec prevOriginals(1, flippedFixedEdge); + const unordered_map::const_iterator originalsIt = + pieceToOriginals.find(flippedFixedEdge); + if(originalsIt != pieceToOriginals.end()) + { + prevOriginals = originalsIt->second; + } +#ifdef CDT_CXX11_IS_SUPPORTED + remaining.emplace_back(flippedFixedEdge, prevOriginals, prevOverlaps); +#else + remaining.push_back( + make_tuple(flippedFixedEdge, prevOriginals, prevOverlaps)); +#endif + } +} + +template +void Triangulation::conformToEdge( + Edge edge, + EdgeVec originals, + BoundaryOverlapCount overlaps, + std::vector& remaining) +{ + // use iteration over recursion to avoid stack overflows + remaining.clear(); +#ifdef CDT_CXX11_IS_SUPPORTED + remaining.emplace_back(edge, originals, overlaps); +#else + remaining.push_back(make_tuple(edge, originals, overlaps)); +#endif + while(!remaining.empty()) + { + tie(edge, originals, overlaps) = remaining.back(); + remaining.pop_back(); + conformToEdgeIteration(edge, originals, overlaps, remaining); + } +} + +/*! + * Returns: + * - intersected triangle index + * - index of point on the left of the line + * - index of point on the right of the line + * If left point is right on the line: no triangle is intersected: + * - triangle index is no-neighbor (invalid) + * - index of point on the line + * - index of point on the right of the line + */ +template +tuple +Triangulation::intersectedTriangle( + const VertInd iA, + const V2d& a, + const V2d& b, + const T orientationTolerance) const +{ + const TriInd startTri = m_vertTris[iA]; + TriInd iT = startTri; + do + { + const Triangle t = triangles[iT]; + const Index i = vertexInd(t.vertices, iA); + const VertInd iP2 = t.vertices[ccw(i)]; + const T orientP2 = orient2D(vertices[iP2], a, b); + const PtLineLocation::Enum locP2 = classifyOrientation(orientP2); + if(locP2 == PtLineLocation::Right) + { + const VertInd iP1 = t.vertices[cw(i)]; + const T orientP1 = orient2D(vertices[iP1], a, b); + const PtLineLocation::Enum locP1 = classifyOrientation(orientP1); + if(locP1 == PtLineLocation::OnLine) + { + return make_tuple(noNeighbor, iP1, iP1); + } + if(locP1 == PtLineLocation::Left) + { + if(orientationTolerance) + { + T closestOrient; + VertInd iClosestP; + if(std::abs(orientP1) <= std::abs(orientP2)) + { + closestOrient = orientP1; + iClosestP = iP1; + } + else + { + closestOrient = orientP2; + iClosestP = iP2; + } + if(classifyOrientation( + closestOrient, orientationTolerance) == + PtLineLocation::OnLine) + { + return make_tuple(noNeighbor, iClosestP, iClosestP); + } + } + return make_tuple(iT, iP1, iP2); + } + } + iT = t.next(iA).first; + } while(iT != startTri); + + throw Error( + "Could not find vertex triangle intersected by an edge.", + CDT_SOURCE_LOCATION); +} + +template +void Triangulation::addSuperTriangle(const Box2d& box) +{ + m_nTargetVerts = 3; + m_superGeomType = SuperGeometryType::SuperTriangle; + + const V2d center = { + (box.min.x + box.max.x) / T(2), (box.min.y + box.max.y) / T(2)}; + const T w = box.max.x - box.min.x; + const T h = box.max.y - box.min.y; + T r = std::max(w, h); // incircle radius upper bound + + // Note: make sure radius is big enough. Constants chosen experimentally. + // - for tiny bounding boxes: use 1.0 as the smallest radius + // - multiply radius by 2.0 for extra safety margin + r = std::max(T(2) * r, T(1)); + + // Note: for very large floating point numbers rounding can lead to wrong + // super-triangle coordinates. This is a very rare corner-case so the + // handling is very primitive. + { // note: '<=' means '==' but avoids the warning + while(center.y <= center.y - r) + r *= T(2); + } + + const T R = T(2) * r; // excircle radius + const T cos_30_deg = T(0.8660254037844386); // note: (std::sqrt(3.0) / 2.0) + const T shiftX = R * cos_30_deg; + const V2d posV1 = {center.x - shiftX, center.y - r}; + const V2d posV2 = {center.x + shiftX, center.y - r}; + const V2d posV3 = {center.x, center.y + R}; + addNewVertex(posV1, TriInd(0)); + addNewVertex(posV2, TriInd(0)); + addNewVertex(posV3, TriInd(0)); + const Triangle superTri = { + {VertInd(0), VertInd(1), VertInd(2)}, + {noNeighbor, noNeighbor, noNeighbor}}; + addTriangle(superTri); + if(m_vertexInsertionOrder != VertexInsertionOrder::Auto) + { + m_nearPtLocator.initialize(vertices); + } +} + +template +void Triangulation::addNewVertex( + const V2d& pos, + const TriInd iT) +{ + vertices.push_back(pos); + m_vertTris.push_back(iT); +} + +template +std::vector +Triangulation::insertVertex_FlipFixedEdges( + const VertInd iV1) +{ + std::vector flippedFixedEdges; + + const V2d& v1 = vertices[iV1]; + const VertInd startVertex = m_nearPtLocator.nearPoint(v1, vertices); + array trisAt = walkingSearchTrianglesAt(iV1, startVertex); + std::stack triStack = + trisAt[1] == noNeighbor ? insertVertexInsideTriangle(iV1, trisAt[0]) + : insertVertexOnEdge(iV1, trisAt[0], trisAt[1]); + + TriInd iTopo, n1, n2, n3, n4; + VertInd iV2, iV3, iV4; + while(!triStack.empty()) + { + const TriInd iT = triStack.top(); + triStack.pop(); + + edgeFlipInfo(iT, iV1, iTopo, iV2, iV3, iV4, n1, n2, n3, n4); + if(iTopo != noNeighbor && isFlipNeeded(iV1, iV2, iV3, iV4)) + { + // if flipped edge is fixed, remember it + const Edge flippedEdge(iV2, iV4); + if(!fixedEdges.empty() && + fixedEdges.find(flippedEdge) != fixedEdges.end()) + { + flippedFixedEdges.push_back(flippedEdge); + } + + flipEdge(iT, iTopo, iV1, iV2, iV3, iV4, n1, n2, n3, n4); + triStack.push(iT); + triStack.push(iTopo); + } + } + + tryAddVertexToLocator(iV1); + return flippedFixedEdges; +} + +template +void Triangulation::insertVertex( + const VertInd iVert, + const VertInd walkStart) +{ + const array trisAt = walkingSearchTrianglesAt(iVert, walkStart); + std::stack triStack = + trisAt[1] == noNeighbor + ? insertVertexInsideTriangle(iVert, trisAt[0]) + : insertVertexOnEdge(iVert, trisAt[0], trisAt[1]); + ensureDelaunayByEdgeFlips(iVert, triStack); +} + +template +void Triangulation::insertVertex(const VertInd iVert) +{ + const V2d& v = vertices[iVert]; + const VertInd walkStart = m_nearPtLocator.nearPoint(v, vertices); + insertVertex(iVert, walkStart); + tryAddVertexToLocator(iVert); +} + +template +void Triangulation::ensureDelaunayByEdgeFlips( + const VertInd iV1, + std::stack& triStack) +{ + TriInd iTopo, n1, n2, n3, n4; + VertInd iV2, iV3, iV4; + while(!triStack.empty()) + { + const TriInd iT = triStack.top(); + triStack.pop(); + + edgeFlipInfo(iT, iV1, iTopo, iV2, iV3, iV4, n1, n2, n3, n4); + if(iTopo != noNeighbor && isFlipNeeded(iV1, iV2, iV3, iV4)) + { + flipEdge(iT, iTopo, iV1, iV2, iV3, iV4, n1, n2, n3, n4); + triStack.push(iT); + triStack.push(iTopo); + } + } +} + +/* + * v4 original edge: (v1, v3) + * /|\ flip-candidate edge: (v, v2) + * / | \ + * n3 / | \ n4 + * / | \ + * new vertex--> v1 T | Topo v3 + * \ | / + * n1 \ | / n2 + * \ | / + * \|/ + * v2 + */ +template +void Triangulation::edgeFlipInfo( + const TriInd iT, + const VertInd iV1, + TriInd& iTopo, + VertInd& iV2, + VertInd& iV3, + VertInd& iV4, + TriInd& n1, + TriInd& n2, + TriInd& n3, + TriInd& n4) +{ + /* v[2] + / \ + n[2]/ \n[1] + /_____\ + v[0] n[0] v[1] */ + const Triangle& t = triangles[iT]; + if(t.vertices[0] == iV1) + { + iV2 = t.vertices[1]; + iV4 = t.vertices[2]; + n1 = t.neighbors[0]; + n3 = t.neighbors[2]; + iTopo = t.neighbors[1]; + } + else if(t.vertices[1] == iV1) + { + iV2 = t.vertices[2]; + iV4 = t.vertices[0]; + n1 = t.neighbors[1]; + n3 = t.neighbors[0]; + iTopo = t.neighbors[2]; + } + else + { + iV2 = t.vertices[0]; + iV4 = t.vertices[1]; + n1 = t.neighbors[2]; + n3 = t.neighbors[1]; + iTopo = t.neighbors[0]; + } + if(iTopo == noNeighbor) + return; + const Triangle& tOpo = triangles[iTopo]; + if(tOpo.neighbors[0] == iT) + { + iV3 = tOpo.vertices[2]; + n2 = tOpo.neighbors[1]; + n4 = tOpo.neighbors[2]; + } + else if(tOpo.neighbors[1] == iT) + { + iV3 = tOpo.vertices[0]; + n2 = tOpo.neighbors[2]; + n4 = tOpo.neighbors[0]; + } + else + { + iV3 = tOpo.vertices[1]; + n2 = tOpo.neighbors[0]; + n4 = tOpo.neighbors[1]; + } +} + +/*! + * Handles super-triangle vertices. + * Super-tri points are not infinitely far and influence the input points + * Three cases are possible: + * 1. If one of the opposed vertices is super-tri: no flip needed + * 2. One of the shared vertices is super-tri: + * check if on point is same side of line formed by non-super-tri + * vertices as the non-super-tri shared vertex + * 3. None of the vertices are super-tri: normal circumcircle test + */ +/* + * v4 original edge: (v2, v4) + * /|\ flip-candidate edge: (v1, v3) + * / | \ + * / | \ + * / | \ + * new vertex--> v1 | v3 + * \ | / + * \ | / + * \ | / + * \|/ + * v2 + */ +template +bool Triangulation::isFlipNeeded( + const VertInd iV1, + const VertInd iV2, + const VertInd iV3, + const VertInd iV4) const +{ + if(fixedEdges.count(Edge(iV2, iV4))) + return false; // flip not needed if the original edge is fixed + const V2d& v1 = vertices[iV1]; + const V2d& v2 = vertices[iV2]; + const V2d& v3 = vertices[iV3]; + const V2d& v4 = vertices[iV4]; + if(m_superGeomType == SuperGeometryType::SuperTriangle) + { + // If flip-candidate edge touches super-triangle in-circumference + // test has to be replaced with orient2d test against the line + // formed by two non-artificial vertices (that don't belong to + // super-triangle) + if(iV1 < 3) // flip-candidate edge touches super-triangle + { + // does original edge also touch super-triangle? + if(iV2 < 3) + return locatePointLine(v2, v3, v4) == + locatePointLine(v1, v3, v4); + if(iV4 < 3) + return locatePointLine(v4, v2, v3) == + locatePointLine(v1, v2, v3); + return false; // original edge does not touch super-triangle + } + if(iV3 < 3) // flip-candidate edge touches super-triangle + { + // does original edge also touch super-triangle? + if(iV2 < 3) + { + return locatePointLine(v2, v1, v4) == + locatePointLine(v3, v1, v4); + } + if(iV4 < 3) + { + return locatePointLine(v4, v2, v1) == + locatePointLine(v3, v2, v1); + } + return false; // original edge does not touch super-triangle + } + // flip-candidate edge does not touch super-triangle + if(iV2 < 3) + return locatePointLine(v2, v3, v4) == locatePointLine(v1, v3, v4); + if(iV4 < 3) + return locatePointLine(v4, v2, v3) == locatePointLine(v1, v2, v3); + } + return isInCircumcircle(v1, v2, v3, v4); +} + +/* Flip edge between T and Topo: + * + * v4 | - old edge + * /|\ ~ - new edge + * / | \ + * n3 / T' \ n4 + * / | \ + * / | \ + * T -> v1 ~~~~~~~~ v3 <- Topo + * \ | / + * \ | / + * n1 \Topo'/ n2 + * \ | / + * \|/ + * v2 + */ +template +void Triangulation::flipEdge( + const TriInd iT, + const TriInd iTopo, + const VertInd v1, + const VertInd v2, + const VertInd v3, + const VertInd v4, + const TriInd n1, + const TriInd n2, + const TriInd n3, + const TriInd n4) +{ + // change vertices and neighbors + using detail::arr3; + triangles[iT] = Triangle::make(arr3(v4, v1, v3), arr3(n3, iTopo, n4)); + triangles[iTopo] = Triangle::make(arr3(v2, v3, v1), arr3(n2, iT, n1)); + // adjust neighboring triangles and vertices + changeNeighbor(n1, iT, iTopo); + changeNeighbor(n4, iTopo, iT); + // only adjust adjacent triangles if triangulation is not finalized: + // can happen when called from outside on an already finalized + // triangulation + if(!isFinalized()) + { + setAdjacentTriangle(v4, iT); + setAdjacentTriangle(v2, iTopo); + } +} + +/* Insert point into triangle: split into 3 triangles: + * - create 2 new triangles + * - re-use old triangle for the 3rd + * v3 + * / | \ + * / | \ <-- original triangle (t) + * / | \ + * n3 / | \ n2 + * /newT2|newT1\ + * / v \ + * / __/ \__ \ + * / __/ \__ \ + * / _/ t' \_ \ + * v1 ___________________ v2 + * n1 + */ +template +std::stack +Triangulation::insertVertexInsideTriangle( + VertInd v, + TriInd iT) +{ + const TriInd iNewT1 = addTriangle(); + const TriInd iNewT2 = addTriangle(); + + Triangle& t = triangles[iT]; + const array vv = t.vertices; + const array nn = t.neighbors; + const VertInd v1 = vv[0], v2 = vv[1], v3 = vv[2]; + const TriInd n1 = nn[0], n2 = nn[1], n3 = nn[2]; + // make two new triangles and convert current triangle to 3rd new + // triangle + using detail::arr3; + triangles[iNewT1] = Triangle::make(arr3(v2, v3, v), arr3(n2, iNewT2, iT)); + triangles[iNewT2] = Triangle::make(arr3(v3, v1, v), arr3(n3, iT, iNewT1)); + t = Triangle::make(arr3(v1, v2, v), arr3(n1, iNewT1, iNewT2)); + // adjust adjacent triangles + setAdjacentTriangle(v, iT); + setAdjacentTriangle(v3, iNewT1); + // change triangle neighbor's neighbors to new triangles + changeNeighbor(n2, iT, iNewT1); + changeNeighbor(n3, iT, iNewT2); + // return newly added triangles + std::stack newTriangles; + newTriangles.push(iT); + newTriangles.push(iNewT1); + newTriangles.push(iNewT2); + return newTriangles; +} + +/* Inserting a point on the edge between two triangles + * T1 (top) v1 + * /|\ + * n1 / | \ n4 + * / | \ + * / T1' | Tnew1\ + * v2-------v-------v4 + * \ T2' | Tnew2/ + * \ | / + * n2 \ | / n3 + * \|/ + * T2 (bottom) v3 + */ +template +std::stack Triangulation::insertVertexOnEdge( + VertInd v, + TriInd iT1, + TriInd iT2) +{ + const TriInd iTnew1 = addTriangle(); + const TriInd iTnew2 = addTriangle(); + + Triangle& t1 = triangles[iT1]; + Triangle& t2 = triangles[iT2]; + Index i = opposedVertexInd(t1.neighbors, iT2); + const VertInd v1 = t1.vertices[i]; + const VertInd v2 = t1.vertices[ccw(i)]; + const TriInd n1 = t1.neighbors[i]; + const TriInd n4 = t1.neighbors[cw(i)]; + i = opposedVertexInd(t2.neighbors, iT1); + const VertInd v3 = t2.vertices[i]; + const VertInd v4 = t2.vertices[ccw(i)]; + const TriInd n3 = t2.neighbors[i]; + const TriInd n2 = t2.neighbors[cw(i)]; + // add new triangles and change existing ones + using detail::arr3; + t1 = Triangle::make(arr3(v, v1, v2), arr3(iTnew1, n1, iT2)); + t2 = Triangle::make(arr3(v, v2, v3), arr3(iT1, n2, iTnew2)); + triangles[iTnew1] = Triangle::make(arr3(v, v4, v1), arr3(iTnew2, n4, iT1)); + triangles[iTnew2] = Triangle::make(arr3(v, v3, v4), arr3(iT2, n3, iTnew1)); + // adjust adjacent triangles + setAdjacentTriangle(v, iT1); + setAdjacentTriangle(v4, iTnew1); + // adjust neighboring triangles and vertices + changeNeighbor(n4, iT1, iTnew1); + changeNeighbor(n3, iT2, iTnew2); + // return newly added triangles + std::stack newTriangles; + newTriangles.push(iT1); + newTriangles.push(iTnew2); + newTriangles.push(iT2); + newTriangles.push(iTnew1); + return newTriangles; +} + +template +array +Triangulation::trianglesAt(const V2d& pos) const +{ + array out = {noNeighbor, noNeighbor}; + for(TriInd i = TriInd(0); i < TriInd(triangles.size()); ++i) + { + const Triangle& t = triangles[i]; + const V2d& v1 = vertices[t.vertices[0]]; + const V2d& v2 = vertices[t.vertices[1]]; + const V2d& v3 = vertices[t.vertices[2]]; + const PtTriLocation::Enum loc = locatePointTriangle(pos, v1, v2, v3); + if(loc == PtTriLocation::Outside) + continue; + out[0] = i; + if(isOnEdge(loc)) + out[1] = t.neighbors[edgeNeighbor(loc)]; + return out; + } + throw Error("No triangle was found at position", CDT_SOURCE_LOCATION); +} + +template +TriInd Triangulation::walkTriangles( + const VertInd startVertex, + const V2d& pos) const +{ + // begin walk in search of triangle at pos + TriInd currTri = m_vertTris[startVertex]; + bool found = false; + detail::SplitMix64RandGen prng; + while(!found) + { + const Triangle& t = triangles[currTri]; + found = true; + // stochastic offset to randomize which edge we check first + const Index offset(prng() % 3); + for(Index i_(0); i_ < Index(3); ++i_) + { + const Index i((i_ + offset) % 3); + const V2d& vStart = vertices[t.vertices[i]]; + const V2d& vEnd = vertices[t.vertices[ccw(i)]]; + const PtLineLocation::Enum edgeCheck = + locatePointLine(pos, vStart, vEnd); + const TriInd iN = t.neighbors[i]; + if(edgeCheck == PtLineLocation::Right && iN != noNeighbor) + { + found = false; + currTri = iN; + break; + } + } + } + return currTri; +} + +template +array Triangulation::walkingSearchTrianglesAt( + const VertInd iV, + const VertInd startVertex) const +{ + const V2d v = vertices[iV]; + array out = {noNeighbor, noNeighbor}; + const TriInd iT = walkTriangles(startVertex, v); + // Finished walk, locate point in current triangle + const Triangle& t = triangles[iT]; + const V2d& v1 = vertices[t.vertices[0]]; + const V2d& v2 = vertices[t.vertices[1]]; + const V2d& v3 = vertices[t.vertices[2]]; + const PtTriLocation::Enum loc = locatePointTriangle(v, v1, v2, v3); + + if(loc == PtTriLocation::Outside) + throw Error("No triangle was found at position", CDT_SOURCE_LOCATION); + if(loc == PtTriLocation::OnVertex) + { + const VertInd iDupe = v1 == v ? t.vertices[0] + : v2 == v ? t.vertices[1] + : t.vertices[2]; + throw DuplicateVertexError( + iV - m_nTargetVerts, iDupe - m_nTargetVerts, CDT_SOURCE_LOCATION); + } + + out[0] = iT; + if(isOnEdge(loc)) + out[1] = t.neighbors[edgeNeighbor(loc)]; + return out; +} + +/* Flip edge between T and Topo: + * + * v4 | - old edge + * /|\ ~ - new edge + * / | \ + * n3 / T' \ n4 + * / | \ + * / | \ + * T -> v1~~~~~~~~~v3 <- Topo + * \ | / + * \ | / + * n1 \Topo'/ n2 + * \ | / + * \|/ + * v2 + */ +template +void Triangulation::flipEdge( + const TriInd iT, + const TriInd iTopo) +{ + Triangle& t = triangles[iT]; + Triangle& tOpo = triangles[iTopo]; + const array& triNs = t.neighbors; + const array& triOpoNs = tOpo.neighbors; + const array& triVs = t.vertices; + const array& triOpoVs = tOpo.vertices; + // find vertices and neighbors + Index i = opposedVertexInd(t.neighbors, iTopo); + const VertInd v1 = triVs[i]; + const VertInd v2 = triVs[ccw(i)]; + const TriInd n1 = triNs[i]; + const TriInd n3 = triNs[cw(i)]; + i = opposedVertexInd(tOpo.neighbors, iT); + const VertInd v3 = triOpoVs[i]; + const VertInd v4 = triOpoVs[ccw(i)]; + const TriInd n4 = triOpoNs[i]; + const TriInd n2 = triOpoNs[cw(i)]; + // change vertices and neighbors + using detail::arr3; + t = Triangle::make(arr3(v4, v1, v3), arr3(n3, iTopo, n4)); + tOpo = Triangle::make(arr3(v2, v3, v1), arr3(n2, iT, n1)); + // adjust neighboring triangles and vertices + changeNeighbor(n1, iT, iTopo); + changeNeighbor(n4, iTopo, iT); + // only adjust adjacent triangles if triangulation is not finalized: + // can happen when called from outside on an already finalized + // triangulation + if(!isFinalized()) + { + setAdjacentTriangle(v4, iT); + setAdjacentTriangle(v2, iTopo); + } +} + +template +void Triangulation::changeNeighbor( + const TriInd iT, + const TriInd oldNeighbor, + const TriInd newNeighbor) +{ + if(iT == noNeighbor) + return; + NeighborsArr3& nn = triangles[iT].neighbors; + assert( + nn[0] == oldNeighbor || nn[1] == oldNeighbor || nn[2] == oldNeighbor); + if(nn[0] == oldNeighbor) + nn[0] = newNeighbor; + else if(nn[1] == oldNeighbor) + nn[1] = newNeighbor; + else + nn[2] = newNeighbor; +} + +template +void Triangulation::changeNeighbor( + const TriInd iT, + const VertInd iVedge1, + const VertInd iVedge2, + const TriInd newNeighbor) +{ + assert(iT != noNeighbor); + Triangle& t = triangles[iT]; + t.neighbors[edgeNeighborInd(t.vertices, iVedge1, iVedge2)] = newNeighbor; +} + +template +void Triangulation::triangulatePseudoPolygon( + const std::vector& poly, + unordered_map& outerTris, + TriInd iT, + TriInd iN, + std::vector& iterations) +{ + assert(poly.size() > 2); + // note: uses iteration instead of recursion to avoid stack overflows + iterations.clear(); + iterations.push_back(make_tuple( + IndexSizeType(0), + static_cast(poly.size() - 1), + iT, + iN, + Index(0))); + while(!iterations.empty()) + { + triangulatePseudoPolygonIteration(poly, outerTris, iterations); + } +} + +template +void Triangulation::triangulatePseudoPolygonIteration( + const std::vector& poly, + unordered_map& outerTris, + std::vector& iterations) +{ + IndexSizeType iA, iB; + TriInd iT, iParent; + Index iInParent; + assert(!iterations.empty()); + tie(iA, iB, iT, iParent, iInParent) = iterations.back(); + iterations.pop_back(); + assert(iB - iA > 1 && iT != noNeighbor && iParent != noNeighbor); + Triangle& t = triangles[iT]; + // find Delaunay point + const IndexSizeType iC = findDelaunayPoint(poly, iA, iB); + + const VertInd a = poly[iA]; + const VertInd b = poly[iB]; + const VertInd c = poly[iC]; + + // split pseudo-polygon in two parts and triangulate them + // note: second part needs to be pushed on stack first to be processed first + + // second part: points after the Delaunay point + if(iB - iC > 1) + { + const TriInd iNext = addTriangle(); + iterations.push_back(make_tuple(iC, iB, iNext, iT, Index(1))); + } + else // pseudo-poly is reduced to a single outer edge + { + const Edge outerEdge(b, c); + const TriInd outerTri = outerTris.at(outerEdge); + if(outerTri != noNeighbor) + { + assert(outerTri != iT); + t.neighbors[1] = outerTri; + changeNeighbor(outerTri, c, b, iT); + } + else + outerTris.at(outerEdge) = iT; + } + // first part: points before the Delaunay point + if(iC - iA > 1) + { // add next triangle and add another iteration + const TriInd iNext = addTriangle(); + iterations.push_back(make_tuple(iA, iC, iNext, iT, Index(2))); + } + else + { // pseudo-poly is reduced to a single outer edge + const Edge outerEdge(c, a); + const TriInd outerTri = outerTris.at(outerEdge); + if(outerTri != noNeighbor) + { + assert(outerTri != iT); + t.neighbors[2] = outerTri; + changeNeighbor(outerTri, c, a, iT); + } + else + outerTris.at(outerEdge) = iT; + } + // Finalize triangle + // note: only when triangle is finalized to we add it as a neighbor to + // parent to maintain triangulation topology consistency + triangles[iParent].neighbors[iInParent] = iT; + t.neighbors[0] = iParent; + t.vertices = detail::arr3(a, b, c); + setAdjacentTriangle(c, iT); +} + +template +IndexSizeType Triangulation::findDelaunayPoint( + const std::vector& poly, + const IndexSizeType iA, + const IndexSizeType iB) const +{ + assert(iB - iA > 1); + const V2d& a = vertices[poly[iA]]; + const V2d& b = vertices[poly[iB]]; + IndexSizeType out = iA + 1; + const V2d* c = &vertices[poly[out]]; // caching for better performance + for(IndexSizeType i = iA + 1; i < iB; ++i) + { + const V2d& v = vertices[poly[i]]; + if(isInCircumcircle(v, a, b, *c)) + { + out = i; + c = &v; + } + } + assert(out > iA && out < iB); // point is between ends + return out; +} + +template +void Triangulation::insertVertices( + const std::vector >& newVertices) +{ + return insertVertices( + newVertices.begin(), newVertices.end(), getX_V2d, getY_V2d); +} + +template +bool Triangulation::isFinalized() const +{ + return m_vertTris.empty() && !vertices.empty(); +} + +template +unordered_map +Triangulation::peelLayer( + std::stack seeds, + const LayerDepth layerDepth, + std::vector& triDepths) const +{ + unordered_map behindBoundary; + while(!seeds.empty()) + { + const TriInd iT = seeds.top(); + seeds.pop(); + triDepths[iT] = std::min(triDepths[iT], layerDepth); + behindBoundary.erase(iT); + const Triangle& t = triangles[iT]; + for(Index i(0); i < Index(3); ++i) + { + const Edge opEdge(t.vertices[ccw(i)], t.vertices[cw(i)]); + const TriInd iN = t.neighbors[opoNbr(i)]; + if(iN == noNeighbor || triDepths[iN] <= layerDepth) + continue; + if(fixedEdges.count(opEdge)) + { + const unordered_map::const_iterator cit = + overlapCount.find(opEdge); + const LayerDepth triDepth = cit == overlapCount.end() + ? layerDepth + 1 + : layerDepth + cit->second + 1; + behindBoundary[iN] = triDepth; + continue; + } + seeds.push(iN); + } + } + return behindBoundary; +} + +template +std::vector +Triangulation::calculateTriangleDepths() const +{ + std::vector triDepths( + triangles.size(), std::numeric_limits::max()); + std::stack seeds(TriDeque(1, m_vertTris[0])); + LayerDepth layerDepth = 0; + LayerDepth deepestSeedDepth = 0; + + unordered_map seedsByDepth; + do + { + const unordered_map& newSeeds = + peelLayer(seeds, layerDepth, triDepths); + + seedsByDepth.erase(layerDepth); + typedef unordered_map::const_iterator Iter; + for(Iter it = newSeeds.begin(); it != newSeeds.end(); ++it) + { + deepestSeedDepth = std::max(deepestSeedDepth, it->second); + seedsByDepth[it->second].insert(it->first); + } + const TriIndUSet& nextLayerSeeds = seedsByDepth[layerDepth + 1]; + seeds = std::stack( + TriDeque(nextLayerSeeds.begin(), nextLayerSeeds.end())); + ++layerDepth; + } while(!seeds.empty() || deepestSeedDepth > layerDepth); + + return triDepths; +} + +template +void Triangulation::insertVertices_AsProvided( + VertInd superGeomVertCount) +{ + for(VertInd iV = superGeomVertCount; iV < vertices.size(); ++iV) + { + insertVertex(iV); + } +} + +template +void Triangulation::insertVertices_Randomized( + VertInd superGeomVertCount) +{ + std::size_t vertexCount = vertices.size() - superGeomVertCount; + std::vector ii(vertexCount); + detail::iota(ii.begin(), ii.end(), superGeomVertCount); + detail::random_shuffle(ii.begin(), ii.end()); + for(std::vector::iterator it = ii.begin(); it != ii.end(); ++it) + { + insertVertex(*it); + } +} + +namespace detail +{ + +// log2 implementation backwards compatible with pre c++11 +template +inline double log2_bc(T x) +{ +#ifdef CDT_CXX11_IS_SUPPORTED + return std::log2(x); +#else + static double log2_constant = std::log(2.0); + return std::log(static_cast(x)) / log2_constant; +#endif +} + +/// Since KD-tree bulk load builds a balanced tree the maximum length of a +/// queue can be pre-calculated: it is calculated as size of a completely +/// filled tree layer plus the number of the nodes on a completely filled +/// layer that have two children. +inline std::size_t maxQueueLengthBFSKDTree(const std::size_t vertexCount) +{ + const int filledLayerPow2 = + static_cast(std::floor(log2_bc(vertexCount)) - 1); + const std::size_t nodesInFilledTree = + static_cast(std::pow(2., filledLayerPow2 + 1) - 1); + const std::size_t nodesInLastFilledLayer = + static_cast(std::pow(2., filledLayerPow2)); + const std::size_t nodesInLastLayer = vertexCount - nodesInFilledTree; + return nodesInLastLayer >= nodesInLastFilledLayer + ? nodesInLastFilledLayer + nodesInLastLayer - + nodesInLastFilledLayer + : nodesInLastFilledLayer; +} + +template +class FixedCapacityQueue +{ +public: + FixedCapacityQueue(const std::size_t capacity) + : m_vec(capacity) + , m_front(m_vec.begin()) + , m_back(m_vec.begin()) + , m_size(0) + {} + bool empty() const + { + return m_size == 0; + } + const T& front() const + { + return *m_front; + } + void pop() + { + assert(m_size > 0); + ++m_front; + if(m_front == m_vec.end()) + m_front = m_vec.begin(); + --m_size; + } + void push(const T& t) + { + assert(m_size < m_vec.size()); + *m_back = t; + ++m_back; + if(m_back == m_vec.end()) + m_back = m_vec.begin(); + ++m_size; + } +#ifdef CDT_CXX11_IS_SUPPORTED + void push(const T&& t) + { + assert(m_size < m_vec.size()); + *m_back = t; + ++m_back; + if(m_back == m_vec.end()) + m_back = m_vec.begin(); + ++m_size; + } +#endif +private: + std::vector m_vec; + typename std::vector::iterator m_front; + typename std::vector::iterator m_back; + std::size_t m_size; +}; + +template +class less_than_x +{ + const std::vector >& m_vertices; + +public: + less_than_x(const std::vector >& vertices) + : m_vertices(vertices) + {} + bool operator()(const VertInd a, const VertInd b) const + { + return m_vertices[a].x < m_vertices[b].x; + } +}; + +template +class less_than_y +{ + const std::vector >& m_vertices; + +public: + less_than_y(const std::vector >& vertices) + : m_vertices(vertices) + {} + bool operator()(const VertInd a, const VertInd b) const + { + return m_vertices[a].y < m_vertices[b].y; + } +}; + +} // namespace detail + +template +void Triangulation::insertVertices_KDTreeBFS( + VertInd superGeomVertCount, + V2d boxMin, + V2d boxMax) +{ + // calculate original indices + const VertInd vertexCount = + static_cast(vertices.size()) - superGeomVertCount; + if(vertexCount <= 0) + return; + std::vector ii(vertexCount); + detail::iota(ii.begin(), ii.end(), superGeomVertCount); + + typedef std::vector::iterator It; + detail::FixedCapacityQueue, V2d, VertInd> > queue( + detail::maxQueueLengthBFSKDTree(vertexCount)); + queue.push(make_tuple(ii.begin(), ii.end(), boxMin, boxMax, VertInd(0))); + + It first, last; + V2d newBoxMin, newBoxMax; + VertInd parent, mid; + + const detail::less_than_x cmpX(vertices); + const detail::less_than_y cmpY(vertices); + + while(!queue.empty()) + { + tie(first, last, boxMin, boxMax, parent) = queue.front(); + queue.pop(); + assert(first != last); + + const std::ptrdiff_t len = std::distance(first, last); + if(len == 1) + { + insertVertex(*first, parent); + continue; + } + const It midIt = first + len / 2; + if(boxMax.x - boxMin.x >= boxMax.y - boxMin.y) + { + detail::portable_nth_element(first, midIt, last, cmpX); + mid = *midIt; + const T split = vertices[mid].x; + newBoxMin.x = split; + newBoxMin.y = boxMin.y; + newBoxMax.x = split; + newBoxMax.y = boxMax.y; + } + else + { + detail::portable_nth_element(first, midIt, last, cmpY); + mid = *midIt; + const T split = vertices[mid].y; + newBoxMin.x = boxMin.x; + newBoxMin.y = split; + newBoxMax.x = boxMax.x; + newBoxMax.y = split; + } + insertVertex(mid, parent); + if(first != midIt) + { + queue.push(make_tuple(first, midIt, boxMin, newBoxMax, mid)); + } + if(midIt + 1 != last) + { + queue.push(make_tuple(midIt + 1, last, newBoxMin, boxMax, mid)); + } + } +} + +template +std::pair Triangulation::edgeTriangles( + const VertInd a, + const VertInd b) const +{ + const TriInd triStart = m_vertTris[a]; + assert(triStart != noNeighbor); + TriInd iT = triStart, iTNext = triStart; + VertInd iV = noVertex; + do + { + const Triangle& t = triangles[iT]; + tie(iTNext, iV) = t.next(a); + assert(iTNext != noNeighbor); + if(iV == b) + { + return std::make_pair(iT, iTNext); + } + iT = iTNext; + } while(iT != triStart); + return std::make_pair(invalidIndex, invalidIndex); +} + +template +bool Triangulation::hasEdge( + const VertInd a, + const VertInd b) const +{ + return edgeTriangles(a, b).first != invalidIndex; +} + +template +void Triangulation::setAdjacentTriangle( + const VertInd v, + const TriInd t) +{ + assert(t != noNeighbor); + m_vertTris[v] = t; + assert( + triangles[t].vertices[0] == v || triangles[t].vertices[1] == v || + triangles[t].vertices[2] == v); +} + +template +void Triangulation::pivotVertexTriangleCW(const VertInd v) +{ + assert(m_vertTris[v] != noNeighbor); + m_vertTris[v] = triangles[m_vertTris[v]].next(v).first; + assert(m_vertTris[v] != noNeighbor); + assert( + triangles[m_vertTris[v]].vertices[0] == v || + triangles[m_vertTris[v]].vertices[1] == v || + triangles[m_vertTris[v]].vertices[2] == v); +} + +template +void Triangulation::tryAddVertexToLocator(const VertInd v) +{ + if(!m_nearPtLocator.empty()) // only if locator is initialized already + m_nearPtLocator.addPoint(v, vertices); +} + +template +void Triangulation::tryInitNearestPointLocator() +{ + if(!vertices.empty() && m_nearPtLocator.empty()) + { + m_nearPtLocator.initialize(vertices); + } +} + +} // namespace CDT diff --git a/graphics/src/CDT/portable_nth_element.hpp b/graphics/src/CDT/portable_nth_element.hpp new file mode 100644 index 000000000..d560f29ac --- /dev/null +++ b/graphics/src/CDT/portable_nth_element.hpp @@ -0,0 +1,313 @@ +// This code is copied from LLVM's libc++ +// https://github.com/llvm-mirror/libcxx/blob/4dde9ccef57d50e50620408a0b7a902f0aba803e/include/algorithm +// It is needed to provide portable nth_element on different platforms + +// -*- C++ -*- +//===-------------------------- algorithm ---------------------------------===// +// +// Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions. +// See https://llvm.org/LICENSE.txt for license information. +// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception +// +//===----------------------------------------------------------------------===// + +#ifndef CDT_PORTABLE_NTH_ELEMENT +#define CDT_PORTABLE_NTH_ELEMENT + +#include +#include +#ifdef CDT_CXX11_IS_SUPPORTED +#include +#else +#include +#endif + +namespace CDT +{ +namespace detail +{ + +// sort + +// stable, 2-3 compares, 0-2 swaps + +template +unsigned +sort3(ForwardIterator x, ForwardIterator y, ForwardIterator z, Compare c) +{ + unsigned r = 0; + if(!c(*y, *x)) // if x <= y + { + if(!c(*z, *y)) // if y <= z + return r; // x <= y && y <= z + // x <= y && y > z + std::swap(*y, *z); // x <= z && y < z + r = 1; + if(c(*y, *x)) // if x > y + { + std::swap(*x, *y); // x < y && y <= z + r = 2; + } + return r; // x <= y && y < z + } + if(c(*z, *y)) // x > y, if y > z + { + std::swap(*x, *z); // x < y && y < z + r = 1; + return r; + } + std::swap(*x, *y); // x > y && y <= z + r = 1; // x < y && x <= z + if(c(*z, *y)) // if y > z + { + std::swap(*y, *z); // x <= y && y < z + r = 2; + } + return r; +} // x <= y && y <= z + +// Assumes size > 0 +template +void selection_sort( + BirdirectionalIterator first, + BirdirectionalIterator last, + Compare comp) +{ + BirdirectionalIterator lm1 = last; + for(--lm1; first != lm1; ++first) + { +#ifdef CDT_CXX11_IS_SUPPORTED + BirdirectionalIterator i = std::min_element< + BirdirectionalIterator, + typename std::add_lvalue_reference::type>( + first, last, comp); +#else + BirdirectionalIterator i = std::min_element< + BirdirectionalIterator, + typename boost::add_lvalue_reference::type>( + first, last, comp); +#endif + if(i != first) + std::swap(*first, *i); + } +} + +// nth_element + +template +void nth_element( + RandomAccessIterator first, + RandomAccessIterator nth, + RandomAccessIterator last, + Compare comp) +{ + // Compare is known to be a reference type + typedef typename std::iterator_traits::difference_type + difference_type; + const difference_type limit = 7; + while(true) + { + restart: + if(nth == last) + return; + difference_type len = last - first; + switch(len) + { + case 0: + case 1: + return; + case 2: + if(comp(*--last, *first)) + std::swap(*first, *last); + return; + case 3: { + RandomAccessIterator m = first; + detail::sort3(first, ++m, --last, comp); + return; + } + } + if(len <= limit) + { + detail::selection_sort(first, last, comp); + return; + } + // len > limit >= 3 + RandomAccessIterator m = first + len / 2; + RandomAccessIterator lm1 = last; + unsigned n_swaps = detail::sort3(first, m, --lm1, comp); + // *m is median + // partition [first, m) < *m and *m <= [m, last) + // (this inhibits tossing elements equivalent to m around + // unnecessarily) + RandomAccessIterator i = first; + RandomAccessIterator j = lm1; + // j points beyond range to be tested, *lm1 is known to be <= *m + // The search going up is known to be guarded but the search coming + // down isn't. Prime the downward search with a guard. + if(!comp(*i, *m)) // if *first == *m + { + // *first == *m, *first doesn't go in first part + // manually guard downward moving j against i + while(true) + { + if(i == --j) + { + // *first == *m, *m <= all other elements + // Parition instead into [first, i) == *first and + // *first < [i, last) + ++i; // first + 1 + j = last; + if(!comp(*first, *--j)) // we need a guard if + // *first == *(last-1) + { + while(true) + { + if(i == j) + return; // [first, last) all equivalent + // elements + if(comp(*first, *i)) + { + std::swap(*i, *j); + ++n_swaps; + ++i; + break; + } + ++i; + } + } + // [first, i) == *first and *first < [j, + // last) and j == last - 1 + if(i == j) + return; + while(true) + { + while(!comp(*first, *i)) + ++i; + while(comp(*first, *--j)) + ; + if(i >= j) + break; + std::swap(*i, *j); + ++n_swaps; + ++i; + } + // [first, i) == *first and *first < [i, + // last) The first part is sorted, + if(nth < i) + return; + // nth_element the secod part + // nth_element(i, nth, last, comp); + first = i; + goto restart; + } + if(comp(*j, *m)) + { + std::swap(*i, *j); + ++n_swaps; + break; // found guard for downward moving j, now use + // unguarded partition + } + } + } + ++i; + // j points beyond range to be tested, *lm1 is known to be <= *m + // if not yet partitioned... + if(i < j) + { + // known that *(i - 1) < *m + while(true) + { + // m still guards upward moving i + while(comp(*i, *m)) + ++i; + // It is now known that a guard exists for downward moving + // j + while(!comp(*--j, *m)) + ; + if(i >= j) + break; + std::swap(*i, *j); + ++n_swaps; + // It is known that m != j + // If m just moved, follow it + if(m == i) + m = j; + ++i; + } + } + // [first, i) < *m and *m <= [i, last) + if(i != m && comp(*m, *i)) + { + std::swap(*i, *m); + ++n_swaps; + } + // [first, i) < *i and *i <= [i+1, last) + if(nth == i) + return; + if(n_swaps == 0) + { + // We were given a perfectly partitioned sequence. Coincidence? + if(nth < i) + { + // Check for [first, i) already sorted + j = m = first; + while(++j != i) + { + if(comp(*j, *m)) + // not yet sorted, so sort + goto not_sorted; + m = j; + } + // [first, i) sorted + return; + } + else + { + // Check for [i, last) already sorted + j = m = i; + while(++j != last) + { + if(comp(*j, *m)) + // not yet sorted, so sort + goto not_sorted; + m = j; + } + // [i, last) sorted + return; + } + } + not_sorted: + // nth_element on range containing nth + if(nth < i) + { + // nth_element(first, nth, i, comp); + last = i; + } + else + { + // nth_element(i+1, nth, last, comp); + first = ++i; + } + } +} + +template +inline void portable_nth_element( + _RandomAccessIterator first, + _RandomAccessIterator nth, + _RandomAccessIterator last, + _Compare comp) +{ +#ifdef CDT_CXX11_IS_SUPPORTED + detail::nth_element::type>( + first, nth, last, comp); +#else + detail::nth_element::type>( + first, nth, last, comp); +#endif +} + +} // namespace detail +} // namespace CDT + +#endif // CDT_PORTABLE_NTH_ELEMENT diff --git a/graphics/src/CDT/predicates.h b/graphics/src/CDT/predicates.h new file mode 100644 index 000000000..bcdd3a2f2 --- /dev/null +++ b/graphics/src/CDT/predicates.h @@ -0,0 +1,915 @@ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * + * Copyright (c) 2019, William C. Lenthe * + * All rights reserved. * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met: * + * * + * 1. Redistributions of source code must retain the above copyright notice, this * + * list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright notice, * + * this list of conditions and the following disclaimer in the documentation * + * and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from * + * this software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE * + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, * + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * + * * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + +#ifndef PREDICATES_H_INCLUDED +#define PREDICATES_H_INCLUDED + +//@reference: https://www.cs.cmu.edu/~quake/robust.html + +namespace predicates { + //@brief: geometric predicates using arbitrary precision arithmetic + //@note : these are provided primarily for illustrative purposes and adaptive routines should be preferred + namespace exact { + //@brief : determine if the 2d point c is above, on, or below the line defined by a and b + //@param ax: X-coordinate of a + //@param ay: Y-coordinate of a + //@param bx: X-coordinate of b + //@param by: Y-coordinate of b + //@param cx: X-coordinate of c + //@param cy: Y-coordinate of c + //@return : determinant of {{ax - cx, ay - cy}, {bx - cx, by - cy}} + //@note : positive, 0, negative result for c above, on, or below the line defined by a -> b + template T orient2d(T const ax, T const ay, T const bx, T const by, T const cx, T const cy); + + //@brief : determine if the 2d point c is above, on, or below the line defined by a and b + //@param pa: pointer to a as {x, y} + //@param pb: pointer to b as {x, y} + //@param pc: pointer to c as {x, y} + //@return : determinant of {{ax - cx, ay - cy}, {bx - cx, by - cy}} + //@note : positive, 0, negative result for c above, on, or below the line defined by a -> b + template T orient2d(T const*const pa, T const*const pb, T const*const pc); + + //@brief : determine if the 2d point d is inside, on, or outside the circle defined by a, b, and c + //@param ax: X-coordinate of a + //@param ay: Y-coordinate of a + //@param bx: X-coordinate of b + //@param by: Y-coordinate of b + //@param cx: X-coordinate of c + //@param cy: Y-coordinate of c + //@param dx: X-coordinate of d + //@param dy: Y-coordinate of d + //@return : determinant of {{ax - dx, ay - dy, (ax - dx)^2 + (ay - dy)^2}, {bx - dx, by - dy, (bx - dx)^2 + (by - dy)^2}, {cx - dx, cy - dy, (cx - dx)^2 + (cy - dy)^2}} + //@note : positive, 0, negative result for d inside, on, or outside the circle defined by a, b, and c + template T incircle(T const ax, T const ay, T const bx, T const by, T const cx, T const cy, T const dx, T const dy); + + //@brief : determine if the 2d point d is inside, on, or outside the circle defined by a, b, and c + //@param pa: pointer to a as {x, y} + //@param pb: pointer to b as {x, y} + //@param pc: pointer to c as {x, y} + //@param pc: pointer to d as {x, y} + //@return : determinant of {{ax - dx, ay - dy, (ax - dx)^2 + (ay - dy)^2}, {bx - dx, by - dy, (bx - dx)^2 + (by - dy)^2}, {cx - dx, cy - dy, (cx - dx)^2 + (cy - dy)^2}} + //@note : positive, 0, negative result for d inside, on, or outside the circle defined by a, b, and c + template T incircle(T const*const pa, T const*const pb, T const*const pc, T const*const pd); + + //@brief : determine if the 3d point d is above, on, or below the plane defined by a, b, and c + //@param pa: pointer to a as {x, y, z} + //@param pb: pointer to b as {x, y, z} + //@param pc: pointer to c as {x, y, z} + //@param pd: pointer to d as {x, y, z} + //@return : determinant of {{ax - dx, ay - dy, az - dz}, {bx - dx, by - dy, bz - dz}, {cx - dx, cy - dy, cz - dz}} + //@note : positive, 0, negative result for c above, on, or below the plane defined by a, b, and c + template T orient3d(T const*const pa, T const*const pb, T const*const pc, T const*const pd); + + //@brief : determine if the 3d point e is inside, on, or outside the sphere defined by a, b, c, and d + //@param pa: pointer to a as {x, y, z} + //@param pb: pointer to b as {x, y, z} + //@param pc: pointer to c as {x, y, z} + //@param pd: pointer to d as {x, y, z} + //@param pe: pointer to e as {x, y, z} + //@return : determinant of {{ax - ex, ay - ey, az - ez, (ax - ex)^2 + (ay - ey)^2 + (az - ez)^2}, {bx - ex, by - ey, bz - ez, (bx - ex)^2 + (by - ey)^2 + (bz - ez)^2}, {cx - ex, cy - ey, cz - ez, (cx - ex)^2 + (cy - ey)^2 + (cz - ez)^2}, {dx - ex, dy - ey, dz - ez, (dx - ex)^2 + (dy - ey)^2 + (dz - ez)^2}} + //@note : positive, 0, negative result for d inside, on, or outside the circle defined by a, b, and c + template T insphere(T const*const pa, T const*const pb, T const*const pc, T const*const pd, T const*const pe); + } + + //@brief: geometric predicates using normal floating point arithmetic but falling back to arbitrary precision when needed + //@note : these should have the same accuracy but are significantly faster when determinants are large + namespace adaptive { + //@brief : determine if the 2d point c is above, on, or below the line defined by a and b + //@param ax: X-coordinate of a + //@param ay: Y-coordinate of a + //@param bx: X-coordinate of b + //@param by: Y-coordinate of b + //@param cx: X-coordinate of c + //@param cy: Y-coordinate of c + //@return : determinant of {{ax - cx, ay - cy}, {bx - cx, by - cy}} + //@note : positive, 0, negative result for c above, on, or below the line defined by a -> b + template T orient2d(T const ax, T const ay, T const bx, T const by, T const cx, T const cy); + + //@brief : determine if the 2d point c is above, on, or below the line defined by a and b + //@param pa: pointer to a as {x, y} + //@param pb: pointer to b as {x, y} + //@param pc: pointer to c as {x, y} + //@return : determinant of {{ax - cx, ay - cy}, {bx - cx, by - cy}} + //@note : positive, 0, negative result for c above, on, or below the line defined by a -> b + template T orient2d(T const*const pa, T const*const pb, T const*const pc); + + //@brief : determine if the 2d point d is inside, on, or outside the circle defined by a, b, and c + //@param ax: X-coordinate of a + //@param ay: Y-coordinate of a + //@param bx: X-coordinate of b + //@param by: Y-coordinate of b + //@param cx: X-coordinate of c + //@param cy: Y-coordinate of c + //@param dx: X-coordinate of d + //@param dy: Y-coordinate of d + //@return : determinant of {{ax - dx, ay - dy, (ax - dx)^2 + (ay - dy)^2}, {bx - dx, by - dy, (bx - dx)^2 + (by - dy)^2}, {cx - dx, cy - dy, (cx - dx)^2 + (cy - dy)^2}} + //@note : positive, 0, negative result for d inside, on, or outside the circle defined by a, b, and c + template T incircle(T const ax, T const ay, T const bx, T const by, T const cx, T const cy, T const dx, T const dy); + + //@brief : determine if the 2d point d is inside, on, or outside the circle defined by a, b, and c + //@param pa: pointer to a as {x, y} + //@param pb: pointer to b as {x, y} + //@param pc: pointer to c as {x, y} + //@param pc: pointer to d as {x, y} + //@return : determinant of {{ax - dx, ay - dy, (ax - dx)^2 + (ay - dy)^2}, {bx - dx, by - dy, (bx - dx)^2 + (by - dy)^2}, {cx - dx, cy - dy, (cx - dx)^2 + (cy - dy)^2}} + //@note : positive, 0, negative result for d inside, on, or outside the circle defined by a, b, and c + template T incircle(T const*const pa, T const*const pb, T const*const pc, T const*const pd); + + //@brief : determine if the 3d point d is above, on, or below the plane defined by a, b, and c + //@param pa: pointer to a as {x, y, z} + //@param pb: pointer to b as {x, y, z} + //@param pc: pointer to c as {x, y, z} + //@param pd: pointer to d as {x, y, z} + //@return : determinant of {{ax - dx, ay - dy, az - dz}, {bx - dx, by - dy, bz - dz}, {cx - dx, cy - dy, cz - dz}} + //@note : positive, 0, negative result for c above, on, or below the plane defined by a, b, and c + template T orient3d(T const*const pa, T const*const pb, T const*const pc, T const*const pd); + + //@brief : determine if the 3d point e is inside, on, or outside the sphere defined by a, b, c, and d + //@param pa: pointer to a as {x, y, z} + //@param pb: pointer to b as {x, y, z} + //@param pc: pointer to c as {x, y, z} + //@param pd: pointer to d as {x, y, z} + //@param pe: pointer to e as {x, y, z} + //@return : determinant of {{ax - ex, ay - ey, az - ez, (ax - ex)^2 + (ay - ey)^2 + (az - ez)^2}, {bx - ex, by - ey, bz - ez, (bx - ex)^2 + (by - ey)^2 + (bz - ez)^2}, {cx - ex, cy - ey, cz - ez, (cx - ex)^2 + (cy - ey)^2 + (cz - ez)^2}, {dx - ex, dy - ey, dz - ez, (dx - ex)^2 + (dy - ey)^2 + (dz - ez)^2}} + //@note : positive, 0, negative result for d inside, on, or outside the circle defined by a, b, and c + template T insphere(T const*const pa, T const*const pb, T const*const pc, T const*const pd, T const*const pe); + } +} + +#include //abs, fma +#include +#include //pair +#include //accumulate +#include //transform, copy_n, merge +#include //negate + +// a macro based static assert for pre c++11 +#define PREDICATES_PORTABLE_STATIC_ASSERT(condition, message) typedef char message[(condition) ? 1 : -1] + +// check if c++11 is supported +#if !defined(__cplusplus) && !defined(_MSC_VER) + PREDICATES_PORTABLE_STATIC_ASSERT(false, couldnt_parse_cxx_standard) +#endif +#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900) + #define PREDICATES_CXX11_IS_SUPPORTED +#endif + +// choose to use c++11 features or their backports +#ifdef PREDICATES_CXX11_IS_SUPPORTED +#include +#include // is_same, enable_if +#undef PREDICATES_PORTABLE_STATIC_ASSERT +#define PREDICATES_TOKEN_TO_STRING1(x) #x +#define PREDICATES_TOKEN_TO_STRING(x) PREDICATES_TOKEN_TO_STRING1(x) +#define PREDICATES_PORTABLE_STATIC_ASSERT(condition, message) static_assert(condition, PREDICATES_TOKEN_TO_STRING(message)) +namespace predicates { +namespace stdx { + using std::array; + using std::copy_n; +} +#else +namespace predicates { +namespace stdx { + // array + template + class array { + T buff[N]; + public: + T& operator[](const size_t& i) { return buff[i]; } + const T& operator[](const size_t& i) const { return buff[i]; } + + T * data() { return buff; } + T const * data() const { return buff; } + + T * begin() { return buff; } + T const * cbegin() const { return buff; } + }; + // copy_n + template< class InputIt, class Size, class OutputIt> + OutputIt copy_n(InputIt first, Size count, OutputIt result) + { + if (count > 0) { + *result++ = *first; + for (Size i = 1; i < count; ++i) { + *result++ = *++first; + } + } + return result; + } +} +#endif // PREDICATES_CXX11_IS_SUPPORTED + +namespace detail { + template class ExpansionBase; + + //@brief: class to exactly represent the result of a sequence of arithmetic operations as an sequence of values that sum to the result + template + class Expansion : private ExpansionBase, public stdx::array { + private: + public: + size_t m_size; + template friend class ExpansionBase;//access for base class + template friend class Expansion;//access for expansions of different size + + Expansion() : m_size(0) {} + template Expansion& operator=(const Expansion& e) { + PREDICATES_PORTABLE_STATIC_ASSERT(M <= N, cannot_assign_a_larger_expansion_to_a_smaller_expansion); + stdx::copy_n(e.cbegin(), e.size(), stdx::array::begin()); + m_size = e.size(); + return *this; + } + + //vector like convenience functions + size_t size() const {return m_size;} + bool empty() const {return 0 == m_size;} + void push_back(const T v) {stdx::array::operator[](m_size++) = v;} + + public: + //estimates of expansion value + T mostSignificant() const {return empty() ? T(0) : stdx::array::operator[](m_size - 1);} + T estimate() const {return std::accumulate(stdx::array::cbegin(), stdx::array::cbegin() + size(), T(0));} + + template Expansion operator+(const Expansion& f) const { + Expansion h; + h.m_size = ExpansionBase::ExpansionSum(this->data(), this->size(), f.data(), f.size(), h.data()); + return h; + } + + void negate() {std::transform(stdx::array::cbegin(), stdx::array::cbegin() + size(), stdx::array::begin(), std::negate());} + Expansion operator-() const {Expansion e = *this; e.negate(); return e;} + template Expansion operator-(const Expansion& f) const {return operator+(-f);} + + Expansion operator*(const T b) const { + Expansion h; + h.m_size = ExpansionBase::ScaleExpansion(this->data(), this->size(), b, h.data()); + return h; + } + }; + +// See: https://stackoverflow.com/a/40765925/1597714 +// Standard defines: https://en.cppreference.com/w/cpp/numeric/math/fma +#if defined(__FMA__) || defined(__FMA4__) || defined(__AVX2__) || (defined(FP_FAST_FMA) && defined(FP_FAST_FMAF)) +#define PREDICATES_FAST_FMA 1 +#endif + + //@brief : helper function to sort by absolute value + //@param a: lhs item to compare + //@param b: rhs item to compare + //@return : true if |a| < |b| + //@note : defined since lambda functions aren't allow in c++03 + template bool absLess(const T& a, const T& b) {return std::abs(a) < std::abs(b);} + + template + class ExpansionBase { + private: + static const T Splitter; + + PREDICATES_PORTABLE_STATIC_ASSERT(std::numeric_limits::is_iec559, Requires_IEC_559_IEEE_754_floating_point_type); + PREDICATES_PORTABLE_STATIC_ASSERT(2 == std::numeric_limits::radix, Requires_base_2_floating_point_type); + + //combine result + roundoff error into expansion + static inline Expansion MakeExpansion(const T value, const T tail) { + Expansion e; + if(T(0) != tail) e.push_back(tail); + if(T(0) != value) e.push_back(value); + return e; + } + + protected: + //add 2 expansions + static size_t ExpansionSum(T const * const e, const size_t n, T const * const f, const size_t m, T * const h) { + std::merge(e, e + n, f, f + m, h, absLess); + if(m == 0) return n; + if(n == 0) return m; + size_t hIndex = 0; + T Q = h[0]; + T Qnew = h[1] + Q; + T hh = FastPlusTail(h[1], Q, Qnew); + Q = Qnew; + if(T(0) != hh) h[hIndex++] = hh; + for(size_t g = 2; g != n + m; ++g) { + Qnew = Q + h[g]; + hh = PlusTail(Q, h[g], Qnew); + Q = Qnew; + if(T(0) != hh) h[hIndex++] = hh; + } + if(T(0) != Q) h[hIndex++] = Q; + return hIndex; + } + + //scale an expansion by a constant + static size_t ScaleExpansion(T const * const e, const size_t n, const T b, T * const h) { + if(n == 0 || T(0) == b) return 0; + size_t hIndex = 0; + T Q = e[0] * b; + const std::pair bSplit = Split(b); + T hh = MultTailPreSplit(e[0], b, bSplit, Q); + if(T(0) != hh) h[hIndex++] = hh; + for(size_t eIndex = 1; eIndex < n; ++eIndex) { + T Ti = e[eIndex] * b; + T ti = MultTailPreSplit(e[eIndex], b, bSplit, Ti); + T Qi = Q + ti; + hh = PlusTail(Q, ti, Qi); + if(T(0) != hh) h[hIndex++] = hh; + Q = Ti + Qi; + hh = FastPlusTail(Ti, Qi, Q); + if(T(0) != hh) h[hIndex++] = hh; + } + if(T(0) != Q) h[hIndex++] = Q; + return hIndex; + } + + public: + //roundoff error of x = a + b + static inline T PlusTail(const T a, const T b, const T x) { + const T bVirtual = x - a; + const T aVirtual = x - bVirtual; + const T bRoundoff = b - bVirtual; + const T aRoundoff = a - aVirtual; + return aRoundoff + bRoundoff; + } + + //roundoff error of x = a + b if |a| > |b| + static inline T FastPlusTail(const T a, const T b, const T x) { + const T bVirtual = x - a; + return b - bVirtual; + } + + //roundoff error of x = a - b + static inline T MinusTail(const T a, const T b, const T x) { + const T bVirtual = a - x; + const T aVirtual = x + bVirtual; + const T bRoundoff = bVirtual - b; + const T aRoundoff = a - aVirtual; + return aRoundoff + bRoundoff; + } + + //split a into 2 nonoverlapping values + static inline std::pair Split(const T a) { + const T c = a * Splitter; + const T aBig = c - a; + const T aHi = c - aBig; + return std::pair(aHi, a - aHi); + } + + //roundoff error of x = a * b via dekkers product + static inline T DekkersProduct(const T /*a*/, const std::pair aSplit, const T /*b*/, const std::pair bSplit, const T p) { + T y = p - T(aSplit.first * bSplit.first); + y -= T(aSplit.second * bSplit.first); + y -= T(aSplit.first * bSplit.second); + return T(aSplit.second * bSplit.second) - y; + } + + //roundoff error of x = a * b +#if defined(PREDICATES_CXX11_IS_SUPPORTED) && defined(PREDICATES_FAST_FMA) + static T MultTail(const T a, const T b, const T p) {return std::fma(a, b, -p);} + static T MultTailPreSplit(const T a, const T b, const std::pair /*bSplit*/, const T p) {return std::fma(a, b, -p);} +#else + static T MultTail(const T a, const T b, const T p) {return DekkersProduct(a, Split(a), b, Split(b), p);} + static T MultTailPreSplit(const T a, const T b, const std::pair bSplit, const T p) {return DekkersProduct(a, Split(a), b, bSplit, p);} +#endif + //expand a + b + static inline Expansion Plus(const T a, const T b) { + const T x = a + b; + return MakeExpansion(x, PlusTail(a, b, x)); + } + + //expand a - b + static inline Expansion Minus(const T a, const T b) {return Plus(a, -b);} + + //expand a * b + static inline Expansion Mult(const T a, const T b) { + const T x = a * b; + return MakeExpansion(x, MultTail(a, b, x)); + } + + //expand the determinant of {{ax, ay}, {bx, by}} (unrolled Mult(ax, by) - Mult(ay, bx)) + static inline Expansion TwoTwoDiff(const T ax, const T by, const T ay, const T bx) { + const T axby1 = ax * by; + const T axby0 = MultTail(ax, by, axby1); + const T bxay1 = bx * ay; + const T bxay0 = MultTail(bx, ay, bxay1); + const T _i0 = axby0 - bxay0; + const T x0 = MinusTail(axby0, bxay0, _i0); + const T _j = axby1 + _i0; + const T _0 = PlusTail(axby1, _i0, _j); + const T _i1 = _0 - bxay1; + const T x1 = MinusTail(_0, bxay1, _i1); + const T x3 = _j + _i1; + const T x2 = PlusTail(_j, _i1, x3); + Expansion e; + if(T(0) != x0) e.push_back(x0); + if(T(0) != x1) e.push_back(x1); + if(T(0) != x2) e.push_back(x2); + if(T(0) != x3) e.push_back(x3); + return e; + } + + //TwoTwoDiff checking for zeros to avoid extra splitting + static inline Expansion TwoTwoDiffZeroCheck(const T ax, const T by, const T ay, const T bx) { + Expansion e; + if(T(0) == ax && T(0) == ay) return e; + else if(T(0) == ax) e = Mult(ay, bx); + else if(T(0) == ay) e = Mult(ax, by); + else e = TwoTwoDiff(ax, by, ay, bx); + return e; + } + + //(a * b) * c checking for zeros + static inline Expansion ThreeProd(const T a, const T b, const T c) {return (T(0) == a || T(0) == b || T(0) == c) ? Expansion() : Mult(a, b) * c;} + }; + + template const T ExpansionBase::Splitter = +#ifdef PREDICATES_CXX11_IS_SUPPORTED + static_cast(std::exp2(std::numeric_limits::digits/2 + 1)); +#else + static_cast(std::ldexp(T(1), std::numeric_limits::digits/2 + 1)); +#endif +} + + namespace exact { + template T orient2d(T const ax, T const ay, T const bx, T const by, T const cx, T const cy) + { + const detail::Expansion aterms = detail::ExpansionBase::TwoTwoDiff(ax, by, ax, cy); + const detail::Expansion bterms = detail::ExpansionBase::TwoTwoDiff(bx, cy, bx, ay); + const detail::Expansion cterms = detail::ExpansionBase::TwoTwoDiff(cx, ay, cx, by); + const detail::Expansion w = aterms + bterms + cterms; + return w.mostSignificant(); + } + + template T orient2d(T const*const pa, T const*const pb, T const*const pc) { + return orient2d(pa[0], pa[1], pb[0], pb[1], pc[0], pc[1]); + } + + template T incircle(T const ax, T const ay, T const bx, T const by, T const cx, T const cy, T const dx, T const dy) { + const detail::Expansion ab = detail::ExpansionBase::TwoTwoDiff(ax, by, bx, ay); + const detail::Expansion bc = detail::ExpansionBase::TwoTwoDiff(bx, cy, cx, by); + const detail::Expansion cd = detail::ExpansionBase::TwoTwoDiff(cx, dy, dx, cy); + const detail::Expansion da = detail::ExpansionBase::TwoTwoDiff(dx, ay, ax, dy); + const detail::Expansion ac = detail::ExpansionBase::TwoTwoDiff(ax, cy, cx, ay); + const detail::Expansion bd = detail::ExpansionBase::TwoTwoDiff(bx, dy, dx, by); + + const detail::Expansion abc = ab + bc - ac; + const detail::Expansion bcd = bc + cd - bd; + const detail::Expansion cda = cd + da + ac; + const detail::Expansion dab = da + ab + bd; + + const detail::Expansion adet = bcd * ax * ax + bcd * ay * ay; + const detail::Expansion bdet = cda * bx * -bx + cda * by * -by; + const detail::Expansion cdet = dab * cx * cx + dab * cy * cy; + const detail::Expansion ddet = abc * dx * -dx + abc * dy * -dy; + + const detail::Expansion deter = (adet + bdet) + (cdet + ddet); + return deter.mostSignificant(); + } + + template T incircle(T const*const pa, T const*const pb, T const*const pc, T const*const pd) { + return incircle(pa[0], pa[1], pb[0], pb[1], pc[0], pc[1], pd[0], pd[1]); + } + + //@brief : determine if the 3d point d is above, on, or below the plane defined by a, b, and c + //@param pa: pointer to a as {x, y, z} + //@param pb: pointer to b as {x, y, z} + //@param pc: pointer to c as {x, y, z} + //@param pd: pointer to d as {x, y, z} + //@return : determinant of {{ax - dx, ay - dy, az - dz}, {bx - dx, by - dy, bz - dz}, {cx - dx, cy - dy, cz - dz}} + //@note : positive, 0, negative result for c above, on, or below the plane defined by a, b, and c + template T orient3d(T const*const pa, T const*const pb, T const*const pc, T const*const pd) { + const detail::Expansion ab = detail::ExpansionBase::TwoTwoDiff(pa[0], pb[1], pb[0], pa[1]); + const detail::Expansion bc = detail::ExpansionBase::TwoTwoDiff(pb[0], pc[1], pc[0], pb[1]); + const detail::Expansion cd = detail::ExpansionBase::TwoTwoDiff(pc[0], pd[1], pd[0], pc[1]); + const detail::Expansion da = detail::ExpansionBase::TwoTwoDiff(pd[0], pa[1], pa[0], pd[1]); + const detail::Expansion ac = detail::ExpansionBase::TwoTwoDiff(pa[0], pc[1], pc[0], pa[1]); + const detail::Expansion bd = detail::ExpansionBase::TwoTwoDiff(pb[0], pd[1], pd[0], pb[1]); + + const detail::Expansion abc = ab + bc - ac; + const detail::Expansion bcd = bc + cd - bd; + const detail::Expansion cda = cd + da + ac; + const detail::Expansion dab = da + ab + bd; + + const detail::Expansion adet = bcd * pa[2]; + const detail::Expansion bdet = cda * -pb[2]; + const detail::Expansion cdet = dab * pc[2]; + const detail::Expansion ddet = abc * -pd[2]; + + const detail::Expansion deter = (adet + bdet) + (cdet + ddet); + return deter.mostSignificant(); + } + + //@brief : determine if the 3d point e is inside, on, or outside the sphere defined by a, b, c, and d + //@param pa: pointer to a as {x, y, z} + //@param pb: pointer to b as {x, y, z} + //@param pc: pointer to c as {x, y, z} + //@param pd: pointer to d as {x, y, z} + //@param pe: pointer to e as {x, y, z} + //@return : determinant of {{ax - ex, ay - ey, az - ez, (ax - ex)^2 + (ay - ey)^2 + (az - ez)^2}, {bx - ex, by - ey, bz - ez, (bx - ex)^2 + (by - ey)^2 + (bz - ez)^2}, {cx - ex, cy - ey, cz - ez, (cx - ex)^2 + (cy - ey)^2 + (cz - ez)^2}, {dx - ex, dy - ey, dz - ez, (dx - ex)^2 + (dy - ey)^2 + (dz - ez)^2}} + //@note : positive, 0, negative result for d inside, on, or outside the circle defined by a, b, and c + template T insphere(T const*const pa, T const*const pb, T const*const pc, T const*const pd, T const*const pe) { + const detail::Expansion ab = detail::ExpansionBase::TwoTwoDiff(pa[0], pb[1], pb[0], pa[1]); + const detail::Expansion bc = detail::ExpansionBase::TwoTwoDiff(pb[0], pc[1], pc[0], pb[1]); + const detail::Expansion cd = detail::ExpansionBase::TwoTwoDiff(pc[0], pd[1], pd[0], pc[1]); + const detail::Expansion de = detail::ExpansionBase::TwoTwoDiff(pd[0], pe[1], pe[0], pd[1]); + const detail::Expansion ea = detail::ExpansionBase::TwoTwoDiff(pe[0], pa[1], pa[0], pe[1]); + const detail::Expansion ac = detail::ExpansionBase::TwoTwoDiff(pa[0], pc[1], pc[0], pa[1]); + const detail::Expansion bd = detail::ExpansionBase::TwoTwoDiff(pb[0], pd[1], pd[0], pb[1]); + const detail::Expansion ce = detail::ExpansionBase::TwoTwoDiff(pc[0], pe[1], pe[0], pc[1]); + const detail::Expansion da = detail::ExpansionBase::TwoTwoDiff(pd[0], pa[1], pa[0], pd[1]); + const detail::Expansion eb = detail::ExpansionBase::TwoTwoDiff(pe[0], pb[1], pb[0], pe[1]); + + const detail::Expansion abc = bc * pa[2] + ac * -pb[2] + ab * pc[2]; + const detail::Expansion bcd = cd * pb[2] + bd * -pc[2] + bc * pd[2]; + const detail::Expansion cde = de * pc[2] + ce * -pd[2] + cd * pe[2]; + const detail::Expansion dea = ea * pd[2] + da * -pe[2] + de * pa[2]; + const detail::Expansion eab = ab * pe[2] + eb * -pa[2] + ea * pb[2]; + const detail::Expansion abd = bd * pa[2] + da * pb[2] + ab * pd[2]; + const detail::Expansion bce = ce * pb[2] + eb * pc[2] + bc * pe[2]; + const detail::Expansion cda = da * pc[2] + ac * pd[2] + cd * pa[2]; + const detail::Expansion deb = eb * pd[2] + bd * pe[2] + de * pb[2]; + const detail::Expansion eac = ac * pe[2] + ce * pa[2] + ea * pc[2]; + + const detail::Expansion bcde = (cde + bce) - (deb + bcd); + const detail::Expansion cdea = (dea + cda) - (eac + cde); + const detail::Expansion deab = (eab + deb) - (abd + dea); + const detail::Expansion eabc = (abc + eac) - (bce + eab); + const detail::Expansion abcd = (bcd + abd) - (cda + abc); + + const detail::Expansion adet = bcde * pa[0] * pa[0] + bcde * pa[1] * pa[1] + bcde * pa[2] * pa[2]; + const detail::Expansion bdet = cdea * pb[0] * pb[0] + cdea * pb[1] * pb[1] + cdea * pb[2] * pb[2]; + const detail::Expansion cdet = deab * pc[0] * pc[0] + deab * pc[1] * pc[1] + deab * pc[2] * pc[2]; + const detail::Expansion ddet = eabc * pd[0] * pd[0] + eabc * pd[1] * pd[1] + eabc * pd[2] * pd[2]; + const detail::Expansion edet = abcd * pe[0] * pe[0] + abcd * pe[1] * pe[1] + abcd * pe[2] * pe[2]; + + const detail::Expansion deter = (adet + bdet) + ((cdet + ddet) + edet); + return deter.mostSignificant(); + } + } + + template + const T& Epsilon() + { + static const T epsilon = +#ifdef PREDICATES_CXX11_IS_SUPPORTED + static_cast(std::exp2(-std::numeric_limits::digits)); +#else + static_cast(std::ldexp(T(1), -std::numeric_limits::digits)); +#endif + return epsilon; + } + + template + class Constants { + public: + static const T epsilon, resulterrbound; + static const T ccwerrboundA, ccwerrboundB, ccwerrboundC; + static const T o3derrboundA, o3derrboundB, o3derrboundC; + static const T iccerrboundA, iccerrboundB, iccerrboundC; + static const T isperrboundA, isperrboundB, isperrboundC; + }; + + template const T Constants::epsilon = Epsilon(); + template const T Constants::resulterrbound = (T( 3) + T( 8) * Epsilon()) * Epsilon(); + template const T Constants::ccwerrboundA = (T( 3) + T( 16) * Epsilon()) * Epsilon(); + template const T Constants::ccwerrboundB = (T( 2) + T( 12) * Epsilon()) * Epsilon(); + template const T Constants::ccwerrboundC = (T( 9) + T( 64) * Epsilon()) * Epsilon() * Epsilon(); + template const T Constants::o3derrboundA = (T( 7) + T( 56) * Epsilon()) * Epsilon(); + template const T Constants::o3derrboundB = (T( 3) + T( 28) * Epsilon()) * Epsilon(); + template const T Constants::o3derrboundC = (T(26) + T( 288) * Epsilon()) * Epsilon() * Epsilon(); + template const T Constants::iccerrboundA = (T(10) + T( 96) * Epsilon()) * Epsilon(); + template const T Constants::iccerrboundB = (T( 4) + T( 48) * Epsilon()) * Epsilon(); + template const T Constants::iccerrboundC = (T(44) + T( 576) * Epsilon()) * Epsilon() * Epsilon(); + template const T Constants::isperrboundA = (T(16) + T( 224) * Epsilon()) * Epsilon(); + template const T Constants::isperrboundB = (T( 5) + T( 72) * Epsilon()) * Epsilon(); + template const T Constants::isperrboundC = (T(71) + T(1408) * Epsilon()) * Epsilon() * Epsilon(); + + namespace adaptive { + template T orient2d(T const ax, T const ay, T const bx, T const by, T const cx, T const cy) { + const T acx = ax - cx; + const T bcx = bx - cx; + const T acy = ay - cy; + const T bcy = by - cy; + const T detleft = acx * bcy; + const T detright = acy * bcx; + T det = detleft - detright; + if((detleft < 0) != (detright < 0)) return det; + if(T(0) == detleft || T(0) == detright) return det; + + const T detsum = std::abs(detleft + detright); + T errbound = Constants::ccwerrboundA * detsum; + if(std::abs(det) >= std::abs(errbound)) return det; + + const detail::Expansion B = detail::ExpansionBase::TwoTwoDiff(acx, bcy, acy, bcx); + det = B.estimate(); + errbound = Constants::ccwerrboundB * detsum; + if(std::abs(det) >= std::abs(errbound)) return det; + + const T acxtail = detail::ExpansionBase::MinusTail(ax, cx, acx); + const T bcxtail = detail::ExpansionBase::MinusTail(bx, cx, bcx); + const T acytail = detail::ExpansionBase::MinusTail(ay, cy, acy); + const T bcytail = detail::ExpansionBase::MinusTail(by, cy, bcy); + if(T(0) == acxtail && T(0) == bcxtail && T(0) == acytail && T(0) == bcytail) return det; + + errbound = Constants::ccwerrboundC * detsum + Constants::resulterrbound * std::abs(det); + det += (acx * bcytail + bcy * acxtail) - (acy * bcxtail + bcx * acytail); + if(std::abs(det) >= std::abs(errbound)) return det; + + const detail::Expansion D = ((B + detail::ExpansionBase::TwoTwoDiff(acxtail, bcy, acytail, bcx)) + detail::ExpansionBase::TwoTwoDiff(acx, bcytail, acy, bcxtail)) + detail::ExpansionBase::TwoTwoDiff(acxtail, bcytail, acytail, bcxtail); + return D.mostSignificant(); + } + + template T orient2d(T const*const pa, T const*const pb, T const*const pc) { + return orient2d(pa[0], pa[1], pb[0], pb[1], pc[0], pc[1]); + } + + template T incircle(T const ax, T const ay, T const bx, T const by, T const cx, T const cy, T const dx, T const dy) { + const T adx = ax - dx; + const T bdx = bx - dx; + const T cdx = cx - dx; + const T ady = ay - dy; + const T bdy = by - dy; + const T cdy = cy - dy; + const T bdxcdy = bdx * cdy; + const T cdxbdy = cdx * bdy; + const T cdxady = cdx * ady; + const T adxcdy = adx * cdy; + const T adxbdy = adx * bdy; + const T bdxady = bdx * ady; + const T alift = adx * adx + ady * ady; + const T blift = bdx * bdx + bdy * bdy; + const T clift = cdx * cdx + cdy * cdy; + T det = alift * (bdxcdy - cdxbdy) + blift * (cdxady - adxcdy) + clift * (adxbdy - bdxady); + const T permanent = (std::abs(bdxcdy) + std::abs(cdxbdy)) * alift + + (std::abs(cdxady) + std::abs(adxcdy)) * blift + + (std::abs(adxbdy) + std::abs(bdxady)) * clift; + T errbound = Constants::iccerrboundA * permanent; + if(std::abs(det) >= std::abs(errbound)) return det; + + const detail::Expansion bc = detail::ExpansionBase::TwoTwoDiff(bdx, cdy, cdx, bdy); + const detail::Expansion ca = detail::ExpansionBase::TwoTwoDiff(cdx, ady, adx, cdy); + const detail::Expansion ab = detail::ExpansionBase::TwoTwoDiff(adx, bdy, bdx, ady); + const detail::Expansion adet = bc * adx * adx + bc * ady * ady; + const detail::Expansion bdet = ca * bdx * bdx + ca * bdy * bdy; + const detail::Expansion cdet = ab * cdx * cdx + ab * cdy * cdy; + const detail::Expansion fin1 = adet + bdet + cdet; + det = fin1.estimate(); + errbound = Constants::iccerrboundB * permanent; + if(std::abs(det) >= std::abs(errbound)) return det; + + const T adxtail = detail::ExpansionBase::MinusTail(ax, dx, adx); + const T adytail = detail::ExpansionBase::MinusTail(ay, dy, ady); + const T bdxtail = detail::ExpansionBase::MinusTail(bx, dx, bdx); + const T bdytail = detail::ExpansionBase::MinusTail(by, dy, bdy); + const T cdxtail = detail::ExpansionBase::MinusTail(cx, dx, cdx); + const T cdytail = detail::ExpansionBase::MinusTail(cy, dy, cdy); + if(T(0) == adxtail && T(0) == bdxtail && T(0) == cdxtail && T(0) == adytail && T(0) == bdytail && T(0) == cdytail) return det; + + errbound = Constants::iccerrboundC * permanent + Constants::resulterrbound * std::abs(det); + det += ((adx * adx + ady * ady) * ((bdx * cdytail + cdy * bdxtail) - (bdy * cdxtail + cdx * bdytail)) + + (bdx * cdy - bdy * cdx) * (adx * adxtail + ady * adytail) * T(2)) + + ((bdx * bdx + bdy * bdy) * ((cdx * adytail + ady * cdxtail) - (cdy * adxtail + adx * cdytail)) + + (cdx * ady - cdy * adx) * (bdx * bdxtail + bdy * bdytail) * T(2)) + + ((cdx * cdx + cdy * cdy) * ((adx * bdytail + bdy * adxtail) - (ady * bdxtail + bdx * adytail)) + + (adx * bdy - ady * bdx) * (cdx * cdxtail + cdy * cdytail) * T(2)); + if(std::abs(det) >= std::abs(errbound)) return det; + return exact::incircle(ax, ay, bx, by, cx, cy, dx, dy); + } + + template T incircle(T const*const pa, T const*const pb, T const*const pc, T const*const pd) { + return incircle(pa[0], pa[1], pb[0], pb[1], pc[0], pc[1], pd[0], pd[1]); + } + + //@brief : determine if the 3d point d is above, on, or below the plane defined by a, b, and c + //@param pa: pointer to a as {x, y, z} + //@param pb: pointer to b as {x, y, z} + //@param pc: pointer to c as {x, y, z} + //@param pd: pointer to d as {x, y, z} + //@return : determinant of {{ax - dx, ay - dy, az - dz}, {bx - dx, by - dy, bz - dz}, {cx - dx, cy - dy, cz - dz}} + //@note : positive, 0, negative result for c above, on, or below the plane defined by a, b, and c + template T orient3d(T const*const pa, T const*const pb, T const*const pc, T const*const pd) { + const T adx = pa[0] - pd[0]; + const T bdx = pb[0] - pd[0]; + const T cdx = pc[0] - pd[0]; + const T ady = pa[1] - pd[1]; + const T bdy = pb[1] - pd[1]; + const T cdy = pc[1] - pd[1]; + const T adz = pa[2] - pd[2]; + const T bdz = pb[2] - pd[2]; + const T cdz = pc[2] - pd[2]; + const T bdxcdy = bdx * cdy; + const T cdxbdy = cdx * bdy; + const T cdxady = cdx * ady; + const T adxcdy = adx * cdy; + const T adxbdy = adx * bdy; + const T bdxady = bdx * ady; + T det = adz * (bdxcdy - cdxbdy) + bdz * (cdxady - adxcdy) + cdz * (adxbdy - bdxady); + const T permanent = (std::abs(bdxcdy) + std::abs(cdxbdy)) * std::abs(adz) + (std::abs(cdxady) + std::abs(adxcdy)) * std::abs(bdz) + (std::abs(adxbdy) + std::abs(bdxady)) * std::abs(cdz); + T errbound = Constants::o3derrboundA * permanent; + if(std::abs(det) >= std::abs(errbound)) return det; + + const detail::Expansion bc = detail::ExpansionBase::TwoTwoDiff(bdx, cdy, cdx, bdy); + const detail::Expansion ca = detail::ExpansionBase::TwoTwoDiff(cdx, ady, adx, cdy); + const detail::Expansion ab = detail::ExpansionBase::TwoTwoDiff(adx, bdy, bdx, ady); + const detail::Expansion fin1 = (bc * adz + ca * bdz) + ab * cdz; + det = fin1.estimate(); + errbound = Constants::o3derrboundB * permanent; + if(std::abs(det) >= std::abs(errbound)) return det; + + const T adxtail = detail::ExpansionBase::MinusTail(pa[0], pd[0], adx); + const T bdxtail = detail::ExpansionBase::MinusTail(pb[0], pd[0], bdx); + const T cdxtail = detail::ExpansionBase::MinusTail(pc[0], pd[0], cdx); + const T adytail = detail::ExpansionBase::MinusTail(pa[1], pd[1], ady); + const T bdytail = detail::ExpansionBase::MinusTail(pb[1], pd[1], bdy); + const T cdytail = detail::ExpansionBase::MinusTail(pc[1], pd[1], cdy); + const T adztail = detail::ExpansionBase::MinusTail(pa[2], pd[2], adz); + const T bdztail = detail::ExpansionBase::MinusTail(pb[2], pd[2], bdz); + const T cdztail = detail::ExpansionBase::MinusTail(pc[2], pd[2], cdz); + if(T(0) == adxtail && T(0) == adytail && T(0) == adztail && + T(0) == bdxtail && T(0) == bdytail && T(0) == bdztail && + T(0) == cdxtail && T(0) == cdytail && T(0) == cdztail) return det; + + errbound = Constants::o3derrboundC * permanent + Constants::resulterrbound * std::abs(det); + det += (adz * ((bdx * cdytail + cdy * bdxtail) - (bdy * cdxtail + cdx * bdytail)) + adztail * (bdx * cdy - bdy * cdx)) + + (bdz * ((cdx * adytail + ady * cdxtail) - (cdy * adxtail + adx * cdytail)) + bdztail * (cdx * ady - cdy * adx)) + + (cdz * ((adx * bdytail + bdy * adxtail) - (ady * bdxtail + bdx * adytail)) + cdztail * (adx * bdy - ady * bdx)); + if(std::abs(det) >= std::abs(errbound)) return det; + + const detail::Expansion bct = detail::ExpansionBase::TwoTwoDiffZeroCheck(bdxtail, cdy, bdytail, cdx) + detail::ExpansionBase::TwoTwoDiffZeroCheck(cdytail, bdx, cdxtail, bdy); + const detail::Expansion cat = detail::ExpansionBase::TwoTwoDiffZeroCheck(cdxtail, ady, cdytail, adx) + detail::ExpansionBase::TwoTwoDiffZeroCheck(adytail, cdx, adxtail, cdy); + const detail::Expansion abt = detail::ExpansionBase::TwoTwoDiffZeroCheck(adxtail, bdy, adytail, bdx) + detail::ExpansionBase::TwoTwoDiffZeroCheck(bdytail, adx, bdxtail, ady); + const detail::Expansion fin2 = fin1 + bct * adz + cat * bdz + abt * cdz + bc * adztail + ca * bdztail + ab * cdztail + + detail::ExpansionBase::ThreeProd( adxtail, bdytail, cdz) + detail::ExpansionBase::ThreeProd( adxtail, bdytail, cdztail) + + detail::ExpansionBase::ThreeProd(-adxtail, cdytail, bdz) + detail::ExpansionBase::ThreeProd(-adxtail, cdytail, bdztail) + + detail::ExpansionBase::ThreeProd( bdxtail, cdytail, adz) + detail::ExpansionBase::ThreeProd( bdxtail, cdytail, adztail) + + detail::ExpansionBase::ThreeProd(-bdxtail, adytail, cdz) + detail::ExpansionBase::ThreeProd(-bdxtail, adytail, cdztail) + + detail::ExpansionBase::ThreeProd( cdxtail, adytail, bdz) + detail::ExpansionBase::ThreeProd( cdxtail, adytail, bdztail) + + detail::ExpansionBase::ThreeProd(-cdxtail, bdytail, adz) + detail::ExpansionBase::ThreeProd(-cdxtail, bdytail, adztail) + + bct * adztail + cat * bdztail + abt * cdztail; + return fin2.mostSignificant(); + } + + //@brief : determine if the 3d point e is inside, on, or outside the sphere defined by a, b, c, and d + //@param pa: pointer to a as {x, y, z} + //@param pb: pointer to b as {x, y, z} + //@param pc: pointer to c as {x, y, z} + //@param pd: pointer to d as {x, y, z} + //@param pe: pointer to e as {x, y, z} + //@return : determinant of {{ax - ex, ay - ey, az - ez, (ax - ex)^2 + (ay - ey)^2 + (az - ez)^2}, {bx - ex, by - ey, bz - ez, (bx - ex)^2 + (by - ey)^2 + (bz - ez)^2}, {cx - ex, cy - ey, cz - ez, (cx - ex)^2 + (cy - ey)^2 + (cz - ez)^2}, {dx - ex, dy - ey, dz - ez, (dx - ex)^2 + (dy - ey)^2 + (dz - ez)^2}} + //@note : positive, 0, negative result for d inside, on, or outside the circle defined by a, b, and c + template T insphere(T const*const pa, T const*const pb, T const*const pc, T const*const pd, T const*const pe) { + T permanent; + const T aex = pa[0] - pe[0]; + const T bex = pb[0] - pe[0]; + const T cex = pc[0] - pe[0]; + const T dex = pd[0] - pe[0]; + const T aey = pa[1] - pe[1]; + const T bey = pb[1] - pe[1]; + const T cey = pc[1] - pe[1]; + const T dey = pd[1] - pe[1]; + const T aez = pa[2] - pe[2]; + const T bez = pb[2] - pe[2]; + const T cez = pc[2] - pe[2]; + const T dez = pd[2] - pe[2]; + { + const T aexbey = aex * bey; + const T bexaey = bex * aey; + const T bexcey = bex * cey; + const T cexbey = cex * bey; + const T cexdey = cex * dey; + const T dexcey = dex * cey; + const T dexaey = dex * aey; + const T aexdey = aex * dey; + const T aexcey = aex * cey; + const T cexaey = cex * aey; + const T bexdey = bex * dey; + const T dexbey = dex * bey; + const T ab = aexbey - bexaey; + const T bc = bexcey - cexbey; + const T cd = cexdey - dexcey; + const T da = dexaey - aexdey; + const T ac = aexcey - cexaey; + const T bd = bexdey - dexbey; + const T abc = aez * bc - bez * ac + cez * ab; + const T bcd = bez * cd - cez * bd + dez * bc; + const T cda = cez * da + dez * ac + aez * cd; + const T dab = dez * ab + aez * bd + bez * da; + const T alift = aex * aex + aey * aey + aez * aez; + const T blift = bex * bex + bey * bey + bez * bez; + const T clift = cex * cex + cey * cey + cez * cez; + const T dlift = dex * dex + dey * dey + dez * dez; + const T det = (dlift * abc - clift * dab) + (blift * cda - alift * bcd); + const T aezplus = std::abs(aez); + const T bezplus = std::abs(bez); + const T cezplus = std::abs(cez); + const T dezplus = std::abs(dez); + const T aexbeyplus = std::abs(aexbey); + const T bexaeyplus = std::abs(bexaey); + const T bexceyplus = std::abs(bexcey); + const T cexbeyplus = std::abs(cexbey); + const T cexdeyplus = std::abs(cexdey); + const T dexceyplus = std::abs(dexcey); + const T dexaeyplus = std::abs(dexaey); + const T aexdeyplus = std::abs(aexdey); + const T aexceyplus = std::abs(aexcey); + const T cexaeyplus = std::abs(cexaey); + const T bexdeyplus = std::abs(bexdey); + const T dexbeyplus = std::abs(dexbey); + permanent = ((cexdeyplus + dexceyplus) * bezplus + (dexbeyplus + bexdeyplus) * cezplus + (bexceyplus + cexbeyplus) * dezplus) * alift + + ((dexaeyplus + aexdeyplus) * cezplus + (aexceyplus + cexaeyplus) * dezplus + (cexdeyplus + dexceyplus) * aezplus) * blift + + ((aexbeyplus + bexaeyplus) * dezplus + (bexdeyplus + dexbeyplus) * aezplus + (dexaeyplus + aexdeyplus) * bezplus) * clift + + ((bexceyplus + cexbeyplus) * aezplus + (cexaeyplus + aexceyplus) * bezplus + (aexbeyplus + bexaeyplus) * cezplus) * dlift; + const T errbound = Constants::isperrboundA * permanent; + if(std::abs(det) >= std::abs(errbound)) return det; + } + + const detail::Expansion ab = detail::ExpansionBase::TwoTwoDiff(aex, bey, bex, aey); + const detail::Expansion bc = detail::ExpansionBase::TwoTwoDiff(bex, cey, cex, bey); + const detail::Expansion cd = detail::ExpansionBase::TwoTwoDiff(cex, dey, dex, cey); + const detail::Expansion da = detail::ExpansionBase::TwoTwoDiff(dex, aey, aex, dey); + const detail::Expansion ac = detail::ExpansionBase::TwoTwoDiff(aex, cey, cex, aey); + const detail::Expansion bd = detail::ExpansionBase::TwoTwoDiff(bex, dey, dex, bey); + const detail::Expansion temp24a = bc * dez + (cd * bez + bd * -cez); + const detail::Expansion temp24b = cd * aez + (da * cez + ac * dez); + const detail::Expansion temp24c = da * bez + (ab * dez + bd * aez); + const detail::Expansion temp24d = ab * cez + (bc * aez + ac * -bez); + const detail::Expansion adet = temp24a * aex * -aex + temp24a * aey * -aey + temp24a * aez * -aez; + const detail::Expansion bdet = temp24b * bex * bex + temp24b * bey * bey + temp24b * bez * bez; + const detail::Expansion cdet = temp24c * cex * -cex + temp24c * cey * -cey + temp24c * cez * -cez; + const detail::Expansion ddet = temp24d * dex * dex + temp24d * dey * dey + temp24d * dez * dez; + const detail::Expansion fin1 = (adet + bdet) + (cdet + ddet); + T det = fin1.estimate(); + T errbound = Constants::isperrboundB * permanent; + if(std::abs(det) >= std::abs(errbound)) return det; + + const T aextail = detail::ExpansionBase::MinusTail(pa[0], pe[0], aex); + const T aeytail = detail::ExpansionBase::MinusTail(pa[1], pe[1], aey); + const T aeztail = detail::ExpansionBase::MinusTail(pa[2], pe[2], aez); + const T bextail = detail::ExpansionBase::MinusTail(pb[0], pe[0], bex); + const T beytail = detail::ExpansionBase::MinusTail(pb[1], pe[1], bey); + const T beztail = detail::ExpansionBase::MinusTail(pb[2], pe[2], bez); + const T cextail = detail::ExpansionBase::MinusTail(pc[0], pe[0], cex); + const T ceytail = detail::ExpansionBase::MinusTail(pc[1], pe[1], cey); + const T ceztail = detail::ExpansionBase::MinusTail(pc[2], pe[2], cez); + const T dextail = detail::ExpansionBase::MinusTail(pd[0], pe[0], dex); + const T deytail = detail::ExpansionBase::MinusTail(pd[1], pe[1], dey); + const T deztail = detail::ExpansionBase::MinusTail(pd[2], pe[2], dez); + if (T(0) == aextail && T(0) == aeytail && T(0) == aeztail && + T(0) == bextail && T(0) == beytail && T(0) == beztail && + T(0) == cextail && T(0) == ceytail && T(0) == ceztail && + T(0) == dextail && T(0) == deytail && T(0) == deztail) return det; + + errbound = Constants::isperrboundC * permanent + Constants::resulterrbound * std::abs(det); + const T abeps = (aex * beytail + bey * aextail) - (aey * bextail + bex * aeytail); + const T bceps = (bex * ceytail + cey * bextail) - (bey * cextail + cex * beytail); + const T cdeps = (cex * deytail + dey * cextail) - (cey * dextail + dex * ceytail); + const T daeps = (dex * aeytail + aey * dextail) - (dey * aextail + aex * deytail); + const T aceps = (aex * ceytail + cey * aextail) - (aey * cextail + cex * aeytail); + const T bdeps = (bex * deytail + dey * bextail) - (bey * dextail + dex * beytail); + const T ab3 = ab.mostSignificant(); + const T bc3 = bc.mostSignificant(); + const T cd3 = cd.mostSignificant(); + const T da3 = da.mostSignificant(); + const T ac3 = ac.mostSignificant(); + const T bd3 = bd.mostSignificant(); + det += ( ( (bex * bex + bey * bey + bez * bez) * ((cez * daeps + dez * aceps + aez * cdeps) + (ceztail * da3 + deztail * ac3 + aeztail * cd3)) + + (dex * dex + dey * dey + dez * dez) * ((aez * bceps - bez * aceps + cez * abeps) + (aeztail * bc3 - beztail * ac3 + ceztail * ab3)) ) + - ( (aex * aex + aey * aey + aez * aez) * ((bez * cdeps - cez * bdeps + dez * bceps) + (beztail * cd3 - ceztail * bd3 + deztail * bc3)) + + (cex * cex + cey * cey + cez * cez) * ((dez * abeps + aez * bdeps + bez * daeps) + (deztail * ab3 + aeztail * bd3 + beztail * da3)) ) ) + + T(2) * ( ( (bex * bextail + bey * beytail + bez * beztail) * (cez * da3 + dez * ac3 + aez * cd3) + + (dex * dextail + dey * deytail + dez * deztail) * (aez * bc3 - bez * ac3 + cez * ab3)) + - ( (aex * aextail + aey * aeytail + aez * aeztail) * (bez * cd3 - cez * bd3 + dez * bc3) + + (cex * cextail + cey * ceytail + cez * ceztail) * (dez * ab3 + aez * bd3 + bez * da3))); + if(std::abs(det) >= std::abs(errbound)) return det; + return exact::insphere(pa, pb, pc, pd, pe); + } + } +} + +#endif diff --git a/graphics/src/CDT/remove_at.hpp b/graphics/src/CDT/remove_at.hpp new file mode 100644 index 000000000..8b960b583 --- /dev/null +++ b/graphics/src/CDT/remove_at.hpp @@ -0,0 +1,55 @@ +#ifndef REMOVE_AT_HPP +#define REMOVE_AT_HPP + +// check if c++11 is supported +#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900) +#define REMOVE_AT_CXX11_IS_SUPPORTED +#elif !defined(__cplusplus) && !defined(_MSC_VER) +typedef char couldnt_parse_cxx_standard[-1]; +#endif + +#include +#include + +/*! + * Remove elements in the range [first; last) with indices from the sorted + * unique range [ii_first, ii_last) + */ +template +inline ForwardIt remove_at( + ForwardIt first, + ForwardIt last, + SortUniqIndsFwdIt ii_first, + SortUniqIndsFwdIt ii_last) +{ + if(ii_first == ii_last) // no indices-to-remove are given + return last; + typedef typename std::iterator_traits::difference_type diff_t; + typedef typename std::iterator_traits::value_type ind_t; + ForwardIt destination = first + static_cast(*ii_first); + while(ii_first != ii_last) + { + // advance to an index after a chunk of elements-to-keep + for(ind_t cur = *ii_first++; ii_first != ii_last; ++ii_first) + { + const ind_t nxt = *ii_first; + if(nxt - cur > 1) + break; + cur = nxt; + } + // move the chunk of elements-to-keep to new destination + const ForwardIt source_first = + first + static_cast(*(ii_first - 1)) + 1; + const ForwardIt source_last = + ii_first != ii_last ? first + static_cast(*ii_first) : last; +#ifdef REMOVE_AT_CXX11_IS_SUPPORTED + std::move(source_first, source_last, destination); +#else + std::copy(source_first, source_last, destination); // c++98 version +#endif + destination += source_last - source_first; + } + return destination; +} + +#endif // REMOVE_AT_HPP diff --git a/graphics/src/CMakeLists.txt b/graphics/src/CMakeLists.txt index b6de4d97b..cd146f8a4 100644 --- a/graphics/src/CMakeLists.txt +++ b/graphics/src/CMakeLists.txt @@ -8,11 +8,13 @@ target_link_libraries(${graphics_target} gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} PRIVATE ${GzAssimp_LIBRARIES} - GTS::GTS FreeImage::FreeImage TINYXML2::TINYXML2 ) +# CDT does a few float comparisons that cause warnings +target_compile_options(${graphics_target} PRIVATE -Wno-float-equal) + gz_build_tests( TYPE UNIT SOURCES ${gtest_sources} diff --git a/graphics/src/DelaunayTriangulation.cc b/graphics/src/DelaunayTriangulation.cc new file mode 100644 index 000000000..d432f9839 --- /dev/null +++ b/graphics/src/DelaunayTriangulation.cc @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +#include "CDT/CDT.h" + +////////////////////////////////////////////////// +bool gz::common::DelaunayTriangulation( + const std::vector &_vertices, + const std::vector &_edges, + gz::common::SubMesh *_subMesh) +{ + if (_edges.empty() || _vertices.empty()) + { + gzerr << "Unable to create an extruded outline mesh with " + << "no paths\n"; + return false; + } + + if (_subMesh == nullptr) + { + gzerr << "No submesh to populate\n"; + return false; + } + + std::vector> cdt_verts; + std::vector cdt_edges; + + cdt_verts.reserve(_vertices.size()); + cdt_edges.reserve(_edges.size()); + + for (const auto &vert : _vertices) + { + cdt_verts.push_back({vert.X(), vert.Y()}); + } + + for (const auto &edge : _edges) + { + cdt_edges.emplace_back(edge.X(), edge.Y()); + } + + CDT::Triangulation cdt; + cdt.insertVertices(cdt_verts); + cdt.insertEdges(cdt_edges); + cdt.eraseOuterTrianglesAndHoles(); + + for (const auto & vert : cdt.vertices) + { + _subMesh->AddVertex(vert.x, vert.y, 0.0); + } + + for (const auto & tri : cdt.triangles) + { + _subMesh->AddIndex(tri.vertices[0]); + _subMesh->AddIndex(tri.vertices[1]); + _subMesh->AddIndex(tri.vertices[2]); + } + + return true; +} diff --git a/graphics/src/GTSMeshUtils_TEST.cc b/graphics/src/DelaunayTriangulation_TEST.cc similarity index 79% rename from graphics/src/GTSMeshUtils_TEST.cc rename to graphics/src/DelaunayTriangulation_TEST.cc index ed32d8cd4..df919d392 100644 --- a/graphics/src/GTSMeshUtils_TEST.cc +++ b/graphics/src/DelaunayTriangulation_TEST.cc @@ -18,23 +18,17 @@ #include #include -#include "gz/common/GTSMeshUtils.hh" +#include "gz/common/DelaunayTriangulation.hh" #include "gz/common/Mesh.hh" #include "gz/common/SubMesh.hh" #include "gz/common/testing/AutoLogFixture.hh" -using namespace gz; - -class GTSMeshUtils : public common::testing::AutoLogFixture { }; +class DelaunayTriangulation: public gz::common::testing::AutoLogFixture { }; ///////////////////////////////////////////////// -TEST_F(GTSMeshUtils, DelaunayTriangulation) +TEST_F(DelaunayTriangulation, DelaunayTriangulation) { - #ifdef _WIN32 - std::cerr << "Skipping test on windows due to issue #50" << std::endl; - return; - #endif // test triangulation of a path with two subpaths: // a smaller square inside a bigger square. @@ -63,13 +57,13 @@ TEST_F(GTSMeshUtils, DelaunayTriangulation) edges.push_back(gz::math::Vector2i(6, 7)); edges.push_back(gz::math::Vector2i(7, 4)); - auto mesh = std::make_unique(); + auto mesh = std::make_unique(); mesh->SetName("extruded"); - common::SubMesh subMesh; + gz::common::SubMesh subMesh; - bool result = common::GTSMeshUtils::DelaunayTriangulation(vertices, - edges, - &subMesh); + bool result = gz::common::DelaunayTriangulation(vertices, + edges, + &subMesh); EXPECT_TRUE(result); // same as number of vertices in the path diff --git a/graphics/src/GTSMeshUtils.cc b/graphics/src/GTSMeshUtils.cc deleted file mode 100644 index fbfdc72f9..000000000 --- a/graphics/src/GTSMeshUtils.cc +++ /dev/null @@ -1,248 +0,0 @@ -/* - * Copyright (C) 2016 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ -#include - -#include - -#include -#include -#include -#include - -using namespace gz; -using namespace common; - -////////////////////////////////////////////////// -static int FillVertex(GtsPoint *_p, gpointer *_data) -{ - // create a vertex from GTS_POINT and add it to the submesh - SubMesh *subMesh = reinterpret_cast(_data[0]); - GHashTable* vIndex = reinterpret_cast(_data[2]); - subMesh->AddVertex(GTS_POINT(_p)->x, GTS_POINT(_p)->y, GTS_POINT(_p)->z); - // fill the hash table which will later be used for adding indices to the - // submesh in the FillFace function. - g_hash_table_insert(vIndex, _p, - GUINT_TO_POINTER((*(reinterpret_cast(_data[1])))++)); - - return 0; -} - -////////////////////////////////////////////////// -static int FillFace(GtsTriangle *_t, gpointer *_data) -{ - SubMesh *subMesh = reinterpret_cast(_data[0]); - GHashTable *vIndex = reinterpret_cast(_data[2]); - GtsVertex *v1, *v2, *v3; - gts_triangle_vertices(_t, &v1, &v2, &v3); - subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v1))); - subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v3))); - subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v2))); - return 0; -} - -////////////////////////////////////////////////////////////////////////// -static int AddConstraint(GtsConstraint *_c, GtsSurface *_s) -{ - gts_delaunay_add_constraint(_s, _c); - return 0; -} - -////////////////////////////////////////////////// -static int Intersection(GtsEdge *_c, gpointer *_data) -{ - double x = *reinterpret_cast(_data[0]); - double y = *reinterpret_cast(_data[1]); - int *intersection = reinterpret_cast(_data[2]); - - GtsVertex *v1, *v2; - v1 = _c->segment.v1; - v2 = _c->segment.v2; - - double x1 = v1->p.x; - double x2 = v2->p.x; - double y1 = v1->p.y; - double y2 = v2->p.y; - - double xmin = std::min(x1, x2); - double xmax = (x1 + x2) - xmin; - double ymin = std::min(y1, y2); - double ymax = (y1 + y2) - ymin; - double xBound = xmax+1; - - if (y < ymax && y >= ymin) - { - double xdiff1, ydiff1, xdiff2, ydiff2; - xdiff1 = x2 - x1; - ydiff1 = y2 - y1; - xdiff2 = xBound - x; - ydiff2 = 0; - - double s, t; - s = (-ydiff1 * (x1 - x) + xdiff1 * (y1 - y)) / - (-xdiff2 * ydiff1 + xdiff1 * ydiff2); - t = (xdiff2 * (y1 - y) - ydiff2 * (x1 - x)) / - (-xdiff2 * ydiff1 + xdiff1 * ydiff2); - - if (s >= 0 && s <= 1 && t >= 0 && t <= 1) - (*intersection)++; - } - - return 0; -} - -////////////////////////////////////////////////// -int TriangleIsHole(GtsTriangle *_t, GtsFifo *_edgeList) -{ - GtsEdge *e1, *e2, *e3; - GtsVertex *v1, *v2, *v3; - - gts_triangle_vertices_edges(_t, nullptr, &v1, &v2, &v3, &e1, &e2, &e3); - - double xCenter = (v1->p.x + v2->p.x + v3->p.x) / 3.0; - double yCenter = (v1->p.y + v2->p.y + v3->p.y) / 3.0; - - int intersections = 0; - gpointer data[3]; - data[0] = &xCenter; - data[1] = &yCenter; - data[2] = &intersections; - - gts_fifo_foreach(_edgeList, (GtsFunc) Intersection, data); - - if (intersections % 2) - return 0; - else - return 1; -} - -////////////////////////////////////////////////// -GtsSurface *GTSMeshUtils::DelaunayTriangulation( - const std::vector &_vertices, - const std::vector &_edges) -{ - GSList *l, *verticesList = nullptr; - - GtsSurface *surface; - - for (const auto &vertex : _vertices) - { - verticesList = g_slist_append(verticesList, - gts_vertex_new(gts_vertex_class(), - vertex.X(), vertex.Y(), 0)); - } - - GtsFifo *edgeList; - edgeList = gts_fifo_new(); - for (const auto &edge : _edges) - { - gts_fifo_push(edgeList, - gts_edge_new(GTS_EDGE_CLASS(gts_constraint_class()), - reinterpret_cast - (g_slist_nth_data(verticesList, edge.X())), - reinterpret_cast - (g_slist_nth_data(verticesList, edge.Y())))); - } - - - GtsVertex *v1, *v2, *v3; - GtsTriangle *tri = gts_triangle_enclosing( - gts_triangle_class(), verticesList, 100.); - gts_triangle_vertices(tri, &v1, &v2, &v3); - - surface = gts_surface_new(gts_surface_class(), - gts_face_class(), - gts_edge_class(), - gts_vertex_class()); - - gts_surface_add_face(surface, - gts_face_new(gts_face_class(), - tri->e1, tri->e2, tri->e3)); - - l = verticesList; - while (l) - { - GtsVertex *v_in = reinterpret_cast(l->data); - GtsVertex *v_out = gts_delaunay_add_vertex(surface, v_in, nullptr); - if (v_out != nullptr) - { - gts_vertex_replace(v_in, v_out); - } - l = l->next; - } - - // add constraints - gts_fifo_foreach(edgeList, (GtsFunc) AddConstraint, surface); - - // delete the enclosing triangle - gts_allow_floating_vertices = true; - gts_object_destroy(GTS_OBJECT(v1)); - gts_object_destroy(GTS_OBJECT(v2)); - gts_object_destroy(GTS_OBJECT(v3)); - gts_allow_floating_vertices = false; - - // Remove edges on the boundary which are not constraints - gts_delaunay_remove_hull(surface); - - // remove triangles that are inside holes - gts_surface_foreach_face_remove(surface, (GtsFunc) TriangleIsHole, - edgeList); - - gts_fifo_destroy(edgeList); - g_slist_free(verticesList); - return surface; -} - -////////////////////////////////////////////////// -bool GTSMeshUtils::DelaunayTriangulation( - const std::vector &_vertices, - const std::vector &_edges, - SubMesh *_subMesh) -{ - if (_edges.empty() || _vertices.empty()) - { - gzerr << "Unable to create an extruded outline mesh with " - << "no paths\n"; - return false; - } - - if (!_subMesh) - _subMesh = new SubMesh(); - - GtsSurface *surface = GTSMeshUtils::DelaunayTriangulation(_vertices, _edges); - // tip: to debug the triangulation, it is possible to save the suface - // to view it in Meshlab - // FILE *fp = fopen ("surface.gts", "wt"); - // gts_surface_write (surface, fp); - // fclose(fp); - - // fill the submesh with data generated by GTS - unsigned int n2 = 0; - gpointer data[3]; - GHashTable *vIndex = g_hash_table_new(nullptr, nullptr); - - data[0] = _subMesh; - data[1] = &n2; - data[2] = vIndex; - gts_surface_foreach_vertex(surface, (GtsFunc) FillVertex, data); - n2 = 0; - gts_surface_foreach_face(surface, (GtsFunc) FillFace, data); - - g_hash_table_destroy(vIndex); - gts_object_destroy(GTS_OBJECT(surface)); - return true; -} - diff --git a/graphics/src/MeshCSG.cc b/graphics/src/MeshCSG.cc deleted file mode 100644 index 91c6d937d..000000000 --- a/graphics/src/MeshCSG.cc +++ /dev/null @@ -1,357 +0,0 @@ -/* - * Copyright (C) 2016 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#include - -#include - -#include "gz/common/Console.hh" -#include "gz/common/Mesh.hh" -#include "gz/common/SubMesh.hh" -#include "gz/common/MeshCSG.hh" - -using namespace gz; -using namespace common; - -////////////////////////////////////////////////// -MeshCSG::MeshCSG() -{ -} - -////////////////////////////////////////////////// -MeshCSG::~MeshCSG() -{ -} - -////////////////////////////////////////////////// -void MeshCSG::MergeVertices(GPtrArray * _vertices, double _epsilon) -{ - GPtrArray *array; - GNode *kdtree; - GtsVertex **verticesData = reinterpret_cast(_vertices->pdata); - array = g_ptr_array_new(); - for (unsigned int i = 0; i < _vertices->len; ++i) - g_ptr_array_add(array, verticesData[i]); - kdtree = gts_kdtree_new(array, nullptr); - g_ptr_array_free(array, true); - - // for each vertex, do a bbox kdtree search to find nearby vertices within - // _epsilon, merge vertices if they are within the bbox - for (unsigned int i = 0; i < _vertices->len; ++i) - { - GtsVertex *v = reinterpret_cast(verticesData[i]); - - // make sure vertex v is active (because they could be marked inactive - // due to previous merge operation) - if (!GTS_OBJECT (v)->reserved) - { - GtsBBox *bbox; - GSList *selected, *j; - - // build bounding box - bbox = gts_bbox_new(gts_bbox_class(), - v, - GTS_POINT(v)->x - _epsilon, - GTS_POINT(v)->y - _epsilon, - GTS_POINT(v)->z - _epsilon, - GTS_POINT(v)->x + _epsilon, - GTS_POINT(v)->y + _epsilon, - GTS_POINT(v)->z + _epsilon); - - // select vertices which are inside bbox using kdtree - j = selected = gts_kdtree_range(kdtree, bbox, nullptr); - while (j) - { - GtsVertex *sv = reinterpret_cast(j->data); - // mark sv as inactive (merged) - if (sv != v && !GTS_OBJECT(sv)->reserved) - GTS_OBJECT(sv)->reserved = v; - j = j->next; - } - g_slist_free(selected); - gts_object_destroy(GTS_OBJECT(bbox)); - } - } - - gts_kdtree_destroy(kdtree); - - // destroy inactive vertices - // we want to control vertex destruction - gts_allow_floating_vertices = true; - for (unsigned int i = 0; i < _vertices->len; ++i) - { - GtsVertex * v = reinterpret_cast(verticesData[i]); - // v is inactive - if (GTS_OBJECT(v)->reserved) - { - verticesData[i] = - reinterpret_cast(GTS_OBJECT(v)->reserved); - gts_object_destroy(GTS_OBJECT(v)); - } - } - gts_allow_floating_vertices = false; -} - -////////////////////////////////////////////////// -static int FillVertex(GtsPoint *_p, gpointer *_data) -{ - // create a vertex from GTS_POINT and add it to the submesh - SubMesh *subMesh = reinterpret_cast(_data[0]); - GHashTable* vIndex = reinterpret_cast(_data[2]); - subMesh->AddVertex(GTS_POINT(_p)->x, GTS_POINT(_p)->y, GTS_POINT(_p)->z); - // fill the hash table which will later be used for adding indices to the - // submesh in the FillFace function. - g_hash_table_insert(vIndex, _p, - GUINT_TO_POINTER((*(reinterpret_cast(_data[1])))++)); - return 0; -} - -////////////////////////////////////////////////// -static int FillFace(GtsTriangle *_t, gpointer *_data) -{ - SubMesh *subMesh = reinterpret_cast(_data[0]); - GHashTable* vIndex = reinterpret_cast(_data[2]); - - GtsVertex * v1, * v2, * v3; - gts_triangle_vertices(_t, &v1, &v2, &v3); - - subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v1))); - subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v2))); - subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v3))); - return 0; -} - -////////////////////////////////////////////////////////////////////////// -static int TriangleRevert(GtsTriangle *_t, void *) -{ - gts_triangle_revert(_t); - return 0; -} - -////////////////////////////////////////////////// -Mesh *MeshCSG::CreateBoolean(const Mesh *_m1, const Mesh *_m2, int _operation, - const gz::math::Pose3d &_offset) -{ - GtsSurface *s1, *s2, *s3; - GtsSurfaceInter *si; - GNode *tree1, *tree2; - - gboolean closed = true; - bool isOpen1 = false; - bool isOpen2 = false; - - s1 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(), - gts_vertex_class()); - s2 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(), - gts_vertex_class()); - s3 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(), - gts_vertex_class()); - - this->ConvertMeshToGTS(_m1, s1); - - if (_offset != gz::math::Pose3d::Zero) - { - Mesh *m2 = new Mesh(); - for (unsigned int i = 0; i < _m2->SubMeshCount(); ++i) - { - SubMesh m2SubMesh; - - auto subMesh = _m2->SubMeshByIndex(i).lock(); - if (subMesh->VertexCount() <= 2) - continue; - for (unsigned int j = 0; j < subMesh->VertexCount(); ++j) - { - m2SubMesh.AddVertex(_offset.Pos() + - _offset.Rot()*subMesh->Vertex(j)); - } - for (unsigned int j = 0; j < subMesh->IndexCount(); ++j) - { - m2SubMesh.AddIndex(subMesh->Index(j)); - } - - m2->AddSubMesh(m2SubMesh); - } - this->ConvertMeshToGTS(m2, s2); - delete m2; - } - else - { - this->ConvertMeshToGTS(_m2, s2); - } - - // build bounding box tree for first surface - tree1 = gts_bb_tree_surface(s1); - isOpen1 = gts_surface_volume(s1) < 0. ? true : false; - - // build bounding box tree for second surface - tree2 = gts_bb_tree_surface(s2); - isOpen2 = gts_surface_volume(s2) < 0. ? true : false; - - si = gts_surface_inter_new(gts_surface_inter_class(), s1, s2, tree1, tree2, - isOpen1, isOpen2); - if (!gts_surface_inter_check(si, &closed)) - { - gzerr << "si is not an orientable manifold\n"; - return nullptr; - } - - if (!closed) - { - gzerr << "the intersection of " << _m1->Name() << " and " - << _m2->Name() << " is not a closed curve\n"; - return nullptr; - } - -// FILE *output1 = fopen("output3.gts", "w"); -// gts_surface_write(s1, output1); -// fclose(output1); - -// FILE *output2 = fopen("output4.gts", "w"); -// gts_surface_write(s2, output2); -// fclose(output2); - - if (_operation == MeshCSG::UNION) - { - gts_surface_inter_boolean(si, s3, GTS_1_OUT_2); - gts_surface_inter_boolean(si, s3, GTS_2_OUT_1); - } - else if (_operation == MeshCSG::INTERSECTION) - { - gts_surface_inter_boolean(si, s3, GTS_1_IN_2); - gts_surface_inter_boolean(si, s3, GTS_2_IN_1); - } - else if (_operation == MeshCSG::DIFFERENCE) - { - gts_surface_inter_boolean(si, s3, GTS_1_OUT_2); - gts_surface_inter_boolean(si, s3, GTS_2_IN_1); - gts_surface_foreach_face(si->s2, (GtsFunc) TriangleRevert, nullptr); - gts_surface_foreach_face(s2, (GtsFunc) TriangleRevert, nullptr); - } - -// FILE *output = fopen("output.gts", "w"); -// gts_surface_write(s3, output); -// fclose(output); - - // create the boolean mesh - Mesh *mesh = new Mesh(); - SubMesh subMesh; - - // fill the submesh with data generated by GTS - unsigned int n = 0; - gpointer data[3]; - GHashTable *vIndex = g_hash_table_new(nullptr, nullptr); - - data[0] = &subMesh; - data[1] = &n; - data[2] = vIndex; - gts_surface_foreach_vertex(s3, (GtsFunc)FillVertex, data); - n = 0; - gts_surface_foreach_face(s3, (GtsFunc)FillFace, data); - g_hash_table_destroy(vIndex); - - mesh->RecalculateNormals(); - - // destroy surfaces - gts_object_destroy(GTS_OBJECT(s1)); - gts_object_destroy(GTS_OBJECT(s2)); - gts_object_destroy(GTS_OBJECT(s3)); - gts_object_destroy(GTS_OBJECT(si)); - - // destroy bounding box trees (including bounding boxes) - gts_bb_tree_destroy(tree1, true); - gts_bb_tree_destroy(tree2, true); - - mesh->AddSubMesh(subMesh); - return mesh; -} - -////////////////////////////////////////////////// -void MeshCSG::ConvertMeshToGTS(const Mesh *_mesh, GtsSurface *_surface) -{ - if (!_surface) - { - gzerr << _mesh->Name() << ": Surface is null\n"; -// _surface = gts_surface_new(gts_surface_class(), gts_face_class(), -// gts_edge_class(), gts_vertex_class()); - return; - } - - GPtrArray *vertices = g_ptr_array_new(); - - for (unsigned int i = 0; i < _mesh->SubMeshCount(); ++i) - { - auto subMesh = _mesh->SubMeshByIndex(i).lock(); - unsigned int indexCount = subMesh->IndexCount(); - if (subMesh->VertexCount() <= 2) - continue; - - for (unsigned int j = 0; j < subMesh->VertexCount(); ++j) - { - gz::math::Vector3d vertex = subMesh->Vertex(j); - g_ptr_array_add(vertices, gts_vertex_new(gts_vertex_class(), vertex.X(), - vertex.Y(), vertex.Z())); - } - - // merge duplicate vertices, otherwise gts produces undesirable results - this->MergeVertices(vertices, 0.01); - - GtsVertex **verticesData = - reinterpret_cast(vertices->pdata); - for (unsigned int j = 0; j < indexCount/3; ++j) - { - GtsEdge *e1 = GTS_EDGE(gts_vertices_are_connected( - verticesData[subMesh->Index(3*j)], - verticesData[subMesh->Index(3*j+1)])); - GtsEdge *e2 = GTS_EDGE(gts_vertices_are_connected( - verticesData[subMesh->Index(3*j+1)], - verticesData[subMesh->Index(3*j+2)])); - GtsEdge *e3 = GTS_EDGE(gts_vertices_are_connected( - verticesData[subMesh->Index(3*j+2)], - verticesData[subMesh->Index(3*j)])); - if (e1 == nullptr && verticesData[subMesh->Index(3*j)] - != verticesData[subMesh->Index(3*j+1)]) - { - e1 = gts_edge_new(_surface->edge_class, - verticesData[subMesh->Index(3*j)], - verticesData[subMesh->Index(3*j+1)]); - } - if (e2 == nullptr && verticesData[subMesh->Index(3*j+1)] - != verticesData[subMesh->Index(3*j+2)]) - { - e2 = gts_edge_new(_surface->edge_class, - verticesData[subMesh->Index(3*j+1)], - verticesData[subMesh->Index(3*j+2)]); - } - if (e3 == nullptr && verticesData[subMesh->Index(3*j+2)] - != verticesData[subMesh->Index(3*j)]) - { - e3 = gts_edge_new(_surface->edge_class, - verticesData[subMesh->Index(3*j+2)], - verticesData[subMesh->Index(3*j)]); - } - if (e1 != nullptr && e2 != nullptr && e3 != nullptr) - { - gts_surface_add_face(_surface, gts_face_new(_surface->face_class, e1, - e2, e3)); - } - else - { - gzwarn << _mesh->Name() << ": Ignoring degenerate facet!"; - } - } - } -} diff --git a/graphics/src/MeshManager.cc b/graphics/src/MeshManager.cc index bb6173e41..b7ac44143 100644 --- a/graphics/src/MeshManager.cc +++ b/graphics/src/MeshManager.cc @@ -39,11 +39,6 @@ #pragma GCC diagnostic pop #endif -#ifndef _WIN32 - #include "gz/common/GTSMeshUtils.hh" - #include "gz/common/MeshCSG.hh" -#endif - #include "gz/common/Console.hh" #include "gz/common/Mesh.hh" #include "gz/common/SubMesh.hh" @@ -57,6 +52,7 @@ #include "gz/common/config.hh" #include "gz/common/MeshManager.hh" +#include "gz/common/DelaunayTriangulation.hh" using namespace gz::common; @@ -616,7 +612,7 @@ void MeshManager::CreateExtrudedPolyline(const std::string &_name, tol, vertices, edges); - if (!GTSMeshUtils::DelaunayTriangulation(vertices, edges, &subMesh)) + if (!gz::common::DelaunayTriangulation(vertices, edges, &subMesh)) { gzerr << "Unable to triangulate polyline." << std::endl; delete mesh; @@ -1580,23 +1576,8 @@ void MeshManager::Tesselate2DMesh(SubMesh *sm, int meshWidth, int meshHeight, vInc = -vInc; } } - ////////////////////////////////////////////////// -void MeshManager::CreateBoolean(const std::string &_name, const Mesh *_m1, - const Mesh *_m2, int _operation, const gz::math::Pose3d &_offset) -{ - if (this->HasMesh(_name)) - return; -#ifndef _WIN32 - MeshCSG csg; - Mesh *mesh = csg.CreateBoolean(_m1, _m2, _operation, _offset); - mesh->SetName(_name); - this->dataPtr->meshes.insert(std::make_pair(_name, mesh)); -#endif -} - -////////////////////////////////////////////////// size_t MeshManager::AddUniquePointToVerticesTable( std::vector &_vertices, const gz::math::Vector2d &_p,