From 53096017f5c43c5f65b78ac3d985ca6b7ab80f27 Mon Sep 17 00:00:00 2001 From: Thomas Krijnen Date: Mon, 16 Oct 2023 21:28:38 +0200 Subject: [PATCH] Update nef_to_halfspace_tree.h --- .../kernels/cgal/nef_to_halfspace_tree.h | 528 ++++++++++++++---- 1 file changed, 426 insertions(+), 102 deletions(-) diff --git a/src/ifcgeom/kernels/cgal/nef_to_halfspace_tree.h b/src/ifcgeom/kernels/cgal/nef_to_halfspace_tree.h index 2dbcf9af75..feb522991e 100644 --- a/src/ifcgeom/kernels/cgal/nef_to_halfspace_tree.h +++ b/src/ifcgeom/kernels/cgal/nef_to_halfspace_tree.h @@ -28,6 +28,9 @@ #include #include #include +#include +#include +#include #include #include @@ -43,9 +46,11 @@ #include #include +#include #include #include +// Functor to lexicographically sort Plane_3 template struct PlaneLess { bool operator()(const typename Kernel::Plane_3& lhs, const typename Kernel::Plane_3& rhs) const { @@ -61,6 +66,7 @@ struct PlaneLess { } }; +// Functor to hash Plane_3 template struct PlaneHash { size_t operator()(const CGAL::Plane_3& plane) const @@ -75,45 +81,41 @@ struct PlaneHash { } }; +// Utility function to return Nef facet information as string template std::string dump_facet(typename CGAL::Nef_polyhedron_3::Halffacet_const_handle h) { - typedef CGAL::Nef_polyhedron_3::SHalfedge_const_handle SHalfedge_const_handle; - typedef CGAL::Nef_polyhedron_3::SHalfedge_around_facet_const_circulator SHalfedge_around_facet_const_circulator; + typedef typename CGAL::Nef_polyhedron_3::SHalfedge_const_handle SHalfedge_const_handle; + typedef typename CGAL::Nef_polyhedron_3::SHalfedge_around_facet_const_circulator SHalfedge_around_facet_const_circulator; std::ostringstream oss; const auto& p = h->plane(); - oss << "F plane=" << p << std::endl; - h->facet_cycles_begin(); - + oss << "Facet plane=" << p << std::endl; + auto fc = h->facet_cycles_begin(); auto se = SHalfedge_const_handle(fc); CGAL_assertion(se != 0); SHalfedge_around_facet_const_circulator hc_start(se); SHalfedge_around_facet_const_circulator hc_end(hc_start); CGAL_For_all(hc_start, hc_end) { - oss << " co=" << hc_start->source()->center_vertex()->point() << std::endl; + oss << " co=" << hc_start->source()->center_vertex()->point() << std::endl; } oss << std::endl; return oss.str(); } +// Boolean operations enum halfspace_operation { OP_UNION, OP_SUBTRACTION, OP_INTERSECTION }; +// Map of Plane_3 -> Plane_3 used when applied snapping template using plane_map = std::map>; -template -class halfspace_tree { -public: - virtual CGAL::Nef_polyhedron_3 evaluate(int level = 0) const = 0; - virtual void accumulate(std::list&) const = 0; - virtual std::unique_ptr map(const plane_map&) const = 0; -}; - +// Snap halfspace planes +// search_radius: max cartesian distance in plane equation parameters as 4d points in space template plane_map snap_halfspaces(const std::list>& planes, double search_radius) { // @todo this should incorporate some recursive or actual clustering approach so that @@ -127,6 +129,8 @@ plane_map snap_halfspaces(const std::list>& planes plane_map result; + std::map> neighbours; + std::map>> originals; std::vector planes_as_point; for (auto& p : planes) { @@ -135,6 +139,7 @@ plane_map snap_halfspaces(const std::list>& planes // @todo how to properly initialize using p._().exact() without converting to double? Point_d pp(CGAL::to_double(p.a()) / l, CGAL::to_double(p.b()) / l, CGAL::to_double(p.c()) / l, CGAL::to_double(p.d()) / l); planes_as_point.push_back(pp); + originals[pp].push_back(p); } // @todo should we have a proper distance metric for plane equations @@ -142,42 +147,48 @@ plane_map snap_halfspaces(const std::list>& planes auto plit = planes.begin(); for (size_t i = 0; i < planes.size(); ++i) { + if (result.find(*plit++) != result.end()) { + continue; + } + auto& query = planes_as_point[i]; + Fuzzy_sphere fs(query, search_radius, 0.); // std::cout << "q " << query << std::endl; + + std::list results_pos, results_neg; + kdtree.search(std::back_inserter(results_pos), fs); - std::list results; - kdtree.search(std::back_inserter(results), fs); - for (auto& r : results) { - // std::cout << " " << r << std::endl; - } - auto sum = std::accumulate(++results.begin(), results.end(), results.front(), [](Point_d a, Point_d b) {return Point_d(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]); }); - int N = results.size(); - results.clear(); - - // Search for the negation of the query point as well. - // @todo should we rather make sure planes are filtered to one hemisphere before inserted? Point_d n(-query[0], -query[1], -query[2], -query[3]); Fuzzy_sphere fsn(n, search_radius, 0.); - kdtree.search(std::back_inserter(results), fsn); - for (auto& r : results) { - // std::cout << " " << r << std::endl; + kdtree.search(std::back_inserter(results_neg), fsn); + + auto sum = std::accumulate(++results_pos.begin(), results_pos.end(), results_pos.front(), [](Point_d a, Point_d b) {return Point_d(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]); }); + int N = results_pos.size(); + auto sum2 = std::accumulate(results_neg.begin(), results_neg.end(), sum, [](Point_d a, Point_d b) {return Point_d(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]); }); + N += results_neg.size(); + + auto avg = CGAL::Plane_3(sum2[0] / N, sum2[1] / N, sum2[2] / N, sum2[3] / N); + + for (auto& p : results_pos) { + for (auto& pl : originals[p]) { + result.insert({ pl, avg }); + } + } + for (auto& p : results_neg) { + for (auto& pl : originals[p]) { + result.insert({ pl, avg.opposite() }); + } } - N += results.size(); - auto sum2 = std::accumulate(results.begin(), results.end(), sum, [](Point_d a, Point_d b) {return Point_d(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]); }); - - // It is imperative that there are no rounding errors, I think that's covered by using the Point_d - // (even if we populated it inaccurately using doubles and sqrt). - auto avg = Kernel::Plane_3(sum[0] / N, sum[1] / N, sum[2] / N, sum[3] / N); - - // std::cout << *plit << " -> " << avg << std::endl; - result.insert({ *plit++, avg }); } return result; } - +// Snap halfspace planes +// planes_fixed: candidates +// planes: planes that can be moved to planes_fixed when distance permits +// search_radius: max cartesian distance in plane equation parameters as 4d points in space template plane_map snap_halfspaces_2(const std::list>& planes_fixed, const std::list>& planes, double search_radius) { // @todo this should incorporate some recursive or actual clustering approach so that @@ -200,7 +211,7 @@ plane_map snap_halfspaces_2(const std::list>& plan // @todo how to properly initialize using p._().exact() without converting to double? Point_d pp(CGAL::to_double(p.a()) / l, CGAL::to_double(p.b()) / l, CGAL::to_double(p.c()) / l, CGAL::to_double(p.d()) / l); planes_as_point.push_back(pp); - normalized_to_original.insert({pp, p}); + normalized_to_original.insert({ pp, p }); } // @todo should we have a proper distance metric for plane equations @@ -233,7 +244,20 @@ plane_map snap_halfspaces_2(const std::list>& plan return result; } +enum tree_type { TT_NARY_BRANCH, TT_PLANE }; +// Abstract base class for halfspace tree component +template +class halfspace_tree { +public: + virtual CGAL::Nef_polyhedron_3 evaluate(int level = 0) const = 0; + virtual void accumulate(std::list&) const = 0; + virtual std::unique_ptr map(const plane_map&) const = 0; + virtual tree_type kind() const = 0; + virtual void merge(CGAL::Nef_polyhedron_3&) const = 0; +}; + +// Halfspace tree component as n-ary operands template class halfspace_tree_nary_branch : public halfspace_tree { private: @@ -241,6 +265,14 @@ private: std::list>> operands_; public: + virtual tree_type kind() const { + return TT_NARY_BRANCH; + } + + virtual void merge(CGAL::Nef_polyhedron_3&) const { + throw std::runtime_error("not implemented"); + } + halfspace_tree_nary_branch(halfspace_operation operation, std::list>>&& operands) : operation_(operation) , operands_(std::move(operands)) @@ -263,11 +295,33 @@ public: } result = builder.get_union(); } else if (operation_ == OP_INTERSECTION) { - CGAL::Nef_nary_intersection_3> builder; + bool is_all_planes = true; for (auto& op : operands_) { - builder.add_polyhedron(op->evaluate(level + 1)); + if (op->kind() != TT_PLANE) { + is_all_planes = false; + break; + } + } + + if (is_all_planes) { + // Instead of creating an operand based on the intersection of plane and cube + // which results in two intersection operations. Accumulate the result directly. + bool first = true; + for (auto& op : operands_) { + if (first) { + result = op->evaluate(level + 1); + first = false; + } else { + op->merge(result); + } + } + } else { + CGAL::Nef_nary_intersection_3> builder; + for (auto& op : operands_) { + builder.add_polyhedron(op->evaluate(level + 1)); + } + result = builder.get_intersection(); } - result = builder.get_intersection(); /* CGAL::Nef_polyhedron_3 r; @@ -302,6 +356,7 @@ public: } }; +// Utility function to extrude a polyhedral facet template void extrude(LoopType bottom, const CGAL::Vector_3& V, CGAL::Polyhedron_3& P) { std::list face_list = { bottom }; @@ -351,6 +406,7 @@ void extrude(LoopType bottom, const CGAL::Vector_3& V, CGAL::Polyhedron_ CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh(unique_points, facet_vertices, P); } +// Create cube of half-distance d template void createCube(CGAL::Polyhedron_3& P, double d) { typedef CGAL::Point_3 Point; @@ -363,17 +419,22 @@ void createCube(CGAL::Polyhedron_3& P, double d) { Point(-d, +d, -d) } }; - typename Kernel::Vector_3 V(0, 0, d * 2); + CGAL::Vector_3 V(0, 0, d * 2); extrude(bottom, V, P); } +// Leaf of halfspace tree stored as a Plane_3 template class halfspace_tree_plane : public halfspace_tree { private: typename Kernel::Plane_3 plane_; public: + virtual tree_type kind() const { + return TT_PLANE; + } + halfspace_tree_plane(const typename Kernel::Plane_3& plane) : plane_(plane) {} @@ -381,7 +442,7 @@ public: // std::cout << std::string(level * 2, ' ') << "p " << plane_ << std::endl; if constexpr(CGAL::Is_extended_kernel::value_type::value) { - static_assert(false, "Not implemented yet"); + throw std::runtime_error("Not implemented yet"); // typename Kernel::Plane_3 plane(plane_.a().exact(), plane_.b().exact(), plane_.c().exact(), plane_.d().exact()); // CGAL::Nef_polyhedron_3 plane_nef(plane, CGAL::Nef_polyhedron_3::Boundary::INCLUDED); // CGAL::Nef_polyhedron_3 full_nef(full); @@ -393,9 +454,12 @@ public: createCube(P, 10000); return CGAL::Nef_polyhedron_3(P); }(); - return almost_complete.intersection(plane_.opposite(), CGAL::Nef_polyhedron_3::CLOSED_HALFSPACE); + return almost_complete.intersection(plane_.opposite(), CGAL::Nef_polyhedron_3::OPEN_HALFSPACE).closure(); } } + virtual void merge(CGAL::Nef_polyhedron_3& a) const { + a = a.intersection(plane_.opposite(), CGAL::Nef_polyhedron_3::OPEN_HALFSPACE).closure(); + } virtual void accumulate(std::list& points) const { points.push_back(plane_); } @@ -409,9 +473,10 @@ public: } }; +// Triangulate a nef facet. Used for intersection check to find convex subcomponent template std::vector> triangulate_nef_facet(typename CGAL::Nef_polyhedron_3::Halffacet_const_handle f) { - typedef CGAL::Nef_polyhedron_3::SHalfedge_around_facet_const_circulator SHalfedge_around_facet_const_circulator; + typedef typename CGAL::Nef_polyhedron_3::SHalfedge_around_facet_const_circulator SHalfedge_around_facet_const_circulator; std::vector ps; SHalfedge_around_facet_const_circulator it(f->facet_cycles_begin()); @@ -446,9 +511,11 @@ using Graph = boost::adjacency_list, boost::property>; +// Build a boost graph with vertex corresponding to Nef facet, edge corresponding to Nef edge +// marked as reflex or not. template Graph build_facet_edge_graph(const CGAL::Nef_polyhedron_3& poly) { - typedef CGAL::Nef_polyhedron_3::Halffacet_const_handle Halffacet_const_handle; + typedef typename CGAL::Nef_polyhedron_3::Halffacet_const_handle Halffacet_const_handle; Graph G; @@ -499,11 +566,14 @@ Graph build_facet_edge_graph(const CGAL::Nef_polyhedron_3& poly) } } + return G; +} + +template +void dump_facets(Graph& G) { for (size_t ii = 0; ii < boost::num_vertices(G); ++ii) { // std::cout << ii << " " << dump_facet(G[ii].facet) << std::endl; } - - return G; } /* @@ -524,6 +594,23 @@ struct Intersection_visitor { }; */ +template +struct Segment_collector { + typedef void result_type; + boost::optional> segment; + + void operator()(const CGAL::Point_3&) + { + } + void operator()(const CGAL::Segment_3& s) + { + segment = s; + } + void operator()(const CGAL::Triangle_3&) + { + } +}; + template class convex_subcomponent_visitor : public boost::default_bfs_visitor { @@ -536,7 +623,7 @@ public: : edgetype_(edgetype), components_(component) {} template - void tree_edge(Edge e, const Graph& g) { + bool tree_edge(Edge e, const Graph& g) { if (boost::get(boost::edge_weight, g, e) == edgetype_) { auto srcid = boost::source(e, g); auto tgtid = boost::target(e, g); @@ -549,6 +636,8 @@ public: std::swap(src, tgt); } + // std::cout << " v " << srcid << " -> " << tgtid << " ??" << std::endl; + if (*tgt == -1) { bool tgt_has_any_reflex_edge = false; @@ -556,11 +645,14 @@ public: for (boost::tie(ei, ei_end) = boost::out_edges(tgtid, g); ei != ei_end; ++ei) { if (boost::get(boost::edge_weight, g, *ei) != edgetype_) { tgt_has_any_reflex_edge = true; + // std::cout << " reflex: " << boost::source(*ei, g) << " -- " << boost::target(*ei, g) << std::endl; break; } } - if (!tgt_has_any_reflex_edge) { + if (tgt_has_any_reflex_edge) { + // std::cout << " x has reflex edge" << std::endl; + } else { // topological check completed, now check geometry, non topologically connected facets should not geometrically intersect bool any_intersecting = false; auto& plane_tgt = g[tgtid].facet->plane(); @@ -578,44 +670,113 @@ public: if (!has_edge) { auto triangles_i = triangulate_nef_facet(g[i].facet); - // std::cout << "i " << i << ": " << std::endl; - for (auto& t : triangles_i) { - // std::cout << " " << t << std::endl; + std::list> edges_i; + + for (auto fc = g[i].facet->facet_cycles_begin(); fc != g[i].facet->facet_cycles_end(); ++fc) { + auto se = typename CGAL::Nef_polyhedron_3::SHalfedge_const_handle(fc); + CGAL_assertion(se != 0); + typename CGAL::Nef_polyhedron_3::SHalfedge_around_facet_const_circulator hc(se); + typename CGAL::Nef_polyhedron_3::SHalfedge_around_facet_const_circulator hc_end(hc); + + CGAL_For_all(hc, hc_end) { + edges_i.emplace_back(hc->target()->center_vertex()->point(), hc->source()->center_vertex()->point()); + } } - if (std::any_of(triangles_i.begin(), triangles_i.end(), [&plane_tgt](CGAL::Triangle_3& t) { + if (std::any_of(triangles_i.begin(), triangles_i.end(), [&plane_tgt, &edges_i](CGAL::Triangle_3& t) { auto x = CGAL::intersection(plane_tgt, t); if (x) { - // std::cout << "t " << t << " x " << std::endl; + // std::cout << " triangle: " << t << std::endl; // Intersection_visitor v; - // boost::apply_visitor([](auto x) {std::cout << x << std::endl; })(*x); + // boost::apply_visitor([](auto x) {std::cout << " intersects: " << x << std::endl; })(*x); + Segment_collector sc; + boost::apply_visitor(sc)(*x); + if (sc.segment) { + if (!std::any_of(edges_i.begin(), edges_i.end(), [&sc](CGAL::Segment_3& s) { + // When intersecting with the boundary of a facet we likely multiple co-planar facets. Exclude intersection. + auto xy = CGAL::intersection(s, *sc.segment); + Segment_collector scy; + boost::apply_visitor(scy)(*xy); + return (bool)scy.segment; + })) { + return true; + } + } } - return (bool)x; + return false; })) { + // std::cout << " intersects with: " << i << ": " << std::endl; + for (auto& t : triangles_i) { + // std::cout << " " << t << std::endl; + } + any_intersecting = true; break; } } } } - if (!any_intersecting) { - // std::cout << "v " << srcid << " -> " << tgtid << std::endl; + if (any_intersecting) { + // std::cout << " x has intersection" << std::endl; + } else { + // std::cout << "v " << srcid << " -> " << tgtid << " !!" << std::endl; // std::cout << "(" << *src << " " << *tgt << ")" << std::endl; *tgt = *src; + return true; } } } } + return false; } }; +// bfs implementation to that respects a predicate on determining whether an edge is applicable +template +void bfs(Graph& g, size_t start_vertex, Fn& fn) { + std::queue queue; + queue.push(start_vertex); + + std::set visited; + visited.insert(start_vertex); + + while (!queue.empty()) { + auto cur = queue.front(); + queue.pop(); + + typename boost::graph_traits>::out_edge_iterator ei, ei_end; + for (boost::tie(ei, ei_end) = boost::out_edges(cur, g); ei != ei_end; ++ei) { + auto s = boost::source(*ei, g); + auto t = boost::target(*ei, g); + + // @todo is this necessary? + if (cur == t) { + std::swap(s, t); + } + + if (visited.find(t) == visited.end()) { + // @nb a boolean condition on tree_edge() so that + // we can influence traversal with out topological and geometrical + // constraints. + if (fn.tree_edge(*ei, g)) { + queue.push(t); + visited.insert(t); + } + } + } + } +} + +// builds a tree of halfspaces from an input nef polyhedron +// checks whether output is equivalent and if not uses a convex decomposition first template -std::unique_ptr> build_halfspace_tree(Graph& G, CGAL::Nef_polyhedron_3& poly, bool negate = false) { - typedef boost::filtered_graph, boost::keep_all, std::function::vertex_descriptor)>> FilteredGraph; +std::unique_ptr> build_halfspace_tree(Graph& G, CGAL::Nef_polyhedron_3& poly, bool negate = false, int level=0) { + typedef boost::filtered_graph, boost::keep_all, std::function::vertex_descriptor)>> FilteredGraph; auto edge_trait = negate ? CONCAVE : CONVEX; + /* bool all_convex = true; typename boost::graph_traits>::edge_iterator ei, ei_end; for (boost::tie(ei, ei_end) = boost::edges(G); ei != ei_end; ++ei) { @@ -624,41 +785,7 @@ std::unique_ptr> build_halfspace_tree(Graph& break; } } - - if (!all_convex) { - std::unique_ptr> tree; - std::list>> root_expression; - - CGAL::convex_decomposition_3(poly); - // the first volume is the outer volume, which is - // ignored in the decomposition - auto ci = ++poly.volumes_begin(); - int NN = 0; - - for (; ci != poly.volumes_end(); ++ci, ++NN) { - std::list>> sub_expression; - - if (ci->mark()) { - // @todo couldn't get it to work with the multiple volumes of a complex decomposition - // directly, so for now we need to isolate the individual volumes. - CGAL::Polyhedron_3 P; - poly.convert_inner_shell_to_polyhedron(ci->shells_begin(), P); - CGAL::Nef_polyhedron_3 Pnef(P); - auto Pgraph = build_facet_edge_graph(Pnef); - for (size_t ii = 0; ii < boost::num_vertices(Pgraph); ++ii) { - auto& p0 = Pgraph[ii].facet->plane(); - // @todo this is to convert from kernel to extended kernel, can be if constexpr perhaps? - CGAL::Plane_3 p1(p0.a().exact(), p0.b().exact(), p0.c().exact(), p0.d().exact()); - sub_expression.emplace_back(new halfspace_tree_plane(p1)); - } - } - - root_expression.emplace_back(new halfspace_tree_nary_branch(OP_INTERSECTION, std::move(sub_expression))); - } - - tree.reset(new halfspace_tree_nary_branch(OP_UNION, std::move(root_expression))); - return tree; - } + */ // boost::write_graphviz(std::cout, G); @@ -683,12 +810,26 @@ std::unique_ptr> build_halfspace_tree(Graph& std::vector components(boost::num_vertices(sub_graph_0), -1); int largest_component_idx = -1; - convex_subcomponent_visitor visitor(edge_trait, components); int num_components = 0; + + // @nb we don't just randomly start from an arbitrary seed, but we sort planes by d / | abc | + // for (size_t i = 0; i < boost::num_vertices(sub_graph_0); ++i) { + + std::vector sorted_verts; for (size_t i = 0; i < boost::num_vertices(sub_graph_0); ++i) { + sorted_verts.push_back(i); + } + std::sort(sorted_verts.begin(), sorted_verts.end(), [&G](size_t a, size_t b) { + auto da = G[a].facet->plane().d() / G[a].facet->plane().orthogonal_vector().squared_length(); + auto db = G[b].facet->plane().d() / G[b].facet->plane().orthogonal_vector().squared_length(); + return db < da; + }); + + convex_subcomponent_visitor visitor(edge_trait, components); + for (auto& i : sorted_verts) { if (components[i] == -1) { components[i] = num_components++; - boost::breadth_first_search(sub_graph_0, boost::vertex(i, sub_graph_0), boost::visitor(visitor)); + bfs(sub_graph_0, boost::vertex(i, sub_graph_0), visitor); } } @@ -750,7 +891,7 @@ std::unique_ptr> build_halfspace_tree(Graph& // @nb counting vertices on filtered_graph returns the original amount if (boost::num_vertices(sub_graph)) { - auto remainder = build_halfspace_tree(sub_graph, poly, !negate); + auto remainder = build_halfspace_tree(sub_graph, poly, !negate, level+1); std::list>> sub_expression; sub_expression.emplace_back(std::move(tree)); @@ -763,10 +904,193 @@ std::unique_ptr> build_halfspace_tree(Graph& } if (root_expression_0.size() == 1) { - return std::move(root_expression_0.front()); + tree_0 = std::move(root_expression_0.front()); + } else { + tree_0.reset(new halfspace_tree_nary_branch(OP_UNION, std::move(root_expression_0))); } - tree_0.reset(new halfspace_tree_nary_branch(OP_UNION, std::move(root_expression_0))); + + if (false && level == 0) { + auto compare = tree_0->evaluate(); + auto make_vertex_point_it = [](typename CGAL::Nef_polyhedron_3::Vertex_const_iterator p) { + return boost::make_transform_iterator(p, [](auto v) { return v.point(); }); + }; + + std::set> s1(make_vertex_point_it(poly.vertices_begin()), make_vertex_point_it(poly.vertices_end())); + std::set> s2(make_vertex_point_it(compare.vertices_begin()), make_vertex_point_it(compare.vertices_end())); + + if (s1 != s2) { + std::unique_ptr> tree; + std::list>> root_expression; + + CGAL::convex_decomposition_3(poly); + // the first volume is the outer volume, which is + // ignored in the decomposition + auto ci = ++poly.volumes_begin(); + int NN = 0; + + for (; ci != poly.volumes_end(); ++ci, ++NN) { + std::list>> sub_expression; + + if (ci->mark()) { + // @todo couldn't get it to work with the multiple volumes of a complex decomposition + // directly, so for now we need to isolate the individual volumes. + CGAL::Polyhedron_3 P; + poly.convert_inner_shell_to_polyhedron(ci->shells_begin(), P); + CGAL::Nef_polyhedron_3 Pnef(P); + auto Pgraph = build_facet_edge_graph(Pnef); + for (size_t ii = 0; ii < boost::num_vertices(Pgraph); ++ii) { + auto& p0 = Pgraph[ii].facet->plane(); + // @todo this is to convert from kernel to extended kernel, can be if constexpr perhaps? + CGAL::Plane_3 p1(p0.a().exact(), p0.b().exact(), p0.c().exact(), p0.d().exact()); + sub_expression.emplace_back(new halfspace_tree_plane(p1)); + } + } + + root_expression.emplace_back(new halfspace_tree_nary_branch(OP_INTERSECTION, std::move(sub_expression))); + } + + tree.reset(new halfspace_tree_nary_branch(OP_UNION, std::move(root_expression))); + return tree; + } + } + return std::move(tree_0); } +template +size_t edge_contract(Graph& G) { + size_t n = 0; + typename boost::graph_traits>::edge_iterator ei, ei_end; + bool has_contracted = true; + while (has_contracted) { + has_contracted = false; + for (boost::tie(ei, ei_end) = boost::edges(G); ei != ei_end; ++ei) { + auto srcid = boost::source(*ei, G); + auto tgtid = boost::target(*ei, G); + auto a = G[srcid].facet->plane().orthogonal_vector(); + auto b = G[tgtid].facet->plane().orthogonal_vector(); + // std::cout << srcid << " -- " << tgtid << std::endl << G[srcid].facet->plane() << std::endl << G[tgtid].facet->plane() << std::endl << CGAL::approximate_angle(a, b) << std::endl; + if (CGAL::approximate_angle(a, b) < 0.1) { + for (auto oe : boost::make_iterator_range(boost::out_edges(tgtid, G))) { + auto tt = boost::target(oe, G); + if (srcid != tt) { // Avoid self-loop + bool exists = boost::edge(srcid, tt, G).second; + if (!exists) { + boost::add_edge(srcid, tt, G); + } + } + } + ++n; + has_contracted = true; + + boost::clear_vertex(tgtid, G); + boost::remove_vertex(tgtid , G); + + break; + } + } + } + return n; +} + + +// Visitor for Nef_polyhedron_3 shells to convert facets to Polyhedron_3 +// using the Polygon_mesh_processing package and Polygon_triangulation_decomposition_2 +// in case of facets with inner bounds. +template +class Polysoup_builder { +private: + std::map, size_t> verts; + std::vector> facets; +public: + void visit(typename CGAL::Nef_polyhedron_3::Vertex_const_handle) {} + void visit(typename CGAL::Nef_polyhedron_3::Halfedge_const_handle) {} + void visit(typename CGAL::Nef_polyhedron_3::Halffacet_const_handle h) { + boost::optional> pwh; + auto nf = std::distance(h->facet_cycles_begin(), h->facet_cycles_end()); + for (auto fc = h->facet_cycles_begin(); fc != h->facet_cycles_end(); ++fc) { + // std::cout << "h->plane().point() " << h->plane().point() << std::endl; + // std::cout << "h->plane().base1() " << h->plane().base1() << std::endl; + // std::cout << "h->plane().base2() " << h->plane().base2() << std::endl; + + auto se = typename CGAL::Nef_polyhedron_3::SHalfedge_const_handle(fc); + CGAL_assertion(se != 0); + typename CGAL::Nef_polyhedron_3::SHalfedge_around_facet_const_circulator hc(se); + typename CGAL::Nef_polyhedron_3::SHalfedge_around_facet_const_circulator hc_end(hc); + + CGAL::Polygon_2 loop; + if (nf == 1) { + facets.emplace_back(); + } + + CGAL_For_all(hc, hc_end) { + auto p = hc->source()->center_vertex()->point(); + if (nf == 1) { + facets.back().push_back(verts.insert({ p , verts.size() }).first->second); + } else { + // std::cout << "p " << p << std::endl; + auto v = p - h->plane().point(); + // std::cout << "v " << v << std::endl; + CGAL::Point_2 uv(v * h->plane().base1(), v * h->plane().base2()); + // std::cout << "uv " << uv << std::endl; + loop.push_back(uv); + } + } + + if (pwh) { + pwh->add_hole(loop); + } else { + pwh.emplace(loop); + } + } + + if (nf > 1) { + CGAL::Polygon_triangulation_decomposition_2 decompositor; + std::list> decom_polies; + decompositor(*pwh, std::back_inserter(decom_polies)); + for (auto& p : decom_polies) { + facets.emplace_back(); + for (auto it = p.vertices_begin(); it != p.vertices_end(); ++it) { + // std::cout << "*it " << *it << std::endl; + auto du = it->x() * h->plane().base1() / h->plane().base1().squared_length(); + auto dv = it->y() * h->plane().base2() / h->plane().base2().squared_length(); + auto pp = h->plane().point() + du + dv; + // std::cout << "pp " << pp << std::endl; + facets.back().push_back(verts.insert({ pp, verts.size() }).first->second); + } + } + } + } + void visit(typename CGAL::Nef_polyhedron_3::SHalfedge_const_handle) {} + void visit(typename CGAL::Nef_polyhedron_3::SHalfloop_const_handle) {} + void visit(typename CGAL::Nef_polyhedron_3::SFace_const_handle) {} + void build(CGAL::Polyhedron_3& P) { + std::vector> verts_vector(verts.size()); + for (auto& p : verts) { + verts_vector[p.second] = p.first; + } + CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh(verts_vector, facets, P); + } +}; + +// For some reason gives better results then Nef_polyhedron_3.convert_to_polyhedron() in some cases +template +bool convert_to_polyhedron(const CGAL::Nef_polyhedron_3& a, CGAL::Polyhedron_3& b, size_t volume_index=0) { + size_t v = 0; + for (auto it = a.volumes_begin(); it != a.volumes_end(); ++it) { + if (!it->mark()) { + continue; + } + for (auto jt = it->shells_begin(); jt != it->shells_end(); ++jt) { + if (v++ == volume_index) { + Polysoup_builder vis; + a.visit_shell_objects(CGAL::Nef_polyhedron_3::SFace_const_handle(jt), vis); + vis.build(b); + return true; + } + } + } + return false; +} + #endif \ No newline at end of file