diff --git a/src/GEL/Geometry/NeighborUtil.h b/src/GEL/Geometry/NeighborUtil.h new file mode 100644 index 00000000..0566de4a --- /dev/null +++ b/src/GEL/Geometry/NeighborUtil.h @@ -0,0 +1,120 @@ +// +// Created by Cem Akarsubasi on 11/19/25. +// + +/// @file NeighborUtil.h Helper functions for dealing with point cloud neighbors + +#ifndef GEL_NEIGHBORUTIL_H +#define GEL_NEIGHBORUTIL_H + +#include + +#include +#include + +#include + +namespace Geometry +{ +/// @brief Calculate tangent plane projection distance +/// @param edge: the edge to be considered +/// @param this_normal: normal of one vertex +/// @param neighbor_normal: normal of another vertex +/// @return projection distance +inline double tangent_space_distance(const CGLA::Vec3d& edge, const CGLA::Vec3d& this_normal, const CGLA::Vec3d& neighbor_normal) +{ + const double euclidean_distance = edge.length(); + if (std::abs(dot(this_normal, neighbor_normal)) < std::cos(15. / 180. * M_PI)) + return euclidean_distance; + const double neighbor_normal_length = dot(edge, neighbor_normal); + const double normal_length = dot(edge, this_normal); + const double projection_dist = (std::sqrt((euclidean_distance * euclidean_distance) - (normal_length * normal_length)) + + std::sqrt((euclidean_distance * euclidean_distance) - (neighbor_normal_length * neighbor_normal_length))) * 0.5; + return projection_dist; +} + +using Tree = KDTree; +using Record = KDTreeRecord; + +struct NeighborInfo { + AMGraph::NodeID id; + double distance; + + explicit NeighborInfo(const Record& record) noexcept : id(record.v), distance(std::sqrt(record.d)) + {} +}; + +//using NeighborArray = Util::InplaceVector; +using NeighborArray = std::vector; +using NeighborMap = std::vector; + +template +Tree build_kd_tree_of_indices(const std::vector& vertices, const Indices& indices) +{ + Tree kd_tree; + for (const auto idx : indices) { + kd_tree.insert(vertices.at(idx), idx); + } + kd_tree.build(); + return kd_tree; +} + +/// @brief k nearest neighbor search +/// @param query: the coordinate of the point to be queried +/// @param kdTree: kd-tree for knn query +/// @param num: number of nearest neighbors to be queried +/// @param neighbors: [OUT] indices of k nearest neighbors +inline void knn_search(const CGLA::Vec3d& query, const Tree& kdTree, const int num, NeighborArray& neighbors) +{ + // It might be a better idea for the caller to handle this to reduce some clutter + std::vector records = kdTree.m_closest(num, query, INFINITY); + std::sort_heap(records.begin(), records.end()); + + for (auto record : records) { + neighbors.emplace_back(record); + } +} + +template +auto calculate_neighbors( + Util::detail::IExecutor& pool, + const std::vector& vertices, + const Indices& indices, + const Tree& kdTree, + const int k, + NeighborMap&& neighbors_memoized = NeighborMap()) + -> NeighborMap +{ + if (neighbors_memoized.empty()) { + neighbors_memoized = NeighborMap(indices.size()); + for (auto& neighbors : neighbors_memoized) { + neighbors.reserve(k + 2); + } + } else if (neighbors_memoized.at(0).capacity() < k) { + for (auto& neighbors : neighbors_memoized) { + neighbors.reserve(k + 2); + } + } + + auto cache_kNN_search = [&kdTree, k, &vertices](auto index, auto& neighbor) { + auto vertex = vertices.at(index); + knn_search(vertex, kdTree, k, neighbor); + }; + Util::detail::Parallel::foreach2(pool, indices, neighbors_memoized, cache_kNN_search); + return neighbors_memoized; +} + +inline auto calculate_neighbors( + Util::detail::IExecutor& pool, + const std::vector& vertices, + const Tree& kdTree, + const int k, + NeighborMap&& neighbors_memoized = NeighborMap()) + -> NeighborMap +{ + const auto indices = std::ranges::iota_view(0UL, vertices.size()); + return calculate_neighbors(pool, vertices, indices, kdTree, k, std::move(neighbors_memoized)); +} +} + +#endif //GEL_NEIGHBORUTIL_H diff --git a/src/GEL/HMesh/HierarchicalReconstruction.cpp b/src/GEL/HMesh/HierarchicalReconstruction.cpp new file mode 100644 index 00000000..56b519b1 --- /dev/null +++ b/src/GEL/HMesh/HierarchicalReconstruction.cpp @@ -0,0 +1,944 @@ +// +// Created by Cem Akarsubasi on 9/10/25. +// + +#include + +#include + +#include +#include + +#include +#include + +namespace HMesh::RSR +{ +using NodeID = size_t; +using Point = CGLA::Vec3d; +using Vec3 = CGLA::Vec3d; + +using namespace detail; +using namespace Util::detail; + +using Geometry::AMGraph; + +/// Raw collapse is used by the CollapseGraph which +/// needs to track the NodeIDs in addition to the coordinates. +struct RawCollapse { + /// The point whose ID remains in the graph after the collapse + NodeID active; + /// The point whose ID is removed from the graph after the collapse + NodeID latent; + /// The old coordinates of active + Point active_point_coords; + /// The old coordinates of latent + Point latent_point_coords; + /// The new coordinates of the collapsed point. + Point v_bar; +}; + +/// Derives from AMGraph to perform the collapse. The actual edges are +/// stored inside a BTree which serves as a mutable priority queue. +struct CollapseGraph : AMGraph { +private: + struct Vertex { + Point position; + Vec3 normal; + /// The weight of a vertex depends on how many vertices have been collapsed + /// into it. This penalizes continuously collapsing into the same vertex + /// and presumably creates a more balanced graph. + double weight = 1; + }; + + /// Stores edge information + struct Edge { + NodeID from; + NodeID to; + double dist; + + bool operator==(const Edge& rhs) const + { + return (from == rhs.from && to == rhs.to) || (from == rhs.to && to == rhs.from); + } + + bool operator<(const Edge& other) const + { + return dist < other.dist; + } + }; + +public: + /// Stores vertex information + Util::AttribVec m_vertices; + +private: + /// The collapse queue stored as a BTreeSet + Util::BTreeSet m_collapse_queue; + /// Map from an EdgeID to the edge length. Since edges are stored sorted + /// in terms of edge length in m_collapse_queue, extracting them in logarithmic + /// time requires us to store the edge length in two separate places. + Util::AttribVec m_edges; + +public: + /// Insert a vertex + auto add_node(const Point& position, const Vec3& normal) -> NodeID + { + const NodeID n = AMGraph::add_node(); + m_vertices[n] = Vertex{.position = position, .normal = normal}; + return n; + } + + /// Insert an edge into the graph and the priority queue + auto connect_nodes(const NodeID n0, const NodeID n1) -> EdgeID + { + if (n1 > n0) { + const EdgeID e = AMGraph::connect_nodes(n0, n1); + auto [_, dist] = distance_function(n0, n1); + m_collapse_queue.emplace(n0, n1, dist); + m_edges[e] = dist; + return e; + } + return InvalidEdgeID; + } + + /// Perform a single collapse. The edge with the lowest "weight" is removed from + /// the graph and the ends of the edge are combined into a single vertex. All of + /// the edges that used to belong to the two vertices are removed from the priority + /// queue and are reinserted with the new coordinates and vertex weights. + auto collapse_one() -> RawCollapse + { + while (!m_collapse_queue.empty()) { + auto edge_ = m_collapse_queue.begin(); + const auto edge = *edge_; + m_collapse_queue.erase(edge_); + const auto active = edge.from; + const auto latent = edge.to; + + if (AMGraph::find_edge(active, latent) != AMGraph::InvalidEdgeID) { + const auto active_coords = m_vertices[active].position; + const auto latent_coords = m_vertices[latent].position; + GEL_ASSERT_FALSE(active_coords.any([](auto d){ return std::isnan(d); })); + GEL_ASSERT_FALSE(latent_coords.any([](auto d){ return std::isnan(d); })); + + // recalculate current edges + for (auto v : AMGraph::neighbors_lazy(active)) { + auto v0 = std::min(v, active); + auto v1 = std::max(v, active); + auto edge_id = find_edge(v0, v1); + auto dist = m_edges[edge_id]; + m_collapse_queue.extract(Edge{v0, v1, dist}); + } + + for (auto v : AMGraph::neighbors_lazy(latent)) { + auto v0 = std::min(v, latent); + auto v1 = std::max(v, latent); + auto edge_id = find_edge(v0, v1); + auto dist = m_edges[edge_id]; + m_collapse_queue.extract(Edge{v0, v1, dist}); + } + + // combine the two vertices + double total_weight = m_vertices[latent].weight + m_vertices[active].weight; + const auto new_normal = + lerp(m_vertices[latent].normal, m_vertices[active].normal, + m_vertices[active].weight / total_weight); + const auto v_bar = + lerp(m_vertices[latent].position, m_vertices[active].position, + m_vertices[active].weight / total_weight); + m_vertices[active].position = v_bar; + m_vertices[active].normal = new_normal; + m_vertices[active].weight += m_vertices[latent].weight; + // we set the latent positions to NaN to defend against reusing them. + m_vertices[latent].position = Point(std::numeric_limits::signaling_NaN()); + m_vertices[latent].normal = Vec3(std::numeric_limits::signaling_NaN()); + + // recalculate current edges + for (auto v : AMGraph::neighbors_lazy(active)) { + auto [v0, v1] = std::minmax(v, active); + connect_nodes(v0, v1); + } + + for (auto v : AMGraph::neighbors_lazy(latent)) { + auto [v0, v1] = std::minmax(v, active); + connect_nodes(v0, v1); + } + AMGraph::erase_node(latent); + + return RawCollapse{ + .active = active, + .latent = latent, + .active_point_coords = active_coords, + .latent_point_coords = latent_coords, + .v_bar = v_bar + }; + } + } + // Immediately fail if we run out of edges to collapse. This might happen if the graph is initialized + // with an insufficient number of initial nearest neighbors. Not a hugely important case to handle right now. + GEL_ASSERT(false, "Ran out of edges to collapse"); + return RawCollapse{}; + } + + /// Return the remaining points + auto to_point_cloud() -> PointCloud + { + std::vector points; + std::vector normals; + for (auto i = 0UL; i < m_vertices.size(); ++i) { + if (!m_vertices[i].position.any([](const double e) { return std::isnan(e); })) { + points.emplace_back(m_vertices[i].position); + normals.emplace_back(m_vertices[i].normal); + } + } + return PointCloud{std::move(points), std::move(normals)}; + } + +private: + /// Returns the optimal point and the collapse distance for two vertices + [[nodiscard]] + std::pair distance_function(const NodeID n0, const NodeID n1) const + { + GEL_ASSERT(valid_node_id(n0) && valid_node_id(n1)); + if (valid_node_id(n0) && valid_node_id(n1)) { + const auto& v0 = m_vertices[n0]; + const auto& v1 = m_vertices[n1]; + + const auto total_weight = v0.weight + v1.weight; + const auto center = lerp(v0.position, v1.position, v0.weight / total_weight); + const auto tangent_distance = + Geometry::tangent_space_distance(v0.position - v1.position, v0.normal, v1.normal); + + return std::make_pair(center, tangent_distance * total_weight); + } else { + return std::make_pair(CGLA::Vec3d(), CGLA::CGLA_NAN); + } + } +}; + +/// Normalized normal vector of a triangle +Vec3 triangle_normal(const Vec3& p1, const Vec3& p2, const Vec3& p3) +{ + const auto v1 = p2 - p1; + const auto v2 = p3 - p1; + return CGLA::normalize(CGLA::cross(v1, v2)); +} + +/// returns 0 at 180 degrees, 1 at 90 (or 270) degrees +double optimize_dihedral_angle(const Vec3& n1, const Vec3& n2) +{ + const auto angle = CGLA::dot(n1, n2) - 1.0; + return std::abs(angle); +} + +/// returns 0 for an equilateral triangle +double optimize_min_angle( + const Vec3& p1, + const Vec3& p2, + const Vec3& p3, + double angle_factor, + double angle_threshold_penalty, + double angle_threshold_cos +) +{ + const auto e1 = p2 - p1; + const auto e1_len = e1.length(); + const auto e2 = p3 - p1; + const auto e2_len = e2.length(); + + const auto e4 = p3 - p2; + const auto e4_len = e4.length(); + + std::array angles{ + dot(e1, e2) / (e1_len * e2_len), + -dot(e2, e4) / (e2_len * e4_len), + dot(-e4, -e1) / (e4_len * e1_len) + }; + + std::array lengths{e4_len, e1_len, e2_len}; + const auto shortest = std::min(e4_len, std::min(e1_len, e2_len)); + double min_angle_ = 0; + double opposing_distance = 0; + for (auto i = 0; i < 3; ++i) { + if (angles[i] < 0) { + continue; + } else if (angles[i] > min_angle_) { + min_angle_ = angles[i]; + opposing_distance = lengths[i]; + } + } + // Calculating acos directly is actually rather slow + auto min_angle_acos = std::acos(min_angle_); + //1 - min_angle_; // ol reliable + // Attempt to avoid recalculating this. Probably better to get rid of this function altogether + // than to eliminate this. + if (min_angle_ > angle_threshold_cos) { + return angle_threshold_penalty; + } else { + auto score = std::abs( + std::numbers::pi / 3.0 + //0.5 + - min_angle_acos); + return score * shortest * angle_factor; + } +} + +/// Information about a split +struct Split { + HMesh::HalfEdgeID h_in = InvalidHalfEdgeID; + HMesh::HalfEdgeID h_out = InvalidHalfEdgeID; + /// From 0.0 (0 degrees) to 2.0 (180 degrees) + double max_dihedral_angle = 0.0; +}; + +/// An abstraction of a triangle to make it simple to perform a fold operation over a circular +/// list of triangles. +struct Triangle { + /// The length of the edge that this triangle shares with the next triangle in that list + double shared_length = 0; + Vec3 normal = {0, 0, 0}; + + Triangle() = default; + + Triangle(const Vec3& p1, const Vec3& p2, const Vec3& p3, double shared_length) : + shared_length(shared_length), normal(triangle_normal(p1, p2, p3)) + {} +}; + +/// Maximum dihedral angle within a vertex's one ring +auto one_ring_max_dihedral_angle(const Manifold& manifold, const VertexID vid) -> double +{ + auto dihedral_from_hedge = [&manifold](HalfEdgeID h) { + Walker w = manifold.walker(h); + auto v1 = w.vertex(); + auto v2 = w.next().vertex(); + auto v3 = w.next().next().vertex(); + auto v4 = w.opp().vertex(); + auto v5 = w.opp().next().vertex(); + auto v6 = w.opp().next().next().vertex(); + if (v1 == InvalidVertexID || + v2 == InvalidVertexID || + v3 == InvalidVertexID || + v4 == InvalidVertexID || + v5 == InvalidVertexID || + v6 == InvalidVertexID) { + GEL_ASSERT(false, "wat"); + return 0.0; + } + auto n0 = triangle_normal( + manifold.positions[v1], + manifold.positions[v2], + manifold.positions[v3]); + auto n1 = triangle_normal( + manifold.positions[v4], + manifold.positions[v5], + manifold.positions[v6]); + return optimize_dihedral_angle(n0, n1); + }; + + double max_angle = 0; + for (const auto h : manifold.incident_halfedges(vid)) { + GEL_ASSERT_NEQ(h, InvalidHalfEdgeID); + max_angle = std::max(max_angle, dihedral_from_hedge(h)); + } + return max_angle; +} + +/// Find the best edge pair for +Split find_edge_pair(const Manifold& m, const VertexID center_idx, const Vec3& v_new_position, + const Vec3& v_old_position, const ReexpandOpts& opts, double angle_threshold_cos) +{ + const auto angle_factor = opts.min_angle_weight; + const auto angle_threshold_penalty = opts.min_angle_threshold_penalty; + + // Print debug information about a halfedge + const auto print_hedge = [&m](HalfEdgeID he) { + auto v_from = m.positions[m.walker(he).opp().vertex()]; + auto v_to = m.positions[m.walker(he).vertex()]; + std::cout << v_from << " -> " << v_to << " (" << (v_to - v_from) << ")\n"; + }; + + // Optimize the dihedral value + const auto dihedral_one_ring = [](const Triangle& t1, const Triangle& t2) -> std::pair { + auto d = optimize_dihedral_angle(t1.normal, t2.normal); + auto edge_length = t1.shared_length; + return std::make_pair(d, edge_length); + }; + + struct ExpandResult { + double score = INFINITY; + double max_angle = INFINITY; + }; + // Calculate how bad a given expansion would be + const auto expand_score = [&]( + const HalfEdgeID& h_in_opp, + const HalfEdgeID& h_out, + const Point& v_new_position, + const Point& v_old_position) -> ExpandResult { + const auto walker_out = m.walker(h_out); + const auto walker_in_opp = m.walker(h_in_opp); + const auto& v_h_out = m.pos(walker_out.vertex()); + const auto& v_h_in = m.pos(walker_in_opp.vertex()); + + constexpr double EPS = 1e-8; + + const double in_len = std::max((v_old_position - v_h_in).length(), EPS); + const double out_len = std::max((v_new_position - v_h_out).length(), EPS); + const auto tri_center_in = Triangle(v_old_position, v_new_position, v_h_in, in_len); + const auto tri_center_out = Triangle(v_new_position, v_old_position, v_h_out, out_len); + + double angle_cost = 0; + if (angle_factor > 0.0) { + angle_cost += optimize_min_angle(v_old_position, v_new_position, v_h_in, angle_factor, + angle_threshold_penalty, angle_threshold_cos); + angle_cost += optimize_min_angle(v_new_position, v_old_position, v_h_out, angle_factor, + angle_threshold_penalty, angle_threshold_cos); + } + + double dihedral_cost = 0; + double max_angle = 0; + std::array triangles_buffer_; + auto calculate_one_dihedral = [&](const Triangle& tri) { + triangles_buffer_[0] = triangles_buffer_[1]; + triangles_buffer_[1] = tri; + auto [angle, edge_length] = dihedral_one_ring(triangles_buffer_[0], triangles_buffer_[1]); + dihedral_cost += angle * angle * edge_length; + max_angle = std::max(max_angle, angle); + }; + + auto walker_prev = walker_out; + auto walker_next = walker_prev.circulate_vertex_ccw(); + // from out towards in counterclockwise + triangles_buffer_[1] = tri_center_out; + while (walker_prev.halfedge() != walker_in_opp.halfedge()) { + const auto& p2 = m.pos(walker_prev.vertex()); + const auto& p3 = m.pos(walker_next.vertex()); + const auto shared_length = std::max((v_new_position - p3).length(), EPS); + Triangle tri = {v_new_position, p2, p3, shared_length}; + if (angle_factor > 0.0) { + angle_cost += optimize_min_angle(v_new_position, p2, p3, angle_factor, angle_threshold_penalty, + angle_threshold_cos); + } + calculate_one_dihedral(tri); + + walker_prev = walker_prev.circulate_vertex_ccw(); + walker_next = walker_next.circulate_vertex_ccw(); + } + calculate_one_dihedral(tri_center_in); + + // from in towards out counterclockwise + walker_prev = walker_in_opp; + walker_next = walker_in_opp.circulate_vertex_ccw(); + while (walker_prev.halfedge() != walker_out.halfedge()) { + const auto& p2 = m.pos(walker_prev.vertex()); + const auto& p3 = m.pos(walker_next.vertex()); + const auto shared_length = std::max((v_old_position - p3).length(), EPS); + Triangle tri = {v_old_position, p2, p3, shared_length}; + if (angle_factor > 0.0) { + angle_cost += optimize_min_angle(v_old_position, p2, p3, angle_factor, angle_threshold_penalty, + angle_threshold_cos); + } + calculate_one_dihedral(tri); + + walker_prev = walker_prev.circulate_vertex_ccw(); + walker_next = walker_next.circulate_vertex_ccw(); + } + calculate_one_dihedral(tri_center_out); + + const auto [dihedral0_angle, dihedral0_length] = dihedral_one_ring(tri_center_in, tri_center_out); + const auto dihedral0 = dihedral0_angle * dihedral0_angle * dihedral0_length; + max_angle = std::max(max_angle, dihedral0_angle); + + return ExpandResult{ + .score = dihedral_cost + dihedral0 + angle_cost, + .max_angle = max_angle, + }; + }; + + double min_score = INFINITY; + double max_angle = INFINITY; + HalfEdgeID h_in_opp; + HalfEdgeID h_out; + // Threshold for a "degenerate" dihedral angle + constexpr double dihedral_threshold = 1.75; + + double min_score_alt = INFINITY; + double max_angle_alt = INFINITY; + HalfEdgeID h_in_opp_alt = InvalidHalfEdgeID; + HalfEdgeID h_out_alt = InvalidHalfEdgeID; + + // TODO: ruling out "bad pairs" early can speed this up by a lot + auto v_bar = m.positions[center_idx]; + auto ref_length = (v_old_position - v_new_position).length(); + auto norm = m.normal(center_idx); + auto v3 = v_bar + norm * ref_length; + + for (auto h1 : m.incident_halfedges(center_idx)) { + auto v1 = m.positions[m.walker(h1).vertex()]; + for (auto h2 : m.incident_halfedges(center_idx)) { + // Illegal + if (h1 == h2) { + continue; + } + auto v2 = m.positions[m.walker(h2).vertex()]; + // TODO: early bad pair culling + auto plane_norm = CGLA::normalize(CGLA::cross(v3 - v1, v2 - v1)); + //if (CGLA::dot((v_new_position - v_bar), plane_norm) < 0.0 || CGLA::dot((v_old_position - v_bar), plane_norm) > 0.0) + // continue; + + // This duplication performs "thresholding" for bad dihedral angles + // TODO: might be better to move thresholding up instead of having a lot of duplication here + auto score = expand_score(h1, h2, v_new_position, v_old_position); + if (score.score < min_score) { + min_score = score.score; + max_angle = score.max_angle; + h_in_opp = h1; + h_out = h2; + } + if (score.score < min_score_alt && score.max_angle < dihedral_threshold) { + min_score_alt = score.score; + max_angle_alt = score.max_angle; + h_in_opp_alt = h1; + h_out_alt = h2; + } + if (opts.debug_opts.debug_mask & RE_SPLITS) { + std::cout << "h1: "; + print_hedge(h1); + std::cout << "h2: "; + print_hedge(h2); + std::cout << "score : " << score.score << "\n"; + std::cout << "max angle: " << score.max_angle << "\n"; + } + } + } + if (max_angle > dihedral_threshold && max_angle_alt < dihedral_threshold) { + if (opts.debug_opts.debug_mask & RE_SPLIT_RESULTS) { + std::cout << "using alternative split\n"; + std::cout << "h_in_opp: "; + print_hedge(h_in_opp_alt); + std::cout << "h_out: "; + print_hedge(h_out_alt); + } + if (h_out_alt == InvalidHalfEdgeID) { + return {}; + } + return Split{m.walker(h_in_opp_alt).opp().halfedge(), h_out_alt, max_angle_alt}; + } + if (opts.debug_opts.debug_mask & RE_SPLIT_RESULTS) { + std::cout << "using regular split\n"; + std::cout << "h_in_opp: "; + print_hedge(h_in_opp); + std::cout << "h_out: "; + print_hedge(h_out); + } + if (h_out == InvalidHalfEdgeID) { + return {}; + } + return Split{m.walker(h_in_opp).opp().halfedge(), h_out, max_angle}; +} + +/// For the refinement, returns true if an edge flip will not degenerate the mesh and has at least +/// one corner where the angle is below our given threshold angle (in radians). +auto angle_flip_check(const Manifold& manifold, const HalfEdgeID he, double angle_threshold) -> bool +{ + if (!manifold.precond_flip_edge(he)) + return false; + const auto angles = [&](HalfEdgeID he) -> std::pair { + auto walker = manifold.walker(he); + auto v1 = manifold.positions[walker.vertex()]; + auto v2 = manifold.positions[walker.next().vertex()]; + auto v3 = manifold.positions[walker.opp().vertex()]; + auto e_shared = CGLA::normalize(v1 - v3); + auto e_next = CGLA::normalize(v2 - v1); + auto e_prev = CGLA::normalize(v2 - v3); + + auto angle1 = CGLA::dot(e_next, -e_shared); + auto angle2 = CGLA::dot(e_prev, e_shared); + + return {angle1, angle2}; + }; + const auto he_opp = manifold.walker(he).opp().halfedge(); + auto [angle1, angle2] = angles(he); + auto [angle3, angle4] = angles(he_opp); + + if (angle1 < 0.0 || angle2 < 0.0 || angle3 < 0.0 || angle4 < 0.0) { + return false; + } + + // Really expensive, perhaps use a fast approximation or do everything in the cosine domain + if (std::acos(angle1) < angle_threshold || + std::acos(angle2) < angle_threshold || + std::acos(angle3) < angle_threshold || + std::acos(angle4) < angle_threshold) { + return true; + } + return false; +} + +auto collapse_points(const std::vector& vertices, const std::vector& normals, + const CollapseOpts& opts) -> std::pair +{ + if (opts.max_iterations == 0) { + return std::make_pair(Collapse(), PointCloud(vertices, normals)); + } + std::cout << "Collapsing..." << std::endl; + GEL_ASSERT_EQ(vertices.size(), normals.size()); + ImmediatePool pool; + CollapseGraph graph; + + // initialize graph + for (auto i = 0UL; i < vertices.size(); ++i) { + graph.add_node(vertices[i], normals[i]); + } + auto indices = [&vertices] { + std::vector temp(vertices.size()); + std::iota(temp.begin(), temp.end(), 0); + return temp; + }(); + + const auto kd_tree = Geometry::build_kd_tree_of_indices(vertices, indices); + const auto neighbor_map = Geometry::calculate_neighbors(pool, vertices, kd_tree, opts.initial_neighbors); + + // This also initializes distances + for (const auto& neighbors : neighbor_map) { + const NodeID this_id = neighbors[0].id; + for (const auto& neighbor : neighbors | std::views::drop(1)) { + // kNN connection + graph.connect_nodes(this_id, neighbor.id); + } + } + + std::vector> collapses; + size_t total_collapses = 0; + for (size_t iter = 0; iter < opts.max_iterations; ++iter) { + // TODO: stricter checking + const size_t max_collapses = + [&]() -> size_t { + return vertices.size() * std::pow(0.5, iter) * opts.reduction_per_iteration; + }(); + + std::vector activity; + + size_t count = 0; + while (count < max_collapses) { + total_collapses++; + count++; + auto [active, latent, active_point_coords, latent_point_coords, v_bar] = graph.collapse_one(); + + activity.emplace_back(active_point_coords, latent_point_coords, v_bar); + if (total_collapses == max_collapses) { + break; + } + } + collapses.emplace_back(std::move(activity)); + std::cout << "Collapsed " << count << " of " << max_collapses << std::endl; + if (total_collapses == max_collapses) { + break; + } + } + Collapse collapse(std::move(collapses)); + return std::make_pair(std::move(collapse), graph.to_point_cloud()); +} + +struct PointHash { + size_t operator()(const Point& point) const + { + const auto h1 = std::hash{}(point[0]); + const auto h2 = std::hash{}(point[1]); + const auto h3 = std::hash{}(point[2]); + return h1 ^ (h2 << 1) ^ (h3 << 2); + } +}; + +struct PointEquals { + size_t operator()(const Point& left, const Point& right) const + { + return (left[0] == right[0]) && (left[1] == right[1]) && (left[2] == right[2]); + } +}; + +/// If an edge plane would be crossed, returns that edge to be flipped +std::optional find_crossed_edge( + const Manifold& manifold, + const VertexID id, + const Point& starting_pos, + const Point& end_pos, + const ReexpandOpts& opts) +{ + for (auto he : manifold.incident_halfedges(id)) { + auto walker = manifold.walker(he); + auto flip_maybe = walker.next().halfedge(); + auto v1 = manifold.positions[walker.vertex()]; + auto v2 = manifold.positions[walker.next().vertex()]; + auto tangent = normalize(v1 - v2); + if (opts.debug_opts.debug_mask & RE_CROSSING_FLIP) { + std::cout << v1 << "\n"; + std::cout << v2 << "\n"; + } + + auto normal = CGLA::normalize(CGLA::cross((starting_pos - v1), tangent)); + + // normal of the plane + Vec3 binormal = CGLA::cross(tangent, normal); + // point on the plane + const Point& p0 = (v1 + v2) * 0.5; + const double r = (v1 - v2).length() * 0.5; + + // starting point in our line + const Point& l0 = starting_pos; + const Vec3 l = normalize(end_pos - starting_pos); + + auto den = dot(l, binormal); + if (std::abs(den) < 1e-8) { + continue; + } // parallel case + auto len = (end_pos - starting_pos).length(); + auto d = dot((p0 - l0), binormal) / den; + + auto p = l0 + d * l; + auto distance = (p - p0).length(); + if (opts.debug_opts.debug_mask & RE_CROSSING_FLIP) { + std::cout << "d : " << d << "\n"; + std::cout << "len: " << len << "\n"; + } + if (distance > r) { + continue; + } + + if (d > 0 && len > (d * 0.90) && manifold.precond_flip_edge(flip_maybe)) { + if (opts.debug_opts.debug_mask & RE_CROSSING_FLIP) { + std::cout << "flipped!" << "\n"; + } + return flip_maybe; + } + } + return std::nullopt; +} + +// Fast quick sort implementations from cppreference +// The reason why this is here is using std::sort results in memory corruption which probably has to do with +// incorrect NaN handling or some other poorly specified behavior in operator< for double. +namespace +{ + template > + void quick_sort(std::forward_iterator auto first, std::forward_iterator auto last, Comp comp = std::less<>{}) + { + if (first == last) + return; + + auto pivot = *std::next(first, std::distance(first, last) / 2); + auto middle1 = std::partition(first, last, [&](const auto& elem) { + return comp(elem, pivot); + }); + auto middle2 = std::partition(middle1, last, [&](const auto& elem) { + return !comp(pivot, elem); + }); + + quick_sort(first, middle1, comp); + quick_sort(middle2, last, comp); + } + + template {})> + void quick_sort(Range&& arr, Comp comp = std::less<>{}) + { + if (std::ranges::distance(arr) == 0) { + return; + } + quick_sort(arr.begin(), arr.end(), comp); + } +} + +void reexpand_points(Manifold& manifold, const Collapse& collapse, const ReexpandOpts& opts) +{ + std::cout << "reexpanding" << std::endl; + const auto& manifold_positions = manifold.positions; + + std::unordered_multimap point_to_manifold_ids; + for (auto manifold_vid : manifold.vertices()) { + auto pos = manifold_positions[manifold_vid]; + point_to_manifold_ids.emplace(pos, manifold_vid); + } + auto position_to_manifold_iter = [&](const Point& point) { + auto [fst, snd] = point_to_manifold_ids.equal_range(point); + return std::ranges::subrange(fst, snd) | std::views::values; + }; + + // insert latent point to stored latent position + // update active point position to the stored coordinate + + double angle_threshold_cos = std::cos(opts.min_angle_threshold); + // Now we need to consider two position candidates + + size_t expansion_failures = 0; + size_t bad_expansions = 0; + size_t flips = 0; + int iteration = 0; + std::vector one_ring; + std::vector circle; + std::vector two_ring; + for (const auto& collapse_iter : collapse.collapses | std::views::reverse) { + for (auto single_collapse : collapse_iter | std::views::reverse) { + iteration++; + // find the manifold_ids for the active vertex + + const auto active_pos = single_collapse.active_point_coords; + const auto latent_pos = single_collapse.latent_point_coords; + const auto v_bar = single_collapse.v_bar; + // FIXME: Debug info + if (opts.debug_opts.debug_mask & RE_ITERATION) { + std::cout << "--------------------------------\n"; + std::cout << "@iteration: " << iteration << "\n"; + std::cout << "active pos: " << active_pos << "\n"; + std::cout << "latent pos: " << latent_pos << "\n"; + std::cout << "combin pos: " << v_bar << "\n"; + } + if (opts.debug_opts.debug_mask & RE_MARK_SPLITS) { + manifold.add_face({active_pos, latent_pos, v_bar}); + } + if (opts.debug_opts.debug_mask & RE_CROSSING_FLIP) { + std::cout << "First flip:\n"; + } + // repair local geometry maybe + const auto manifold_ids = position_to_manifold_iter(v_bar); + for (const auto id : manifold_ids) { + auto maybe1 = find_crossed_edge(manifold, id, latent_pos, active_pos, opts); + if (maybe1) { + manifold.flip_edge(*maybe1); + } + auto maybe2 = find_crossed_edge(manifold, id, active_pos, latent_pos, opts); + if (maybe2) { + manifold.flip_edge(*maybe2); + } + + manifold.positions[id] = active_pos; + } + + // actually do the expansion + const auto new_vid = [&]() -> VertexID { + // we want to get as close to 90 degrees as possible here + for (const auto this_vert : manifold_ids) { + const auto candidate = find_edge_pair(manifold, this_vert, latent_pos, active_pos, opts, + angle_threshold_cos); + if (candidate.h_in != InvalidHalfEdgeID) { + const auto vnew = manifold.split_vertex(candidate.h_in, candidate.h_out); + GEL_ASSERT_NEQ(vnew, InvalidVertexID); + return vnew; + } + } + return InvalidVertexID; + }(); + + if (new_vid == InvalidVertexID) { + expansion_failures++; + continue; + } + + manifold.positions[new_vid] = latent_pos; + + // Update the point to manifold id map + // we need to copy the data to avoid invalidating iterators when we mutate point_to_manifold_ids + if (std::ranges::distance(manifold_ids) < 4) { + InplaceVector copy(manifold_ids.begin(), manifold_ids.end()); + for (auto id : copy) { + point_to_manifold_ids.emplace(active_pos, id); + } + } else { + std::vector copy(manifold_ids.begin(), manifold_ids.end()); + for (auto id : copy) { + point_to_manifold_ids.emplace(active_pos, id); + } + } + point_to_manifold_ids.emplace(latent_pos, new_vid); + point_to_manifold_ids.erase(v_bar); + + // populate a bunch of vectors for the optimization phase + one_ring.clear(); + circle.clear(); + two_ring.clear(); + circulate_vertex_ccw(manifold, new_vid, [&](Walker& w) { + const HalfEdgeID one_ring_he = w.halfedge(); + const HalfEdgeID circle_he = w.next().halfedge(); + const HalfEdgeID two_ring_he = w.next().opp().next().halfedge(); + const HalfEdgeID two_ring_he2 = w.next().opp().prev().halfedge(); + + one_ring.push_back(one_ring_he); + circle.push_back(circle_he); + two_ring.push_back(two_ring_he); + two_ring.push_back(two_ring_he2); + }); + + // sort from longest to shortest edge + const auto& manifold_cref = manifold; + auto cmp = [&manifold_cref](const HalfEdgeID& e1, const HalfEdgeID& e2) -> bool { + // TODO: There is a serious soundness issue here + GEL_ASSERT(manifold_cref.in_use(e1), "%ld", e1.get_index()); + GEL_ASSERT(manifold_cref.in_use(e2), "%ld", e2.get_index()); + auto len1 = manifold_cref.length(e1); + auto len2 = manifold_cref.length(e2); + return len1 < len2; + }; + + // perform optimization/refinement + for (int i = 0; i < opts.refinement_iterations; ++i) { + auto threshold = opts.refinement_angle_threshold; + quick_sort(one_ring, cmp); + for (HalfEdgeID h : one_ring | std::views::reverse) { + bool flipped = angle_flip_check(manifold, h, threshold); + if (flipped) { + manifold.flip_edge(h); + } + flips += flipped; + } + quick_sort(circle, cmp); + for (HalfEdgeID h : circle | std::views::reverse) { + bool flipped = angle_flip_check(manifold, h, threshold); + if (flipped) { + manifold.flip_edge(h); + } + flips += flipped; + } + quick_sort(two_ring, cmp); + for (HalfEdgeID h : two_ring | std::views::reverse) { + bool flipped = angle_flip_check(manifold, h, threshold); + if (flipped) { + manifold.flip_edge(h); + } + flips += flipped; + } + } + + if (opts.debug_opts.debug_mask & RE_ERRORS) { + const auto v_new_max_angle = one_ring_max_dihedral_angle(manifold, new_vid); + const auto v_old_max_angle = one_ring_max_dihedral_angle(manifold, manifold_ids.front()); + if (v_new_max_angle > opts.debug_opts.angle_bad_threshold || v_old_max_angle > opts.debug_opts. + angle_bad_threshold) { + bad_expansions++; + if (opts.debug_opts.debug_mask & RE_ERRORS) { + auto normal = manifold.normal(new_vid); + manifold.add_face({ + active_pos, latent_pos, v_bar + (active_pos - latent_pos).length() * normal + }); + std::cout << "v_new max angle: " << v_new_max_angle << "\n"; + std::cout << "v_old max angle: " << v_old_max_angle << "\n"; + std::cout << "failed at iteration " << iteration << "\n"; + } + if (opts.debug_opts.early_stop_at_error && opts.debug_opts.stop_at_error == 0) { + goto EXIT; + } + if (opts.debug_opts.stop_at_error > 0 && opts.debug_opts.stop_at_error == bad_expansions) { + goto EXIT; + } + } + } + + if (opts.debug_opts.stop_at_iteration > 0 && iteration == opts.debug_opts.stop_at_iteration) { + std::cout << "stopped early at " << iteration << "\n"; + goto EXIT; + } + } + } +EXIT: + std::cout << "flips: " << flips << "\n"; + std::cerr << "failures: " << expansion_failures << "\n"; +} +} diff --git a/src/GEL/HMesh/HierarchicalReconstruction.h b/src/GEL/HMesh/HierarchicalReconstruction.h new file mode 100644 index 00000000..34c18311 --- /dev/null +++ b/src/GEL/HMesh/HierarchicalReconstruction.h @@ -0,0 +1,188 @@ +// +// Created by Cem Akarsubasi on 7/11/25. +// Some Collapse data structures and functions that can be separated from RsR + +#ifndef GEL_HMESH_COLLAPSE_H +#define GEL_HMESH_COLLAPSE_H + +#include + +#include +#include + +namespace HMesh::RSR +{ + +/// Type of distance function to use during reconstruction +/// Euclidean tends to perform better on smooth meshes while +/// Tangent distance performs better on noisy meshes. +enum class Distance { + Euclidean, + Tangent, +}; + +/// Options struct for the collapse phase of hierarchical reconstruction +struct CollapseOpts { + /// Number of collapse iterations to run. During each iteration, a percentage + /// of vertices determined by the reduction_per_iteration will be removed from + /// the point cloud. Removing more than about 90% of the vertices (4 iterations + /// at 0.5 reduction) will typically provide diminishing returns in terms of + /// performance improvements and may affect the reconstruction results negatively + /// as the RSR step may fail to cope with the lack of input points. + /// + /// The separation of this from reduction_per_iteration is to allow for future + /// spatial splitting schemes for parallelization. + size_t max_iterations = 4; + /// Ratio of vertices that will be _removed_ after each iteration. See the + /// documentation for max_iterations for details. + double reduction_per_iteration = 0.5; + /// Number of initial neighbors to seed for the collapse graph. All collapses + /// should happen within the one ring of a given vertex which under most + /// circumstances should be the nearest 4-6 vertices. Using values larger than + /// 6 will typically not provide any quality improvement and will substantially + /// slow down the collapse phase. + size_t initial_neighbors = 5; + /// Maximum number of collapses to perform. If set to a value other than 0, this + /// will stop the collapse phase after that number of collapses are performed. + /// For debug purposes. + size_t max_collapses = 0; + /// Distance function to be used for smoothing and normal estimation. Not directly + /// used by the collapse phase. + Distance distance = Distance::Euclidean; +}; + +/// Debug options for the reconstruction phase. +/// Use operator| to combine debug options. +enum ReexpandDebugOpts { + /// No debug options + RE_NONE = 0, + /// Debug prints for each iteration. Very verbose. + RE_ITERATION = 1 << 0, + /// Debug prints for each vertex split. Very verbose. + RE_SPLITS = 1 << 1, + /// Debug prints for the chosen vertex split. Relatively verbose. + RE_SPLIT_RESULTS = 1 << 2, + /// When enabled, the reconstruction will check to see if any reexpansion + /// creates a dihedral angle that is worse than the "angle_bad_threshold". If + /// found, information will be printed about the expansion and a somewhat + /// prominent floating triangle will be inserted to where the bad expansion + /// occurred. + RE_ERRORS = 1 << 3, + /// Debug prints about whether an edge flip is performed during the edge + /// crossing checks. Very verbose. + RE_CROSSING_FLIP = 1 << 4, + /// When enabled, a floating triangle will be inserted for every performed vertex + /// split. The points of the vertex are placed to both the reexpansion points and + /// the shared point before the reexpansion. This can be used to visualize the + /// progression of the reexpansion phase. + RE_MARK_SPLITS = 1 << 6, +}; + +struct ReexpandDebug { + bool early_stop_at_error = false; + ReexpandDebugOpts debug_mask = RE_NONE; + int stop_at_error = 0; + size_t stop_at_iteration = 0; + double angle_bad_threshold = 1.25; +}; + +/// Options struct for the reexpansion phase of hierarchical reconstruction +struct ReexpandOpts { + /// Whether to perform the reexpansion at all. You probably want to disable + /// this if you want to get a decimated reexpansion or want to debug the + /// collapse phase. + bool enabled = true; + /// Number of iterations of the optimization algorithm to run after a vertex + /// split. Setting this to 0 will substantial reduce the reconstruction + /// quality. Setting this to more than 1 will typically not improve the + /// quality much but substantially increase the runtime. + unsigned refinement_iterations = 1; + /// During the refinement phase, if an edge flip candidate has a corner + /// angle in radians that is _lower_ than this value, that edge will flipped + /// (if such a flip will not degenerate the mesh). Values around 22.5 degrees + /// seem to provide the best results. This option will most likely be removed + /// in the future. + double refinement_angle_threshold = std::numbers::pi / 180.0 * 22.5; + /// How much weight should be given to the minimum angles during the vertex + /// splitting phase. Giving some weights to minimum angles seem to avoid a + /// small number of degenerate cases but giving large weights to this will + /// typically not improve quality and reducing it might improve quality instead. + /// This option will most likely be removed in the future. + double min_angle_weight = 1; + /// If a triangle with a minimum angle in radians lower than min_angle_threshold + /// is created during the vertex splitting phase, that expansion is penalized with + /// min_angle_threshold_penalty. + double min_angle_threshold = std::numbers::pi / 180.0 * 3.0; + /// How much to penalize vertex splits that create triangles with minimum + /// angles below min_angle_threshold. The value is absolute so pretty much + /// any nonzero value will rule out those triangles. + double min_angle_threshold_penalty = 0.1; + /// Debug options + ReexpandDebug debug_opts = ReexpandDebug(); +}; + +/// Information about a single collapse. +struct SingleCollapse { + /// Old coordinates of the active point + CGLA::Vec3d active_point_coords; + /// Old coordinates of the latent point + CGLA::Vec3d latent_point_coords; + /// Current coordinates of the active point + CGLA::Vec3d v_bar; +}; + +/// A point cloud consists of pairs of coordinates and their +/// associated normal vectors. +struct PointCloud { + std::vector points; + std::vector normals; +}; + + +/// Contains data needed for a reexpansion +struct Collapse { +private: + std::vector> collapses; + + // Private constructors can only be called from friend functions + Collapse() = default; + explicit Collapse(std::vector>&& _collapses) : collapses(_collapses) {} + +public: + friend auto collapse_points( + const std::vector& vertices, + const std::vector& normals, + const CollapseOpts& opts + ) -> std::pair; + + friend void reexpand_points( + Manifold& manifold, + const Collapse& collapse, + const ReexpandOpts& opts); +}; + +/// Perform the collapse phase of the hierarchical reconstruction. The vertices must be provided +/// with the correct associated normals. +/// @param vertices Vertices of the point cloud to collapse +/// @param normals Normals of the point cloud to collapse, must be the same length as the vertices +/// @param opts Collapse options +/// @return A pair consisting of the collapse information and the new point cloud after the collapse +auto collapse_points( + const std::vector& vertices, + const std::vector& normals, + const CollapseOpts& opts = CollapseOpts() +) -> std::pair; + +/// Perform the reexpansion phase of the hierarchical reconstruction, using a manifold acquired +/// from the point set based reconstruction of the collapsed point cloud and the collapse +/// information acquired during the collapse phase. +/// @param manifold The manifold to perform the reexpansion. +/// @param collapse The collapse information acquired from the collapse phase. +/// @param opts Reexpansion options +void reexpand_points( + Manifold& manifold, + const Collapse& collapse, + const ReexpandOpts& opts = ReexpandOpts()); +} // namespace HMesh + +#endif // GEL_HMESH_COLLAPSE_H diff --git a/src/GEL/HMesh/RSRExperimental.cpp b/src/GEL/HMesh/RSRExperimental.cpp new file mode 100644 index 00000000..1a79db4f --- /dev/null +++ b/src/GEL/HMesh/RSRExperimental.cpp @@ -0,0 +1,2239 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include // std::views +#include +//#include + + +namespace HMesh::RSR +{ +using Vec3 = CGLA::Vec3d; +using Point = Vec3; +using Geometry::AMGraph; + +using NodeID = AMGraph::NodeID; + +using namespace detail; +using namespace Geometry; +using uint = Geometry::uint; + +inline constexpr NodeID InvalidNodeID = AMGraph::InvalidNodeID; +inline constexpr NodeID InvalidEdgeID = AMGraph::InvalidEdgeID; + +template +using OrderedSet = Util::BTreeSet; + +template +using UnorderedSet = Util::HashSet; + +template +using Map = std::unordered_map; + +template +using OrderedMap = Util::BTreeMap; + +double cal_radians_3d(const Vec3& branch, const Vec3& normal); +double cal_radians_3d(const Vec3& branch, const Vec3& normal, const Vec3& ref_vec); + +/// Graph definition. The RsR graph here is integrated with the rotation system based on AMGraph +struct Vertex { + using NormalRep = std::int64_t; + + NormalRep normal_rep = InvalidNormalRep; + Vec3 coords = Vec3(0., 0., 0.); + Vec3 normal = Vec3(0., 0., 0.); + + static constexpr NormalRep InvalidNormalRep = -1; + static constexpr NormalRep CollisionRep = -2; +}; + +struct Neighbor { + double angle = 0.0; + uint v = -1; + uint tree_id = 0; + + Neighbor(const Vertex& u, const Vertex& v, const uint id) + { + this->v = id; + this->angle = cal_radians_3d(v.coords - u.coords, u.normal); + } + + // friend size_t hash_value(const Neighbor& p) + // { + // return std::hash()(p.v); + // } + bool operator==(const Neighbor& rhs) const + { + return v == rhs.v; + } + + bool operator<(const Neighbor& rhs) const + { + return angle < rhs.angle; + } + + // std::weak_ordering operator<=>(const Neighbor& rhs) const + // { + // if (this->v == rhs.v) return std::weak_ordering::equivalent; + // if (this->angle < rhs.angle) return std::weak_ordering::less; + // else return std::weak_ordering::greater; + // } +}; + +static_assert(std::is_trivially_destructible_v); + +struct Edge { + NodeID source = InvalidNodeID; + NodeID target = InvalidNodeID; + int ref_time = 0; +}; + +class SimpGraph /*: public Util::SparseGraph*/ { + AMGraph graph; + std::vector m_edges; // Edge weight + +public: + using NodeID = decltype(graph)::NodeID; + static constexpr auto InvalidEdgeID = decltype(graph)::InvalidEdgeID; + + void reserve(size_t vertices, int k) + { + m_edges.reserve(vertices * k); + } + + // connected components + [[nodiscard]] + auto inner() const -> const decltype(graph)& + { + return graph; + } + + // generic algorithms + auto add_node() -> NodeID + { + return graph.add_node(); + } + + auto connect_nodes(const NodeID source, const NodeID target, const double weight = 0.) + { + const auto id = + graph.connect_nodes(source, target); + if (id == m_edges.size()) + m_edges.push_back(weight); + else + m_edges[id] = weight; + return id; + } + + [[nodiscard]] double get_weight(const NodeID n1, const NodeID n2) const + { + return m_edges[graph.find_edge(n1, n2)]; + } + + /// Disconnect nodes. This operation removes the edge from the edge maps of the two formerly connected + /// vertices, but the number of edges reported by the super class AMGraph is not decremented, so the edge is only + /// invalidated. Call cleanup to finalize removal. + void disconnect_nodes(const NodeID n0, const NodeID n1) + { + //return graph.remove_edge(n0, n1); + //remove_edge(n0, n1); + // if (graph.valid_node_id(n0) && graph.valid_node_id(n1)) { + // graph.edge_map[n0].erase(n1); + // graph.edge_map[n1].erase(n0); + // } + graph.erase_edge(n0, n1); + } + + [[nodiscard]] + auto find_edge(NodeID from, NodeID to) const + { + return graph.find_edge(from, to); + } + + [[nodiscard]] + auto node_ids() const + { + return graph.node_ids(); + } + + [[nodiscard]] + auto no_nodes() const -> size_t + { + return graph.no_nodes(); + } + + [[nodiscard]] + auto no_edges() const -> size_t + { + return graph.no_edges(); + } + + [[nodiscard]] + auto neighbors_lazy(NodeID from) const + { + return graph.neighbors_lazy(from); + } + + auto collapse(NodeID to_keep, NodeID to_remove) + { + for (auto n : neighbors_lazy(to_remove)) { + double weight = get_weight(n, to_remove); + connect_nodes(to_keep, n, weight); + } + graph.erase_node(to_remove); + } +}; + +class RSGraph : private AMGraph { +public: + static constexpr auto InvalidEdgeID = std::nullopt; + + Geometry::ETF etf; + std::vector m_vertices; + std::vector> m_neighbors; + std::vector m_edges; + + void reserve(size_t nodes, int k) + { + m_vertices.reserve(nodes * k); + } + + [[nodiscard]] + auto no_nodes() const -> size_t + { + return AMGraph::no_nodes(); + } + + [[nodiscard]] + auto neighbors_lazy(NodeID n) const + { + return AMGraph::neighbors_lazy(n); + } + + [[nodiscard]] + auto shared_neighbors_lazy(NodeID n1, NodeID n2) const + { + return AMGraph::shared_neighbors_lazy(n1, n2); + } + + [[nodiscard]] + auto valence(NodeID n) const + { + return AMGraph::valence(n); + } + + EdgeID add_edge(const NodeID source, const NodeID target) + { + const EdgeID id = this->connect_nodes(source, target); + GEL_ASSERT_NEQ(id, AMGraph::InvalidEdgeID); + if (m_edges.size() == id) + m_edges.emplace_back(source, target); + else + m_edges[id] = Edge{.source = source, .target = target}; + + // insert neighbors + m_neighbors[source].emplace(Neighbor(m_vertices[source], m_vertices[target], target)); + m_neighbors[target].emplace(Neighbor(m_vertices[target], m_vertices[source], source)); + + //return { source, target }; + return id; + } + + NodeID add_node(const Vec3& p, const Vec3& in_normal = Vec3(0., 0., 0.)) + { + const NodeID n = AMGraph::add_node(); + GEL_ASSERT_EQ(m_vertices.size(), n); + m_vertices.emplace_back(Vertex::InvalidNormalRep, p, in_normal); + m_neighbors.emplace_back(); + //m_vertices[n] = Vertex{.id = n, .normal_rep = Vertex::InvalidNormalRep, .coords = p, .normal = in_normal }; + return n; + } + + void increment_ref_time(NodeID n1, NodeID n2) + { + auto edge = AMGraph::find_edge(n1, n2); + if (edge != AMGraph::InvalidEdgeID) { + m_edges[edge].ref_time += 1; + } + } + + [[nodiscard]] + std::optional find_edge(NodeID n1, NodeID n2) const + { + const EdgeID id = AMGraph::find_edge(n1, n2); + if (id == AMGraph::InvalidEdgeID) { + return std::nullopt; + } + return m_edges[id]; + } + + /// @brief Get last neighbor information + /// @param root: root vertex index + /// @param branch: current outgoing branch + /// @return reference to last neighbor struct + // TODO: const correctness + [[nodiscard]] + Neighbor& predecessor(const NodeID& root, const NodeID& branch) const + { + GEL_ASSERT(m_vertices.size() > root); + GEL_ASSERT(m_vertices.size() > branch); + auto& u = m_vertices.at(root); + auto& v = m_vertices.at(branch); + GEL_ASSERT(!m_neighbors[root].empty()); // we need at least one neighbor to return + Neighbor temp = {u, v, static_cast(branch)}; + auto iter = m_neighbors[root].lower_bound(temp); + if (iter == m_neighbors[root].begin()) iter = m_neighbors[root].end(); // Wrap around + //auto& f = iter->first; + //auto& s = iter->second; + //return {f, s}; + //return std::tie(f, s); + return const_cast(*(std::prev(iter))); + } + + /// @brief Get the next neighbor information + /// + /// @param root: root vertex index + /// @param branch: current outgoing branch + /// + /// @return reference to the next neighbor struct + [[nodiscard]] + const Neighbor& successor(const NodeID& root, const NodeID& branch) const + { + auto& u = m_vertices.at(root); + auto& v = m_vertices.at(branch); + GEL_ASSERT(!m_neighbors[root].empty()); // we need at least one neighbor to return + auto iter = m_neighbors[root].upper_bound(Neighbor(u, v, branch)); + if (iter == m_neighbors[root].end()) iter = m_neighbors[root].begin(); // Wrap around + //auto& f = iter->first; + //auto& s = iter->second; + //return {f, s}; + //return std::tie(f, s); + //return const_cast(*(iter)); + return (*iter); + } + +private: + /// @brief Get the neighbor information + /// @param root: root vertex index + /// @param branch: the outgoing branch + /// @return reference to the neighbor struct + Neighbor& get_neighbor_info(const NodeID root, const NodeID branch) + { + auto& u = this->m_vertices.at(root); + auto& v = this->m_vertices.at(branch); + auto iter = m_neighbors[root].lower_bound({u, v, static_cast(branch)}); + // TODO: tree_id does not invalidate ordered_neighbors, but it still has thread safety issues + //auto& f = iter->first; + //auto& s = iter->second; + //return {f, s}; + return const_cast(*(iter)); + } + +public: + void maintain_face_loop(const NodeID source, const NodeID target) + { + auto& this_v_tree = this->predecessor(source, target).tree_id; + auto& neighbor_tree = this->predecessor(target, source).tree_id; + + auto [fst, snd] = this->etf.insert(this_v_tree, neighbor_tree); + + get_neighbor_info(source, target).tree_id = fst; + get_neighbor_info(target, source).tree_id = snd; + } +}; + + +//using namespace detail; +using namespace Util::detail; + +using Geometry::estimateNormal; +using namespace ::HMesh; +//using namespace Util::Ranges; +using Util::AttribVec; +using HMesh::Manifold; + +using FaceType = std::array; + +struct FaceComparator { + constexpr bool operator()(const std::pair& left, + const std::pair& right) const + { + return (left.second) > (right.second); + } +}; + +using FacePriorityQueue = std::priority_queue< + std::pair, + std::vector>, + FaceComparator>; + +struct TEdge { + NodeID from; + NodeID to; +}; + +struct PEdgeLength { + TEdge edge; + float length; +}; + +struct FaceConnectionKey { + uint tree_id; + uint to_tree_id; + + constexpr bool operator==(const FaceConnectionKey& other) const noexcept + { + return tree_id == other.tree_id && to_tree_id == other.to_tree_id; + } +}; + +struct FaceConnectionKeyHasher { + std::size_t operator()(const FaceConnectionKey& self) const noexcept + { + constexpr std::hash hasher; + return (hasher(self.tree_id) << 1) ^ hasher(self.to_tree_id); + } +}; + +using FacePair = std::pair; +using NeighborPair = std::pair; + +constexpr bool edge_comparator(const PEdgeLength& l, const PEdgeLength& r) +{ + return l.length < r.length; +} + +constexpr bool face_comparator(const FacePair& l, const FacePair& r) +{ + return l.first > r.first; +} + +constexpr bool neighbor_comparator(const NeighborPair& l, const NeighborPair& r) +{ + return l.first > r.first; +} + +/// @brief Calculate the reference vector for the rotation system +/// @param normal: normal direction for the target vertex +/// @return the reference vector +constexpr Vec3 calculate_ref_vec(const Vec3& normal) +{ + constexpr double eps = 1e-6; + if (normal[2] == 1.0) + return Vec3(0.0, 1.0, 0.0);; + const double second = (normal[1] == 0.0) ? eps : normal[1]; + const auto ref_vec = Vec3(0.0, -normal[2] / second, 1.0); + return CGLA::normalize(ref_vec); +} + +/// @brief Calculate the radian given the reference vector +/// @param branch: vector of the outgoing edge +/// @param normal: normal of the root vertex +/// @param ref_vec: the reference vector +/// @return radian +double cal_radians_3d(const Vec3& branch, const Vec3& normal, const Vec3& ref_vec) +{ + const Vec3 proj_vec = branch - dot(normal, branch) * normal; + const double proj_vec_len = proj_vec.length(); + if (proj_vec_len < 1e-8) + return 0.; + + const Vec3 proj_ref = ref_vec - dot(normal, ref_vec) * normal; + const auto value = std::clamp( + dot(proj_vec, proj_ref) / proj_vec_len / + proj_ref.length(), -1.0, 1.0); + // We clamped value to [-1,1], so radian cannot be NaN unless normal is 0 + double radian = std::acos(value); + if (dot(cross(proj_vec, proj_ref), normal) > 0.0) + radian = 2.0 * M_PI - radian; + return radian; +} + +/// @brief Calculate the radian in the rotation system +/// @param branch: vector of the outgoing edge +/// @param normal: normal of the root vertex +/// @return radian +double cal_radians_3d(const Vec3& branch, const Vec3& normal) +{ + const auto ref_vec = calculate_ref_vec(normal); + return cal_radians_3d(branch, normal, ref_vec); +} + +/// @brief Calculate projection distance +/// @param edge: the edge to be considered +/// @param this_normal: normal of one vertex +/// @param neighbor_normal: normal of another vertex +/// @return projection distance +double cal_proj_dist(const Vec3& edge, const Vec3& this_normal, const Vec3& neighbor_normal) +{ + const double euclidean_distance = edge.length(); + if (std::abs(dot(this_normal, neighbor_normal)) < std::cos(15. / 180. * M_PI)) + return euclidean_distance; + const double neighbor_normal_length = dot(edge, neighbor_normal); + const double normal_length = dot(edge, this_normal); + const double projection_dist = (std::sqrt( + (euclidean_distance * euclidean_distance) - (normal_length * normal_length)) + + std::sqrt((euclidean_distance * euclidean_distance) - (neighbor_normal_length * neighbor_normal_length))) + * + 0.5; + return projection_dist; +} + +void filter_out_cross_connection( + NeighborArray& neighbors, + const std::vector& normals, + const NodeID this_idx, + const double cross_conn_thresh, + const bool is_euclidean) +{ + const auto& this_normal = normals[this_idx]; + std::erase_if(neighbors, [&](const auto& neighbor_info) { + const auto& neighbor_normal = normals[neighbor_info.id]; + const double cos_theta = dot(this_normal, neighbor_normal); + const double cos_thresh = + (is_euclidean) ? 0.0 : std::cos(cross_conn_thresh / 180. * M_PI); + return cos_theta < cos_thresh; + }); +} + +struct ConnectionLength { + /// the distance of the longest connection each vertex involved + double max_length = 0.0; + /// the maximum length of connection before connecting handles (conservative connection) + double pre_max_length = 0.0; +}; + +/// @brief initialize the graph and related information +/// @param vertices: vertices of the component +/// @param smoothed_v: smoothed vertices of the component +/// @param normals: normals of this component +/// @param neighbor_map +/// @param connection_lengths [OUT] Distance of each vertex's longest connection and the maximum length before connecting handles +/// @param k +/// @param is_euclidean +/// @return a light-weight graph with the essential connections for building MST +SimpGraph init_graph( + const std::vector& smoothed_v, + const std::vector& normals, + const NeighborMap& neighbor_map, + std::vector& connection_lengths, + const int k, + const double cross_connection_threshold, + const bool is_euclidean) +{ + SimpGraph dist_graph; + AMGraph::NodeSet sets; + dist_graph.reserve(smoothed_v.size(), k); + //GEL_ASSERT_EQ(vertices.size(), smoothed_v.size()); + GEL_ASSERT_EQ(smoothed_v.size(), normals.size()); + + for (int i = 0; i < smoothed_v.size(); i++) { + auto node = dist_graph.add_node(); + sets.insert(node); + } + + for (NodeID id_this = 0UL; id_this < smoothed_v.size(); ++id_this) { + const Point& vertex = smoothed_v[id_this]; + const Vec3& this_normal = normals[id_this]; + auto neighbors = neighbor_map[id_this]; + + connection_lengths[id_this].pre_max_length = neighbors[static_cast(double(k) * (2.0 / 3.0))].distance; + + filter_out_cross_connection(neighbors, normals, id_this, cross_connection_threshold, is_euclidean); + //GEL_ASSERT(neighbors.size() > 1); + for (const auto& neighbor : neighbors | std::views::drop(1)) { + const auto id_other = neighbor.id; + //if (id_other <= id_this) continue; + //GEL_ASSERT_NEQ(id_other, id_this, "Vertex connects back to its own"); + + const Vec3& neighbor_normal = normals[id_other]; + const Point& neighbor_pos = smoothed_v[id_other]; + const Vec3 edge = neighbor_pos - vertex; + const auto weight = + (is_euclidean) + ? (edge.length()) + : (cal_proj_dist(edge, this_normal, neighbor_normal)); + if (weight > connection_lengths[id_this].max_length) + connection_lengths[id_this].max_length = weight; + + if (weight > connection_lengths[id_other].max_length) + connection_lengths[id_other].max_length = weight; + + if (weight < 1e-8) [[unlikely]] + std::cerr << __func__ << " [ERROR] weight: " << weight << std::endl; + + // if (dist_graph.find_edge(id_this, id_other) != SimpGraph::InvalidEdgeID) + // continue; + + dist_graph.connect_nodes(id_this, id_other, weight); + } + } + + const auto components_vec = connected_components(dist_graph.inner(), sets); + GEL_ASSERT(components_vec.size() == 1, "%d", components_vec.size()); + return dist_graph; +} + +/// @brief Find the shortest path from one vertex to another in the graph +/// @param mst: the graph +/// @param start: the source vertex +/// @param target: the target vertex +/// @return number of steps in the shortest path +int find_shortest_path(const RSGraph& mst, const NodeID start, const NodeID target) +{ + std::queue q; + std::vector dist(mst.no_nodes(), -1); // Distance from the start to each node + UnorderedSet visited; + + dist[start] = 0; + q.push(start); + + while (!q.empty()) { + const auto u = q.front(); + q.pop(); + + // If the target node is reached, stop early + if (u == target) + break; + // If the node has already been visited, skip it + if (visited.contains(u)) + continue; + + visited.insert(u); + + // Explore neighbors + for (const auto& v : mst.neighbors_lazy(u)) { + if (dist[v] == -1) { + // If the node hasn't been visited + dist[v] = dist[u] + 1; // Increment distance + q.push(v); + } + } + } + + return dist[target]; +} + +/// @brief weighted smoothing method using defined neighborhood with tangential distance weighted +/// @param pool thread pool +/// @param vertices: vertices of the point cloud +/// @param normals: normal of the point cloud +/// @param neighbors_map +/// @param smoothed_v: [OUT] vertices after smoothing +void weighted_smooth( + IExecutor& pool, + const std::vector& vertices, + const std::vector& normals, + const NeighborMap& neighbors_map, + std::vector& smoothed_v) +{ + auto _debug = __func__; + auto lambda = [_debug, &normals, &vertices](const size_t idx, const Point& vertex, const NeighborArray& neighbors) { + const Vec3 normal = normals[idx]; + + double weight_sum = 0.; + double amp_sum = 0.; + double max_dist = 0.; + + struct LengthWeight { + double vert_length = 0.0; + double weight = 0.0; + }; + InplaceVector length_weights; + const std::intptr_t limit = (neighbors.size() < 192) ? static_cast(neighbors.size()) : 192; + length_weights.reserve(limit); + for (const auto& neighbor : neighbors | std::views::take(192)) { + const Point neighbor_pos = vertices[neighbor.id]; + const Vec3 n2this = neighbor_pos - vertex; + if (dot(normals[neighbor.id], normal) < std::cos(30. / 180. * M_PI)) { + continue; + } + const double vertical = CGLA::dot(n2this, normal); + const double n_dist = (neighbor_pos - vertex).length(); + + const double tangential_square = n_dist * n_dist - + vertical * vertical; + const double tangential_dist = (tangential_square > 0.) ? std::sqrt(tangential_square) : 0.0; + + if (!std::isfinite(tangential_dist)) [[unlikely]] { + std::cerr << n_dist << " " << vertical << std::endl; + std::cerr << _debug << ": error" << std::endl; + } + + const double weight = -tangential_dist; + if (tangential_dist > max_dist) + max_dist = tangential_dist; + + length_weights.emplace_back(vertical, weight); + weight_sum += weight; + } + for (const auto& length_weight : length_weights) { + amp_sum += length_weight.vert_length * (length_weight.weight + max_dist); + } + weight_sum += (max_dist * static_cast(length_weights.size())); + + weight_sum = (weight_sum == 0.) ? 1. : weight_sum; + + amp_sum /= weight_sum; + if (!std::isfinite(amp_sum)) [[unlikely]] + std::cout << _debug << ": error" << std::endl; + const Vec3 move = amp_sum * normal; + return vertex + move; + }; + Parallel::enumerate_map2(pool, vertices, neighbors_map, smoothed_v, lambda); +} + +auto normalize_normals(std::vector& normals) -> void +{ + for (auto& normal : normals) { + normal.normalize(); + } +} + +void estimate_normal_no_normals_memoized( + IExecutor& pool, + const std::vector& vertices, + const NeighborMap& neighbors, + std::vector& normals) +{ + normals.clear(); + // Data type transfer & Cal diagonal size + auto _debug = __func__; + auto lambda = [&](const NeighborArray& neighbors_of_this) { + // need id, distance and coords anyway + + auto neighbor_coords = neighbors_of_this | std::views::transform([&](const auto& neighbor) { + return vertices[neighbor.id]; + }); + // FIXME: the radius parameter might be needed in the future + const Vec3 normal = estimateNormal(neighbor_coords, 0.0); + + if (std::isnan(normal.length())) [[unlikely]] { + std::cerr << _debug << ": error" << std::endl; + std::cerr << neighbors_of_this.size() << std::endl; + } + return normal; + }; + Parallel::map(pool, neighbors, normals, lambda); +} + +/// @brief Calculate cos angle weight for correcting normal orientation +/// @param this_normal: normal of current vertex +/// @param neighbor_normal: normal of its neighbor vertex +/// @return angle weight calculated +double cal_angle_based_weight(const Vec3& this_normal, const Vec3& neighbor_normal) +{ + const double dot_pdt = std::abs( + dot(this_normal, neighbor_normal) / (this_normal.length() * neighbor_normal.length())); + const double dot_pdt_clamped = std::clamp(dot_pdt, 0., 1.0); + return 1.0 - dot_pdt_clamped; +} + +/// Generic template for creating an MST +template */ typename TargetGraph, + /*std::derived_from*/ typename SourceGraph, + std::invocable NodeInserter, + // TargetGraph -> NodeID -> () + std::invocable DistanceFunc, + // SourceGraph -> NodeID -> NodeID -> double + std::invocable EdgeInserterFunc> +// TargetGraph -> NodeID -> NodeID -> () +TargetGraph make_mst( + const SourceGraph& g, + NodeID root, + NodeInserter&& node_inserter, + DistanceFunc&& distance_function, + EdgeInserterFunc&& edge_insertion) +{ + GEL_ASSERT_NEQ(root, AMGraph::InvalidNodeID); + TargetGraph gn; + struct QElem { + double distance; + NodeID from; + NodeID to; + + bool operator>(const QElem& rhs) + { + return distance > rhs.distance; + } + }; + struct Bool { + bool inner; + }; + for (auto n : g.node_ids()) + node_inserter(gn, n); + std::vector in_tree(gn.no_nodes(), Bool(false)); + in_tree[root].inner = true; + + std::priority_queue, std::greater<>> queue; + for (auto neighbor : g.neighbors_lazy(root)) { + const auto distance = distance_function(g, neighbor, root); + queue.emplace(distance, root, neighbor); + } + + while (!queue.empty()) { + auto [distance, id1, id2] = queue.top(); + queue.pop(); + + if (!in_tree[id2].inner) { + in_tree[id2].inner = true; + + edge_insertion(gn, id1, id2); + + GEL_ASSERT(std::ranges::distance(g.neighbors_lazy(id2)) > 0); + // FIXME: no idea why MSVC is dying at this + auto filtered_neighbors = g.neighbors_lazy(id2) + | std::views::filter([&](auto nb) { return !in_tree[nb].inner; }); + for (auto neighbor : filtered_neighbors) { + const auto distance2 = distance_function(g, neighbor, id2); + queue.emplace(distance2, id2, neighbor); + } + } + } + return gn; +} + +RSGraph minimum_spanning_tree( + const SimpGraph& g, + const NodeID root, + const std::vector& normals, + const std::vector& vertices, + const bool is_euclidean) +{ + GEL_ASSERT_NEQ(root, AMGraph::InvalidNodeID); + auto gn = make_mst( + g, root, + [&](RSGraph& graph, NodeID n) { graph.add_node(vertices[n], normals[n]); }, + [&](const SimpGraph& graph, NodeID n, NodeID m) { + return CGLA::sqr_length(vertices[m] - vertices[n]); + }, + [&](RSGraph& graph, NodeID id1, NodeID id2) { + graph.add_edge(id1, id2); + } + ); + // by the definition of an mst + GEL_ASSERT_EQ(gn.m_vertices.size(), gn.m_edges.size() + 1); + return gn; +} + +SimpGraph minimum_spanning_tree(const SimpGraph& g, NodeID root) +{ + auto gn = make_mst(g, root, + [](SimpGraph& gn, NodeID n) { gn.add_node(); }, + [](const SimpGraph& g, NodeID n, NodeID m) { return g.get_weight(n, m); }, + [](SimpGraph& gn, NodeID n, NodeID m) { gn.connect_nodes(n, m); }); + return gn; +} + +/// @brief Determine the normal orientation +/// @param pool Thread pool +/// @param kdTree +/// @param in_smoothed_v +/// @param normals: [OUT] normal of the point cloud with orientation corrected +/// @param k +void correct_normal_orientation( + IExecutor& pool, + const Tree& kdTree, + const std::vector& in_smoothed_v, + std::vector& normals, + const int k) +{ + /// The graph has the angles as weights + const auto [g_angle, sets] = [&] { + SimpGraph g_angle_temp; + AMGraph::NodeSet sets_temp; + // sets_temp.reserve(in_smoothed_v.size()); // TODO: flat_hash_set can be reserved + + for (int i = 0; i < in_smoothed_v.size(); i++) { + sets_temp.insert(g_angle_temp.add_node()); + } + + const auto all_neighbors = calculate_neighbors(pool, in_smoothed_v, kdTree, k); + + // Init angle based graph + for (int i = 0; i < in_smoothed_v.size(); i++) { + const auto& neighbors = all_neighbors[i]; + const auto& this_normal = normals[i]; + + for (const auto neighbor : neighbors + | std::views::drop(1) + | std::views::filter([&i, &g_angle_temp](auto&& nb) { + return g_angle_temp.find_edge(i, nb.id) == AMGraph::InvalidEdgeID; + }) + ) { + //if (g_angle_temp.find_edge(i, neighbor.id) != AMGraph::InvalidEdgeID) + // continue; + const auto& neighbor_normal = normals[neighbor.id]; + const double angle_weight = cal_angle_based_weight(this_normal, neighbor_normal); + + g_angle_temp.connect_nodes(i, neighbor.id, angle_weight); + } + } + + return std::make_tuple(g_angle_temp, sets_temp); + }(); + + const auto components_vec = connected_components(g_angle.inner(), sets); + + // The number of components and their relative sizes is inconsistent, don't parallelize this + for (const auto& i : components_vec) { + NodeID root = *i.begin(); + SimpGraph mst_angle = minimum_spanning_tree(g_angle, root); + + /// A trivial wrapper over bool to avoid the std::vector specialization + struct Boolean { + bool inner; + }; + auto visited_vertex = std::vector(g_angle.no_nodes(), Boolean{false}); + + // This uses the MST to visit every node + // Start from the root + std::queue to_visit; + to_visit.push(root); + while (!to_visit.empty()) { + const NodeID node_id = to_visit.front(); + to_visit.pop(); + + visited_vertex[node_id].inner = true; + const Vec3 this_normal = normals[node_id]; + for (auto vd : mst_angle.neighbors_lazy(node_id)) { + if (!visited_vertex[vd].inner) { + to_visit.push(vd); + const Vec3 neighbor_normal = normals[vd]; + if (dot(this_normal, neighbor_normal) < 0) { + normals[vd] = -normals[vd]; + } + } + } + } + } +} + +void init_face_loop_label(RSGraph& g) +{ + constexpr NodeID start_v = 0; + NodeID last_vertex = start_v; + auto loop_step = 0UL; + // TODO: unsafe access + NodeID current_vertex = g.m_neighbors[start_v].begin()->v; + do { + auto& next_neighbor = g.predecessor(current_vertex, last_vertex); + + next_neighbor.tree_id = g.etf.accumulate(); + + last_vertex = current_vertex; + current_vertex = next_neighbor.v; + + loop_step++; + } while (current_vertex != g.m_neighbors[start_v].begin()->v || last_vertex != start_v); + + std::cout << "Loop step initialization finished after " + std::to_string(loop_step) + " steps." << std::endl; +} + +/// @brief Project a vector to a plane +/// @param input: Vector to be projected +/// @param normal: normal to the plane +/// @return projected Vector +Vec3 projected_vector(const Vec3& input, const Vec3& normal) +{ + const auto normal_normed = CGLA::normalize(normal); + return input - CGLA::dot(input, normal_normed) * normal_normed; + //return input - dot(input, normal) * normal; +} + +/// @brief Check if two segments are intersecting on both planes (defined by their normal) they belong +/// @param mst: graph and vertex information +/// @param v1: 1st vertex of segment 1 +/// @param v2: 2nd vertex of segment 1 +/// @param v3: 1st vertex of segment 2 +/// @param v4: 2nd vertex of segment 2 +/// @return if they are intersecting with each other +bool is_intersecting(const RSGraph& mst, const NodeID v1, const NodeID v2, const NodeID v3, const NodeID v4) +{ + const auto& p1 = mst.m_vertices[v1].coords; + const auto& p2 = mst.m_vertices[v2].coords; + const auto& n1 = mst.m_vertices[v1].normal; + const auto& n2 = mst.m_vertices[v2].normal; + const Vec3 normal_12 = (n1 + n2) / 2.; + + const auto& p3 = mst.m_vertices[v3].coords; + const auto& p4 = mst.m_vertices[v4].coords; + const auto& n3 = mst.m_vertices[v3].normal; + const auto& n4 = mst.m_vertices[v4].normal; + const Vec3 normal_34 = (n3 + n4) / 2.; + + auto check_intersection = [](auto&& p1, auto&& p2, auto&& p3, auto&& p4, auto&& normal) -> bool { + const Vec3 midpoint = p1 + (p2 - p1) / 2.0; + const Vec3 edge1 = p1 - midpoint; + const Vec3 edge2 = p3 - midpoint; + const Vec3 edge3 = p4 - midpoint; + const Vec3 proj_edge1 = projected_vector(edge1, normal); + const Vec3 proj_edge2 = projected_vector(edge2, normal); + const Vec3 proj_edge3 = projected_vector(edge3, normal); + const Vec3 pro1 = CGLA::cross(proj_edge2, proj_edge1); + const Vec3 pro2 = CGLA::cross(proj_edge3, proj_edge1); + return CGLA::dot(pro1, pro2) <= 0; + }; + + // On the plane of edge 12 + if (check_intersection(p1, p2, p3, p4, normal_12) && + check_intersection(p3, p4, p1, p2, normal_12)) + return true; + + // On the plane of edge 34 + if (check_intersection(p1, p2, p3, p4, normal_34) && + check_intersection(p3, p4, p1, p2, normal_34)) + return true; + + return false; +} + +/// @brief Geometry check for connection +/// @param mst: graph and vertex information +/// @param candidate: the edge to be checked +/// @param kd_tree: kd-tree for knn query +/// @return if the candidate pass the check +bool geometry_check( + const RSGraph& mst, + const TEdge& candidate, + const Tree& kd_tree, + std::vector>& neighbors) +{ + const NodeID& v1 = candidate.from; + const NodeID& v2 = candidate.to; + const Point& p1 = mst.m_vertices[v1].coords; + const Point& p2 = mst.m_vertices[v2].coords; + const Vec3& n1 = mst.m_vertices[v1].normal; + const Vec3& n2 = mst.m_vertices[v2].normal; + + const Vec3 mean_normal = (n1 + n2) * 0.5; + + const Point search_center = (p1 + p2) * 0.5; + const double radius = (p2 - p1).length() * 0.5; + + neighbors.clear(); + kd_tree.in_sphere(search_center, radius * 3.0, neighbors); + + auto in_rejection_set = [&](NodeID neighbor) -> bool { + return + (neighbor != v1 && + neighbor != v2 && + CGLA::dot(mst.m_vertices[neighbor].normal, mean_normal) > 0.5); // std::cos(60. / 180. * M_PI)); + }; + for (const auto neighbor : neighbors + | std::views::values + | std::views::filter(in_rejection_set)) { + if (std::ranges::any_of( + mst.m_neighbors[neighbor] | + std::views::transform([](auto&& x) { return x.v; }) | + std::views::filter(in_rejection_set), [&](const auto& rej_neighbor_neighbor) { + return is_intersecting(mst, v1, v2, neighbor, rej_neighbor_neighbor); + } + )) + return false; + } + return true; +} + +bool geometry_check( + const RSGraph& mst, + const TEdge& candidate, + const Tree& kd_tree) +{ + std::vector> neighbors; + return geometry_check(mst, candidate, kd_tree, neighbors); +} + +bool topology_check(const RSGraph& mst, const TEdge& candidate) +{ + const auto [this_v, neighbor] = candidate; + + // Topology check + const auto this_v_tree = mst.predecessor(this_v, neighbor).tree_id; + const auto neighbor_tree = mst.predecessor(neighbor, this_v).tree_id; + + if (!mst.etf.connected(this_v_tree, neighbor_tree)) { + return false; + } else { + return true; + } +} + +bool vanilla_check( + const RSGraph& mst, + const TEdge& candidate, + const Tree& kd_tree, + std::vector>& neighbors) +{ + const auto [this_v, neighbor] = candidate; + + // Topology check + const auto this_v_tree = mst.predecessor(this_v, neighbor).tree_id; + const auto neighbor_tree = mst.predecessor(neighbor, this_v).tree_id; + + if (!mst.etf.connected(this_v_tree, neighbor_tree)) { + return false; + } + + return geometry_check(mst, candidate, kd_tree, neighbors); +} + +bool routine_check(const RSGraph& mst, const FaceType& triangle) +{ + const Point& p1 = mst.m_vertices[triangle[0]].coords; + const Point& p2 = mst.m_vertices[triangle[1]].coords; + const Point& p3 = mst.m_vertices[triangle[2]].coords; + const auto len_ui = (p1 - p2).length(); + const auto len_wi = (p3 - p2).length(); + const auto len_uw = (p1 - p3).length(); + + auto max_value = std::acos(std::clamp( + dot((p3 - p2), (p1 - p2)) / len_ui / + len_wi, -1, 1)); + auto radian = std::acos(std::clamp( + dot((p2 - p1), (p3 - p1)) / len_ui / + len_uw, -1, 1)); + if (radian > max_value) + max_value = radian; + radian = std::acos(std::clamp( + dot((p1 - p3), (p2 - p3)) / len_uw / + len_wi, -1, 1)); + if (radian > max_value) + max_value = radian; + + if (max_value > 175. / 180. * M_PI || + mst.find_edge(triangle[0], triangle[2]).value().ref_time == 2 || + mst.find_edge(triangle[1], triangle[2]).value().ref_time == 2) + return true; + else + return false; +} + +void add_face(RSGraph& graph, const FaceType& item, + std::vector& faces) +{ + const auto& v_i = item[0]; + const auto& v_u = item[1]; + const auto& v_w = item[2]; + + graph.increment_ref_time(v_i, v_w); + graph.increment_ref_time(v_u, v_i); + graph.increment_ref_time(v_w, v_u); + faces.push_back(v_i); + faces.push_back(v_u); + faces.push_back(v_w); +} + +struct RegisterFaceResult { + InplaceVector faces; +}; + +auto register_face(const RSGraph& mst, const NodeID v1, const NodeID v2) -> std::optional +{ + GEL_ASSERT_EQ(mst.find_edge(v1, v2), RSGraph::InvalidEdgeID); + auto shared_neighbors = mst.shared_neighbors_lazy(v1, v2); + if (shared_neighbors.empty()) { + return RegisterFaceResult(); // true + } + + const Point& p1 = mst.m_vertices[v1].coords; + const Point& p2 = mst.m_vertices[v2].coords; + + const auto possible_root1 = mst.predecessor(v1, v2).v; + const auto angle1 = cal_radians_3d(p1 - mst.m_vertices[possible_root1].coords, + mst.m_vertices[possible_root1].normal, + p2 - mst.m_vertices[possible_root1].coords); + const auto possible_root2 = mst.predecessor(v2, v1).v; + const auto angle2 = cal_radians_3d(p2 - mst.m_vertices[possible_root2].coords, + mst.m_vertices[possible_root2].normal, + p1 - mst.m_vertices[possible_root2].coords); + + InplaceVector temp; + for (const auto v3 : shared_neighbors) { + FaceType triangle{v1, v2, v3}; + if (v3 == possible_root1 && angle1 < M_PI) { + if (routine_check(mst, triangle) || + mst.successor(v2, v1).v != v3 || + mst.successor(possible_root1, v2).v != v1) { + return std::nullopt; // false + } + temp.emplace_back(FaceType{v1, v3, v2}); + } + + if (v3 == possible_root2 && angle2 < M_PI) { + if (routine_check(mst, triangle) || + mst.successor(v1, v2).v != v3 || + mst.successor(possible_root2, v1).v != v2) { + return std::nullopt; // false + } + temp.emplace_back(FaceType{v1, v2, v3}); + } + } + + if (temp.empty()) + return std::nullopt; // false + + return RegisterFaceResult{ + temp, + }; // true +} + +/// @brief Connect handle to raise the genus number +/// @param smoothed_v: smoothed vertices of the point cloud +/// @param mst: graph and vertex information +/// @param kd_tree: kd-tree for knn query +/// @param connected_handle_root: [OUT] log the connected handles +/// @param k: number of kNN search +/// @param isEuclidean: if to use Euclidean distance +/// @param expected_genus expected genus of the mesh +/// @param step_thresh: step threshold for shortest distance path early stop +void connect_handle( + const std::vector& smoothed_v, + const Tree& kd_tree, + RSGraph& mst, + std::vector& connected_handle_root, + const int k, + const int step_thresh, + const bool isEuclidean, + const int expected_genus) +{ + std::vector imp_node; + size_t num = 0; + size_t edge_num = 0; + // Collect vertices w/ an open angle larger than pi + for (int i = 0; i < mst.no_nodes(); i++) { + const auto& neighbors = mst.m_neighbors[i]; + // FIXME: Clean this up + float last_angle = (*(--neighbors.end())).angle; + float this_angle; + + for (auto& neighbor : neighbors) { + this_angle = neighbor.angle; + float angle_diff = this_angle - last_angle; + if (angle_diff < 0) + angle_diff += 2 * M_PI; + if (angle_diff > M_PI) + imp_node.push_back(i); + last_angle = this_angle; + } + //for (const auto [neighbor_old, neighbor_next] : slide2_wraparound(neighbors)) { + // auto last_angle = neighbor_old.angle; + // auto this_angle = neighbor_next.angle; + // auto angle_diff = this_angle - last_angle; + // if (angle_diff < 0) + // angle_diff += 2 * M_PI; + // if (angle_diff > M_PI) + // imp_node.push_back(i); + //} + } + + struct Connection { + NodeID p_from; + NodeID p_to; + uint tree_from; + uint tree_to; + }; + std::vector connect; + + ThreadPool pool; + const auto neighbors_map = calculate_neighbors(pool, smoothed_v, imp_node, kd_tree, k); + + GEL_ASSERT_EQ(imp_node.size(), neighbors_map.size()); + for (auto i = 0UL; i < imp_node.size(); i++) { + const auto& this_v = imp_node[i]; + const auto& neighbors = neighbors_map[i]; + + // Potential handle collection + for (auto& neighbor : neighbors | std::views::drop(1)) { + if (mst.find_edge(this_v, neighbor.id) != RSGraph::InvalidEdgeID) + continue; + const auto tree = mst.etf.representative(mst.predecessor(this_v, neighbor.id).tree_id); + const auto to_tree = mst.etf.representative(mst.predecessor(neighbor.id, this_v).tree_id); + const TEdge candidate(this_v, neighbor.id); + if (tree != to_tree && geometry_check(mst, candidate, kd_tree)) { + // TODO: Check if any tree shares root, and return corresponding edges + connect.emplace_back(this_v, neighbor.id, tree, to_tree); + break; + } + } + } + std::cout << "connections: " << connect.size() << "\n"; + + // Select one handle + Map, FaceConnectionKeyHasher> face_connections; + for (int i = 0; i < connect.size(); i++) { + uint tree = connect[i].tree_from; //tree_id[i]; + uint to_tree = connect[i].tree_to; //to_tree_id[i]; + if (to_tree > tree) + std::swap(tree, to_tree); + const auto key = FaceConnectionKey{tree, to_tree}; + if (!face_connections.contains(key)) + face_connections[key] = std::vector{i}; + else { + face_connections[key].push_back(i); + } + } + + // Sort + std::vector sorted_face; + for (const auto& key : face_connections | std::views::keys) { + auto length = face_connections[key].size(); + sorted_face.emplace_back(length, key); + } + std::ranges::sort(sorted_face, face_comparator); + for (const auto& val : sorted_face | std::views::values) { + const std::vector& idx_vec = face_connections[val]; + if (idx_vec.size() <= 5 || (expected_genus >= 0 && num >= expected_genus)) + break; + + bool isFind = false; + for (const int idx : idx_vec) { + const NodeID& this_v = connect[idx].p_from; + const Point& query = mst.m_vertices[this_v].coords; + const NodeID& connected_neighbor = connect[idx].p_to; + // TODO: path and step_thresh are both dead + const int steps = find_shortest_path(mst, this_v, connected_neighbor); + if (steps > step_thresh) { + isFind = true; + const auto candidate = TEdge(this_v, connected_neighbor); + if (geometry_check(mst, candidate, kd_tree)) { + mst.add_edge(this_v, connected_neighbor); + connected_handle_root.push_back(this_v); + connected_handle_root.push_back(connected_neighbor); + + edge_num++; + } + } + } + if (isFind) { + num++; + } + } + + std::cout << "Handle Connection done :)" << std::endl; + std::cout << std::to_string(num) << " pairs of faces are connected." << std::endl; + std::cout << std::to_string(edge_num) << " edges are connected." << std::endl; +} + +bool explore( + const RSGraph& G, + const NodeID i, + FacePriorityQueue& queue, + const std::vector& length_thresh, + const bool is_euclidean) +{ + const NodeID v_i = i; + bool isFound = false; + for (const auto& neighbor : G.m_neighbors[i]) { + const NodeID v_u = neighbor.v; + const NodeID v_w = G.successor(i, v_u).v; + + const Point& w_pos = G.m_vertices[v_w].coords; + const Point& u_pos = G.m_vertices[v_u].coords; + const Point& i_pos = G.m_vertices[v_i].coords; + const Vec3& i_normal = G.m_vertices[v_i].normal; + const Vec3& u_normal = G.m_vertices[v_u].normal; + const Vec3& w_normal = G.m_vertices[v_w].normal; + const auto angle = cal_radians_3d(w_pos - i_pos, i_normal, + u_pos - i_pos); + const bool isLargerThanPi = angle < M_PI; + const FaceType face_vector{v_i, v_u, v_w}; + if (v_u != v_w && + isLargerThanPi && + G.find_edge(v_u, v_w) == RSGraph::InvalidEdgeID) { + const auto score = (is_euclidean) + ? (G.m_vertices[v_u].coords - G.m_vertices[v_w].coords).length() + : cal_proj_dist(G.m_vertices[v_u].coords - G.m_vertices[v_w].coords, + u_normal, w_normal); + if (score > length_thresh[v_u].max_length || score > length_thresh[v_w].max_length) + continue; + else if (score >= 0) { + queue.emplace(face_vector, score); + isFound = true; + } + } + } + + return isFound; +} + +bool check_branch_validity(const RSGraph& G, const NodeID root, const NodeID branch1, const NodeID branch2) +{ + constexpr auto angle_thresh = 0. / 180. * M_PI; + + const Point& pos_u = G.m_vertices[branch1].coords; + const Point& pos_w = G.m_vertices[branch2].coords; + const Vec3& normal_u = G.m_vertices[branch1].normal; + const Vec3& normal_w = G.m_vertices[branch2].normal; + + const auto is_valid = [&root](auto this_radian, auto former, auto next) { + if (next.v == root) { + auto diff = next.angle - this_radian; + if (diff < 0) + diff += 2 * M_PI; + if (diff < M_PI) + return true; + } + if (former.v == root) { + auto diff = -former.angle + this_radian; + if (diff < 0) + diff += 2 * M_PI; + if (diff < M_PI) + return true; + } + return false; + }; + + // Check u's RS validity + { + const auto this_radian = cal_radians_3d(pos_w - pos_u, normal_u); + const auto& former = G.predecessor(branch1, branch2); + const auto& next = G.successor(branch1, branch2); + if (!is_valid(this_radian, former, next)) + return false; + + // Thresh on angle + auto diff_angle_thresh = this_radian - former.angle; + if (diff_angle_thresh < 0) + diff_angle_thresh += M_PI * 2.; + if (diff_angle_thresh < angle_thresh) + return false; + } + + // Check w's RS validity + { + const auto this_radian = cal_radians_3d(pos_u - pos_w, normal_w); + const auto& former = G.predecessor(branch2, branch1); + const auto& next = G.successor(branch2, branch1); + if (!is_valid(this_radian, former, next)) + return false; + + // Thresh on angle + if (const auto diff_angle_thresh2 = -this_radian + next.angle + 2. * M_PI; + diff_angle_thresh2 < angle_thresh) + return false; + } + + return true; +} + + +// TODO: validity of what? +bool check_validity( + const RSGraph& graph, + const std::pair& item) +{ + const NodeID v_i = item.first[0]; + const NodeID v_u = item.first[1]; + const NodeID v_w = item.first[2]; + const Point& pos_i = graph.m_vertices[v_i].coords; + const Point& pos_u = graph.m_vertices[v_u].coords; + const Point& pos_w = graph.m_vertices[v_w].coords; + const Vec3& normal_i = graph.m_vertices[v_i].normal; + + if (graph.find_edge(v_u, v_w) != RSGraph::InvalidEdgeID) + return false; + + // Non-manifold edge check + if (graph.find_edge(v_i, v_u).value().ref_time == 2 || + graph.find_edge(v_i, v_w).value().ref_time == 2) + return false; + + // Check this rotation system + if (graph.successor(v_i, v_u).v != v_w) + return false; + const auto angle = cal_radians_3d(pos_w - pos_i, normal_i, pos_u - pos_i); + if (angle > M_PI) + return false; + + // Check the rotation system's validity of branch nodes + if (!check_branch_validity(graph, v_i, v_u, v_w)) { + return false; + } + + return true; +} + +void triangulate( + std::vector& faces, + RSGraph& graph, + const bool is_euclidean, + const std::vector& length_thresh, + const std::vector& connected_handle_root) +{ + // Since we are clearing every element here, a TreeSet has much better performance than a Swiss table + OrderedSet to_visit; + FacePriorityQueue queue; + + // Init priority queue + for (auto i = 0UL; i < graph.no_nodes(); i++) { + to_visit.insert(i); + } + + for (const auto i : connected_handle_root) { + explore(graph, i, queue, length_thresh, is_euclidean); + to_visit.erase(i); + } + + std::cout << "Global init done :)" << std::endl; + + while (!to_visit.empty()) { + while (!queue.empty()) { + std::pair item = queue.top(); + queue.pop(); + + if (item.second >= 0.0 && !check_validity(graph, item)) { + continue; + } + + // Add the edge + const NodeID v_i = item.first[0]; + const NodeID v_u = item.first[1]; + const NodeID v_w = item.first[2]; + + if (graph.find_edge(v_u, v_w) == RSGraph::InvalidEdgeID) { + graph.add_edge(v_u, v_w); + add_face(graph, item.first, faces); + } else { + continue; + } + + // Deal with incident triangles + for (const NodeID incident_root : graph.shared_neighbors_lazy(v_u, v_w)) { + if (incident_root == v_i) + continue; + std::array face{incident_root, v_w, v_u}; + + // Non-manifold edge check + if (graph.find_edge(incident_root, v_u).value().ref_time == 2 || + graph.find_edge(incident_root, v_w).value().ref_time == 2 || + graph.find_edge(v_u, v_w).value().ref_time == 2) + continue; + + add_face(graph, face, faces); + } + + to_visit.erase(v_u); + to_visit.erase(v_w); + + // Explore and sanity check + explore(graph, v_u, queue, length_thresh, is_euclidean); + explore(graph, v_w, queue, length_thresh, is_euclidean); + } + + if (!to_visit.empty()) { + NodeID pick = *to_visit.begin(); + to_visit.erase(pick); + explore(graph, pick, queue, length_thresh, is_euclidean); + } + } +} + +/// @brief Build minimum spanning tree (MST) +/// @param out_mst: [OUT] constructed MST +/// @param g: connection information of the mst +/// @param root root node +/// @param is_euclidean: if to use Euclidean distance +/// @param vertices: coordinates of the point cloud +/// @param normals: normal of the point cloud +void build_mst( + const SimpGraph& g, + const NodeID root, + RSGraph& out_mst, + const std::vector& normals, + const std::vector& vertices, + const bool is_euclidean) +{ + RSGraph temp = minimum_spanning_tree(g, root, normals, vertices, is_euclidean); + + // Fix strong ambiguous points + if (!is_euclidean) { + for (int i = 0; i < temp.m_edges.size(); i++) { + const NodeID& source = temp.m_edges[i].source; + const NodeID& target = temp.m_edges[i].target; + const Vec3& normal1 = temp.m_vertices[source].normal; + const Vec3& normal2 = temp.m_vertices[target].normal; + const Point& pos1 = temp.m_vertices[source].coords; + const Point& pos2 = temp.m_vertices[target].coords; + if (temp.valence(source) >= 2 && temp.valence(target) >= 2) + continue; + const Vec3 edge = pos2 - pos1; + + const Vec3 normal_sum = (normal1 + normal2) * 0.5; + const auto cos_angle = std::abs(dot(edge, normal_sum / edge.length())); + if (cos_angle > std::cos(10. / 180. * M_PI)) { + NodeID parent; + if (temp.valence(source) == 1) { + temp.m_vertices[source].normal = temp.m_vertices[target].normal; + parent = target; + } else { + temp.m_vertices[target].normal = temp.m_vertices[source].normal; + parent = source; + } + + for (const auto neighbor : temp.neighbors_lazy(parent)) { + if (temp.m_vertices[neighbor].normal_rep == Vertex::InvalidNormalRep) { + // this conversion is fine since we need 10^18 vertices for it to matter + temp.m_vertices[neighbor].normal_rep = parent; + } else { + // Collision! + temp.m_vertices[neighbor].normal_rep = Vertex::CollisionRep; + } + } + } + } + for (int i = 0; i < temp.m_vertices.size(); i++) { + if (temp.m_vertices[i].normal_rep >= 0) + temp.m_vertices[i].normal = temp.m_vertices[temp.m_vertices[i].normal_rep].normal; + } + } + + // Build corrected MST + size_t verts = 0; + for (const auto& vertex : temp.m_vertices) { + out_mst.add_node(vertex.coords, vertex.normal); + verts++; + } + + size_t edges = 0; + for (const auto& m_edge : temp.m_edges) { + out_mst.add_edge(m_edge.source, m_edge.target); + edges++; + } + GEL_ASSERT_EQ(verts, edges + 1); +} + + +auto estimate_normals_included_normals( + const std::vector& vertices, + std::vector& normals, + const Distance dist) -> std::vector +{ + GEL_ASSERT_EQ(vertices.size(), normals.size()); + ThreadPool pool; + std::vector smoothed_v; + std::vector temp; + const int smoothing_size = std::max(static_cast(static_cast(vertices.size()) / 2000.), 192); + + normalize_normals(normals); + if (dist == Distance::Euclidean) { + smoothed_v = vertices; + } else if (dist == Distance::Tangent) { + const auto indices = std::ranges::iota_view(0UL, vertices.size()); + const auto kdTree = build_kd_tree_of_indices(vertices, indices); + const auto neighbors = + calculate_neighbors(pool, vertices, kdTree, smoothing_size); + weighted_smooth(pool, vertices, normals, neighbors, smoothed_v); + + + temp.reserve(smoothed_v.size()); + std::swap(temp, smoothed_v); + smoothed_v.clear(); + + weighted_smooth(pool, temp, normals, neighbors, smoothed_v); + } else { + GEL_ASSERT(false, "unreachable"); + } + GEL_ASSERT_EQ(vertices.size(), normals.size()); + return smoothed_v; +} + +auto estimate_normals_no_normals( + const std::vector& vertices, + std::vector& normals, + const Distance dist) -> std::vector +{ + GEL_ASSERT_EQ(normals.size(), 0LU); + ThreadPool pool; + const int smoothing_size = std::max(static_cast(static_cast(vertices.size()) / 2000.), 192); + + const auto indices = std::ranges::iota_view(0UL, vertices.size()); + const auto kdTree = build_kd_tree_of_indices(vertices, indices); + auto neighbors = calculate_neighbors(pool, vertices, kdTree, smoothing_size); + + estimate_normal_no_normals_memoized(pool, vertices, neighbors, normals); + + std::vector smoothed_v; + + switch (dist) { + case Distance::Euclidean: + smoothed_v = vertices; + break; + case Distance::Tangent: + std::cout << "Smoothing round 1 ..." << std::endl; + weighted_smooth(pool, vertices, normals, neighbors, smoothed_v); + break; + default: + GEL_ASSERT(false, "unreachable"); + } + + const auto temp_tree1 = build_kd_tree_of_indices(smoothed_v, indices); + neighbors = calculate_neighbors(pool, smoothed_v, temp_tree1, smoothing_size); + estimate_normal_no_normals_memoized(pool, smoothed_v, neighbors, normals); + + if (dist == Distance::Tangent) { + std::vector temp; + temp.reserve(smoothed_v.size()); + std::swap(temp, smoothed_v); + smoothed_v.clear(); + std::cout << "Smoothing round 2 ..." << std::endl; + weighted_smooth(pool, temp, normals, neighbors, smoothed_v); + + const Tree temp_tree2 = build_kd_tree_of_indices(smoothed_v, indices); + neighbors = calculate_neighbors(pool, smoothed_v, temp_tree2, smoothing_size, std::move(neighbors)); + estimate_normal_no_normals_memoized(pool, smoothed_v, neighbors, normals); + } + GEL_ASSERT_EQ(vertices.size(), normals.size()); + return smoothed_v; +} + +[[nodiscard]] +auto estimate_normals_and_smooth( + IExecutor& pool, + const std::vector& org_vertices, + std::vector& org_normals, + Distance dist) -> std::vector +{ + if (org_normals.empty()) { + return estimate_normals_no_normals(org_vertices, org_normals, dist); + } else { + return estimate_normals_included_normals(org_vertices, org_normals, dist); + } +} + +struct Components { + std::vector> vertices; + std::vector> smoothed_v; + std::vector> normals; +}; + +/// @brief Find the number of connected components and separate them +/// @param pool: Thread pool to use +/// @param vertices: vertices of the point cloud +/// @param smoothed_v: smoothed vertices of the point cloud +/// @param normals: normal of the point cloud vertices +/// @param neighbor_map +/// @param opts: theta: (cross-connection threshold) angle threshold to avoid connecting vertices on different surface +/// r: (outlier_thresh) threshold distance(?) to remove an outlier +/// k +/// isEuclidean +/// @return Split components +[[nodiscard]] +auto split_components( + IExecutor& pool, + const NeighborMap& neighbor_map, + std::vector&& vertices, + std::vector&& normals, + std::vector&& smoothed_v, + const RSROpts& opts) + -> Components +{ + GEL_ASSERT_EQ(vertices.size(), normals.size()); + GEL_ASSERT_EQ(vertices.size(), smoothed_v.size()); + std::vector> component_vertices; + std::vector> component_smoothed_v; + std::vector> component_normals; + + // Identifies clusters of vertices which are reconstructed to disparate meshes + const double outlier_thresh = opts.r; + double total_edge_length = 0; + AMGraph::NodeSet sets; + SimpGraph components; + for (int i = 0; i < vertices.size(); i++) { + sets.insert(components.add_node()); + } + + // Construct graph + for (const auto& neighbors : neighbor_map) { + const NodeID this_idx = neighbors[0].id; + for (const auto& neighbor : neighbors | std::views::drop(1)) { + const NodeID idx = neighbor.id; + if (this_idx < idx) continue; + const double length = neighbor.distance; + + total_edge_length += length; + + components.connect_nodes(this_idx, idx); + } + } + const double thresh_r = (total_edge_length * outlier_thresh) / static_cast(components.no_edges()); + // Remove Edges Longer than the threshold + std::vector> edge_rm_v; + for (NodeID vertex1 = 0; vertex1 < components.no_nodes(); ++vertex1) { + for (const auto& vertex2 : components.neighbors_lazy(vertex1)) { + const double edge_length = (vertices[vertex1] - vertices[vertex2]).length(); + + if (edge_length > thresh_r) { + edge_rm_v.emplace_back(vertex1, vertex2); + } + } + } + for (auto& [fst, snd] : edge_rm_v) { + components.disconnect_nodes(fst, snd); + } + // Find Components + const auto components_vec = connected_components(components.inner(), sets); + std::cout << "The input contains " << components_vec.size() << " connected components." << std::endl; + // Valid Components and create new vectors for components + const auto threshold = std::min(vertices.size(), 100); + for (auto& component : components_vec) { + if (component.size() >= threshold) { + std::vector this_normals; + this_normals.reserve(component.size()); + std::vector this_vertices; + this_vertices.reserve(component.size()); + std::vector this_smoothed_v; + this_smoothed_v.reserve(component.size()); + for (const auto& element : component) { + this_vertices.push_back(vertices[element]); + this_smoothed_v.push_back(smoothed_v[element]); + this_normals.push_back(normals[element]); + } + component_normals.emplace_back(std::move(this_normals)); + component_vertices.emplace_back(std::move(this_vertices)); + component_smoothed_v.emplace_back(std::move(this_smoothed_v)); + } + } + std::cout << component_vertices.size() << " of them will be reconstructed." << std::endl; + + return Components( + std::move(component_vertices), + std::move(component_smoothed_v), + std::move(component_normals) + ); +} + +void export_edges(const RSGraph& g, const std::string& out_path) +{ + std::ofstream file(out_path); + // Write vertices + file << "# List of geometric vertices" << std::endl; + for (int i = 0; i < g.m_vertices.size(); i++) { + Point this_coords = g.m_vertices[i].coords; + file << "v " << std::to_string(this_coords[0]) + << " " << std::to_string(this_coords[1]) + << " " << std::to_string(this_coords[2]) << "\n"; + } + + // Write lines + file << std::endl; + file << "# Line element" << std::endl; + for (int i = 0; i < g.m_vertices.size(); i += 2) + file << "l " << i + 1 + << " " << i + 2 << "\n"; + //file.close(); +} + +void export_graph(const RSGraph& g, const std::string& out_path) +{ + std::ofstream file(out_path); + // Write vertices + file << "# List of geometric vertices\n"; + for (int i = 0; i < g.m_vertices.size(); i++) { + Point this_coords = g.m_vertices[i].coords; + file << "v " << std::to_string(this_coords[0]) + << " " << std::to_string(this_coords[1]) + << " " << std::to_string(this_coords[2]) << "\n"; + } + + // Write lines + file << "\n# Line elements\n"; + for (int i = 0; i < g.m_edges.size(); ++i) { + auto edge = g.m_edges[i]; + file << "l " << (edge.source + 1) << " " << (edge.target + 1) << "\n"; + } +} + +bool vec3_eq_(const Vec3& lhs, const Vec3& rhs, double eps = 1e-4) +{ + return lhs.all_le(rhs + Vec3(eps)) && lhs.all_ge(rhs - Vec3(eps)); +} + + +RSGraph from_simp_graph(const SimpGraph& graph, const std::vector& points) +{ + RSGraph copy; + + for (auto id : graph.node_ids()) { + copy.add_node(points.at(id)); + } + for (auto id : graph.node_ids()) { + for (auto neighbor : graph.neighbors_lazy(id)) { + if (id < neighbor) { + copy.add_edge(id, neighbor); + } + } + } + return copy; +} + +auto component_to_manifold( + IExecutor& pool, + const RSROpts& opts, + const std::vector& vertices, + const std::vector& normals, + const std::vector& smoothed_v, + const Tree& kd_tree, + const NeighborMap& neighbor_map +) -> Manifold +{ + Util::RSRTimer inner_timer; + std::cout << "Init mst" << std::endl; + // Initial Structure + RSGraph mst; + mst.reserve(vertices.size(), opts.k); + + std::vector edge_length; + std::vector connection_lengths(vertices.size(), ConnectionLength()); + { + inner_timer.start("init_graph"); + SimpGraph g = init_graph(smoothed_v, normals, neighbor_map, connection_lengths, opts.k, opts.theta, + opts.dist == Distance::Euclidean); + inner_timer.end("init_graph"); + // Generate MST + inner_timer.start("build_mst"); + + build_mst(g, 0, mst, normals, smoothed_v, opts.dist == Distance::Euclidean); + //export_graph(from_simp_graph(g, vertices), "all_connected.obj"); + //export_graph(mst, "mst.obj"); + inner_timer.end("build_mst"); + + // Edge arrays and sort + inner_timer.start("edge_length"); + for (NodeID node : g.node_ids()) { + for (NodeID node_neighbor : g.neighbors_lazy(node)) { + if (node < node_neighbor) { + const Vec3 edge = smoothed_v[node] - smoothed_v[node_neighbor]; + const double len = (opts.dist == Distance::Euclidean) + ? edge.length() + : cal_proj_dist(edge, normals[node], normals[node_neighbor]); + + if (len > connection_lengths[node].pre_max_length || + len > connection_lengths[node_neighbor].pre_max_length) + continue; + edge_length.emplace_back(TEdge(node, node_neighbor), len); + } + } + } + inner_timer.end("edge_length"); + + inner_timer.start("sort"); + std::ranges::sort(edge_length, edge_comparator); + inner_timer.end("sort"); + } + + // Initialize face loop label + mst.etf.reserve(6 * vertices.size()); + init_face_loop_label(mst); + + std::vector flattened_face; + flattened_face.reserve(vertices.size() * 2 * 3); // A fairly reasonable heuristic + + std::vector> neighbors; + inner_timer.start("edge_connection"); + for (auto& [this_edge, edge_len] : edge_length) { + if (mst.find_edge(this_edge.from, this_edge.to) == RSGraph::InvalidEdgeID && + vanilla_check(mst, this_edge, kd_tree, neighbors)) { + auto result = register_face(mst, this_edge.from, this_edge.to); + if (result.has_value()) { + mst.add_edge(this_edge.from, this_edge.to); + mst.maintain_face_loop(this_edge.from, this_edge.to); + for (const auto& face : result->faces) { + add_face(mst, face, flattened_face); + } + } + } + } + inner_timer.end("edge_connection"); + std::cout << "edge length length: " << edge_length.size() << "\n"; + + // Create handles & Triangulation + inner_timer.start("triangulation"); + if (opts.genus != 0) { + std::vector connected_handle_root; + connect_handle(smoothed_v, kd_tree, mst, connected_handle_root, opts.k, opts.n, + opts.dist == Distance::Euclidean, opts.genus); + triangulate(flattened_face, mst, opts.dist == Distance::Euclidean, connection_lengths, connected_handle_root); + } + inner_timer.end("triangulation"); + + inner_timer.start("build_manifold"); + Manifold res; + + build_manifold(res, vertices, flattened_face, 3); + inner_timer.end("build_manifold"); + + std::cout << "\n"; + inner_timer.show(); + std::cout << "\n"; + + return res; +} + +auto point_cloud_to_mesh_impl( + std::vector&& vertices_copy, + std::vector&& normals_copy, + Util::RSRTimer& timer, + ThreadPool& pool, + const RSROpts& opts) -> Manifold +{ + Manifold output; + + auto in_smoothed_v = estimate_normals_and_smooth(pool, vertices_copy, normals_copy, opts.dist); + const Tree kd_tree = build_kd_tree_of_indices(in_smoothed_v, std::views::iota(0UL, in_smoothed_v.size())); + + // Find components + timer.start("Split components"); + std::cout << "Split components\n"; + // Note: the cross connection filtering needs to be synced with the inner loop, else there are issues + auto neighbor_map = calculate_neighbors(pool, in_smoothed_v, kd_tree, opts.k); + Parallel::foreach(pool, std::views::iota(0UL, vertices_copy.size()), [&](const NodeID idx) { + filter_out_cross_connection(neighbor_map[idx], normals_copy, idx, opts.theta, opts.dist == Distance::Euclidean); + }); + + auto [component_vertices, + component_smoothed_v, + component_normals] = + split_components(pool, + neighbor_map, + std::move(vertices_copy), + std::move(normals_copy), + std::move(in_smoothed_v), + opts); + timer.end("Split components"); + // There is no guarantee that there is more than one component, and components can + // be highly non-uniform in terms of how many primitives they have. That means we cannot + // rely on this loop for good parallelization opportunities. + timer.start("Algorithm"); + for (size_t component_id = 0; component_id < component_vertices.size(); component_id++) { + std::cout << "Reconstructing component " << std::to_string(component_id) << " ... (" << component_vertices[ + component_id].size() << " vertices)" << std::endl; + + std::vector vertices_of_this = std::move(component_vertices[component_id]); + std::vector normals_of_this = std::move(component_normals[component_id]); + std::vector smoothed_v_of_this = std::move(component_smoothed_v[component_id]); + GEL_ASSERT(vertices_of_this.size() == normals_of_this.size()); + GEL_ASSERT(vertices_of_this.size() == smoothed_v_of_this.size()); + + // While I would like to move this up, there are some nontrivial changes made to each component + // inside split_components eve if we only have one component to worry about. + const auto indices_of_this = std::ranges::iota_view(0UL, smoothed_v_of_this.size()); + Tree kd_tree_of_this = build_kd_tree_of_indices(smoothed_v_of_this, indices_of_this); + + auto neighbor_map_of_this = calculate_neighbors(pool, smoothed_v_of_this, kd_tree_of_this, opts.k); + //Parallel::foreach(pool, std::views::iota(0UL, smoothed_v_of_this.size()), [&](NodeID idx) { + // filter_out_cross_connection(neighbor_map_of_this[idx], normals_of_this, idx, opts.theta, + // opts.dist == Distance::EUCLIDEAN); + //}); + + auto res = component_to_manifold( + pool, + opts, + vertices_of_this, + normals_of_this, + smoothed_v_of_this, + kd_tree_of_this, + neighbor_map_of_this); + + output.merge(res); + // if (component_id == 0) { + // std::swap(output, res); + // } else { + // output.merge(res); + // } + } + timer.end("Algorithm"); + + return output; +} + +auto point_cloud_to_mesh( + const std::vector& vertices_in, + const std::vector& normals_in, + const RSROpts& opts) -> Manifold +{ + Util::RSRTimer timer; + ThreadPool pool; + timer.start("Whole process"); + timer.start("Validation"); + if (!normals_in.empty()) { + GEL_ASSERT_EQ(vertices_in.size(), normals_in.size(), "Vertices and normals must be the same size"); + } + for (const auto& point : vertices_in) { + GEL_ASSERT_FALSE(std::isnan(point[0]) || std::isnan(point[1]) || std::isnan(point[2]), "Bad point input"); + } + for (const auto& normal : normals_in) { + GEL_ASSERT_FALSE(std::isnan(normal[0]) || std::isnan(normal[1]) || std::isnan(normal[2]), "Bad normal input"); + GEL_ASSERT_NEQ(normal, Vec3(0.0), "Bad normal input"); + } + auto vertices_copy = vertices_in; + auto normals_copy = normals_in; + timer.end("Validation"); + + + // Estimate normals & orientation & weighted smoothing + timer.start("Estimate and smooth normals"); + std::vector in_smoothed_v = estimate_normals_and_smooth(pool, vertices_copy, normals_copy, opts.dist); + timer.end("Estimate and smooth normals"); + + + const auto indices = std::ranges::iota_view(0UL, in_smoothed_v.size()); + const Tree kd_tree = build_kd_tree_of_indices(in_smoothed_v, indices); + if (normals_in.empty()) { + std::cout << "correct normal orientation\n"; + timer.start("Correct normal orientation"); + correct_normal_orientation(pool, kd_tree, in_smoothed_v, normals_copy, opts.k); + timer.end("Correct normal orientation"); + } + + Manifold result = point_cloud_to_mesh_impl( + std::move(vertices_copy), + std::move(normals_copy), + timer, + pool, + opts); + timer.end("Whole process"); + const std::string line(40, '='); + std::cout << line << "\n\n"; + timer.show(); + return result; +} + +template +auto indexed_select(const Collection& collection, + const IndexRange& indices) -> std::vector +{ + std::vector result; + result.reserve(collection.size()); + for (auto idx : indices) { + result.push_back(collection.at(idx)); + } + return result; +} + +std::vector validate_normals(ThreadPool& pool, const std::vector& vertices, + const std::vector& normals, const RSROpts& reconstruction_options) +{ + auto check = [&]() { + if (!normals.empty() && vertices.size() != normals.size()) { + std::cerr << "Bad normal input: normals must either be empty or the same size as vertices" << "\n"; + return false; + } + for (const auto& point : vertices) { + GEL_ASSERT_FALSE(std::isnan(point[0]) || std::isnan(point[1]) || std::isnan(point[2]), + "Bad point input: Found a NaN coordinate."); + } + for (const auto& normal : normals) { + if (std::isnan(normal[0]) || std::isnan(normal[1]) || std::isnan(normal[2])) { + std::cerr << "Bad normal input: normal is NaN" << "\n"; + return false; + } + if (normal == Vec3(0.0)) { + std::cerr << "Bad normal input: normal is 0 vector" << "\n"; + return false; + } + } + return true; + }; + if (check()) { + return normals; + } else { + std::cout << "generating normals..."; + auto normals_copy = std::vector(); + auto smoothed_points = estimate_normals_and_smooth(pool, vertices, normals_copy, reconstruction_options.dist); + return normals_copy; + } +} + +auto point_cloud_collapse_reexpand( + const std::vector& vertices_in, + const std::vector& normals_in, + const CollapseOpts& collapse_options, + const RSROpts& rsr_opts, + const ReexpandOpts& reexpand_opts) -> Manifold +{ + if (collapse_options.max_iterations == 0) { + return point_cloud_to_mesh(vertices_in, normals_in, rsr_opts); + } + Util::RSRTimer timer; + ThreadPool pool; + + timer.start("Whole process"); + timer.start("Validation"); + if (!normals_in.empty()) { + GEL_ASSERT_EQ(vertices_in.size(), normals_in.size(), "Vertices and normals must be the same size"); + } + for (const auto& point : vertices_in) { + GEL_ASSERT_FALSE(std::isnan(point[0]) || std::isnan(point[1]) || std::isnan(point[2]), "Bad point input"); + } + for (const auto& normal : normals_in) { + GEL_ASSERT_FALSE(std::isnan(normal[0]) || std::isnan(normal[1]) || std::isnan(normal[2]), "Bad normal input"); + GEL_ASSERT_NEQ(normal, Vec3(0.0), "Bad normal input"); + } + auto vertices_copy = vertices_in; + auto normals_copy = normals_in; + timer.end("Validation"); + + + // Estimate normals & orientation & weighted smoothing + timer.start("Estimate and smooth normals"); + std::vector in_smoothed_v = estimate_normals_and_smooth(pool, vertices_copy, normals_copy, + collapse_options.distance); + timer.end("Estimate and smooth normals"); + + + if (normals_in.empty()) { + std::cout << "correct normal orientation\n"; + timer.start("Correct normal orientation"); + const auto indices = std::ranges::iota_view(0UL, in_smoothed_v.size()); + const Tree kd_tree = build_kd_tree_of_indices(in_smoothed_v, indices); + correct_normal_orientation(pool, kd_tree, in_smoothed_v, normals_copy, rsr_opts.k); + timer.end("Correct normal orientation"); + } + + timer.start("Collapse"); + auto [collapse, point_cloud] = collapse_points(vertices_copy, normals_copy, collapse_options); + timer.end("Collapse"); + + auto [points_collapsed, normals_collapsed] = std::move(point_cloud); + + Manifold manifold = point_cloud_to_mesh_impl( + std::move(points_collapsed), + std::move(normals_collapsed), + timer, + pool, + rsr_opts); + + timer.start("Reexpand"); + if (reexpand_opts.enabled) + reexpand_points(manifold, std::move(collapse), reexpand_opts); + timer.end("Reexpand"); + + timer.end("Whole process"); + const std::string line(40, '='); + std::cout << line << "\n\n"; + timer.show(); + return manifold; +} + +auto point_cloud_normal_estimate(const std::vector& vertices, + const std::vector& normals, + const bool is_euclidean) -> NormalEstimationResult +{ + auto normals_copy = normals; + const auto dist = is_euclidean ? Distance::Euclidean : Distance::Tangent; + ThreadPool pool; + const auto smoothed_v = estimate_normals_and_smooth(pool, vertices, normals_copy, dist); + return {vertices, normals_copy, smoothed_v}; +} +} // namespace GEL::HMesh::RsR diff --git a/src/GEL/HMesh/RSRExperimental.h b/src/GEL/HMesh/RSRExperimental.h new file mode 100644 index 00000000..3eaba950 --- /dev/null +++ b/src/GEL/HMesh/RSRExperimental.h @@ -0,0 +1,93 @@ +#ifndef GEL_RSR_EXPERIMENTAL_H +#define GEL_RSR_EXPERIMENTAL_H +#pragma once + +#include + +#include + +/// @brief Rotation System Reconstruction +namespace HMesh::RSR +{ +/// Options struct for point cloud reconstruction +struct RSROpts { + /// Expected genus of the manifold, -1 to auto-detect. This value + /// is used in the handle connection phase to determine how many + /// handles should be inserted. Providing the genus might improve + /// reconstruction quality but only if the reconstruction is of + /// sufficiently good quality. + int32_t genus = -1; + /// Number of nearest neighbors to initialize the graphs with. + /// Higher values can improve reconstruction quality, but they + /// will also substantially increase reconstruction time. Values + /// higher than 30 are likely to result in diminishing returns. + int32_t k = 30; + /// Threshold multiplier for removing overly long edges. When + /// initializing k-nearest neighbor graphs, edges whose length are + /// longer than the average length times r will be removed from + /// the graph. This can prevent outliers and unwanted connections + /// between different components of a manifold. + double r = 20; + /// Cross connection angle threshold in degrees. For the k-nearest + /// neighbor graph, if the angles between the normals of two + /// neighbors is larger than this value, the edge is culled. This + /// can help eliminating edges resulting from noise or edges + /// belonging to different (potentially overlapping) components. + double theta = 60; + /// Minimum step threshold for handle connections. During the handle + /// connection phase, if the shortest path between two vertices of + /// a handle candidate is fewer than this number, the handle candidate + /// is rejected. This can help prevent spurious local handles from + /// being inserted. + int32_t n = 50; + /// Distance function to use. The options are the Euclidean distance + /// which tends to work well with smoother meshes and tangent space + /// distance which tends to work much better with noisy meshes but + /// with substantial quality loss in smooth meshes. + Distance dist = Distance::Euclidean; +}; + +// Algorithm + +/// Convert a point cloud into a Manifold using the rotation system +/// based reconstruction method. +/// @param vertices_in vertices of the point cloud +/// @param normals_in normals of the point cloud or empty vector +/// @param opts reconstruction options +/// @return reconstructed manifold mesh +auto point_cloud_to_mesh(const std::vector& vertices_in, + const std::vector& normals_in, + const RSROpts& opts) -> HMesh::Manifold; + +/// Convert a point cloud into a Manifold using the hierarchical collapse +/// and reexpansion method. Rotation system reconstruction is used to perform +/// the intermediate reconstruction. +/// @param vertices vertices of the point cloud +/// @param normals normals of the point cloud or empty vector +/// @param collapse_options collapse options +/// @param reconstruction_options reconstruction options +/// @param reexpand_options reexpansion options +/// @return reconstructed manifold mesh +auto point_cloud_collapse_reexpand( + const std::vector& vertices, + const std::vector& normals, + const CollapseOpts& collapse_options, + const RSROpts& reconstruction_options, + const ReexpandOpts& reexpand_options) -> HMesh::Manifold; + +// TODO: probably remove these +namespace detail +{ + struct NormalEstimationResult { + std::vector vertices; + std::vector normals; + std::vector smoothed_v; + }; + + auto point_cloud_normal_estimate(const std::vector& vertices, + const std::vector& normals, + bool is_euclidean) -> NormalEstimationResult; +} +} // namespace HMesh::RSR + +#endif // GEL_HMesh_RsR2_hpp diff --git a/src/GEL/Util/RSRTimer.h b/src/GEL/Util/RSRTimer.h new file mode 100644 index 00000000..46684b48 --- /dev/null +++ b/src/GEL/Util/RSRTimer.h @@ -0,0 +1,86 @@ +// +// Created on 4/24/25. +// +#pragma once +#ifndef GEL_HMESH_RSRTIMER_H +#define GEL_HMESH_RSRTIMER_H + +#include +#include +#include +#include + +namespace Util { + /// Timer class + /// FIXME: The other ancient timer should probably be removed from the library and replaced with this + struct RSRTimer { + RSRTimer() = default; + + //RSRTimer(const RSRTimer&) = delete; + //RSRTimer& operator=(const RSRTimer&) = delete; + + private: + using time_point = std::chrono::high_resolution_clock::time_point; + + struct Time { + time_point start; + time_point end; + }; + + std::unordered_map m_times; + + public: + RSRTimer& create(const std::string& name) { + m_times.insert_or_assign( + name, + Time{std::chrono::high_resolution_clock::now(), std::chrono::high_resolution_clock::now()} + ); + + return *this; + } + + RSRTimer& start(const std::string& name) { + if (!m_times.contains(name)) { + this->create(name); + } + m_times[name].start = std::chrono::high_resolution_clock::now(); + return *this; + } + + void end(const std::string& name) { + //int idx = idx_map[name]; + if (m_times.contains(name)) { + m_times[name].end = std::chrono::high_resolution_clock::now(); + } + } + + void show() const + { + for (const auto& [name, time] : m_times) { + const auto count = std::chrono::duration_cast(time.end - time.start).count(); + std::cout << "Spent "<< count << " milliseconds on " << name << "\n"; + } + } + }; + + struct TimerGuard { + TimerGuard() = delete; + + TimerGuard(RSRTimer& timer, const std::string& name) : timer(timer), name(name) + { + timer.start(name); + } + + TimerGuard(const TimerGuard& other) = delete; + + ~TimerGuard() + { + timer.end(name); + } + private: + RSRTimer& timer; + std::string name; + }; +} + +#endif //GEL_HMESH_RSRTIMER_H diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 86f85e0b..db9af61e 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -27,6 +27,8 @@ FetchContent_GetProperties(doctest SOURCE_DIR doctest_source_dir) set(DOCTEST_INCLUDE_DIR ${doctest_source_dir}/doctest CACHE INTERNAL "Path to include folder for doctest") # # Add doctest to a project with: # target_include_directories(some_executable PRIVATE ${DOCTEST_INCLUDE_DIR}) + +# include the doctest_discover_tests() script include(${doctest_source_dir}/scripts/cmake/doctest.cmake) add_subdirectory(CGLA-covariance) @@ -36,9 +38,11 @@ add_subdirectory(CGLA-vec) add_subdirectory(Geometry-kdtree) add_subdirectory(Util-Assert) add_subdirectory(Manifold) + +add_subdirectory(RsR) +add_subdirectory(Util) +add_subdirectory(Util-ThreadPool) + # The following two rely on key input in order to exit #add_subdirectory(CGLA-ogl) #add_subdirectory(GLGraphics-console) - -add_subdirectory(Util-ThreadPool) -add_subdirectory(Util) diff --git a/src/test/RsR/CMakeLists.txt b/src/test/RsR/CMakeLists.txt new file mode 100644 index 00000000..52637597 --- /dev/null +++ b/src/test/RsR/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.30) + +get_filename_component(GEL_INCLUDE_DIR ../.. ABSOLUTE) + +add_executable(RsR_test RsR_test.cpp) + +set_target_properties(RsR_test PROPERTIES + CXX_STANDARD 20 + CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=undefined" +) +target_include_directories(RsR_test PRIVATE "${GEL_INCLUDE_DIR}" "${DOCTEST_INCLUDE_DIR}") + +target_link_libraries(RsR_test PRIVATE GEL nanobench) + +doctest_discover_tests(RsR_test TEST_PREFIX "RsR.") +#add_test(NAME GEL.Test.RsR COMMAND RsR_test) \ No newline at end of file diff --git a/src/test/RsR/RsR_test.cpp b/src/test/RsR/RsR_test.cpp new file mode 100644 index 00000000..42dc74b1 --- /dev/null +++ b/src/test/RsR/RsR_test.cpp @@ -0,0 +1,498 @@ +// +// Created by Cem Akarsubasi on 4/15/25. +// + +#include +#include +#include +#include "../common/RawObj.h" + +#include +#include +#include +#include + +#include + +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include + +using HMesh::RSR::point_cloud_to_mesh; +using namespace HMesh::RSR; +using namespace CGLA; +using NodeID = size_t; + +static constexpr auto FILE_CAPITAL_A = "../../../../data/PointClouds/Capital_A.obj"; +// Not included: +// static constexpr auto FILE_BUNNY_SIMPLE = "../../../../data/bunny_with_normals.obj"; +static constexpr auto FILE_BUNNY_SIMPLE_NO_NORMALS = "../../../../data/bunny.obj"; + +// Not included: +//static constexpr auto FILE_BUNNY_COMPLEX = "../../../../data/PointClouds/bun_complete.obj"; +static constexpr auto FILE_THINGY = "../../../../data/thingy.obj"; +static constexpr auto FILE_AS = "../../../../data/as.obj"; + +template +constexpr auto make_array(Args... args) +{ + constexpr auto size = sizeof...(args); + return std::array{args...}; +} + +static constexpr auto QUICK_TEST_FILES = make_array( + FILE_CAPITAL_A, + //FILE_THINGY, + FILE_AS); + +constexpr auto IS_EUCLIDEAN = false; +constexpr auto K_PARAM = 30; +constexpr auto GENUS = -1; +constexpr auto R_PARAM = 20; +constexpr auto THETA = 60; +constexpr auto N_PARAM = 50; + +auto test_options() +{ + RSROpts opts; + opts.dist = IS_EUCLIDEAN ? Distance::Euclidean : Distance::Tangent; + opts.k = K_PARAM; + opts.genus = GENUS; + opts.r = R_PARAM; + opts.theta = THETA; + opts.n = N_PARAM; + return opts; +} + +template +auto indices_from(const Collection& collection) -> std::vector +{ + const auto indices = [&collection] { + std::vector temp(collection.size()); + std::iota(temp.begin(), temp.end(), 0); + return temp; + }(); + return indices; +} + +template +auto indexed_select(const std::vector& vec, const std::vector& indices) -> std::vector +{ + std::vector result; + result.reserve(indices.size()); + for (auto idx : indices) { + result.push_back(vec.at(idx)); + } + return result; +} + +inline +auto angle_cost(const Vec3d e1, const Vec3d e2) -> double +{ + return std::abs(2.0 * dot(e1, e2) / (e1.length() * e2.length() + 1e-18) - 1.0); +} + +inline +auto edge_cost(const Vec3d e1, const Vec3d e2) -> double +{ + const Vec3d e3 = e1 - e2; + return angle_cost(e1, e2) * e3.length(); +} + +inline +auto triangle_cost(const Vec3d p1, const Vec3d p2, const Vec3d p3) -> double +{ + return edge_cost((p2 - p1), (p3 - p1)) + edge_cost((p1 - p2), (p3 - p2)) + edge_cost((p1 - p3), (p2 - p3)); +} + +auto manifold_cost(const HMesh::Manifold& manifold) -> double +{ + double total_cost = 0.0; + for (const auto f : manifold.faces()) { + std::array arr; + int idx = 0; + HMesh::circulate_face_ccw(manifold, f, [&idx, &arr](const HMesh::VertexID v) { + if (idx < 3) + arr[idx++] = v; + }); + if (idx == 3) { + const auto p1 = manifold.positions[arr[0]]; + const auto p2 = manifold.positions[arr[1]]; + const auto p3 = manifold.positions[arr[2]]; + const auto cost = triangle_cost(p1, p2, p3); + total_cost += cost; + } + } + return total_cost; +} + +auto manifold_average_edge_length(const HMesh::Manifold& manifold) -> double +{ + double total = 0.0; + for (const auto halfedge : manifold.halfedges()) { + HMesh::Walker walker = manifold.walker(halfedge); + auto v1 = walker.vertex(); + auto v2 = walker.opp().vertex(); + GEL_ASSERT_NEQ(v1, v2); + auto p1 = manifold.positions[v1]; + auto p2 = manifold.positions[v2]; + total += std::sqrt(dot((p2 - p1), (p2 - p1))); + } + return total / static_cast(manifold.no_halfedges()); +} + +auto manifold_cost_normalized(const HMesh::Manifold& manifold) -> double +{ + const auto number_of_faces = manifold.no_faces(); + const auto total_cost = manifold_cost(manifold); + + return total_cost / static_cast(number_of_faces) / manifold_average_edge_length(manifold); +} + +auto manifold_is_triangular(const HMesh::Manifold& manifold) -> bool +{ + for (const auto f : manifold.faces()) { + int idx = 0; + HMesh::circulate_face_ccw(manifold, f, [&idx](const HMesh::VertexID v) { + idx++; + }); + if (idx != 3) + return false; + } + return true; +} + +auto manifold_face_vert_ratio(const HMesh::Manifold& manifold) -> double +{ + return static_cast(manifold.no_faces()) / static_cast(manifold.no_vertices()); +} + +/// Estimate quality of a Manifold based on dihedral angles. +/// @details The dihedral angle between each face is used to determine how expensive it would be to "cross" between +/// those two faces. The average of this for every half-edge is used to get the total cost. Boundary edges are considered +/// to be the most expensive to cross. +/// +/// This gives a quality metric in terms of how "smooth" a Manifold is. In an infinite flat plane, this function is 0. +/// However, keep in mind that this is not a generalized quality measure. The Manifold might be intentionally bumpy which +/// will return a high cost. +auto manifold_dihedral_cost(const HMesh::Manifold& manifold) -> std::pair +{ + double total_cost = 0.0; + double edge_scaled_cost = 0.0; + size_t bad_edges = 0; + size_t boundary_edges = 0; + for (const auto half_edge : manifold.halfedges()) { + const auto f1 = manifold.walker(half_edge).face(); + const auto f2 = manifold.walker(half_edge).opp().face(); + if (f1 != HMesh::InvalidFaceID && f2 != HMesh::InvalidFaceID) { + const auto n1 = manifold.normal(f1); + const auto n2 = manifold.normal(f2); + const auto cost = std::abs(CGLA::dot(CGLA::normalize(n1), CGLA::normalize(n2)) - 1.0); + if (!std::isnan(cost)) { + total_cost += cost; + edge_scaled_cost += (cost * manifold.length(half_edge)); + } + else + bad_edges++; + } else { + // boundary edges are considered to be the most expensive + boundary_edges ++; + total_cost += 2.0; + } + } + if (bad_edges > 0) { + std::cerr << "dihedral cost found " << bad_edges << " bad edges\n"; + } + std::cout << "dihedral cost found " << boundary_edges << " boundary edges\n"; + return std::make_pair(total_cost / static_cast(manifold.no_halfedges()), edge_scaled_cost / static_cast(manifold.no_halfedges())); +} + +struct ValencyHistogram { + static constexpr size_t max_tracked_valency = 9; + size_t total = 0; + std::array common_valency{0}; + size_t high = 0; + + friend std::ostream& operator<<(std::ostream& os, const ValencyHistogram& hist); +}; + +std::ostream& operator<<(std::ostream& os, const ValencyHistogram& hist) +{ + os << "Total: " << hist.total << "\n"; + constexpr auto draw_factor = 40; + for (size_t valency = 2; valency < hist.common_valency.size(); ++valency) { + const auto ratio = static_cast(hist.common_valency[valency]) / static_cast(hist.total); + const auto blocks = static_cast(ratio * draw_factor); + os << valency << ": "; + for (int i = 0; i < blocks; ++i) { + os << "#"; + } + for (int i = blocks; i < draw_factor; ++i) { + os << " "; + } + os << " (" << (ratio * 100.0) << "%)\n"; + } + const auto ratio = static_cast(hist.high) / static_cast(hist.total); + const auto blocks = static_cast(ratio * draw_factor); + os << hist.common_valency.size() << "+: "; + for (int i = 0; i < blocks; ++i) { + os << "#"; + } + for (int i = blocks; i < draw_factor; ++i) { + os << " "; + } + os << " (" << (ratio * 100.0) << "%)\n"; + return os; +} + +auto manifold_valency_histogram(const HMesh::Manifold& manifold) -> ValencyHistogram +{ + ValencyHistogram hist; + for (const auto v : manifold.vertices()) { + if (const auto valency = manifold.valency(v); valency > 8) { + hist.high++; + } else { + hist.common_valency[valency]++; + } + } + hist.total = manifold.no_vertices(); + return hist; +} + +auto manifold_valency_histogram_non_boundary(const HMesh::Manifold& manifold) -> ValencyHistogram +{ + ValencyHistogram hist; + size_t non_boundary_vertices = 0; + for (const auto v : manifold.vertices()) { + if (!manifold.boundary(v)) { + non_boundary_vertices++; + if (const auto valency = manifold.valency(v); valency > 8) { + hist.high++; + } else { + hist.common_valency[valency]++; + } + } + } + hist.total = non_boundary_vertices; + return hist; +} + +// Test functions begin + + +auto test_reconstruct_new(const std::string_view file_name, const RSROpts& opts) -> std::optional +{ + std::cout << "======================\n" + << "Begin new function\n"; + auto input_maybe = Util::read_raw_obj(file_name); + if (!input_maybe) { + WARN_FALSE("File not found."); + return std::nullopt; + } + const auto input = *std::move(input_maybe); + + std::cout << "obj vertices: " << input.vertices.size() << "\n"; + std::cout << "obj normals: " << input.normals.size() << "\n"; + + HMesh::Manifold output = point_cloud_to_mesh(input.vertices, {}, opts); + // k: 70 is too large + // r: needs isEuclidean false + std::cout << output.positions.size() << "\n"; + + return output; +} + +auto point_cloud_to_mesh_legacy(std::vector const& points, const std::vector& normals, const RSROpts& opts) -> HMesh::Manifold +{ + auto points_copy = points; + auto normals_copy = normals; + HMesh::Manifold output; + reconstruct_single(output, points_copy, normals_copy, opts.dist == Distance::Euclidean, opts.genus, opts.k, + opts.r, opts.theta, opts.n); + return output; +} + + +auto test_reconstruct_legacy(const std::string_view file_name, const RSROpts& opts) -> std::optional +{ + std::cout << "======================\n" + << "Begin original function\n"; + auto input_maybe = Util::read_raw_obj(file_name); + if (!input_maybe) { + WARN_FALSE("File not found."); + return std::nullopt; + } + auto input = Util::to_triangle_mesh(*input_maybe); + + std::cout << "obj vertices: " << input.vertices.size() << "\n"; + std::cout << "obj normals: " << input.normals.size() << "\n"; + + HMesh::Manifold output; + reconstruct_single(output, + input.vertices, + input.normals, + opts.dist == Distance::Euclidean, opts.genus, opts.k, + opts.r, opts.theta, opts.n); + // k: 70 is too large + // r: needs isEuclidean false + std::cout << output.positions.size() << "\n"; + + return output; +} + +auto test_reconstruct_collapse_reexpand(const std::string_view file_name, const CollapseOpts& collapse_opts, + const RSROpts& rsr_opts, const ReexpandOpts& reexpand) -> std::optional +{ + std::cout << "======================\n" + << "Begin new function\n"; + auto input_maybe = Util::read_raw_obj(file_name); + if (!input_maybe) { + WARN_FALSE("File not found."); + return std::nullopt; + } + const auto input = Util::to_triangle_mesh(*input_maybe); + + std::cout << "obj vertices: " << input.vertices.size() << "\n"; + std::cout << "obj normals: " << input.normals.size() << "\n"; + + HMesh::Manifold output = point_cloud_collapse_reexpand(input.vertices, input.normals, collapse_opts, rsr_opts, + reexpand); + // k: 70 is too large + // r: needs isEuclidean false + std::cout << output.positions.size() << "\n"; + + return output; +} + +auto reconstruct_assertions(const HMesh::Manifold& manifold) -> void +{ + // Manifold properties: + // Triangular + // Approx. 2 face per vertex (if good reconstruction) + // Each vertex around 6 edges + CHECK_EQ(manifold_is_triangular(manifold), true); + auto ratio = manifold_face_vert_ratio(manifold); + CHECK_GT(ratio, 1.5); + CHECK_LT(ratio, 2.25); + auto faces = manifold.no_faces(); + auto cost = manifold_cost(manifold); + auto average_edge = manifold_average_edge_length(manifold); + auto normed_cost = cost / static_cast(faces) / average_edge; + auto valencies = manifold_valency_histogram(manifold); + auto valencies_nb = manifold_valency_histogram_non_boundary(manifold); + auto [dihedral_cost1, dihedral_cost2] = manifold_dihedral_cost(manifold); + std::cout << "ratio: " << ratio << std::endl; + std::cout << "total cost: " << cost << std::endl; + std::cout << "num faces : " << faces << std::endl; + std::cout << "average edge length: " << average_edge << std::endl; + std::cout << "normed cost: " << normed_cost << std::endl; + std::cout << "Valency histogram (all vertices): " << std::endl; + std::cout << valencies; + std::cout << "Valency histogram (non-boundary vertices): " << std::endl; + std::cout << valencies_nb; + std::cout << "Dihedral cost raw: " << dihedral_cost1 << std::endl; + std::cout << "Dihedral cost adj: " << dihedral_cost2 << std::endl; +} + +template +void test_reconstruct(Func&& f, const bool save, const bool all = false) + requires std::is_same_v(), std::declval())), + std::optional> +{ + const auto test_files = std::ranges::subrange(QUICK_TEST_FILES); + + const auto opts = test_options(); + for (const auto file : test_files) { + std::filesystem::path p = file; + auto opts_neighbors = opts; + opts_neighbors.dist = Distance::Tangent; + + // MSVC lacks implicit conversions from fs::path to string + // And if we rely on .string() inline, we end up creating a dangling pointer + std::string case_name = p.stem().concat("_neighbors").string(); + SUBCASE(case_name.c_str()) { + std::optional manifold = f(file, opts_neighbors); + if (manifold.has_value()) { + auto out_path = p.stem().concat("_neighbors").concat(".obj"); + if (save) + HMesh::obj_save(out_path.string(), *manifold); + reconstruct_assertions(*manifold); + } + } + } + for (const auto file : test_files) { + std::filesystem::path p = file; + + auto opts_euclidean = opts; + opts_euclidean.dist = Distance::Euclidean; + + // MSVC lacks implicit conversions from fs::path to string + // And if we rely on .string() inline, we end up creating a dangling pointer + std::string case_name = p.stem().concat("_euclidean").string(); + SUBCASE(case_name.c_str()) { + std::optional manifold = f(file, opts_euclidean); + if (manifold.has_value()) { + auto out_path = p.stem().concat("_euclidean").concat(".obj"); + if (save) + HMesh::obj_save(out_path.string(), *manifold); + reconstruct_assertions(*manifold); + } + } + } +} + +TEST_CASE("reconstruct legacy") +{ + test_reconstruct(test_reconstruct_legacy, false); +} + +TEST_CASE("reconstruct") +{ + test_reconstruct(test_reconstruct_new, true); +} + +TEST_CASE("reconstruct_collapse_reexpand") +{ + auto l = [](T0&& PH1, T1&& PH2) { + return test_reconstruct_collapse_reexpand(std::forward(PH1), CollapseOpts(), std::forward(PH2), ReexpandOpts()); + }; + test_reconstruct(l, true, true); +} + +// TEST_CASE("reconstruct_debug") +// { +// auto collapse_opts = CollapseOpts{}; +// auto opts_euclidean = test_options(); +// auto file = FILE_BUNNY_SIMPLE_NO_NORMALS; +// collapse_opts.max_iterations = 3; +// collapse_opts.initial_neighbors = 8; +// collapse_opts.reduction_per_iteration = 0.50; +// collapse_opts.max_collapses = 0; +// +// auto reexpand_opts = ReexpandOptions(); +// reexpand_opts.enabled = true; +// reexpand_opts.debug_print = false; +// reexpand_opts.angle_threshold = std::numbers::pi / 180.0 * 9; +// reexpand_opts.angle_threshold_penalty = 0.1; +// reexpand_opts.angle_factor *= 1; +// reexpand_opts.early_stop_at_error = false; +// reexpand_opts.stop_at_error = 0; +// reexpand_opts.debug_mask; // |= RE_ERRORS; // | RE_FIRST_FLIP | RE_SECOND_FLIP | RE_ITERATION; +// reexpand_opts.stop_at_iteration = 0; +// +// +// opts_euclidean.dist = Distance::Euclidean; +// auto manifold = test_reconstruct_collapse_reexpand(file, collapse_opts, opts_euclidean, reexpand_opts); +// CHECK(manifold.has_value()); +// if (manifold.has_value()) { +// HMesh::obj_save("debug_obj_reexpand_euclidean.obj", *manifold); +// reconstruct_assertions(*manifold); +// } +// // auto opts_neighbors = test_options(); +// // opts_neighbors.dist = Distance::Tangent; +// // manifold = test_reconstruct_collapse_reexpand(file, collapse_opts, opts_neighbors, reexpand_opts); +// // CHECK(manifold.has_value()); +// // if (manifold.has_value()) { +// // HMesh::obj_save("debug_obj_reexpand_neighbors.obj", *manifold); +// // reconstruct_assertions(*manifold); +// // } +// } \ No newline at end of file