From aaf05585b5f96bdef988a07d65e4f2df428de1fc Mon Sep 17 00:00:00 2001 From: Simon Giraudot Date: Thu, 6 Jun 2019 14:56:45 +0200 Subject: [PATCH] WIP: operations almost all working in regular cases --- .../CGAL/boost/graph/Euler_operations.h | 66 + .../kinetic_precomputed_shapes_example.cpp | 2 +- .../include/CGAL/KSR/debug.h | 106 +- .../include/CGAL/KSR_3/Data_structure.h | 1183 ++++++++++++----- .../include/CGAL/KSR_3/Event.h | 84 +- .../include/CGAL/KSR_3/Event_queue.h | 50 +- .../include/CGAL/KSR_3/Intersection_graph.h | 27 +- .../include/CGAL/KSR_3/Polygon_splitter.h | 6 +- .../include/CGAL/KSR_3/Support_plane.h | 114 +- .../CGAL/Kinetic_shape_reconstruction_3.h | 367 +++-- .../include/CGAL/Surface_mesh/Surface_mesh.h | 2 +- 11 files changed, 1485 insertions(+), 522 deletions(-) diff --git a/BGL/include/CGAL/boost/graph/Euler_operations.h b/BGL/include/CGAL/boost/graph/Euler_operations.h index 12e6ed25252..be549fcf50d 100644 --- a/BGL/include/CGAL/boost/graph/Euler_operations.h +++ b/BGL/include/CGAL/boost/graph/Euler_operations.h @@ -1317,6 +1317,72 @@ flip_edge(typename boost::graph_traits::halfedge_descriptor h, set_halfedge(foh,oh,g); } +/// \todo document me +template +void shift_source (typename boost::graph_traits::halfedge_descriptor h, + Graph& g) +{ + typedef typename boost::graph_traits Traits; + typedef typename Traits::halfedge_descriptor halfedge_descriptor; + typedef typename Traits::vertex_descriptor vertex_descriptor; + typedef typename Traits::face_descriptor face_descriptor; + + + halfedge_descriptor hp = prev (h, g); + halfedge_descriptor ho = opposite (h, g); + halfedge_descriptor hon = next (ho, g); + halfedge_descriptor hpp = prev (hp, g); + vertex_descriptor vppt = target (hpp, g); + + face_descriptor f = face (h, g); + face_descriptor fo = face (ho, g); + + CGAL_assertion (f != Traits::null_face()); + CGAL_assertion (fo != Traits::null_face()); + + if (halfedge (f, g) == hp) + set_halfedge (f, h, g); + + set_next (ho, hp, g); + set_next (hp, hon, g); + set_next (hpp, h, g); + set_target (ho, vppt, g); + set_face (hp, fo, g); +} + +/// \todo document me +template +void shift_target (typename boost::graph_traits::halfedge_descriptor h, + Graph& g) +{ + typedef typename boost::graph_traits Traits; + typedef typename Traits::halfedge_descriptor halfedge_descriptor; + typedef typename Traits::vertex_descriptor vertex_descriptor; + typedef typename Traits::face_descriptor face_descriptor; + + halfedge_descriptor hn = next (h, g); + halfedge_descriptor ho = opposite (h, g); + halfedge_descriptor hop = prev (ho, g); + halfedge_descriptor hnn = next (hn, g); + vertex_descriptor vnt = target (hn, g); + + face_descriptor f = face (h, g); + face_descriptor fo = face (ho, g); + + CGAL_assertion (f != Traits::null_face()); + CGAL_assertion (fo != Traits::null_face()); + + if (halfedge (f, g) == hn) + set_halfedge (f, h, g); + + set_next (hop, hn, g); + set_next (hn, ho, g); + set_next (h, hnn, g); + set_target (h, vnt, g); + set_face (hn, fo, g); +} + + /** * \returns `true` if `e` satisfies the *link condition* \cgalCite{degn-tpec-98}, which guarantees that the surface is also 2-manifold after the edge collapse. */ diff --git a/Kinetic_shape_reconstruction/examples/Kinetic_shape_reconstruction/kinetic_precomputed_shapes_example.cpp b/Kinetic_shape_reconstruction/examples/Kinetic_shape_reconstruction/kinetic_precomputed_shapes_example.cpp index 1f7bb1417d3..cd70a9b082d 100644 --- a/Kinetic_shape_reconstruction/examples/Kinetic_shape_reconstruction/kinetic_precomputed_shapes_example.cpp +++ b/Kinetic_shape_reconstruction/examples/Kinetic_shape_reconstruction/kinetic_precomputed_shapes_example.cpp @@ -2,7 +2,7 @@ #include -#define CGAL_KSR_VERBOSE_LEVEL 3 +#define CGAL_KSR_VERBOSE_LEVEL 4 #include #include diff --git a/Kinetic_shape_reconstruction/include/CGAL/KSR/debug.h b/Kinetic_shape_reconstruction/include/CGAL/KSR/debug.h index e13308d4f3c..6adb4ebc2e2 100644 --- a/Kinetic_shape_reconstruction/include/CGAL/KSR/debug.h +++ b/Kinetic_shape_reconstruction/include/CGAL/KSR/debug.h @@ -53,6 +53,26 @@ void dump_intersection_edges (const DS& data, const std::string& tag = std::stri out << "2 " << data.segment_3 (iedge) << std::endl; } +template +void dump_segmented_edges (const DS& data, const std::string& tag = std::string()) +{ + std::vector out; + for (KSR::size_t i = 0; i < data.nb_intersection_lines(); ++ i) + { + std::string filename = (tag != std::string() ? tag + "_" : "") + "intersection_line_" + std::to_string(i) + ".polylines.txt"; + out.push_back (new std::ofstream (filename)); + out.back()->precision(18); + } + + for (const typename DS::IEdge& iedge : data.iedges()) + { + CGAL_assertion (data.line_idx(iedge) != KSR::no_element()); + *(out[data.line_idx(iedge)]) << "2 " << data.segment_3 (iedge) << std::endl; + } + for (std::ofstream* o : out) + delete o; +} + template void dump_constrained_edges (const DS& data, const std::string& tag = std::string()) { @@ -84,53 +104,53 @@ void dump_polygons (const DS& data, const std::string& tag = std::string()) Uchar_map bbox_green = bbox_mesh.template add_property_map("green", 0).first; Uchar_map bbox_blue = bbox_mesh.template add_property_map("blue", 0).first; - KSR::size_t bbox_nb_vertices = 0; - KSR::size_t nb_vertices = 0; - KSR::vector vertices; + KSR::vector map_vertices; + for (KSR::size_t i = 0; i < data.number_of_support_planes(); ++ i) { if (data.is_bbox_support_plane(i)) { - KSR::size_t new_vertices = 0; + map_vertices.clear(); for (typename DS::PVertex pvertex : data.pvertices(i)) { - bbox_mesh.add_vertex (data.point_3(pvertex)); - ++ new_vertices; + if (map_vertices.size() <= pvertex.second) + map_vertices.resize (pvertex.second + 1); + map_vertices[pvertex.second] = bbox_mesh.add_vertex (data.point_3(pvertex)); } for (typename DS::PFace pface : data.pfaces(i)) { vertices.clear(); for(typename DS::PVertex pvertex : data.pvertices_of_pface(pface)) - vertices.push_back (typename Mesh::Vertex_index(KSR::size_t(pvertex.second) + bbox_nb_vertices)); + vertices.push_back (map_vertices[pvertex.second]); typename Mesh::Face_index face = bbox_mesh.add_face (vertices); std::tie (bbox_red[face], bbox_green[face], bbox_blue[face]) = get_idx_color ((i+1) * (pface.second+1)); } - bbox_nb_vertices += new_vertices; } else { - KSR::size_t new_vertices = 0; + map_vertices.clear(); for (typename DS::PVertex pvertex : data.pvertices(i)) { - mesh.add_vertex (data.point_3 (pvertex)); - ++ new_vertices; + if (map_vertices.size() <= pvertex.second) + map_vertices.resize (pvertex.second + 1); + map_vertices[pvertex.second] = mesh.add_vertex (data.point_3 (pvertex)); } for (typename DS::PFace pface : data.pfaces(i)) { vertices.clear(); + for(typename DS::PVertex pvertex : data.pvertices_of_pface(pface)) - vertices.push_back (typename Mesh::Vertex_index(KSR::size_t(pvertex.second) + nb_vertices)); - + vertices.push_back (map_vertices[pvertex.second]); + typename Mesh::Face_index face = mesh.add_face (vertices); std::tie (red[face], green[face], blue[face]) = get_idx_color (i * (pface.second+1)); } - nb_vertices += new_vertices; } } @@ -139,7 +159,7 @@ void dump_polygons (const DS& data, const std::string& tag = std::string()) CGAL::set_binary_mode (out); CGAL::write_ply(out, mesh); -#if 1 +#if 0 std::string bbox_filename = (tag != std::string() ? tag + "_" : "") + "bbox_polygons.ply"; std::ofstream bbox_out (bbox_filename); CGAL::set_binary_mode (bbox_out); @@ -148,19 +168,57 @@ void dump_polygons (const DS& data, const std::string& tag = std::string()) } +template +void dump_polygon_borders (const DS& data, const std::string& tag = std::string()) +{ + std::string filename = (tag != std::string() ? tag + "_" : "") + "polygon_borders.polylines.txt"; + std::ofstream out (filename); + + for (KSR::size_t i = 6; i < data.number_of_support_planes(); ++ i) + for (const typename DS::PEdge pedge : data.pedges(i)) + out << "2 " << data.segment_3 (pedge) << std::endl; +} + template void dump_event (const DS& data, const Event& ev, const std::string& tag = std::string()) { - std::string lfilename = (tag != std::string() ? tag + "_" : "") + "event_line.polylines.txt"; - std::ofstream lout (lfilename); - lout.precision(18); + if (ev.is_pvertex_to_pvertex()) + { + std::string vfilename = (tag != std::string() ? tag + "_" : "") + "event_pvertex.xyz"; + std::ofstream vout (vfilename); + vout.precision(18); + vout << data.point_3 (ev.pvertex()) << std::endl; - // TODO + std::string ofilename = (tag != std::string() ? tag + "_" : "") + "event_pother.xyz"; + std::ofstream oout (ofilename); + oout.precision(18); + oout << data.point_3 (ev.pother()) << std::endl; + } + else if (ev.is_pvertex_to_iedge()) + { + std::string lfilename = (tag != std::string() ? tag + "_" : "") + "event_iedge.polylines.txt"; + std::ofstream lout (lfilename); + lout.precision(18); + lout << "2 " << data.segment_3 (ev.iedge()) << std::endl; + + std::string vfilename = (tag != std::string() ? tag + "_" : "") + "event_pvertex.xyz"; + std::ofstream vout (vfilename); + vout.precision(18); + vout << data.point_3 (ev.pvertex()); + } + else if (ev.is_pvertex_to_ivertex()) + { + std::string vfilename = (tag != std::string() ? tag + "_" : "") + "event_pvertex.xyz"; + std::ofstream vout (vfilename); + vout.precision(18); + vout << data.point_3 (ev.pvertex()) << std::endl; + + std::string ofilename = (tag != std::string() ? tag + "_" : "") + "event_ivertex.xyz"; + std::ofstream oout (ofilename); + oout.precision(18); + oout << data.point_3 (ev.ivertex()) << std::endl; + } - std::string vfilename = (tag != std::string() ? tag + "_" : "") + "event_vertex.xyz"; - std::ofstream vout (vfilename); - vout.precision(18); -// vout << data.point_of_vertex(ev.vertex_idx()) << std::endl; } template @@ -168,10 +226,12 @@ void dump (const DS& data, const std::string& tag = std::string()) { dump_intersection_edges (data, tag); dump_constrained_edges (data, tag); + dump_polygon_borders (data, tag); dump_polygons (data, tag); } + } // namespace KSR_3 } // namespace CGAL diff --git a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Data_structure.h b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Data_structure.h index d6662eefc31..994a0d5afb7 100644 --- a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Data_structure.h +++ b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Data_structure.h @@ -23,6 +23,8 @@ //#include +#include + #include #include @@ -30,9 +32,9 @@ #include #include -#include -#include +#include +#include #include namespace CGAL @@ -191,7 +193,42 @@ public: { return (support_plane_idx < 6); } bool mesh_is_valid (KSR::size_t support_plane_idx) const - { return mesh(support_plane_idx).is_valid(); } + { + bool is_valid = mesh(support_plane_idx).is_valid(); + if (!is_valid) + return false; + + for (PFace pface : pfaces(support_plane_idx)) + { + std::function unary_f = [&](const PVertex& pvertex) -> Point_2 { return point_2(pvertex); }; + CGAL::Polygon_2 polygon + (boost::make_transform_iterator + (pvertices_of_pface(pface).begin(), unary_f), + boost::make_transform_iterator + (pvertices_of_pface(pface).end(), unary_f)); + + // if (!polygon.is_simple()) + // { + // std::cerr << "PFace(" << pface.first << ":" << pface.second << ") is not simple" << std::endl; + // for (const Point_2& p : polygon) + // std::cerr << to_3d(support_plane_idx,p) << " "; + // std::cerr << to_3d(support_plane_idx,polygon[0]) << " "; + // std::cerr << std::endl; + // return false; + // } + if (!polygon.is_convex()) + { + std::cerr << "PFace(" << pface.first << ":" << pface.second << ") is not convex" << std::endl; + for (const Point_2& p : polygon) + std::cerr << to_3d(support_plane_idx,p) << " "; + std::cerr << to_3d(support_plane_idx,polygon[0]) << " "; + std::cerr << std::endl; + return false; + } + } + + return true; + } KSR::size_t add_support_plane (const Support_plane& new_support_plane) { @@ -235,6 +272,7 @@ public: }); KSR::vector common_planes_idx; + std::map map_lines_idx; KSR::vector vertices; vertices.reserve (intersections.size()); for (std::size_t i = 0; i < intersections.size(); ++ i) @@ -259,6 +297,13 @@ public: CGAL_assertion (common_plane_idx != KSR::no_element()); common_planes_idx.push_back (common_plane_idx); + typename std::map::iterator iter; + bool inserted; + std::tie (iter, inserted) + = map_lines_idx.insert (std::make_pair (common_plane_idx, KSR::no_element())); + if (inserted) + iter->second = m_intersection_graph.add_line(); + vertices.push_back (m_intersection_graph.add_vertex (intersections[i].second).first); } @@ -276,6 +321,8 @@ public: IEdge new_edge = m_intersection_graph.add_edge (vertices[i], vertices[(i+1)%vertices.size()], support_plane_idx).first; m_intersection_graph.intersected_planes(new_edge).insert (common_planes_idx[i]); + m_intersection_graph.set_line (new_edge, map_lines_idx[common_planes_idx[i]]); + support_plane(support_plane_idx).iedges().insert (new_edge); support_plane(common_planes_idx[i]).iedges().insert (new_edge); } @@ -301,8 +348,12 @@ public: for (std::size_t i = 0; i < 4; ++ i) { - IEdge iedge - = m_intersection_graph.add_edge (ivertices[i], ivertices[(i+1)%4], support_plane_idx).first; + IEdge iedge; + bool inserted; + std::tie (iedge, inserted) + = m_intersection_graph.add_edge (ivertices[i], ivertices[(i+1)%4], support_plane_idx); + if (inserted) + m_intersection_graph.set_line (iedge, m_intersection_graph.add_line()); support_plane(support_plane_idx).set_iedge (vertices[i], vertices[(i+1)%4], iedge); @@ -375,14 +426,59 @@ public: } + // Get prev and next of free vertex + PVertex prev (const PVertex& pvertex) const + { + return PVertex (pvertex.first, support_plane(pvertex).prev(pvertex.second)); + } + PVertex next (const PVertex& pvertex) const + { + return PVertex (pvertex.first, support_plane(pvertex).next(pvertex.second)); + } + + // Get prev and next of constrained vertex + std::pair prev_and_next (const PVertex& pvertex) const + { + std::pair out (null_pvertex(), null_pvertex()); + + for (Halfedge_index hi : halfedges_around_target(halfedge(pvertex.second, mesh(pvertex)), mesh(pvertex))) + { + IEdge iedge = support_plane(pvertex).iedge (mesh(pvertex).edge(hi)); + if (iedge == this->iedge(pvertex)) + continue; + if (out.first == null_pvertex()) + out.first = PVertex (pvertex.first, mesh(pvertex).source(hi)); + else + { + out.second = PVertex (pvertex.first, mesh(pvertex).source(hi)); + return out; + } + } + + return out; + } + + std::pair border_prev_and_next (const PVertex& pvertex) const + { + Halfedge_index hi = mesh(pvertex).halfedge(pvertex.second); + if (mesh(pvertex).face(hi) != Face_index()) + hi = mesh(pvertex).prev (mesh(pvertex).opposite(hi)); + + CGAL_assertion (mesh(pvertex).face(hi) == Face_index()); + return std::make_pair (PVertex (pvertex.first, mesh(pvertex).source (hi)), + PVertex (pvertex.first, mesh(pvertex).target (mesh(pvertex).next(hi)))); + } + PVertex add_pvertex (KSR::size_t support_plane_idx, const Point_2& point) { return PVertex (support_plane_idx, mesh(support_plane_idx).add_vertex(point)); } - PFace add_pface (KSR::size_t support_plane_idx, const std::vector& pvertices) + + template + PFace add_pface (const VertexRange& pvertices) { - return PFace (support_plane_idx, - mesh(support_plane_idx).add_face + return PFace (pvertices.front().first, + mesh(pvertices.front()).add_face (CGAL::make_range (boost::make_transform_iterator (pvertices.begin(), @@ -417,7 +513,25 @@ public: CGAL_assertion (mesh(pedge).source(mesh(pedge).halfedge(pedge.second)) == pvertex.second); return PVertex (pedge.first, mesh(pedge).target(mesh(pedge).halfedge(pedge.second))); } - + + PFace pface_of_pvertex (const PVertex& pvertex) const + { + return PFace (pvertex.first, support_plane(pvertex).face (pvertex.second)); + } + + std::pair pfaces_of_pvertex (const PVertex& pvertex) const + { + std::pair out (null_pface(), null_pface()); + + std::tie (out.first.second, out.second.second) + = support_plane(pvertex).faces (pvertex.second); + + if (out.first.second != Face_index()) + out.first.first = pvertex.first; + if (out.second.second != Face_index()) + out.second.first = pvertex.first; + return out; + } PVertices_of_pface pvertices_of_pface (const PFace& pface) const { @@ -448,6 +562,11 @@ public: KSR::size_t& input (const PFace& pface) { return support_plane(pface).input(pface.second); } + const unsigned int& k (const PFace& pface) const + { return support_plane(pface).k(pface.second); } + unsigned int& k (const PFace& pface) + { return support_plane(pface).k(pface.second); } + bool is_frozen (const PVertex& pvertex) const { return support_plane(pvertex).is_frozen (pvertex.second); } const Vector_2& direction (const PVertex& pvertex) const @@ -457,6 +576,39 @@ public: FT speed (const PVertex& pvertex) { return support_plane(pvertex).speed (pvertex.second); } + void freeze (PVertex& pvertex) + { + Point_2 p = point_2 (pvertex, m_current_time); + support_plane(pvertex).direction (pvertex.second) = CGAL::NULL_VECTOR; + support_plane(pvertex).set_point (pvertex.second, p); + } + + bool is_active (const PVertex& pvertex) const + { return support_plane(pvertex).is_active (pvertex.second); } + + void deactivate (const PVertex& pvertex) + { + support_plane(pvertex).set_active (pvertex.second, false); + if (iedge(pvertex) != null_iedge()) + m_intersection_graph.is_active(iedge(pvertex)) = false; + if (ivertex(pvertex) != null_ivertex()) + { + m_intersection_graph.is_active(ivertex(pvertex)) = false; + std::cerr << str(ivertex(pvertex)) << " deactivated" << std::endl; + } + } + void activate (const PVertex& pvertex) + { + support_plane(pvertex).set_active (pvertex.second, true); + if (iedge(pvertex) != null_iedge()) + m_intersection_graph.is_active(iedge(pvertex)) = true; + if (ivertex(pvertex) != null_ivertex()) + { + m_intersection_graph.is_active(ivertex(pvertex)) = true; + std::cerr << str(ivertex(pvertex)) << " activated" << std::endl; + } + } + /******************************* * ISimplices *******************************/ @@ -467,6 +619,10 @@ public: IVertices ivertices() const { return m_intersection_graph.vertices(); } IEdges iedges() const { return m_intersection_graph.edges(); } + KSR::size_t nb_intersection_lines() const { return m_intersection_graph.nb_lines(); } + KSR::size_t line_idx (const IEdge& iedge) const { return m_intersection_graph.line (iedge); } + KSR::size_t line_idx (const PVertex& pvertex) const { return line_idx (iedge(pvertex)); } + IVertex add_ivertex (const Point_3& point, const KSR::Idx_set& support_planes_idx) { KSR::Idx_vector vec_planes; @@ -480,7 +636,7 @@ public: } void add_iedge (const KSR::Idx_set& support_planes_idx, - KSR::vector& vertices) + KSR::vector& vertices) { Point_3 source = m_intersection_graph.point_3 (vertices.front()); @@ -491,6 +647,8 @@ public: < CGAL::squared_distance (source, m_intersection_graph.point_3(b))); }); + KSR::size_t line_idx = m_intersection_graph.add_line(); + for (KSR::size_t i = 0; i < vertices.size() - 1; ++ i) { IEdge iedge; @@ -500,6 +658,7 @@ public: vertices[i+1], support_planes_idx); CGAL_assertion (inserted); + m_intersection_graph.set_line (iedge, line_idx); for (KSR::size_t support_plane_idx : support_planes_idx) support_plane(support_plane_idx).iedges().insert (iedge); @@ -510,6 +669,14 @@ public: { return m_intersection_graph.source (edge); } IVertex target (const IEdge& edge) const { return m_intersection_graph.target (edge); } + IVertex opposite (const IEdge& edge, const IVertex& ivertex) const + { + IVertex out = source (edge); + if (out == ivertex) + return target (edge); + CGAL_assertion (target(edge) == ivertex); + return out; + } Incident_iedges incident_iedges (const IVertex& ivertex) const { return m_intersection_graph.incident_edges(ivertex); } @@ -532,8 +699,12 @@ public: } return out; } - + bool is_active (const IEdge& iedge) const + { return m_intersection_graph.is_active (iedge); } + bool is_active (const IVertex& ivertex) const + { return m_intersection_graph.is_active (ivertex); } + bool is_bbox_iedge (const IEdge& edge) const { for (KSR::size_t support_plane_idx : m_intersection_graph.intersected_planes(edge)) @@ -546,6 +717,11 @@ public: * Connectivity *******************************/ + bool has_ivertex (const PVertex& pvertex) const + { return support_plane(pvertex).has_ivertex(pvertex.second); } + IVertex ivertex (const PVertex& pvertex) const + { return support_plane(pvertex).ivertex(pvertex.second); } + bool has_iedge (const PVertex& pvertex) const { return support_plane(pvertex).has_iedge(pvertex.second); } IEdge iedge (const PVertex& pvertex) const @@ -556,12 +732,98 @@ public: IEdge iedge (const PEdge& pedge) const { return support_plane(pedge).iedge(pedge.second); } + void connect (const PVertex& pvertex, const IVertex& ivertex) { support_plane(pvertex).set_ivertex (pvertex.second, ivertex); } void connect (const PVertex& pvertex, const IEdge& iedge) { support_plane(pvertex).set_iedge (pvertex.second, iedge); } void connect (const PVertex& a, const PVertex& b, const IEdge& iedge) { support_plane(a).set_iedge (a.second, b.second, iedge); } + void connect (const PEdge& pedge, const IEdge& iedge) + { support_plane(pedge).set_iedge (pedge.second, iedge); } + + IVertex disconnect_ivertex (const PVertex& pvertex) + { + IVertex out = ivertex (pvertex); + support_plane(pvertex).set_ivertex (pvertex.second, null_ivertex()); + return out; + } + IEdge disconnect_iedge (const PVertex& pvertex) + { + IEdge out = iedge (pvertex); + support_plane(pvertex).set_iedge (pvertex.second, null_iedge()); + return out; + } + + struct Queue_element + { + PVertex previous; + PVertex pvertex; + bool front; + + Queue_element (const PVertex& previous, const PVertex& pvertex, bool front) + : previous (previous), pvertex (pvertex), front (front) { } + }; + + std::vector pvertices_around_ivertex (const PVertex& pvertex, const IVertex& ivertex) const + { + std::deque vertices; + vertices.push_back (pvertex); + + std::queue todo; + PVertex prev, next; + std::tie (prev, next) = border_prev_and_next (pvertex); + todo.push (Queue_element (pvertex, prev, true)); + todo.push (Queue_element (pvertex, next, false)); + + while (!todo.empty()) + { + PVertex previous = todo.front().previous; + PVertex current = todo.front().pvertex; + bool front = todo.front().front; + todo.pop(); + + IEdge iedge = this->iedge (current); + if (iedge == null_iedge()) + continue; + + if (source(iedge) != ivertex && target(iedge) != ivertex) + continue; + + IVertex other = source(iedge); + if (other == ivertex) + other = target(iedge); + else + CGAL_assertion (target(iedge) == ivertex); + + // Filter backwards vertex + if (direction (current) * Vector_2 (point_2 (current.first, other), + point_2 (current.first, ivertex)) + < 0) + continue; + + if (front) + vertices.push_front (current); + else + vertices.push_back (current); + + std::tie (prev, next) = border_prev_and_next (current); + + if (prev == previous) + { + CGAL_assertion (next != previous); + todo.push (Queue_element (current, next, front)); + } + else + todo.push (Queue_element (current, prev, front)); + } + + std::vector out; + out.reserve (vertices.size()); + std::copy (vertices.begin(), vertices.end(), + std::back_inserter (out)); + return out; + } /******************************* * Conversions @@ -576,6 +838,8 @@ public: { return support_plane(pvertex).point_2 (pvertex.second, time); } Point_2 point_2 (const PVertex& pvertex) const { return point_2 (pvertex, m_current_time); } + Point_2 point_2 (KSR::size_t support_plane_idx, const IVertex& ivertex) const + { return support_plane(support_plane_idx).to_2d(point_3 (ivertex)); } Segment_2 segment_2 (KSR::size_t support_plane_idx, const IEdge& iedge) const { return support_plane(support_plane_idx).to_2d(segment_3(iedge)); } @@ -597,322 +861,584 @@ public: Segment_3 segment_3 (const IEdge& edge) const { return m_intersection_graph.segment_3 (edge); } - + /******************************* + * Predicates + *******************************/ - - -#if 0 - inline bool point_is_inside_bbox_section_of_intersection_line - (const Point_3& point, KSR::size_t intersection_line_idx) const + std::pair collision_occured (const PVertex& pvertex, const IEdge& iedge) const { - Vector_3 ref (meta_vertex(intersection_line(intersection_line_idx).meta_vertices_idx()[0]).point(), - meta_vertex(intersection_line(intersection_line_idx).meta_vertices_idx()[1]).point()); - Vector_3 position (meta_vertex(intersection_line(intersection_line_idx).meta_vertices_idx()[0]).point(), - point); - - if (ref * position < 0) - return false; - - return (position * position) < (ref * ref); - } - - KSR::size_t add_intersection_line (KSR::size_t support_plane_idx, - KSR::size_t meta_vertex_idx_0, - KSR::size_t meta_vertex_idx_1) - { - KSR::size_t intersection_line_idx - = add_intersection_line (Intersection_line (Line_3 (meta_vertex(meta_vertex_idx_0).point(), - meta_vertex(meta_vertex_idx_1).point()))); + bool collision = false; - KSR::size_t common_support_plane_idx = KSR::no_element(); - for (KSR::size_t support_plane_idx_0 : meta_vertex(meta_vertex_idx_0).support_planes_idx()) - for (KSR::size_t support_plane_idx_1 : meta_vertex(meta_vertex_idx_1).support_planes_idx()) - if (support_plane_idx_0 != support_plane_idx - && support_plane_idx_0 == support_plane_idx_1) + for (KSR::size_t support_plane_idx : intersected_planes(iedge)) + { + if (support_plane_idx < 6) + return std::make_pair (true, true); + + for (const PEdge& pedge : pedges(support_plane_idx)) + if (this->iedge(pedge) == iedge) { - common_support_plane_idx = support_plane_idx_0; - break; + Segment_2 iedge_segment = segment_2 (support_plane_idx, iedge); + + Vector_2 source_2_vertex (iedge_segment.source(), point_2(pvertex)); + + FT prod = iedge_segment.to_vector() * source_2_vertex; + if (prod < 0) + continue; + + if (source_2_vertex.squared_length() <= iedge_segment.squared_length()) + { + collision = true; + break; + } } - CGAL_assertion (common_support_plane_idx != KSR::no_element()); - CGAL_assertion (common_support_plane_idx < 6); - - intersection_line(intersection_line_idx).meta_vertices_idx().push_back (meta_vertex_idx_0); - intersection_line(intersection_line_idx).meta_vertices_idx().push_back (meta_vertex_idx_1); - - intersection_line(intersection_line_idx).support_planes_idx().push_back (support_plane_idx); - intersection_line(intersection_line_idx).support_planes_idx().push_back (common_support_plane_idx); - support_plane(support_plane_idx).intersection_lines_idx().push_back (intersection_line_idx); - support_plane(common_support_plane_idx).intersection_lines_idx().push_back (intersection_line_idx); - - KSR::Idx_vector polygons_idx = support_plane(common_support_plane_idx).polygons_idx(); - for (KSR::size_t polygon_idx : polygons_idx) - if (do_intersect (polygon_idx, line_on_support_plane (intersection_line_idx, common_support_plane_idx))) - cut_polygon (polygon_idx, intersection_line_idx); - - return intersection_line_idx; - } - - // Add segment on full intersection line, using 2 extrem meta vertices - KSR::size_t add_segment (KSR::size_t intersection_line_idx, KSR::size_t source_idx, KSR::size_t target_idx, - KSR::size_t other_source_idx = KSR::no_element(), - KSR::size_t other_target_idx = KSR::no_element()) - { - KSR::size_t segment_idx = add_segment (Segment (intersection_line_idx, source_idx, target_idx, - other_source_idx, other_target_idx)); - intersection_line(intersection_line_idx).segments_idx().push_back (segment_idx); - return segment_idx; + } + + return std::make_pair (collision, false); } - void partition (KSR::size_t polygon_idx, const Line_2& line, - KSR::Idx_vector& positive_side, KSR::Idx_vector& negative_side) const + /******************************* + * Operations on polygons + *******************************/ + + PVertex crop_polygon (const PVertex& pvertex, const IEdge& iedge) { - std::vector has_on_positive_side; - has_on_positive_side.reserve(polygon(polygon_idx).vertices_idx().size()); + CGAL_KSR_CERR(3) << "*** Cropping " << str(pvertex) << " along " << str(iedge) << std::endl; - for (KSR::size_t vertex_idx : polygon(polygon_idx).vertices_idx()) - has_on_positive_side.push_back (line.has_on_positive_side(vertex(vertex_idx).point(m_current_time))); + Point_2 future_point_a, future_point_b; + Vector_2 direction_a, direction_b; - KSR::size_t first_positive = KSR::no_element(); - - for (std::size_t i = 0; i <= has_on_positive_side.size(); ++ i) - if (!has_on_positive_side[i] && has_on_positive_side[(i+1) % has_on_positive_side.size()]) - { - first_positive = KSR::size_t((i+1) % has_on_positive_side.size()); - break; - } + compute_future_points_and_directions (pvertex, iedge, + future_point_a, future_point_b, + direction_a, direction_b); + + PEdge pedge (pvertex.first, support_plane(pvertex).split_vertex(pvertex.second)); + CGAL_assertion (source(pedge) == pvertex || target(pedge) == pvertex); - KSR::size_t current_position = first_positive; - do + PVertex other = opposite(pedge, pvertex); + + CGAL_KSR_CERR(3) << "*** New edge " << str(pedge) << " between " << str(pvertex) + << " and " << str(other) << std::endl; + + connect (pedge, iedge); + connect (pvertex, iedge); + connect (other, iedge); + + support_plane(pvertex).set_point (pvertex.second, future_point_a); + support_plane(other).set_point (other.second, future_point_b); + + direction(pvertex) = direction_a; + direction(other) = direction_b; + + return other; + } + + std::array propagate_polygon (const PVertex& pvertex, const IEdge& iedge) + { + CGAL_KSR_CERR(3) << "*** Propagating " << str(pvertex) << " along " << str(iedge) << std::endl; + + Point_2 original_point = point_2 (pvertex, 0); + Vector_2 original_direction = direction(pvertex); + + PVertex other = crop_polygon (pvertex, iedge); + + PVertex propagated = add_pvertex (pvertex.first, original_point); + direction(propagated) = original_direction; + + std::array pvertices; + + pvertices[0] = pvertex; + pvertices[1] = other; + pvertices[2] = propagated; + + PFace pface = add_pface (pvertices); + CGAL_assertion (pface.second != Face_index()); + + CGAL_KSR_CERR(3) << "*** New face " << lstr(pface) << std::endl; + + return pvertices; + } + + void transfer_vertex (const PVertex& pvertex, const PVertex& pother) + { + CGAL_KSR_CERR(3) << "*** Transfering " << str(pother) << " through " << str(pvertex) << std::endl; + + // If pvertex is adjacent to one or two + PFace source_face, target_face; + std::tie (source_face, target_face) = pfaces_of_pvertex (pvertex); + + CGAL_KSR_CERR(3) << "*** Initial faces: " << lstr(source_face) + << " and " << lstr(target_face) << std::endl; + + PFace common_pface = pface_of_pvertex (pother); + + if (common_pface == target_face) + std::swap (source_face, target_face); + CGAL_assertion (common_pface == source_face); + + if (target_face == null_pface()) { - if (has_on_positive_side[std::size_t(current_position)]) - positive_side.push_back (polygon(polygon_idx).vertices_idx()[current_position]); - else - negative_side.push_back (polygon(polygon_idx).vertices_idx()[current_position]); - - current_position = (current_position + 1) % has_on_positive_side.size(); - } - while (current_position != first_positive); - - CGAL_assertion (!positive_side.empty() && !negative_side.empty()); - CGAL_assertion (positive_side.size() + negative_side.size() >= 3); - } - - std::tuple - compute_constrained_points_along_line (const Line_2& line_2, - const KSR::Idx_vector& positive_side, - const KSR::Idx_vector& negative_side) const - { - Line_2 line_0 (vertex(positive_side.back()).point(m_current_time), - vertex(negative_side.front()).point(m_current_time)); - Line_2 line_1 (vertex(negative_side.back()).point(m_current_time), - vertex(positive_side.front()).point(m_current_time)); - - Point_2 inter_0 = KSR::intersection_2 (line_0, line_2); - Point_2 inter_1 = KSR::intersection_2 (line_1, line_2); - - // Compute speeds - Line_2 future_line_0 (vertex(positive_side.back()).point(m_current_time + 1), - vertex(negative_side.front()).point(m_current_time + 1)); - Line_2 future_line_1 (vertex(negative_side.back()).point(m_current_time + 1), - vertex(positive_side.front()).point(m_current_time + 1)); - - Point_2 future_inter_0 = KSR::intersection_2 (future_line_0, line_2); - Point_2 future_inter_1 = KSR::intersection_2 (future_line_1, line_2); - - Vector_2 direction_0 (inter_0, future_inter_0); - Vector_2 direction_1 (inter_1, future_inter_1); - - return std::make_tuple (inter_0, direction_0, inter_1, direction_1); - } - - void cut_polygon (KSR::size_t polygon_idx, KSR::size_t intersection_line_idx) - { - CGAL_KSR_CERR(3) << "** Cutting " << polygon_str(polygon_idx) << std::endl; - - Line_2 line_2 = line_on_support_plane (intersection_line_idx, polygon(polygon_idx).support_plane_idx()); - // Partition - KSR::Idx_vector positive_side, negative_side; - partition (polygon_idx, line_2, positive_side, negative_side); - - // Position + Direction of V1 + V2 - Point_2 point_0, point_1; - Vector_2 direction_0, direction_1; - std::tie (point_0, direction_0, point_1, direction_1) - = compute_constrained_points_along_line (line_2, positive_side, negative_side); - - KSR::size_t new_polygon_idx = add_polygon (Polygon(polygon(polygon_idx).input_idx(), polygon(polygon_idx).support_plane_idx())); - support_plane(polygon(polygon_idx).support_plane_idx()).polygons_idx().push_back (new_polygon_idx); - - for (KSR::size_t vertex_idx : positive_side) - vertex(vertex_idx).polygon_idx() = polygon_idx; - for (KSR::size_t vertex_idx : negative_side) - vertex(vertex_idx).polygon_idx() = new_polygon_idx; - - KSR::size_t segment_idx = add_segment (intersection_line_idx, - number_of_vertices(), number_of_vertices() + 1, - number_of_vertices() + 3, number_of_vertices() + 2); - - positive_side.push_back (add_vertex (Vertex (point_0 - m_current_time * direction_0, polygon_idx))); - m_vertices.back().segment_idx() = segment_idx; - m_vertices.back().direction() = direction_0; - - positive_side.push_back (add_vertex (Vertex (point_1 - m_current_time * direction_1, polygon_idx))); - m_vertices.back().segment_idx() = segment_idx; - m_vertices.back().direction() = direction_1; - - negative_side.push_back (add_vertex (Vertex (point_1 - m_current_time * direction_1, new_polygon_idx))); - m_vertices.back().segment_idx() = segment_idx; - m_vertices.back().direction() = direction_1; - - negative_side.push_back (add_vertex (Vertex (point_0 - m_current_time * direction_0, new_polygon_idx))); - m_vertices.back().segment_idx() = segment_idx; - m_vertices.back().direction() = direction_0; - - if (direction_0 == CGAL::NULL_VECTOR) - { - KSR::size_t meta_vertex_idx = add_meta_vertex (support_plane_of_polygon(polygon_idx).to_3d (point_0), - polygon(polygon_idx).support_plane_idx()); - attach_vertex_to_meta_vertex (m_vertices.size() - 4, meta_vertex_idx); - attach_vertex_to_meta_vertex (m_vertices.size() - 1, meta_vertex_idx); - } - - if (direction_1 == CGAL::NULL_VECTOR) - { - KSR::size_t meta_vertex_idx = add_meta_vertex (support_plane_of_polygon(polygon_idx).to_3d (point_1), - polygon(polygon_idx).support_plane_idx()); - attach_vertex_to_meta_vertex (m_vertices.size() - 3, meta_vertex_idx); - attach_vertex_to_meta_vertex (m_vertices.size() - 2, meta_vertex_idx); - } - - polygon(polygon_idx).vertices_idx().swap (positive_side); - polygon(new_polygon_idx).vertices_idx().swap (negative_side); - - CGAL_KSR_CERR(3) << "*** new polygons:"; - for (KSR::size_t i : { polygon_idx, new_polygon_idx }) - CGAL_KSR_CERR(3) << " " << polygon_str(i); - CGAL_KSR_CERR(3) << std::endl; - } - - KSR::size_t crop_polygon (KSR::size_t polygon_idx, KSR::size_t intersection_line_idx, - KSR::Idx_vector& vertices, - const Point_2& point_0, const Vector_2& direction_0, - const Point_2& point_1, const Vector_2& direction_1) - { - m_vertices[vertices.back()] = Vertex (point_1 - m_current_time * direction_1, polygon_idx); - m_vertices[vertices.back()].segment_idx() = number_of_segments(); - m_vertices[vertices.back()].direction() = direction_1; - - vertices.push_back (add_vertex (Vertex (point_0 - m_current_time * direction_0, polygon_idx))); - m_vertices.back().segment_idx() = number_of_segments(); - m_vertices.back().direction() = direction_0; - - KSR::size_t segment_idx = add_segment (intersection_line_idx, - vertices[vertices.size() - 2], - vertices[vertices.size() - 1]); - - polygon(polygon_idx).vertices_idx().swap (vertices); - - return segment_idx; - } - - KSR::size_t propagate_polygon (KSR::size_t segment_idx, - const Point_2& point, - const Vector_2& direction) - { - KSR::size_t source_idx = segment(segment_idx).source_idx(); - KSR::size_t target_idx = segment(segment_idx).target_idx(); - KSR::size_t polygon_idx = vertex(source_idx).polygon_idx(); - - KSR::size_t new_polygon_idx = add_polygon (Polygon(polygon(polygon_idx).input_idx(), polygon(polygon_idx).support_plane_idx())); - support_plane(polygon(polygon_idx).support_plane_idx()).polygons_idx().push_back (new_polygon_idx); - - // Copy segment vertices - segment(segment_idx).other_source_idx() = add_vertex (vertex(source_idx)); - segment(segment_idx).other_target_idx() = add_vertex (vertex(target_idx)); - - vertex(segment(segment_idx).other_source_idx()).segment_idx() = segment_idx; - vertex(segment(segment_idx).other_target_idx()).segment_idx() = segment_idx; - - vertex(segment(segment_idx).other_source_idx()).polygon_idx() = new_polygon_idx; - vertex(segment(segment_idx).other_target_idx()).polygon_idx() = new_polygon_idx; - - KSR::size_t new_vertex_idx = add_vertex (Vertex (point - m_current_time * direction, new_polygon_idx)); - - polygon(new_polygon_idx).vertices_idx().push_back (segment(segment_idx).other_target_idx()); - polygon(new_polygon_idx).vertices_idx().push_back (segment(segment_idx).other_source_idx()); - polygon(new_polygon_idx).vertices_idx().push_back (new_vertex_idx); - - return new_vertex_idx; - } - - std::pair - transfer_vertex (KSR::size_t vertex_idx, KSR::size_t segment_border_idx, - const Point_2& point, const Vector_2& direction) - { - KSR::size_t segment_idx = vertex(segment_border_idx).segment_idx(); - KSR::size_t mirror_segment_border_idx = segment(segment_idx).mirror_vertex (segment_border_idx); - KSR::size_t polygon_idx = vertex(vertex_idx).polygon_idx(); - - if (mirror_segment_border_idx == KSR::no_element()) // Border reached - { - vertex(segment_border_idx) = Vertex (point - m_current_time * direction, polygon_idx); - vertex(segment_border_idx).segment_idx() = segment_idx; - vertex(segment_border_idx).direction() = CGAL::NULL_VECTOR; - - vertex(vertex_idx) = Vertex (point - m_current_time * direction, polygon_idx); - vertex(vertex_idx).direction() = direction; - - return std::make_pair(vertex_idx, KSR::no_element()); + CGAL_assertion_msg (false, "TODO: create target face"); } else { - KSR::size_t other_polygon_idx = vertex(mirror_segment_border_idx).polygon_idx(); + PVertex pthird = next(pother); + if (pthird == pvertex) + pthird = prev(pother); - polygon(polygon_idx).vertices_idx().erase - (std::find (polygon(polygon_idx).vertices_idx().begin(), polygon(polygon_idx).vertices_idx().end(), vertex_idx)); + IEdge iedge = disconnect_iedge (pvertex); - vertex(vertex_idx).polygon_idx() = other_polygon_idx; - - vertex(segment_border_idx) = Vertex (point - m_current_time * direction, polygon_idx); - vertex(segment_border_idx).segment_idx() = segment_idx; - vertex(segment_border_idx).direction() = direction; - - vertex(mirror_segment_border_idx) = Vertex (point - m_current_time * direction, other_polygon_idx); - vertex(mirror_segment_border_idx).segment_idx() = segment_idx; - vertex(mirror_segment_border_idx).direction() = direction; - - for (KSR::size_t i = 0; i < polygon(other_polygon_idx).vertices_idx().size(); ++ i) - { - KSR::size_t vertex_idx_0 = polygon(other_polygon_idx).vertices_idx()[i]; - KSR::size_t vertex_idx_1 = polygon(other_polygon_idx).vertices_idx()[(i+1) % polygon(other_polygon_idx).vertices_idx().size()]; - - if ((vertex_idx_0 == mirror_segment_border_idx && vertex(vertex_idx_1).segment_idx() != segment_idx) - || (vertex_idx_1 == mirror_segment_border_idx && vertex(vertex_idx_0).segment_idx() != segment_idx)) + PEdge pedge = null_pedge(); + for (PEdge pe : pedges_around_pvertex (pvertex)) + if (this->iedge(pe) == iedge) { - polygon(other_polygon_idx).vertices_idx().insert - (polygon(other_polygon_idx).vertices_idx().begin() + i + 1, vertex_idx); - return std::make_pair (segment_border_idx, mirror_segment_border_idx); + pedge = pe; + break; + } + CGAL_assertion (pedge != null_pedge()); + + Halfedge_index hi = mesh(pedge).halfedge(pedge.second); + if (mesh(pedge).face(hi) != common_pface.second) + hi = mesh(pedge).opposite(hi); + CGAL_assertion (mesh(pedge).face(hi) == common_pface.second); + + if (mesh(pedge).target(hi) == pvertex.second) + { + CGAL::Euler::shift_target (hi, mesh(pedge)); + } + else + { + CGAL_assertion (mesh(pedge).source(hi) == pvertex.second); + CGAL::Euler::shift_source (hi, mesh(pedge)); + } + + Vector_2 new_direction; + + Line_2 iedge_line = segment_2(pother.first, iedge).supporting_line(); + Point_2 pinit = iedge_line.projection(point_2 (pother, m_current_time)); + + direction(pvertex) = direction(pother); + support_plane(pother).set_point (pvertex.second, + pinit - direction(pvertex) * m_current_time); + + Line_2 future_line (point_2 (pvertex, m_current_time + 1), + point_2 (pthird, m_current_time + 1)); + + Point_2 future_point = KSR::intersection_2 (future_line, iedge_line); + + direction(pother) = Vector_2 (pinit, future_point); + support_plane(pother).set_point (pother.second, + pinit - direction(pother) * m_current_time); + + connect (pother, iedge); + } + + CGAL_KSR_CERR(3) << "*** New faces: " << lstr(source_face) + << " and " << lstr(target_face) << std::endl; + + } + + void merge_pvertices (const PVertex& pvertex, const PVertex& pother) + { + CGAL_KSR_CERR(3) << "*** Merging " << str(pvertex) << " with " << str(pother) << std::endl; + + Halfedge_index hi = mesh(pvertex).halfedge(pother.second, pvertex.second); + disconnect_ivertex (pother); + CGAL::Euler::join_vertex(hi, mesh(pvertex)); + } + + std::vector merge_pvertices_on_ivertex (std::vector& pvertices, + const IVertex& ivertex) + { + KSR::size_t support_plane_idx = pvertices.front().first; + + // Get prev and next along border + PVertex prev, next, ignored; + std::tie (prev, ignored) = border_prev_and_next (pvertices.front()); + if (prev == pvertices[1]) + std::swap (prev, ignored); + + std::tie (next, ignored) = border_prev_and_next (pvertices.back()); + if (next == pvertices[pvertices.size() - 2]) + std::swap (next, ignored); + + // Copy front/back to remember position/direction + PVertex front (support_plane_idx, support_plane(support_plane_idx).duplicate_vertex(pvertices.front().second)); + PVertex back (support_plane_idx,support_plane(support_plane_idx).duplicate_vertex(pvertices.back().second)); + + if (CGAL::orientation (point_2 (prev), point_2 (support_plane_idx, ivertex), point_2 (next)) == CGAL::LEFT_TURN) + { + std::swap (prev, next); + std::swap (front, back); + } + + // Freeze vertices + for (PVertex& pvertex : pvertices) + { + Point_2 p = point_2 (support_plane_idx, ivertex); + support_plane(pvertex).direction (pvertex.second) = CGAL::NULL_VECTOR; + support_plane(pvertex).set_point (pvertex.second, p); + } + + PVertex pvertex = pvertices.front(); + connect (pvertex, ivertex); + + + // Join vertices + for (std::size_t i = 1; i < pvertices.size(); ++ i) + { + Halfedge_index hi = mesh(support_plane_idx).halfedge(pvertices[i].second, pvertex.second); + disconnect_ivertex (pvertices[i]); + CGAL::Euler::join_vertex(hi, mesh(support_plane_idx)); + } + + Incident_iedges i_iedges = incident_iedges (ivertex); + std::vector > iedges; + std::copy (i_iedges.begin(), i_iedges.end(), + boost::make_function_output_iterator + ([&](const IEdge& ie) -> void + { + if (intersected_planes(ie).find (support_plane_idx) + == intersected_planes(ie).end()) + return; + + Direction_2 dir (point_2 (support_plane_idx, opposite (ie, ivertex)) + - point_2 (support_plane_idx, ivertex)); + iedges.push_back (std::make_pair (ie, dir)); + })); + + std::sort (iedges.begin(), iedges.end(), + [&](const std::pair& a, + const std::pair& b) -> bool + { + return a.second < b.second; + }); + + bool back_constrained = false; + if (iedge(next) != null_iedge() + && (source(iedge(next)) == ivertex || target(iedge(next)) == ivertex)) + back_constrained = true; + + bool front_constrained = false; + if (iedge(prev) != null_iedge() + && (source(iedge(prev)) == ivertex || target(iedge(prev)) == ivertex)) + front_constrained = true; + + std::cerr << "Prev = " << point_2 (prev) << " / " << direction (prev) << std::endl + << "Front = " << point_2 (front) << " / " << direction (front) << std::endl + << "Back = " << point_2 (back) << " / " << direction (back) << std::endl + << "Next = " << point_2 (next) << " / " << direction (next) << std::endl; + + std::vector new_vertices; + + if (back_constrained && front_constrained) // Closing case + { + CGAL_assertion_msg (false, "TODO: closing"); + } + else if (back_constrained) // Border case + { + std::cerr << "Back border case" << std::endl; + KSR::size_t other_side_limit = line_idx(next); + + Direction_2 dir (point_2(prev) - point_2 (pvertex)); + + std::reverse (iedges.begin(), iedges.end()); + + KSR::size_t first_idx = KSR::no_element(); + for (std::size_t i = 0; i < iedges.size(); ++ i) + { + if (dir.counterclockwise_in_between(iedges[(i+1)%iedges.size()].second, + iedges[i].second)) + { + first_idx = (i+1)%iedges.size(); + break; + } + } + + std::ofstream ("first.polylines.txt") + << "2 " << segment_3 (iedges[first_idx].first) << std::endl; + + CGAL_assertion (first_idx != KSR::no_element()); + + std::vector crossed; + + KSR::size_t iedge_idx = first_idx; + while (true) + { + const IEdge& iedge = iedges[iedge_idx].first; + + bool collision, bbox_reached; + std::tie (collision, bbox_reached) = collision_occured (pvertex, iedge); + bool limit_reached = (line_idx(iedge) == other_side_limit); + + crossed.push_back (iedge); + + std::ofstream ("next.polylines.txt") + << "2 " << segment_3 (iedge) << std::endl; + if (limit_reached || bbox_reached) + break; + + iedge_idx = (iedge_idx + 1) % iedges.size(); + } + + std::cerr << "IEdges crossed = " << crossed.size() << std::endl; + + std::vector future_points (crossed.size()); + std::vector future_directions (crossed.size()); + for (std::size_t i = 0; i < crossed.size(); ++ i) + compute_future_point_and_direction (back, prev, crossed[i], future_points[i], future_directions[i]); + + PVertex previous = null_pvertex(); + + for (std::size_t i = 0; i < crossed.size(); ++ i) + { + if (i == 0) // crop + { + PVertex cropped (support_plane_idx, support_plane(pvertex).split_edge (pvertex.second, prev.second)); + + PEdge pedge (support_plane_idx, support_plane(pvertex).edge (pvertex.second, cropped.second)); + + new_vertices.push_back (cropped); + + connect (pedge, crossed[i]); + connect (cropped, crossed[i]); + + support_plane(cropped).set_point (cropped.second, future_points[i]); + direction(cropped) = future_directions[i]; + previous = cropped; + std::cerr << point_2 (cropped) << " -> " << direction(cropped) << std::endl; + } + else // create triangle face + { + PVertex propagated = add_pvertex (pvertex.first, future_points[i]); + direction(propagated) = future_directions[i]; + new_vertices.push_back (propagated); + + add_pface (std::array{ pvertex, previous, propagated }); + previous = propagated; } } } + else if (front_constrained) // Border case + { + std::cerr << "Front border case" << std::endl; + + KSR::size_t other_side_limit = line_idx(prev); + + Direction_2 dir (point_2(next) - point_2 (pvertex)); + + KSR::size_t first_idx = KSR::no_element(); + for (std::size_t i = 0; i < iedges.size(); ++ i) + { + if (dir.counterclockwise_in_between(iedges[i].second, + iedges[(i+1)%iedges.size()].second)) + + { + first_idx = (i+1)%iedges.size(); + break; + } + } + + std::ofstream ("first.polylines.txt") + << "2 " << segment_3 (iedges[first_idx].first) << std::endl; + + CGAL_assertion (first_idx != KSR::no_element()); + + std::vector crossed; + + KSR::size_t iedge_idx = first_idx; + while (true) + { + const IEdge& iedge = iedges[iedge_idx].first; + + bool collision, bbox_reached; + std::tie (collision, bbox_reached) = collision_occured (pvertex, iedge); + bool limit_reached = (line_idx(iedge) == other_side_limit); + + std::ofstream ("next.polylines.txt") + << "2 " << segment_3 (iedge) << std::endl; + crossed.push_back (iedge); + + if (limit_reached || bbox_reached) + break; + + iedge_idx = (iedge_idx + 1) % iedges.size(); + } + + std::cerr << "IEdges crossed = " << crossed.size() << std::endl; + + std::vector future_points (crossed.size()); + std::vector future_directions (crossed.size()); + for (std::size_t i = 0; i < crossed.size(); ++ i) + compute_future_point_and_direction (front, next, crossed[i], future_points[i], future_directions[i]); + + PVertex previous = null_pvertex(); + + for (std::size_t i = 0; i < crossed.size(); ++ i) + { + if (i == 0) // crop + { + PVertex cropped (support_plane_idx, support_plane(pvertex).split_edge (pvertex.second, next.second)); + + PEdge pedge (support_plane_idx, support_plane(pvertex).edge (pvertex.second, cropped.second)); + + CGAL_assertion (cropped != pvertex); + + new_vertices.push_back (cropped); - // This should never be reached - CGAL_assertion(false); - return std::make_pair (KSR::no_element(), KSR::no_element()); + connect (pedge, crossed[i]); + connect (cropped, crossed[i]); + + support_plane(cropped).set_point (cropped.second, future_points[i]); + direction(cropped) = future_directions[i]; + previous = cropped; + std::cerr << point_2 (cropped) << " -> " << direction(cropped) << std::endl; + } + else // create triangle face + { + PVertex propagated = add_pvertex (pvertex.first, future_points[i]); + direction(propagated) = future_directions[i]; + new_vertices.push_back (propagated); + + add_pface (std::array{ pvertex, previous, propagated }); + previous = propagated; + } + } + } + else // Open case + { + std::cerr << "Open case" << std::endl; + + auto pvertex_to_point = + [&](const PVertex& a) -> Point_2 + { + return point_2(a); + }; + // If open case, prev/pvertex/next are collinear, so the + // orientation is not reliable. + PFace fprev = pface_of_pvertex(prev); + Point_2 pprev = CGAL::centroid + (boost::make_transform_iterator (pvertices_of_pface(fprev).begin(), pvertex_to_point), + boost::make_transform_iterator (pvertices_of_pface(fprev).end(), pvertex_to_point)); + PFace fnext = pface_of_pvertex(next); + Point_2 pnext = CGAL::centroid + (boost::make_transform_iterator (pvertices_of_pface(fnext).begin(), pvertex_to_point), + boost::make_transform_iterator (pvertices_of_pface(fnext).end(), pvertex_to_point)); + + if (CGAL::orientation (pprev, point_2 (support_plane_idx, ivertex), pnext) == CGAL::LEFT_TURN) + { + std::swap (prev, next); + std::swap (front, back); + } + + Direction_2 dir_next (point_2(next) - point_2 (pvertex)); + Direction_2 dir_prev (point_2(prev) - point_2 (pvertex)); + KSR::size_t first_idx = KSR::no_element(); + for (std::size_t i = 0; i < iedges.size(); ++ i) + { + if (dir_next.counterclockwise_in_between(iedges[i].second, + iedges[(i+1)%iedges.size()].second)) + + { + first_idx = (i+1)%iedges.size(); + break; + } + } + + std::vector crossed; + + KSR::size_t iedge_idx = first_idx; + while (true) + { + const IEdge& iedge = iedges[iedge_idx].first; + const Direction_2& dir = iedges[iedge_idx].second; + + if (!dir.counterclockwise_in_between (dir_next, dir_prev)) + break; + + crossed.push_back (iedge); + + iedge_idx = (iedge_idx + 1) % iedges.size(); + } + + std::cerr << "IEdges crossed = " << crossed.size() << std::endl; + + std::vector future_points (crossed.size()); + std::vector future_directions (crossed.size()); + for (std::size_t i = 0; i < crossed.size(); ++ i) + compute_future_point_and_direction (pvertex, prev, next, crossed[i], future_points[i], future_directions[i]); + + { + PVertex cropped (support_plane_idx, support_plane(pvertex).split_edge (pvertex.second, next.second)); + + PEdge pedge (support_plane_idx, support_plane(pvertex).edge (pvertex.second, cropped.second)); + + new_vertices.push_back (cropped); + + connect (pedge, crossed.front()); + connect (cropped, crossed.front()); + + support_plane(cropped).set_point (cropped.second, future_points.front()); + direction(cropped) = future_directions.front(); + } + + for (std::size_t i = 1; i < crossed.size() - 1; ++ i) + { + PVertex propagated = add_pvertex (pvertex.first, future_points[i]); + direction(propagated) = future_directions[i]; + new_vertices.push_back (propagated); + } + + { + PVertex cropped (support_plane_idx, support_plane(pvertex).split_edge (pvertex.second, prev.second)); + + PEdge pedge (support_plane_idx, support_plane(pvertex).edge (pvertex.second, cropped.second)); + + new_vertices.push_back (cropped); + + connect (pedge, crossed.back()); + connect (cropped, crossed.back()); + + support_plane(cropped).set_point (cropped.second, future_points.back()); + direction(cropped) = future_directions.back(); + } + std::cerr << new_vertices.size() << " new vertice(s)" << std::endl; + for (std::size_t i = 0; i < new_vertices.size() - 1; ++ i) + add_pface (std::array{ new_vertices[i], new_vertices[i+1], pvertex }); + } + + support_plane(support_plane_idx).remove_vertex(front.second); + support_plane(support_plane_idx).remove_vertex(back.second); + + new_vertices.push_back (pvertex); // We push it just so that its + // IVertex is deactivated before computing new events + return new_vertices; } - void cut_segment_of_vertex (KSR::size_t vertex_idx) - { - KSR::size_t segment_idx = vertex(vertex_idx).segment_idx(); - - } -#endif void update_positions (FT time) { m_current_time = time; } + inline std::string str (const PVertex& pvertex) const + { return "PVertex(" + std::to_string(pvertex.first) + ":v" + std::to_string(pvertex.second) + ")"; } + inline std::string str (const PEdge& pedge) const + { return "PEdge(" + std::to_string(pedge.first) + ":e" + std::to_string(pedge.second) + ")"; } + inline std::string str (const PFace& pface) const + { return "PFace(" + std::to_string(pface.first) + ":f" + std::to_string(pface.second) + ")"; } + inline std::string str (const IVertex& ivertex) const + { return "IVertex(" + std::to_string(ivertex) + ")"; } + inline std::string str (const IEdge& iedge) const + { std::ostringstream oss; oss << "IEdge" << iedge; return oss.str(); } + + inline std::string lstr (const PFace& pface) const + { + std::string out = "PFace(" + std::to_string(pface.first) + ":f" + std::to_string(pface.second) + ")["; + for (PVertex pvertex : pvertices_of_pface (pface)) + out += "v" + std::to_string(pvertex.second); + out += "]"; + return out; + } private: template @@ -928,7 +1454,68 @@ private: template Mesh& mesh (const PSimplex& psimplex) { return mesh(psimplex.first); } Mesh& mesh (KSR::size_t support_plane_idx) { return support_plane(support_plane_idx).mesh(); } - + + void compute_future_points_and_directions (const PVertex& pvertex, const IEdge& iedge, + Point_2& future_point_a, Point_2& future_point_b, + Vector_2& direction_a, Vector_2& direction_b) const + { + PVertex prev (pvertex.first, support_plane(pvertex).prev(pvertex.second)); + PVertex next (pvertex.first, support_plane(pvertex).next(pvertex.second)); + + Line_2 iedge_line = segment_2(pvertex.first, iedge).supporting_line(); + Point_2 pinit = iedge_line.projection(point_2 (pvertex, m_current_time)); + + Line_2 future_line_prev (point_2 (prev, m_current_time + 1), + point_2 (pvertex, m_current_time + 1)); + Line_2 future_line_next (point_2 (next, m_current_time + 1), + point_2 (pvertex, m_current_time + 1)); + + future_point_a = KSR::intersection_2 (future_line_prev, iedge_line); + future_point_b = KSR::intersection_2 (future_line_next, iedge_line); + direction_a = Vector_2 (pinit, future_point_a); + direction_b = Vector_2 (pinit, future_point_b); + future_point_a = pinit - m_current_time * direction_a; + future_point_b = pinit - m_current_time * direction_b; + } + + void compute_future_point_and_direction (const PVertex& pvertex, const PVertex& next, + const IEdge& iedge, + Point_2& future_point, Vector_2& direction) const + { + if (this->iedge(pvertex) != null_iedge() + && line_idx(pvertex) == line_idx(iedge)) + { + std::cerr << "Found limit" << std::endl; + future_point = point_2 (pvertex, 0); + direction = this->direction (pvertex); + return; + } + Line_2 iedge_line = segment_2(pvertex.first, iedge).supporting_line(); + Point_2 pinit = iedge_line.projection(point_2 (pvertex, m_current_time)); + + Line_2 future_line_next (point_2 (next, m_current_time + 1), + point_2 (pvertex, m_current_time + 1)); + + future_point = KSR::intersection_2 (future_line_next, iedge_line); + direction = Vector_2 (pinit, future_point); + future_point = pinit - m_current_time * direction; + } + + void compute_future_point_and_direction (const PVertex& pvertex, + const PVertex& prev, const PVertex& next, + const IEdge& iedge, + Point_2& future_point, Vector_2& direction) const + { + Line_2 iedge_line = segment_2(pvertex.first, iedge).supporting_line(); + Point_2 pinit = iedge_line.projection(point_2 (pvertex, m_current_time)); + + Line_2 future_line (point_2 (next, m_current_time + 1), + point_2 (prev, m_current_time + 1)); + + future_point = KSR::intersection_2 (future_line, iedge_line); + direction = Vector_2 (pinit, future_point); + future_point = pinit - m_current_time * direction; + } }; diff --git a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Event.h b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Event.h index 8921a8d641d..6661b4eb8e8 100644 --- a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Event.h +++ b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Event.h @@ -48,40 +48,94 @@ public: typedef Event_queue Queue; friend Queue; - - enum Type - { - FREE_VERTEX_TO_INTERSECTION_EDGE, - CONSTRAINED_VERTEX_TO_FREE_VERTEX, - CONSTRAINED_VERTEX_TO_INTERSECTION_VERTEX, - CONSTRAINED_VERTEX_TO_CONSTRAINED_VERTEX, - EDGE_TO_INTERSECTION_EDGE - }; - + private: PVertex m_pvertex; + + PVertex m_pother; IEdge m_iedge; + IVertex m_ivertex; + FT m_time; public: - Event () { } + Event () + : m_pvertex (Data::null_pvertex()) + , m_pother (Data::null_pvertex()) + , m_iedge (Data::null_iedge()) + , m_ivertex (Data::null_ivertex()) + , m_time (0) + { } + + Event (PVertex pvertex, PVertex pother, FT time) + : m_pvertex (pvertex) + , m_pother (pother) + , m_iedge (Data::null_iedge()) + , m_ivertex (Data::null_ivertex()) + , m_time (time) + { } Event (PVertex pvertex, IEdge iedge, FT time) : m_pvertex (pvertex) - , m_iedge (iedge), m_time (time) + , m_pother (Data::null_pvertex()) + , m_iedge (iedge) + , m_ivertex (Data::null_ivertex()) + , m_time (time) + { } + + Event (PVertex pvertex, IVertex ivertex, FT time) + : m_pvertex (pvertex) + , m_pother (Data::null_pvertex()) + , m_iedge (Data::null_iedge()) + , m_ivertex (ivertex) + , m_time (time) + { } + + Event (PVertex pvertex, PVertex pother, IVertex ivertex, FT time) + : m_pvertex (pvertex) + , m_pother (pother) + , m_iedge (Data::null_iedge()) + , m_ivertex (ivertex) + , m_time (time) { } PVertex pvertex() const { return m_pvertex; } + PVertex pother() const { return m_pother; } IEdge iedge() const { return m_iedge; } + IVertex ivertex() const { return m_ivertex; } FT time() const { return m_time; } + + bool is_pvertex_to_pvertex() const { return (m_pother != Data::null_pvertex()); } + bool is_pvertex_to_iedge() const { return (m_iedge != Data::null_iedge()); } + + bool is_pvertex_to_ivertex() const { return (m_pother == Data::null_pvertex() + && m_ivertex != Data::null_ivertex()); } + bool is_pvertices_to_ivertex() const { return (m_pother != Data::null_pvertex() + && m_ivertex != Data::null_ivertex()); } friend std::ostream& operator<< (std::ostream& os, const Event& ev) { - os << "Event at t=" << ev.m_time << " between vertex (" - << ev.m_pvertex.first << ":" << ev.m_pvertex.second - << ") and intersection edge " << ev.m_iedge; + if (ev.is_pvertex_to_pvertex()) + os << "Event at t=" << ev.m_time << " between PVertex(" + << ev.m_pvertex.first << ":" << ev.m_pvertex.second + << ") and PVertex(" << ev.m_pother.first << ":" << ev.m_pother.second << ")"; + else if (ev.is_pvertex_to_iedge()) + os << "Event at t=" << ev.m_time << " between PVertex(" + << ev.m_pvertex.first << ":" << ev.m_pvertex.second + << ") and IEdge" << ev.m_iedge; + else if (ev.is_pvertex_to_ivertex()) + os << "Event at t=" << ev.m_time << " between PVertex(" + << ev.m_pvertex.first << ":" << ev.m_pvertex.second + << ") and IVertex(" << ev.m_ivertex << ")"; + else if (ev.is_pvertices_to_ivertex()) + os << "Event at t=" << ev.m_time << " between PVertex(" + << ev.m_pvertex.first << ":" << ev.m_pvertex.second + << "), PVertex(" << ev.m_pother.first << ":" << ev.m_pother.second + << " and IVertex(" << ev.m_ivertex << ")"; + else + os << "Invalid event at t=" << ev.m_time; return os; } diff --git a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Event_queue.h b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Event_queue.h index 7d70737fb33..0ca2265fae4 100644 --- a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Event_queue.h +++ b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Event_queue.h @@ -59,15 +59,19 @@ private: boost::multi_index::ordered_non_unique >, boost::multi_index::ordered_non_unique - > + >, + boost::multi_index::ordered_non_unique + > > > Queue; typedef typename Queue::iterator Queue_iterator; typedef typename Queue::template nth_index<0>::type Queue_by_time; typedef typename Queue_by_time::iterator Queue_by_time_iterator; - typedef typename Queue::template nth_index<1>::type Queue_by_event_idx; - typedef typename Queue_by_event_idx::iterator Queue_by_event_idx_iterator; + typedef typename Queue::template nth_index<1>::type Queue_by_pvertex_idx; + typedef typename Queue_by_pvertex_idx::iterator Queue_by_pvertex_idx_iterator; + typedef typename Queue::template nth_index<2>::type Queue_by_pother_idx; + typedef typename Queue_by_pother_idx::iterator Queue_by_pother_idx_iterator; Queue m_queue; @@ -80,13 +84,16 @@ public: void push (const Event& ev) { + CGAL_KSR_CERR(4) << "**** Pushing " << ev << std::endl; m_queue.insert (ev); } const Queue_by_time& queue_by_time() const { return m_queue.template get<0>(); } - const Queue_by_event_idx& queue_by_event_idx() const { return m_queue.template get<1>(); } + const Queue_by_pvertex_idx& queue_by_pvertex_idx() const { return m_queue.template get<1>(); } + const Queue_by_pother_idx& queue_by_pother_idx() const { return m_queue.template get<2>(); } Queue_by_time& queue_by_time() { return m_queue.template get<0>(); } - Queue_by_event_idx& queue_by_event_idx() { return m_queue.template get<1>(); } + Queue_by_pvertex_idx& queue_by_pvertex_idx() { return m_queue.template get<1>(); } + Queue_by_pother_idx& queue_by_pother_idx() { return m_queue.template get<2>(); } Event pop () { @@ -102,23 +109,28 @@ public: std::cerr << e << std::endl; } - void erase_vertex_events (KSR::size_t support_plane_idx, PVertex pvertex) + void erase_vertex_events (PVertex pvertex) { - std::pair - range = queue_by_event_idx().equal_range(pvertex); - queue_by_event_idx().erase (range.first, range.second); + { + std::pair + range = queue_by_pvertex_idx().equal_range(pvertex); + + for (const auto& ev : CGAL::make_range(range)) + CGAL_KSR_CERR(4) << "**** Erasing " << ev << std::endl; + + queue_by_pvertex_idx().erase (range.first, range.second); + } + { + std::pair + range = queue_by_pother_idx().equal_range(pvertex); + + for (const auto& ev : CGAL::make_range(range)) + CGAL_KSR_CERR(4) << "**** Erasing " << ev << std::endl; + + queue_by_pother_idx().erase (range.first, range.second); + } } - void erase_vertex_events (KSR::size_t support_plane_idx, PVertex pvertex, - KSR::vector& events) - { - std::pair - range = queue_by_event_idx().equal_range(pvertex); - - std::copy (range.first, range.second, std::back_inserter (events)); - queue_by_event_idx().erase (range.first, range.second); - } - }; diff --git a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Intersection_graph.h b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Intersection_graph.h index c4f5520ff5d..1729a58ffd1 100644 --- a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Intersection_graph.h +++ b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Intersection_graph.h @@ -51,14 +51,19 @@ public: struct Vertex_property { Point_3 point; + bool active; - Vertex_property () { } - Vertex_property (const Point_3& point) : point (point) { } + Vertex_property () : active(true) { } + Vertex_property (const Point_3& point) : point (point), active(true) { } }; struct Edge_property { + KSR::size_t line; KSR::Idx_set planes; + bool active; + + Edge_property() : line (KSR::no_element()), active(true) { } }; typedef boost::adjacency_list m_map_points; std::map m_map_vertices; public: - Intersection_graph() { } + Intersection_graph() : m_nb_lines(0) { } static Vertex_descriptor null_ivertex() { return boost::graph_traits::null_vertex(); } @@ -121,6 +127,9 @@ public: return std::make_pair (iter->second, inserted); } + KSR::size_t add_line() { return (m_nb_lines ++); } + KSR::size_t nb_lines() const { return m_nb_lines; } + std::pair add_edge (const Vertex_descriptor& source, const Vertex_descriptor& target, KSR::size_t support_plane_idx) { @@ -144,6 +153,13 @@ public: return add_edge (add_vertex (source).first, add_vertex (target).first); } + void set_line (const Edge_descriptor& edge, KSR::size_t line_idx) + { + m_graph[edge].line = line_idx; + } + + KSR::size_t line (const Edge_descriptor& edge) const { return m_graph[edge].line; } + std::pair split_edge (const Edge_descriptor& edge, const Vertex_descriptor& vertex) { @@ -210,6 +226,11 @@ public: return Line_3 (m_graph[source (edge, m_graph)].point, m_graph[target (edge, m_graph)].point); } + + bool is_active (const Vertex_descriptor& vertex) const { return m_graph[vertex].active; } + bool& is_active (const Vertex_descriptor& vertex) { return m_graph[vertex].active; } + bool is_active (const Edge_descriptor& edge) const { return m_graph[edge].active; } + bool& is_active (const Edge_descriptor& edge) { return m_graph[edge].active; } }; diff --git a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Polygon_splitter.h b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Polygon_splitter.h index 7701f5e531f..dacd0dde10f 100644 --- a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Polygon_splitter.h +++ b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Polygon_splitter.h @@ -101,7 +101,7 @@ public: Polygon_splitter (Data& data) : m_data (data) { } - void split_support_plane (KSR::size_t support_plane_idx) + void split_support_plane (KSR::size_t support_plane_idx, unsigned int k) { // First, insert polygons for (PVertex pvertex : m_data.pvertices(support_plane_idx)) @@ -254,9 +254,11 @@ public: } while (current != edge); - PFace pface = m_data.add_pface (support_plane_idx, new_vertices); + PFace pface = m_data.add_pface (new_vertices); CGAL_assertion (pface != PFace()); + m_data.k(pface) = k; + std::size_t original_idx = 0; if (original_faces.size() != 1) { diff --git a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Support_plane.h b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Support_plane.h index 6ec8f3ab67f..07c50b9c9dd 100644 --- a/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Support_plane.h +++ b/Kinetic_shape_reconstruction/include/CGAL/KSR_3/Support_plane.h @@ -65,8 +65,10 @@ public: typedef typename Mesh::template Property_map V_vector_map; typedef typename Mesh::template Property_map V_ivertex_map; typedef typename Mesh::template Property_map V_iedge_map; + typedef typename Mesh::template Property_map V_bool_map; typedef typename Mesh::template Property_map E_iedge_map; typedef typename Mesh::template Property_map F_index_map; + typedef typename Mesh::template Property_map F_uint_map; private: @@ -78,8 +80,10 @@ private: V_vector_map direction; V_ivertex_map v_ivertex_map; V_iedge_map v_iedge_map; + V_bool_map v_active_map; E_iedge_map e_iedge_map; - F_index_map input; + F_index_map input_map; + F_uint_map k_map; std::set iedges; }; @@ -114,12 +118,14 @@ public: ("v:ivertex", Intersection_graph::null_ivertex()).first; m_data->v_iedge_map = m_data->mesh.template add_property_map ("v:iedge", Intersection_graph::null_iedge()).first; + m_data->v_active_map = m_data->mesh.template add_property_map + ("v:active", true).first; m_data->e_iedge_map = m_data->mesh.template add_property_map ("e:iedge", Intersection_graph::null_iedge()).first; - - bool okay; - std::tie (m_data->input, okay) = m_data->mesh.template add_property_map("f:input", KSR::no_element()); - CGAL_assertion(okay); + m_data->input_map = m_data->mesh.template add_property_map + ("f:input", KSR::no_element()).first; + m_data->k_map = m_data->mesh.template add_property_map + ("f:k", 0).first; } const Plane_3& plane() const { return m_data->plane; } @@ -127,6 +133,39 @@ public: const Mesh& mesh() const { return m_data->mesh; } Mesh& mesh() { return m_data->mesh; } + void set_point (const Vertex_index& vertex_index, const Point_2& point) + { + m_data->mesh.point(vertex_index) = point; + } + + Vertex_index prev (const Vertex_index& vertex_index) const + { + return m_data->mesh.source(m_data->mesh.halfedge(vertex_index)); + } + Vertex_index next (const Vertex_index& vertex_index) const + { + return m_data->mesh.target(m_data->mesh.next(m_data->mesh.halfedge(vertex_index))); + } + + Face_index face (const Vertex_index& vertex_index) const + { + Face_index out = m_data->mesh.face (m_data->mesh.halfedge(vertex_index)); + if (out == Face_index()) + out = m_data->mesh.face (m_data->mesh.opposite(m_data->mesh.halfedge(vertex_index))); + CGAL_assertion (out != Face_index()); + return out; + } + + std::pair faces (const Vertex_index& vertex_index) const + { + for (Halfedge_index hi : halfedges_around_target (halfedge(vertex_index, m_data->mesh), m_data->mesh)) + if (has_iedge (m_data->mesh.edge(hi))) + return std::make_pair (m_data->mesh.face (hi), + m_data->mesh.face (m_data->mesh.opposite(hi))); + CGAL_assertion_msg (false, "No constrained edge found"); + return std::make_pair (Face_index(), Face_index()); + } + Point_2 point_2 (const Vertex_index& vertex_index, FT time) const { return m_data->mesh.point(vertex_index) + time * m_data->direction[vertex_index]; } @@ -146,7 +185,7 @@ public: } void set_iedge (const Vertex_index& a, const Vertex_index& b, - const IEdge& iedge) const + const IEdge& iedge) const { Halfedge_index hi = m_data->mesh.halfedge (a, b); CGAL_assertion (hi != Halfedge_index()); @@ -161,11 +200,17 @@ public: } void set_iedge (const Vertex_index& vertex, - const IEdge& iedge) const + const IEdge& iedge) const { m_data->v_iedge_map[vertex] = iedge; } + void set_iedge (const Edge_index& edge, + const IEdge& iedge) const + { + m_data->e_iedge_map[edge] = iedge; + } + const IEdge& iedge (const Edge_index& edge_index) const { return m_data->e_iedge_map[edge_index]; @@ -176,6 +221,11 @@ public: return m_data->v_iedge_map[vertex_index]; } + const IVertex& ivertex (const Vertex_index& vertex_index) const + { + return m_data->v_ivertex_map[vertex_index]; + } + bool has_iedge (const Edge_index& edge_index) const { return (m_data->e_iedge_map[edge_index] != Intersection_graph::null_iedge()); @@ -184,15 +234,25 @@ public: { return (m_data->v_iedge_map[vertex_index] != Intersection_graph::null_iedge()); } + bool has_ivertex (const Vertex_index& vertex_index) const + { + return (m_data->v_ivertex_map[vertex_index] != Intersection_graph::null_ivertex()); + } const Vector_2& direction (const Vertex_index& vertex_index) const { return m_data->direction[vertex_index]; } Vector_2& direction (const Vertex_index& vertex_index) { return m_data->direction[vertex_index]; } FT speed (const Vertex_index& vertex_index) const { return CGAL::approximate_sqrt (m_data->direction[vertex_index].squared_length()); } - const KSR::size_t& input (const Face_index& face_index) const { return m_data->input[face_index]; } - KSR::size_t& input (const Face_index& face_index) { return m_data->input[face_index]; } + const KSR::size_t& input (const Face_index& face_index) const { return m_data->input_map[face_index]; } + KSR::size_t& input (const Face_index& face_index) { return m_data->input_map[face_index]; } + const unsigned int& k (const Face_index& face_index) const { return m_data->k_map[face_index]; } + unsigned int& k (const Face_index& face_index) { return m_data->k_map[face_index]; } + + bool is_active (const Vertex_index& vertex_index) const { return m_data->v_active_map[vertex_index]; } + void set_active (const Vertex_index& vertex_index, bool value) { m_data->v_active_map[vertex_index] = value; } + bool is_frozen (const Vertex_index& vertex_index) const { return (m_data->direction[vertex_index] == CGAL::NULL_VECTOR); @@ -239,7 +299,7 @@ public: } Face_index fi = m_data->mesh.add_face (vertices); - m_data->input[fi] = KSR::no_element(); + m_data->input_map[fi] = KSR::no_element(); return vertices; } @@ -257,17 +317,14 @@ public: } Face_index fi = m_data->mesh.add_face (vertices); - m_data->input[fi] = input_idx; + m_data->input_map[fi] = input_idx; return KSR::size_t(fi); } Edge_index edge (const Vertex_index& v0, const Vertex_index& v1) { - for (Halfedge_index hi : halfedges_around_target (halfedge(v0, m_data->mesh), m_data->mesh)) - if (target(hi, m_data->mesh) == v1) - return m_data->mesh.edge(hi); - return Edge_index(); + return m_data->mesh.edge (m_data->mesh.halfedge (v0, v1)); } Edge_index add_edge (const Vertex_index& v0, const Vertex_index& v1, @@ -283,9 +340,32 @@ public: return m_data->mesh.add_vertex(point); } - Vertex_index split_edge (const Edge_index& ei) + Vertex_index duplicate_vertex (const Vertex_index& v) { - return m_data->mesh.target (CGAL::Euler::split_edge (m_data->mesh.halfedge (ei), m_data->mesh)); + Vertex_index vi = m_data->mesh.add_vertex (m_data->mesh.point(v)); + m_data->direction[vi] = m_data->direction[v]; + m_data->v_ivertex_map[vi] = m_data->v_ivertex_map[v]; + m_data->v_iedge_map[vi] = m_data->v_iedge_map[v]; + return vi; + } + + void remove_vertex (const Vertex_index& v) + { + m_data->mesh.remove_vertex(v); + } + + Edge_index split_vertex (const Vertex_index& ei) + { + return m_data->mesh.edge + (CGAL::Euler::split_vertex (m_data->mesh.halfedge (ei), + m_data->mesh.opposite(m_data->mesh.next(m_data->mesh.halfedge (ei))), + m_data->mesh)); + } + + Vertex_index split_edge (const Vertex_index& v0, const Vertex_index& v1) + { + return m_data->mesh.target + (CGAL::Euler::split_edge (m_data->mesh.halfedge (v0, v1), m_data->mesh)); } diff --git a/Kinetic_shape_reconstruction/include/CGAL/Kinetic_shape_reconstruction_3.h b/Kinetic_shape_reconstruction/include/CGAL/Kinetic_shape_reconstruction_3.h index 222f807c32b..c977ffd363d 100644 --- a/Kinetic_shape_reconstruction/include/CGAL/Kinetic_shape_reconstruction_3.h +++ b/Kinetic_shape_reconstruction/include/CGAL/Kinetic_shape_reconstruction_3.h @@ -115,8 +115,10 @@ public: KSR_3::dump (m_data, "init"); CGAL_assertion(check_integrity(true)); - make_polygons_intersection_free(); + make_polygons_intersection_free(k); CGAL_assertion(check_integrity(true)); + + KSR_3::dump_segmented_edges (m_data, "init"); KSR_3::dump (m_data, "intersected"); @@ -129,7 +131,7 @@ public: m_min_time = m_max_time; m_max_time += time_step; - CGAL_assertion(check_integrity(true)); +// CGAL_assertion(check_integrity(true)); ++ iter; } exit(0); @@ -151,7 +153,7 @@ public: if (!m_data.mesh_is_valid(i)) { if (verbose) - std::cerr << "ERROR: Mesh " << i << "is invalid" << std::endl; + std::cerr << "ERROR: Mesh " << i << " is invalid" << std::endl; return false; } @@ -161,8 +163,8 @@ public: { if (verbose) std::cerr << "ERROR: Support_plane[" << i - << "] is intersected by IEdge[" << iedge - << "] which claims it does not intersect it" << std::endl; + << "] is intersected by " << m_data.str(iedge) + << " which claims it does not intersect it" << std::endl; return false; } } @@ -175,8 +177,8 @@ public: == m_data.iedges(support_plane_idx).end()) { if (verbose) - std::cerr << "ERROR: IEdge[" << iedge - << "] intersects Support_plane[" << support_plane_idx + std::cerr << "ERROR: " << m_data.str(iedge) + << " intersects Support_plane[" << support_plane_idx << "] which claims it's not intersected by it" << std::endl; return false; } @@ -250,7 +252,7 @@ private: CGAL_assertion (m_data.iedges().size() == 12); } - void make_polygons_intersection_free() + void make_polygons_intersection_free (unsigned int k) { // First, generate all transverse intersection lines typedef std::map > Map; @@ -324,7 +326,7 @@ private: for (KSR::size_t i = 0; i < m_data.number_of_support_planes(); ++ i) { KSR_3::Polygon_splitter splitter (m_data); - splitter.split_support_plane (i); + splitter.split_support_plane (i, k); } } @@ -338,68 +340,14 @@ private: for (KSR::size_t i = 0; i < m_data.number_of_support_planes(); ++ i) { - // To get random access, copy in vector (suboptimal to do this - // all the time, maybe this should be done once and for all and - // replace the set) KSR::vector iedges; - iedges.reserve (m_data.iedges(i).size()); - std::copy (m_data.iedges(i).begin(), - m_data.iedges(i).end(), - std::back_inserter(iedges)); - - // Precompute segments and bboxes KSR::vector segments_2; - segments_2.reserve (iedges.size()); KSR::vector segment_bboxes; - segment_bboxes.reserve (iedges.size()); - for (const IEdge& iedge : iedges) - { - segments_2.push_back (m_data.segment_2 (i, iedge)); - segment_bboxes.push_back (segments_2.back().bbox()); - } + init_search_structures (i, iedges, segments_2, segment_bboxes); for (const PVertex& pvertex : m_data.pvertices(i)) - { - if (m_data.is_frozen(pvertex)) - continue; - - still_running = true; - - if (m_data.has_iedge(pvertex)) // Constrained vertex - { - // Test left and right vertices on mesh face - - // Test end-vertices of intersection edge - } - else // Unconstrained vertex - { - // Test all intersection edges - - Segment_2 si (m_data.point_2 (pvertex, m_min_time), - m_data.point_2 (pvertex, m_max_time)); - CGAL::Bbox_2 si_bbox = si.bbox(); - - for (std::size_t j = 0; j < iedges.size(); ++ j) - { - const IEdge& iedge = iedges[j]; - - if (m_data.iedge(pvertex) == iedge) - continue; - - if (!CGAL::do_overlap (si_bbox, segment_bboxes[j])) - continue; - - Point_2 point; - if (!KSR::intersection_2 (si, segments_2[j], point)) - continue; - - FT dist = CGAL::approximate_sqrt (CGAL::squared_distance (m_data.point_2 (pvertex, m_min_time), point)); - FT time = dist / m_data.speed(pvertex); - - m_queue.push (Event (pvertex, iedge, m_min_time + time)); - } - } - } + if (compute_events_of_vertex (pvertex, iedges, segments_2, segment_bboxes)) + still_running = true; } m_data.update_positions(m_min_time); @@ -407,6 +355,118 @@ private: return still_running; } + void init_search_structures (KSR::size_t i, + KSR::vector& iedges, + KSR::vector& segments_2, + KSR::vector& segment_bboxes) + { + // To get random access, copy in vector (suboptimal to do this + // all the time, maybe this should be done once and for all and + // replace the set) + iedges.reserve (m_data.iedges(i).size()); + std::copy (m_data.iedges(i).begin(), + m_data.iedges(i).end(), + std::back_inserter(iedges)); + + // Precompute segments and bboxes + segments_2.reserve (iedges.size()); + segment_bboxes.reserve (iedges.size()); + for (const IEdge& iedge : iedges) + { + segments_2.push_back (m_data.segment_2 (i, iedge)); + segment_bboxes.push_back (segments_2.back().bbox()); + } + } + + bool compute_events_of_vertex (const PVertex& pvertex, + const KSR::vector& iedges, + const KSR::vector& segments_2, + const KSR::vector& segment_bboxes) + { + if (m_data.is_frozen(pvertex)) + return false; + + Segment_2 sv (m_data.point_2 (pvertex, m_min_time), + m_data.point_2 (pvertex, m_max_time)); + CGAL::Bbox_2 sv_bbox = sv.bbox(); + + if (m_data.has_iedge(pvertex)) // Constrained vertex + { + // Test left and right vertices on mesh face + PVertex prev; + PVertex next; + std::tie (prev, next) = m_data.prev_and_next (pvertex); + + for (const PVertex& pother : { prev, next }) + { + if (pother == Data::null_pvertex() + || !m_data.is_active(pother) + || m_data.has_iedge (pother)) + continue; + + Segment_2 so (m_data.point_2 (pother, m_min_time), + m_data.point_2 (pother, m_max_time)); + CGAL::Bbox_2 so_bbox = so.bbox(); + + if (!do_overlap (sv_bbox, so_bbox)) + continue; + + Point_2 point; + if (!KSR::intersection_2 (sv, so, point)) + continue; + + FT dist = CGAL::approximate_sqrt (CGAL::squared_distance (sv.source(), point)); + FT time = dist / m_data.speed(pvertex); + + m_queue.push (Event (pvertex, pother, m_min_time + time)); + } + + // Test end-vertices of intersection edge + IEdge iedge = m_data.iedge(pvertex); + for (const IVertex& ivertex : { m_data.source(iedge), m_data.target(iedge) }) + { + if (!m_data.is_active(ivertex)) + continue; + Point_2 pi = m_data.to_2d (pvertex.first, ivertex); + if (sv.to_vector() * Vector_2 (sv.source(), pi) < 0) + continue; + + FT dist = CGAL::approximate_sqrt(CGAL::squared_distance (sv.source(), pi)); + FT time = dist / m_data.speed(pvertex); + + if (time < m_max_time - m_min_time) + m_queue.push (Event (pvertex, ivertex, m_min_time + time)); + } + } + else // Unconstrained vertex + { + // Test all intersection edges + for (std::size_t j = 0; j < iedges.size(); ++ j) + { + const IEdge& iedge = iedges[j]; + + if (m_data.iedge(pvertex) == iedge) + continue; + if (!m_data.is_active(iedge)) + continue; + + if (!CGAL::do_overlap (sv_bbox, segment_bboxes[j])) + continue; + + Point_2 point; + if (!KSR::intersection_2 (sv, segments_2[j], point)) + continue; + + FT dist = CGAL::approximate_sqrt (CGAL::squared_distance (m_data.point_2 (pvertex, m_min_time), point)); + FT time = dist / m_data.speed(pvertex); + + m_queue.push (Event (pvertex, iedge, m_min_time + time)); + } + } + return true; + } + + void run() { CGAL_KSR_CERR(1) << "Unstacking queue" << std::endl; @@ -421,10 +481,6 @@ private: FT current_time = ev.time(); - m_data.update_positions (current_time); - - CGAL_KSR_CERR(2) << "* Applying " << iter << ": " << ev << std::endl; - if (iter < 10) { dump (m_data, "iter_0" + std::to_string(iter)); @@ -436,10 +492,20 @@ private: dump_event (m_data, ev, "iter_" + std::to_string(iter)); } + m_data.update_positions (current_time); + + CGAL_KSR_CERR(2) << "* Applying " << iter << ": " << ev << std::endl; + + m_data.update_positions (current_time + 0.01); + dump (m_data, "shifted_" + std::to_string(iter)); + m_data.update_positions (current_time); + ++ iter; - if (iter == 5) + if (iter == 27) + { exit(0); + } apply(ev); @@ -450,95 +516,110 @@ private: void apply (const Event& ev) { PVertex pvertex = ev.pvertex(); - IEdge iedge = ev.iedge(); - } - -#if 0 - void transfer_events (KSR::size_t old_vertex, KSR::size_t new_vertex) - { - CGAL_KSR_CERR(3) << "** Transfering events of vertex " << old_vertex << " to " << new_vertex << std::endl; - - KSR::vector events; - m_queue.erase_vertex_events (old_vertex, events); - - for (Event& ev : events) + if (ev.is_pvertex_to_pvertex()) { - ev.vertex_idx() = new_vertex; - CGAL_KSR_CERR(4) << "**** - Pushing " << ev << std::endl; - m_queue.push (ev); + PVertex pother = ev.pother(); + + remove_events (pvertex); + remove_events (pother); + + CGAL_assertion (m_data.has_iedge(pvertex)); + + if (m_data.has_iedge(pother)) // Two constrained vertices meet + { + CGAL_assertion_msg (false, "TODO: two constrained"); + } + else // One constrained vertex meets a free vertex + { + m_data.transfer_vertex (pvertex, pother); + compute_events_of_vertices (std::array{pvertex, pother}); + } + } + else if (ev.is_pvertex_to_iedge()) + { + remove_events (pvertex); + + IEdge iedge = ev.iedge(); + + PFace pface = m_data.pface_of_pvertex (pvertex); + bool collision, bbox_reached; + std::tie (collision, bbox_reached) + = m_data.collision_occured (pvertex, iedge); + if (collision && m_data.k(pface) > 1) + m_data.k(pface) --; + if (bbox_reached) + m_data.k(pface) = 1; + + if (m_data.k(pface) == 1) // Polygon stops + { + PVertex pvnew = m_data.crop_polygon (pvertex, iedge); + compute_events_of_vertices (std::array{pvertex, pvnew}); + } + else // Polygon continues beyond the edge + { + std::array pvnew = m_data.propagate_polygon (pvertex, iedge); + compute_events_of_vertices (std::array{pvnew[0], pvnew[1], pvnew[2]}); + } + } + else if (ev.is_pvertex_to_ivertex()) + { + // first, let's gather all vertices that will get merged + std::vector pvertices + = m_data.pvertices_around_ivertex (ev.pvertex(), ev.ivertex()); + + CGAL_assertion_msg (pvertices.size() > 1, "Isolated PVertex reaching an IVertex"); + + std::cerr << "Found " << pvertices.size() << " pvertices ready to be merged" << std::endl; + + // Remove associated events + for (const PVertex pvertex : pvertices) + remove_events (pvertex); + + // Merge them and get the newly created vertices + std::vector new_pvertices + = m_data.merge_pvertices_on_ivertex (pvertices, ev.ivertex()); + + // And compute new events + compute_events_of_vertices (new_pvertices); + + } + else + { + CGAL_assertion_msg (false, "Event is invalid"); } } - void update_events (KSR::size_t vertex_idx) + void remove_events (const PVertex& pvertex) { - remove_events (vertex_idx); - compute_new_events (vertex_idx); + m_queue.erase_vertex_events (pvertex); } - void remove_events (KSR::size_t vertex_idx) + template + void compute_events_of_vertices (const PVertexRange& pvertices) { - CGAL_KSR_CERR(3) << "** Removing events of vertex " << vertex_idx << std::endl; - m_queue.erase_vertex_events (vertex_idx); - } - - void compute_new_events (KSR::size_t vertex_idx) - { - const Vertex& vertex = m_data.vertex (vertex_idx); - if (vertex.is_frozen()) - return; - - CGAL_KSR_CERR(3) << "** Computing new events of vertex " << vertex_idx << std::endl; - - FT current_time = m_data.current_time(); + // TODO + m_min_time = m_data.current_time(); m_data.update_positions(m_max_time); - KSR::size_t support_plane_idx = m_data.polygon_of_vertex(vertex_idx).support_plane_idx(); - const Support_plane& support_plane = m_data.support_plane(support_plane_idx); - - // Precompute segments and bboxes + KSR::vector iedges; KSR::vector segments_2; - segments_2.reserve (support_plane.intersection_lines_idx().size()); KSR::vector segment_bboxes; - segment_bboxes.reserve (support_plane.intersection_lines_idx().size()); - for (KSR::size_t intersection_line_idx : support_plane.intersection_lines_idx()) - { - segments_2.push_back (m_data.segment_of_intersection_line_on_support_plane (intersection_line_idx, support_plane_idx)); - segment_bboxes.push_back (segments_2.back().bbox()); - } + init_search_structures (pvertices.front().first, iedges, segments_2, segment_bboxes); + + for (const PVertex& pvertex : pvertices) + m_data.deactivate(pvertex); - Segment_2 si (vertex.point (current_time), vertex.point (m_max_time)); - CGAL::Bbox_2 si_bbox = si.bbox(); - - for (std::size_t j = 0; j < segments_2.size(); ++ j) - { - KSR::size_t intersection_line_idx = support_plane.intersection_lines_idx()[j]; - - if (m_data.intersection_line_idx_of_vertex(vertex_idx) == intersection_line_idx) - continue; - - if (!CGAL::do_overlap (si_bbox, segment_bboxes[j])) - continue; - - Point_2 point; - if (!KSR::intersection_2 (si, segments_2[j], point)) - continue; - - FT dist = CGAL::approximate_sqrt (CGAL::squared_distance (vertex.point (current_time), point)); - FT time = dist / vertex.speed(); - - m_queue.push (Event (vertex_idx, intersection_line_idx, current_time + time)); - } - - m_data.update_positions(current_time); + for (const PVertex& pvertex : pvertices) + compute_events_of_vertex (pvertex, iedges, segments_2, segment_bboxes); + + for (const PVertex& pvertex : pvertices) + m_data.activate(pvertex); + + m_data.update_positions(m_min_time); } - void get_meta_neighbors (KSR::vector >& neighbors) const - { - } -#endif - }; diff --git a/Surface_mesh/include/CGAL/Surface_mesh/Surface_mesh.h b/Surface_mesh/include/CGAL/Surface_mesh/Surface_mesh.h index 22c65544df2..5e9a5d00b22 100644 --- a/Surface_mesh/include/CGAL/Surface_mesh/Surface_mesh.h +++ b/Surface_mesh/include/CGAL/Surface_mesh/Surface_mesh.h @@ -473,7 +473,7 @@ private: //-------------------------------------------------- connectivity types /// \sa `Halfedge_connectivity`, `Face_connectivity` struct Vertex_connectivity { - /// an incoming halfedge per vertex (it will be a border halfedge for border vertices) + /// an incoming halfedge per vlertex (it will be a border halfedge for border vertices) Halfedge_index halfedge_; };