Misc code improvements

Surface_mesh_shortest_path/examples/Surface_mesh_shortest_path/shortest_path_sequence.cpp

@@ -13,9 +13,11 @@
 
 typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
 typedef CGAL::Surface_mesh<Kernel::Point_3> Triangle_mesh;
+
 typedef CGAL::Surface_mesh_shortest_path_traits<Kernel, Triangle_mesh> Traits;
 typedef CGAL::Surface_mesh_shortest_path<Traits> Surface_mesh_shortest_path;
 typedef Traits::Barycentric_coordinates Barycentric_coordinates;
+
 typedef boost::graph_traits<Triangle_mesh> Graph_traits;
 typedef Graph_traits::vertex_iterator vertex_iterator;
 typedef Graph_traits::face_iterator face_iterator;
@@ -34,23 +36,24 @@ struct Sequence_collector
 
   void operator()(halfedge_descriptor he, double alpha)
   {
-
+    sequence.emplace_back(he, alpha);
-    sequence.push_back( std::make_pair(he, alpha) );
   }
 
   void operator()(vertex_descriptor v)
   {
-    sequence.push_back( v );
+    sequence.push_back(v);
   }
 
   void operator()(face_descriptor f, Barycentric_coordinates alpha)
   {
-    sequence.push_back( std::make_pair(f, alpha) );
+    sequence.emplace_back(f, alpha);
   }
 };
 
 // A visitor to print what a variant contains using boost::apply_visitor
