From c887843b9501292522660d08db446aef2477466f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 8 Aug 2014 16:04:41 +0200
Subject: [PATCH] Add method Graph::getEdge
---
include/hpp/manipulation/graph/graph.hh | 3 ++
src/graph/graph.cc | 67 +++++++++++++++++++++++++
2 files changed, 70 insertions(+)
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 2fa1ee2..1140e01 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -47,6 +47,9 @@ namespace hpp {
/// Returns the states of a configuration.
Nodes_t getNode(const Configuration_t config) const;
+ /// Get possible edges between two nodes.
+ std::vector <Edges_t> getEdge(const Nodes_t& from, const Nodes_t& to) const;
+
/// Select randomly outgoing edges of the given nodes.
Edges_t chooseEdge(const Nodes_t& node) const;
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index e61e28d..c2f8f44 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -82,6 +82,73 @@ namespace hpp {
return nodes;
}
+ static void buildPossibleEdges (Edges_t current,
+ const std::vector <Edges_t>& edgesPerNodeSelector,
+ std::vector <Edges_t>& possibleEdges)
+ {
+ size_t d = current.size();
+ if (d == edgesPerNodeSelector.size()) {
+ possibleEdges.push_back (current);
+ return;
+ }
+ Edges_t::const_iterator it = edgesPerNodeSelector[d].begin();
+ while (it != edgesPerNodeSelector[d].end()) {
+ current.push_back (*it);
+ buildPossibleEdges (current, edgesPerNodeSelector, possibleEdges);
+ current.pop_back ();
+ it++;
+ }
+ }
+
+ std::vector <Edges_t> Graph::getEdge(const Nodes_t& from, const Nodes_t& to) const
+ {
+ assert (from.size() == to.size());
+ assert (nodeSelectors_.size() == to.size());
+ size_t numberPossibleEdges = 1;
+ std::vector < Edges_t > edgesPerNodeSelector (nodeSelectors_.size());
+ std::vector < Edges_t >::iterator itEdgePerNodeSelector = edgesPerNodeSelector.begin();
+ Nodes_t::const_iterator itFrom = from.begin (),
+ itTo = to.begin ();
+ // We first iterate through from. For each element of from,
+ // we look for all edges between this element of from and its corresponding
+ // element in to. The resulting set of Edges_t is stored in
+ // edgesPerNodeSelector.
+
+ // Temporary variable.
+ Edges_t edgesInNodeSelector;
+ const Edges_t* neighbors;
+ Edges_t::const_iterator itEdge;
+ while (itFrom != from.end()) {
+ edgesInNodeSelector.clear ();
+ neighbors = &((*itFrom)->neighbors ());
+ itEdge = neighbors->begin();
+ // Find the edges between *itFrom and *itTo
+ while (itEdge != neighbors->end()) {
+ if ( (*itEdge)->to() == (*itTo) )
+ edgesInNodeSelector.push_back (*itEdge);
+ itEdge++;
+ }
+ /// If no Edge is found, the two Node are not connected.
+ if (edgesInNodeSelector.empty ())
+ return std::vector <Edges_t>(0);
+
+ // Store the Edges.
+ numberPossibleEdges *= edgesInNodeSelector.size();
+ *itEdgePerNodeSelector = edgesInNodeSelector;
+
+ itFrom++; itTo++; itEdgePerNodeSelector++;
+ }
+ assert (itTo == to.end());
+ assert (itEdgePerNodeSelector == edgesPerNodeSelector.end());
+
+ // Now, we can create the list of possible Edges_t
+ // between from and to.
+ std::vector <Edges_t> possibleEdges;
+ buildPossibleEdges (Edges_t(), edgesPerNodeSelector, possibleEdges);
+ assert (possibleEdges.size() == numberPossibleEdges);
+ return possibleEdges;
+ }
+
Edges_t Graph::chooseEdge(const Nodes_t& nodes) const
{
Edges_t edges;
--
GitLab