Add Roadmap::nearestNode.

include/hpp/manipulation/roadmap.hh

@@ -21,6 +21,7 @@
 # include <hpp/core/constraint-set.hh>
 
 # include "hpp/manipulation/fwd.hh"
+# include "hpp/manipulation/graph/fwd.hh"
 # include "hpp/manipulation/graph/statistics.hh"
 
 namespace hpp {
@@ -54,6 +55,13 @@ namespace hpp {
         /// Catch event 'New node added'
         void push_node (const core::NodePtr_t& n);
 
+        /// Get the nearest neighbor in a given graph::Node and in a given
+        /// core::ConnectedComponent.
+        core::NodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
+            const ConnectedComponentPtr_t& connectedComponent,
+            const graph::NodePtr_t& node,
+            value_type& minDistance) const;
+
       protected:
         /// Register a new configuration.
         void statInsert (const core::NodePtr_t& n);
@@ -66,6 +74,7 @@ namespace hpp {
         /// Keep track of the leaf that are explored.
         /// There should be one histogram per foliation.
         Histograms histograms_;
+        graph::GraphPtr_t graph_;
     };
     /// \}
   } // namespace manipulation

src/roadmap.cc

@@ -17,6 +17,7 @@
 #include "hpp/manipulation/roadmap.hh"
 
 #include <hpp/util/pointer.hh>
+#include <hpp/core/connected-component.hh>
 
 namespace hpp {
   namespace manipulation {
@@ -65,6 +66,7 @@ namespace hpp {
 
     void Roadmap::constraintGraph (const graph::GraphPtr_t& graph)
     {
+      graph_ = graph;
       Histograms::iterator it = histograms_.begin();
       for (; it != histograms_.end();) {
         if (HPP_DYNAMIC_PTR_CAST (graph::NodeHistogram, *it))
@@ -74,5 +76,27 @@ namespace hpp {
       }
       insertHistogram (graph::HistogramPtr_t (new graph::NodeHistogram (graph)));
     }
+
+    core::NodePtr_t Roadmap::nearestNode (const ConfigurationPtr_t& configuration,
+        const ConnectedComponentPtr_t& connectedComponent,
+        const graph::NodePtr_t& node,
+        value_type& minDistance) const
+    {
+      core::NodePtr_t result = NULL;
+      minDistance = std::numeric_limits <value_type>::infinity ();
+      for (core::Nodes_t::const_iterator itNode =
+          connectedComponent->nodes ().begin ();
+          itNode != connectedComponent->nodes ().end (); ++itNode) {
+        if (graph_->getNode (*(*itNode)->configuration ()) != node)
+          continue;
+        value_type d = (*distance()) (*(*itNode)->configuration (),
+            *configuration);
+        if (d < minDistance) {
+          minDistance = d;
+          result = *itNode;
+        }
+      }
+      return result;
+    }
   } // namespace manipulation
 } // namespace hpp