-struct Print_visitor : public boost::static_visitor<> {
+struct Print_visitor
+  : public boost::static_visitor<>
+{
   int i;
   Triangle_mesh& g;
 
@@ -58,22 +61,25 @@ struct Print_visitor : public boost::static_visitor<> {
 
   void operator()(vertex_descriptor v)
   {
-    std::cout << "#" << ++i << " : Vertex : " << get(boost::vertex_index, g)[v] << "\n";
+    std::cout << "#" << ++i << " Vertex: " << get(boost::vertex_index, g)[v];
+    std::cout << " Position: " << Surface_mesh_shortest_path::point(v, g) << "\n";
   }
 
   void operator()(const std::pair<halfedge_descriptor,double>& h_a)
   {
-    std::cout << "#" << ++i << " : Edge : " << get(CGAL::halfedge_index, g)[h_a.first] << " , ("
+    std::cout << "#" << ++i << " Edge: " << get(CGAL::halfedge_index, g)[h_a.first] << " , ("
-                                            << 1.0 - h_a.second << " , "
+                                           << 1.0 - h_a.second << " , "
-                                            << h_a.second << ")\n";
+                                           << h_a.second << ")";
+    std::cout << " Position: " << Surface_mesh_shortest_path::point(h_a.first, h_a.second, g) << "\n";
   }
 
   void operator()(const std::pair<face_descriptor, Barycentric_coordinates>& f_bc)
   {
-    std::cout << "#" << ++i << " : Face : " << get(CGAL::face_index, g)[f_bc.first] << " , ("
+    std::cout << "#" << ++i << " Face: " << get(CGAL::face_index, g)[f_bc.first] << " , ("
-                                            << f_bc.second[0] << " , "
+                                           << f_bc.second[0] << " , "
-                                            << f_bc.second[1] << " , "
+                                           << f_bc.second[1] << " , "
-                                            << f_bc.second[2] << ")\n";
+                                           << f_bc.second[2] << ")";
+    std::cout << " Position: " << Surface_mesh_shortest_path::point(f_bc.first, f_bc.second, g) << "\n";
   }
 };
 
@@ -100,12 +106,16 @@ int main(int argc, char** argv)
 
   // construct a shortest path query object and add a source point
   Surface_mesh_shortest_path shortest_paths(tmesh);
+
+  std::cout << "Add source: " << Surface_mesh_shortest_path::point(*face_it, face_location, tmesh) << std::endl;
   shortest_paths.add_source_point(*face_it, face_location);
 
   // pick a random target point inside a face
   face_it = faces(tmesh).first;
   std::advance(face_it, rand.get_int(0, static_cast<int>(num_faces(tmesh))));
 
+  std::cout << "Target is: " << Surface_mesh_shortest_path::point(*face_it, face_location, tmesh) << std::endl;
+
   // collect the sequence of simplicies crossed by the shortest path
   Sequence_collector sequence_collector;
   shortest_paths.shortest_path_sequence_to_source_points(*face_it, face_location, sequence_collector);

Surface_mesh_shortest_path/include/CGAL/Surface_mesh_shortest_path/Surface_mesh_shortest_path.h

@@ -14,36 +14,29 @@
 
 #include <CGAL/license/Surface_mesh_shortest_path.h>
 
-#include <CGAL/disable_warnings.h>
-
-#include <iterator>
-#include <vector>
-#include <utility>
-#include <queue>
-#include <algorithm>
-#include <cstddef>
-#include <list>
-
-#include <boost/array.hpp>
-#include <boost/lexical_cast.hpp>
-#include <boost/utility/enable_if.hpp>
-#include <boost/type_traits/is_same.hpp>
-#include <iterator>
-
-#include <CGAL/assertions.h>
-#include <CGAL/AABB_tree.h>
-#include <CGAL/Default.h>
-#include <CGAL/enum.h>
-#include <CGAL/number_utils.h>
-
 #include <CGAL/Surface_mesh_shortest_path/barycentric.h>
 #include <CGAL/Surface_mesh_shortest_path/internal/Cone_tree.h>
 #include <CGAL/Surface_mesh_shortest_path/internal/misc_functions.h>
 
+#include <CGAL/assertions.h>
+#include <CGAL/AABB_tree.h>
 #include <CGAL/boost/graph/helpers.h>
 #include <CGAL/boost/graph/iterator.h>
+#include <CGAL/Default.h>
+#include <CGAL/enum.h>
+#include <CGAL/number_utils.h>
+
+#include <boost/lexical_cast.hpp>
 #include <boost/variant/get.hpp>
 
+#include <algorithm>
+#include <cstddef>
+#include <iterator>
+#include <list>
+#include <queue>
+#include <utility>
+#include <vector>
+
 namespace CGAL {
 
 /*!
@@ -276,8 +269,6 @@ public:
 /// @}
 
 private:
-  typedef typename Graph_traits::vertex_iterator vertex_iterator;
-  typedef typename Graph_traits::halfedge_iterator halfedge_iterator;
   typedef typename Graph_traits::face_iterator face_iterator;
 
   typedef typename Traits::Triangle_3 Triangle_3;
@@ -354,13 +345,11 @@ private:
   Expansion_priqueue m_expansionPriqueue;
 
 #if !defined(NDEBUG)
-
   std::size_t m_currentNodeCount;
   std::size_t m_peakNodeCount;
   std::size_t m_queueAtPeakNodes;
   std::size_t m_peakQueueSize;
   std::size_t m_nodesAtPeakQueue;
-
 #endif
 
 #if !defined(NDEBUG)
@@ -420,7 +409,6 @@ public:
 
 #endif
 
-
 public:
 
   /// \cond
@@ -533,7 +521,8 @@ private:
   }
 
   /*
-    Filtering algorithm described in Xin and Wang (2009) "Improving chen and han's algorithm on the discrete geodesic problem."
+    Filtering algorithm described in Xin and Wang (2009)
+    "Improving chen and han's algorithm on the discrete geodesic problem."
     https://dl.acm.org/citation.cfm?doid=1559755.1559761
   */
   bool window_distance_filter(Cone_tree_node* cone,
@@ -659,7 +648,6 @@ private:
     }
 
     CGAL_assertion(cone->m_pendingLeftSubtree != nullptr);
-
     cone->m_pendingLeftSubtree = nullptr;
 
     if (window_distance_filter(cone, windowSegment, false))
@@ -689,14 +677,13 @@ private:
     typename Traits::Construct_vertex_2 cv2(m_traits.construct_vertex_2_object());
     typename Traits::Construct_triangle_3_along_segment_2_flattening ft3as2(m_traits.construct_triangle_3_along_segment_2_flattening_object());
 
-    CGAL_assertion(cone->m_pendingRightSubtree != nullptr);
-    cone->m_pendingRightSubtree = nullptr;
-
     if (m_debugOutput)
     {
       std::cout << std::endl << " >>>>>>>>>>>>>>>>>>> Expanding RIGHT CHILD <<<<<<<<<<<<<<<<<<<" <<std::endl;
     }
 
+    CGAL_assertion(cone->m_pendingRightSubtree != nullptr);
+    cone->m_pendingRightSubtree = nullptr;
 
     if (window_distance_filter(cone, windowSegment, true))
     {
@@ -729,7 +716,7 @@ private:
 
     std::size_t associatedEdge;
     CGAL::Surface_mesh_shortest_paths_3::Barycentric_coordinates_type type;
-    boost::tie(type, associatedEdge) = classify_barycentric_coordinates(location);
+    std::tie(type, associatedEdge) = classify_barycentric_coordinates(location);
 
     switch (type)
     {
@@ -777,11 +764,13 @@ private:
 
     Cone_tree_node* faceRoot = new Cone_tree_node(m_traits, m_graph, m_rootNodes.size());
     node_created();
-    m_rootNodes.push_back(std::make_pair(faceRoot, sourcePointIt));
+    m_rootNodes.emplace_back(faceRoot, sourcePointIt);
 
     if (m_debugOutput)
     {
       typename Traits::Construct_barycentric_coordinates_weight cbcw(m_traits.construct_barycentric_coordinates_weight_object());
+
+      std::cout << std::endl << " ~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
       std::cout << "\tFace Root Expansion: id = " << get(m_faceIndexMap, f)
                 << " , Location = " << cbcw(faceLocation, 0) << " " << cbcw(faceLocation, 1)
                 << " " << cbcw(faceLocation, 2) << " " << std::endl;
@@ -794,8 +783,12 @@ private:
       const Barycentric_coordinates rotatedFaceLocation(shifted_coordinates(faceLocation, currentVertex));
       const Point_2 sourcePoint(construct_barycenter_in_triangle_2(layoutFace, rotatedFaceLocation));
 
-      Cone_tree_node* child = new Cone_tree_node(m_traits, m_graph, current, layoutFace, sourcePoint,
+      Cone_tree_node* child = new Cone_tree_node(m_traits, m_graph,
-                                                 FT(0), cv2(layoutFace, 0), cv2(layoutFace, 2),
+                                                 current /*entryEdge*/,
+                                                 layoutFace, sourcePoint,
+                                                 FT(0) /*pseudoSourceDistance*/,
+                                                 cv2(layoutFace, 0) /*windowLeft*/,
+                                                 cv2(layoutFace, 2) /*windowRight*/,
                                                  Cone_tree_node::FACE_SOURCE);
       node_created();
       faceRoot->push_middle_child(child);
@@ -803,7 +796,8 @@ private:
       if (m_debugOutput)
       {
         std::cout << "\tExpanding face root #" << currentVertex << " : " << std::endl;;
-        std::cout << "\t\tFace = " << layoutFace << std::endl;
+        std::cout << "\t\t3D Face = " << face3d << std::endl;
+        std::cout << "\t\t2D Face = " << layoutFace << std::endl;
         std::cout << "\t\tLocation = " << sourcePoint << std::endl;
       }
 
@@ -820,15 +814,20 @@ private:
                         const FT t0, const FT t1,
                         Source_point_iterator sourcePointIt)
   {
+    CGAL_precondition(!is_border(baseEdge, m_graph));
+    CGAL_precondition(t0 + t1 == FT(1));
+
     typename Traits::Construct_barycenter_2 cb2(m_traits.construct_barycenter_2_object());
     typename Traits::Construct_vertex_2 cv2(m_traits.construct_vertex_2_object());
     typename Traits::Construct_triangle_3_to_triangle_2_projection pt3t2(m_traits.construct_triangle_3_to_triangle_2_projection_object());
 
     if (m_debugOutput)
     {
+      std::cout << std::endl << " ~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
       std::cout << "\tEdge Root Expansion: faceA = " << get(m_faceIndexMap, face(baseEdge, m_graph))
                 << " , faceB = " << get(m_faceIndexMap, face(opposite(baseEdge, m_graph), m_graph))
                 << " , t0 = " << t0 << " , t1 = " << t1 << std::endl;
+      std::cout << "\t\tBoundary: " << is_border_edge(baseEdge, m_graph) << std::endl;
     }
 
     halfedge_descriptor baseEdges[2];
@@ -850,7 +849,7 @@ private:
 
     Cone_tree_node* edgeRoot = new Cone_tree_node(m_traits, m_graph, m_rootNodes.size());
     node_created();
-    m_rootNodes.push_back(std::make_pair(edgeRoot, sourcePointIt));
+    m_rootNodes.emplace_back(edgeRoot, sourcePointIt);
 
     for (std::size_t side = 0; side < 2; ++side)
     {
@@ -878,6 +877,7 @@ private:
   {
     if (m_debugOutput)
     {
+      std::cout << std::endl << " ~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
       std::cout << "\tVertex Root Expansion: Vertex = " << get(m_vertexIndexMap, vertex) << std::endl;
     }
 
@@ -885,7 +885,7 @@ private:
                                                     prev(halfedge(vertex, m_graph), m_graph));
 
     node_created();
-    m_rootNodes.push_back(std::make_pair(vertexRoot, sourcePointIt));
+    m_rootNodes.emplace_back(vertexRoot, sourcePointIt);
 
     m_closestToVertices[get(m_vertexIndexMap, vertex)] = Node_distance_pair(vertexRoot, FT(0));
 
@@ -894,6 +894,13 @@ private:
 
   /*
     Create child nodes for each face surrounding the vertex occupied by `parent`, and push them to the queue
+
+    By convention, source windows are left & middle (no right) as to not create overlaps.
+    A child is also created for the border halfedge (if any) as to propagate distance
+    to the next vertex on the border (aka `target(next(border_h, g), g)`).
+    This creates a nonsensical triangle made of `source(border_h, g)`, `target(border_h, g)`,
+    and `target(next(border_h, g), g)` but propagation is only done to the vertex (vertex source:
+    no propagation on the right by convention, and left is a border halfedge so no propagation either).
   */
   void expand_pseudo_source(Cone_tree_node* parent)
   {
@@ -911,8 +918,8 @@ private:
 
     if (m_debugOutput)
     {
-      std::cout << "expansionVertex: " << get(m_vertexIndexMap, expansionVertex) << std::endl;
+      std::cout << "Pseudo source: V" << get(m_vertexIndexMap, expansionVertex) << std::endl;
-      std::cout << "Distance from target to root: " << distanceFromTargetToRoot << std::endl;
+      std::cout << "Distance from pseudo source to root: " << distanceFromTargetToRoot << std::endl;
     }
 
     // A potential optimization could be made by only expanding in the 'necessary' range (i.e. the range outside of geodesic visibility), but the
@@ -931,7 +938,7 @@ private:
                   << get(m_vertexIndexMap, source(currentEdge, m_graph)) << " "
                   << get(m_vertexIndexMap, target(currentEdge, m_graph)) << std::endl;
         std::cout << "face id = ";
-        if (face(currentEdge, m_graph) != Graph_traits::null_face())
+        if (!is_border(currentEdge, m_graph))
         {
           std::cout << get(m_faceIndexMap, face(currentEdge, m_graph)) << std::endl;
 
@@ -945,9 +952,11 @@ private:
         }
       }
 
-      Cone_tree_node* child = new Cone_tree_node(m_traits, m_graph, currentEdge, layoutFace,
+      Cone_tree_node* child = new Cone_tree_node(m_traits, m_graph, currentEdge /*entryEdge*/,
-                                                 cv2(layoutFace, 1), distanceFromTargetToRoot,
+                                                 layoutFace, cv2(layoutFace, 1) /*sourceImage*/,
-                                                 cv2(layoutFace, 0), cv2(layoutFace, 2),
+                                                 distanceFromTargetToRoot,
+                                                 cv2(layoutFace, 0) /*windowLeft*/,
+                                                 cv2(layoutFace, 2) /*windowRight*/,
                                                  Cone_tree_node::VERTEX_SOURCE);
 
       node_created();
@@ -1131,18 +1140,18 @@ private:
       std::cout << "\tParent node: " << node->parent() << std::endl;
       std::cout << "\tParent node type: " << node->parent()->node_type() << std::endl;
       std::cout << "\tFace = " << node->layout_face() << std::endl;
-      std::cout << "\tVertices = ";
+      std::cout << "\tVertices =";
       halfedge_descriptor current = node->entry_edge();
       for (std::size_t i = 0; i<3; ++i)
       {
-        std::cout << get(m_vertexIndexMap, source(current, m_graph)) << " ";
+        std::cout << " " << get(m_vertexIndexMap, source(current, m_graph));
         current = next(current, m_graph);
       }
       std::cout << std::endl;
       std::cout << "\tSource Image = " << node->source_image() << std::endl;
-      std::cout << "\tEntry Halfedge = (" << get(m_vertexIndexMap, source(node->entry_edge(), m_graph)) << " "
+      std::cout << "\tEntry Halfedge = (V" << get(m_vertexIndexMap, source(node->entry_edge(), m_graph)) << " V"
                                            << get(m_vertexIndexMap, target(node->entry_edge(), m_graph)) << ")" << std::endl;
-      std::cout << "\tTarget vertex = " << get(m_vertexIndexMap, node->target_vertex()) << std::endl;
+      std::cout << "\tTarget vertex = V" << get(m_vertexIndexMap, node->target_vertex()) << std::endl;
 
       std::cout << "\tWindow Left = " << node->window_left() << std::endl;
       std::cout << "\tWindow Right = " << node->window_right() << std::endl;
@@ -1189,7 +1198,7 @@ private:
 
       std::size_t entryHalfEdgeIndex = get(m_halfedgeIndexMap, node->entry_edge());
 
-      Node_distance_pair currentOccupier = m_vertexOccupiers[entryHalfEdgeIndex];
+      const Node_distance_pair& currentOccupier = m_vertexOccupiers[entryHalfEdgeIndex];
       FT currentNodeDistance = node->distance_from_target_to_root();
 
       if (m_debugOutput)
@@ -1231,11 +1240,11 @@ private:
 
         if (m_debugOutput)
         {
-          std::cout << "\t Current occupier, EH ("
+          std::cout << "\t Current occupier, EH (V"
-                    << get(m_vertexIndexMap, source(currentOccupier.first->entry_edge(), m_graph)) << " "
+                    << get(m_vertexIndexMap, source(currentOccupier.first->entry_edge(), m_graph)) << " V"
                     << get(m_vertexIndexMap, target(currentOccupier.first->entry_edge(), m_graph)) << ")" << std::endl;
           std::cout << "\t Current occupier, Source = " << currentOccupier.first->source_image() << std::endl;
-          std::cout << "\t Current Occupier Distance = " << currentOccupier.second << std::endl;
+          std::cout << "\t Current occupier Distance = " << currentOccupier.second << std::endl;
           std::cout << "\t smaller (-1)/equal (0)/larger (1) comparison? " << c << std::endl;
         }
       }
@@ -1313,9 +1322,9 @@ private:
           }
         }
 
-        // Check if node is now the absolute closest node, and replace the current closest as appropriate
+        // Check if `node` is now the absolute closest node, and replace the current closest as appropriate
         std::size_t targetVertexIndex = get(m_vertexIndexMap, node->target_vertex());
-        Node_distance_pair currentClosest = m_closestToVertices[targetVertexIndex];
+        const Node_distance_pair& currentClosest = m_closestToVertices[targetVertexIndex];
 
         if (m_debugOutput && currentClosest.first != nullptr)
         {
@@ -1324,12 +1333,13 @@ private:
 
         // If equal times, give priority to vertex sources since it's cleaner and simpler to handle than interval windows
         if (currentClosest.first == nullptr ||
-           currentClosest.second > currentNodeDistance ||
+            currentClosest.second > currentNodeDistance ||
-           (currentClosest.second == currentNodeDistance && node->node_type() == Cone_tree_node::VERTEX_SOURCE))
+            (currentClosest.second == currentNodeDistance &&
+             node->node_type() == Cone_tree_node::VERTEX_SOURCE))
         {
           if (m_debugOutput)
           {
-            std::cout << "\t Current node is now the closest at target vertex "
+            std::cout << "\t Current node is now the closest at target vertex V"
                       << get(m_vertexIndexMap, node->target_vertex()) << std::endl;
           }
 
@@ -1338,7 +1348,7 @@ private:
           {
             if (m_debugOutput)
             {
-              std::cout << "\t Vertex " << targetVertexIndex << " is a pseudo-source" << std::endl;
+              std::cout << "\t Vertex V" << targetVertexIndex << " is a pseudo-source" << std::endl;
             }
 
             if (currentClosest.first != nullptr)
@@ -1399,11 +1409,13 @@ private:
     {
       if (propagateLeft)
       {
+        CGAL_assertion(!node->is_null_face());
         push_left_child(node);
       }
 
       if (propagateRight && (!node->is_source_node() || node->node_type() == Cone_tree_node::EDGE_SOURCE))
       {
+        CGAL_assertion(!node->is_source_node());
         push_right_child(node);
       }
 
@@ -1421,9 +1433,17 @@ private:
 
   void push_left_child(Cone_tree_node* parent)
   {
+    if (m_debugOutput)
+    {
+      std::cout << "Tentative push of left child edge "
+                << " (V" << get(m_vertexIndexMap, source(parent->left_child_edge(), m_graph))
+                << " V" << get(m_vertexIndexMap, target(parent->left_child_edge(), m_graph)) << ")" << std::endl;
+      std::cout << "Boundary? " << is_border(parent->left_child_edge(), m_graph) << std::endl;
+    }
+
     typename Traits::Compute_squared_distance_2 csd2(m_traits.compute_squared_distance_2_object());
 
-    if (face(parent->left_child_edge(), m_graph) != Graph_traits::null_face())
+    if (!is_border(parent->left_child_edge(), m_graph))
     {
       Segment_2 leftWindow;
 
@@ -1467,12 +1487,15 @@ private:
   {
     if (m_debugOutput)
     {
-      std::cout << "Tentative push of right child..." << std::endl;
+      std::cout << "Tentative push of right child edge"
+                << " (V" << get(m_vertexIndexMap, source(parent->right_child_edge(), m_graph))
+                << " V" << get(m_vertexIndexMap, target(parent->right_child_edge(), m_graph)) << ")" << std::endl;
+      std::cout << "Boundary? " << is_border(parent->right_child_edge(), m_graph) << std::endl;
     }
 
     typename Traits::Compute_squared_distance_2 csd2(m_traits.compute_squared_distance_2_object());
 
-    if (face(parent->right_child_edge(), m_graph) != Graph_traits::null_face())
+    if (!is_border(parent->right_child_edge(), m_graph))
     {
       Segment_2 rightWindow;
       bool result = clip_to_bounds(parent->right_child_base_segment(), parent->left_boundary(),
@@ -1605,14 +1628,11 @@ private:
 
   void set_vertex_types()
   {
-    vertex_iterator current, end;
+    for(vertex_descriptor v : vertices(m_graph))
-
-    for (boost::tie(current, end) = vertices(m_graph); current != end; ++current)
     {
-      if (halfedge(*current, m_graph)==Graph_traits::null_halfedge())
+      std::size_t vertexIndex = get(m_vertexIndexMap, v);
-        continue;
+      m_vertexIsPseudoSource[vertexIndex] = !is_isolated(v, m_graph) &&
-      std::size_t vertexIndex = get(m_vertexIndexMap, *current);
+                                            (is_saddle_vertex(v) || is_boundary_vertex(v));
-      m_vertexIsPseudoSource[vertexIndex] = (is_saddle_vertex(*current) || is_boundary_vertex(*current));
     }
   }
 
@@ -1623,21 +1643,7 @@ private:
 
   bool is_boundary_vertex(const vertex_descriptor v) const
   {
-    halfedge_descriptor h = halfedge(v, m_graph);
+    return bool(is_border(v, m_graph));
-    halfedge_descriptor first = h;
-
-    do
-    {
-      if (face(h, m_graph) == Graph_traits::null_face() || face(opposite(h, m_graph), m_graph) == Graph_traits::null_face())
-      {
-        return true;
-      }
-
-      h = opposite(next(h, m_graph), m_graph);
-    }
-    while(h != first);
-
-    return false;
   }
 
   void delete_all_nodes()
@@ -1650,11 +1656,8 @@ private:
 
   void reset_algorithm(const bool clearFaceLocations = true)
   {
-    Cone_tree_node* null_value=nullptr;
+    m_closestToVertices.assign(num_vertices(m_graph), Node_distance_pair(nullptr, FT(-1)));
-    m_closestToVertices.resize(num_vertices(m_graph));
+    m_vertexOccupiers.assign(num_halfedges(m_graph), Node_distance_pair(nullptr, FT(-1)));
-    std::fill(m_closestToVertices.begin(), m_closestToVertices.end(), Node_distance_pair(null_value, FT(0)));
-    m_vertexOccupiers.resize(num_halfedges(m_graph));
-    std::fill(m_vertexOccupiers.begin(), m_vertexOccupiers.end(), Node_distance_pair(null_value, FT(0)));
 
     while (!m_expansionPriqueue.empty())
     {
@@ -1671,7 +1674,7 @@ private:
 
     delete_all_nodes();
     m_rootNodes.clear();
-    m_vertexIsPseudoSource.resize(num_vertices(m_graph));
+    m_vertexIsPseudoSource.assign(num_vertices(m_graph), false);
 
 #if !defined(NDEBUG)
     m_currentNodeCount = 0;
@@ -1684,7 +1687,7 @@ private:
   }
 
   template <class Visitor>
-  void visit_shortest_path(Cone_tree_node* startNode,
+  void visit_shortest_path(const Cone_tree_node* startNode,
                            const Point_2& startLocation,
                            Visitor& visitor)
   {
@@ -1695,7 +1698,7 @@ private:
     typename Traits::Construct_target_2 construct_target_2(m_traits.construct_target_2_object());
     typename Traits::Intersect_2 intersect_2(m_traits.intersect_2_object());
 
-    Cone_tree_node* current = startNode;
+    const Cone_tree_node* current = startNode;
     Point_2 currentLocation(startLocation);
 
     while (!current->is_root_node())
@@ -1705,19 +1708,21 @@ private:
         case Cone_tree_node::INTERVAL:
         case Cone_tree_node::EDGE_SOURCE:
         {
-          Segment_2 entrySegment = current->entry_segment();
+          const Segment_2& entrySegment = current->entry_segment();
           const Point_2& currentSourceImage = current->source_image();
           Ray_2 rayToLocation(construct_ray_2(currentSourceImage, currentLocation));
 
-          const auto cgalIntersection = intersect_2(construct_line_2(entrySegment), construct_line_2(rayToLocation));
+          const auto cgalIntersection = intersect_2(construct_line_2(entrySegment),
+                                                    construct_line_2(rayToLocation));
 
           CGAL_assertion(bool(cgalIntersection));
 
           const Point_2* result = boost::get<Point_2>(&*cgalIntersection);
+          if (!result)
+            result = &currentSourceImage;
 
-          if (!result) result = &currentSourceImage;
+          FT t0 = parametric_distance_along_segment_2(construct_source_2(entrySegment),
-
+                                                      construct_target_2(entrySegment), *result);
-          FT t0 = parametric_distance_along_segment_2(construct_source_2(entrySegment), construct_target_2(entrySegment), *result);
 
           if (m_debugOutput)
           {
@@ -1740,8 +1745,8 @@ private:
             std::cout << "Current Right Window: " << current->window_right() << "  ,  "
                       << m_traits.compute_parametric_distance_along_segment_2_object()(entrySegment.start(), entrySegment.end(), current->window_right()) << std::endl;
             std::cout << "Current Segment Intersection: " << *result << std::endl;
-            std::cout << "Edge: (" << get(m_vertexIndexMap, source(current->entry_edge(), m_graph))
+            std::cout << "Edge: (V" << get(m_vertexIndexMap, source(current->entry_edge(), m_graph))
-                      << "," << get(m_vertexIndexMap, target(current->entry_edge(), m_graph)) << ")  :  " << t0 << std::endl;
+                      << ", V" << get(m_vertexIndexMap, target(current->entry_edge(), m_graph)) << ")  :  " << t0 << std::endl;
           }
 
           visitor(current->entry_edge(), t0);
@@ -1866,7 +1871,7 @@ private:
 
     std::size_t associatedEdge;
     CGAL::Surface_mesh_shortest_paths_3::Barycentric_coordinates_type type;
-    boost::tie(type, associatedEdge) = classify_barycentric_coordinates(location);
+    std::tie(type, associatedEdge) = classify_barycentric_coordinates(location);
 
     switch (type)
     {
@@ -1979,42 +1984,35 @@ private:
     reset_algorithm(false);
     set_vertex_types();
 
-    m_vertexOccupiers.resize(num_halfedges(m_graph));
+    m_vertexOccupiers.assign(num_halfedges(m_graph), Node_distance_pair(nullptr, FT(-1)));
-    m_closestToVertices.resize(num_vertices(m_graph));
+    m_closestToVertices.assign(num_vertices(m_graph), Node_distance_pair(nullptr, FT(-1)));
 
     if (m_debugOutput)
     {
-      vertex_iterator current, end;
-
       std::size_t numVertices = 0;
-
+      for (vertex_descriptor v : vertices(m_graph))
-      for (boost::tie(current,end) = vertices(m_graph); current != end; ++current)
       {
         std::cout << "Vertex#" << numVertices
-                  << ": p = " << get(m_vertexPointMap,*current)
+                  << ": p = " << get(m_vertexPointMap, v)
-                  << " , Saddle Vertex: " << (is_saddle_vertex(*current) ? "yes" : "no")
+                  << " , Saddle Vertex: " << (is_saddle_vertex(v) ? "yes" : "no")
-                  << " , Boundary Vertex: " << (is_boundary_vertex(*current) ? "yes" : "no") << std::endl;
+                  << " , Boundary Vertex: " << (is_boundary_vertex(v) ? "yes" : "no") << std::endl;
         ++numVertices;
       }
     }
 
-    face_iterator facesCurrent;
-    face_iterator facesEnd;
-
     if (m_debugOutput)
     {
       std::size_t numFaces = 0;
-
+      for (face_descriptor f : faces(m_graph))
-      for (boost::tie(facesCurrent, facesEnd) = faces(m_graph); facesCurrent != facesEnd; ++facesCurrent)
       {
         std::cout << "Face#" << numFaces << ": Vertices = (";
         ++numFaces;
-        halfedge_descriptor faceEdgesStart = halfedge(*facesCurrent, m_graph);
+        halfedge_descriptor faceEdgesStart = halfedge(f, m_graph);
         halfedge_descriptor faceEdgesCurrent = faceEdgesStart;
 
         do
         {
-          std::cout << get(m_vertexIndexMap, source(faceEdgesCurrent, m_graph));
+          std::cout << "V" << get(m_vertexIndexMap, source(faceEdgesCurrent, m_graph));
 
           faceEdgesCurrent = next(faceEdgesCurrent, m_graph);
 
@@ -2031,15 +2029,15 @@ private:
 
         std::cout << std::endl;
       }
-
     }
 
     for (typename Source_point_list::iterator it = m_faceLocations.begin(); it != m_faceLocations.end(); ++it)
     {
       if (m_debugOutput)
       {
-        std::cout << "Root: " << get(m_faceIndexMap, it->first)
+        std::cout << "Root: F" << get(m_faceIndexMap, it->first)
-                  << " , " << it->second[0] << " " << it->second[1] << " " << it->second[2] << " " << std::endl;
+                  << " , bar " << it->second[0] << " " << it->second[1] << " " << it->second[2] << " "
+                  << ", pos " << point(it->first, it->second) << std::endl;
       }
 
       expand_root(it->first, it->second, Source_point_iterator(it));
@@ -2054,7 +2052,7 @@ private:
 
     }
 
-    while (m_expansionPriqueue.size() > 0)
+    while (!m_expansionPriqueue.empty())
     {
       if (m_debugOutput)
       {
@@ -2077,10 +2075,10 @@ private:
           if (!event->m_cancelled)
           {
             std::cout << " ------ Parent (" << event->m_parent << ") INFO: ";
-            std::cout << "EH = (" << get(m_vertexIndexMap, source(event->m_parent->entry_edge(), m_graph)) << " "
+            std::cout << "EH = (V" << get(m_vertexIndexMap, source(event->m_parent->entry_edge(), m_graph)) << " V"
-                                              << get(m_vertexIndexMap, target(event->m_parent->entry_edge(), m_graph)) << ") ";
+                                  << get(m_vertexIndexMap, target(event->m_parent->entry_edge(), m_graph)) << ") ";
-            std::cout << "S = (" << event->m_parent->source_image() << ") ";
+            std::cout << "Src = (" << event->m_parent->source_image() << ") ";
-            std::cout << "T = " << get(m_vertexIndexMap, target(next(event->m_parent->entry_edge(), m_graph), m_graph));
+            std::cout << "Tar = V" << get(m_vertexIndexMap, target(next(event->m_parent->entry_edge(), m_graph), m_graph));
           }
 
           std::cout << std::endl;
@@ -2114,8 +2112,8 @@ private:
             if (m_debugOutput)
             {
               std::cout << "Left Expansion: Parent = " << parent
-                        << " Edge = (" << get(m_vertexIndexMap, source(event->m_parent->left_child_edge(), m_graph))
+                        << " Edge = (V" << get(m_vertexIndexMap, source(event->m_parent->left_child_edge(), m_graph))
-                        << "," << get(m_vertexIndexMap, target(event->m_parent->left_child_edge(), m_graph))
+                        << ", V" << get(m_vertexIndexMap, target(event->m_parent->left_child_edge(), m_graph))
                         << ") , Distance = " << event->m_distanceEstimate
                         << " , Level = " << event->m_parent->level() + 1 << std::endl;
             }
@@ -2126,8 +2124,8 @@ private:
             if (m_debugOutput)
             {
               std::cout << "Right Expansion: Parent = " << parent
-                        << " , Edge = (" << get(m_vertexIndexMap, source(event->m_parent->right_child_edge(), m_graph))
+                        << " , Edge = (V" << get(m_vertexIndexMap, source(event->m_parent->right_child_edge(), m_graph))
-                        << "," << get(m_vertexIndexMap, target(event->m_parent->right_child_edge(), m_graph))
+                        << ", V" << get(m_vertexIndexMap, target(event->m_parent->right_child_edge(), m_graph))
                         << ") , Distance = " << event->m_distanceEstimate
                         << " , Level = " << event->m_parent->level() + 1 << std::endl;
             }
@@ -2165,7 +2163,7 @@ private:
       for (std::size_t i = 0; i < m_closestToVertices.size(); ++i)
       {
         std::cout << "\tVertex = " << i << std::endl;
-        std::cout << "\tDistance = " << m_closestToVertices[i].second << std::endl;
+        std::cout << "\tDistance = " << m_closestToVertices[i].second << " to " << m_closestToVertices[i].first << std::endl;
       }
 
       std::cout << std::endl;
@@ -2280,6 +2278,13 @@ public:
   Source_point_iterator add_source_point(vertex_descriptor v)
   {
     Face_location location = face_location(v);
+
+    if (m_debugOutput)
+    {
+      std::cout << "Face location from V" << get(m_vertexIndexMap, v) << " is F" << get(m_faceIndexMap, location.first) << " "
+                << location.second[0] << " " << location.second[1] << " " << location.second[2] << std::endl;
+    }
+
     return add_source_point(location);
   }
 
@@ -2306,6 +2311,11 @@ public:
   */
   Source_point_iterator add_source_point(const Face_location& location)
   {
+    if (m_debugOutput)
+    {
+      std::cout << "Add source point at position " << point(location.first, location.second) << std::endl;
+    }
+
     Source_point_underlying_iterator added = m_faceLocations.insert(m_faceLocations.end(), location);
 
     if (m_firstNewSourcePoint == m_faceLocations.end())
@@ -2471,9 +2481,8 @@ public:
   {
     build_sequence_tree();
 
-    Node_distance_pair result = m_closestToVertices[get(m_vertexIndexMap, v)];
+    const Node_distance_pair& result = m_closestToVertices[get(m_vertexIndexMap, v)];
-
+    const Cone_tree_node* current = result.first;
-    Cone_tree_node* current = result.first;
 
     if (current)
     {
@@ -2500,9 +2509,8 @@ public:
   {
     build_sequence_tree();
 
-    std::pair<Node_distance_pair, Barycentric_coordinates> result = nearest_to_location(f, location);
+    const std::pair<Node_distance_pair, Barycentric_coordinates>& result = nearest_to_location(f, location);
-
+    const Cone_tree_node* current = result.first.first;
-    Cone_tree_node* current = result.first.first;
 
     if (current)
     {
@@ -2542,8 +2550,8 @@ public:
   {
     build_sequence_tree();
 
-    Node_distance_pair result = m_closestToVertices[get(m_vertexIndexMap, v)];
+    const Node_distance_pair& result = m_closestToVertices[get(m_vertexIndexMap, v)];
-    Cone_tree_node* current = result.first;
+    const Cone_tree_node* current = result.first;
 
     if (current)
     {
@@ -2739,11 +2747,21 @@ public:
 
   \param vertex A vertex of the input face graph
   */
-  Point_3 point(const vertex_descriptor vertex) const
+  decltype(auto) point(const vertex_descriptor v) const
   {
-    return get(m_vertexPointMap, vertex);
+    return get(m_vertexPointMap, v);
   }
 
+  /// \cond
+
+  static decltype(auto) point(const vertex_descriptor v,
+                              const Triangle_mesh& tm)
+  {
+    return get(CGAL::vertex_point, tm, v);
+  }
+
+  /// \endcond
+
   /// @}
 
   /// \name Surface Face Location Constructions
@@ -2770,7 +2788,7 @@ public:
   {
     typename Traits::Construct_barycentric_coordinates construct_barycentric_coordinates(traits.construct_barycentric_coordinates_object());
     halfedge_descriptor hinit=halfedge(vertex, tm);
-    while (face(hinit, tm) == Graph_traits::null_face())
+    while (is_border(hinit, tm))
       hinit = opposite(next(hinit, tm), tm);
 
     halfedge_descriptor he = next(hinit, tm);
@@ -3043,7 +3061,7 @@ public:
                               Vertex_point_map vertexPointMap)
   {
     face_iterator facesStart, facesEnd;
-    boost::tie(facesStart, facesEnd) = faces(tm);
+    std::tie(facesStart, facesEnd) = faces(tm);
     outTree.rebuild(facesStart, facesEnd, tm, vertexPointMap);
     outTree.build();
   }
@@ -3054,6 +3072,4 @@ public:
 
 } // namespace CGAL
 
-#include <CGAL/enable_warnings.h>
-
 #endif // CGAL_SURFACE_MESH_SHORTEST_PATH_SURFACE_MESH_SHORTEST_PATH_H

Surface_mesh_shortest_path/include/CGAL/Surface_mesh_shortest_path/function_objects.h

@@ -493,7 +493,7 @@ public:
     // In the case of multiple rays reaching the same target, we want to know their respective position
     // so that pruning of branches can be done according to the "one angle one split" idiom.
     // However, the orientation predicate is evaluated in the unfolded 2D plane, which is obtained
-    // via square roots; inconsisnties will exist. We don't want to prune in case it might be wrong,
+    // via square roots; inconsistencies will exist. We don't want to prune in case it might be wrong,
     // so we add a little bit of tolerance on the evaluation of the predicate. If it's almost collinear,
     // return 'collinear' (EQUAL).
     const FT eps = (FT(100) * std::numeric_limits<FT>::epsilon());