diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index d0b5de0ddafd618cc7ab57268e39b3fb64d075d6..2f2468bdd2c9903756ba7f300ce804df4e1012fb 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -62,6 +62,8 @@ namespace hpp {
typedef boost::shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
HPP_PREDEF_CLASS (LeafConnectedComp);
typedef boost::shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
+ typedef boost::shared_ptr<const LeafConnectedComp>
+ LeafConnectedCompConstPtr_t;
typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t;
HPP_PREDEF_CLASS (WeighedLeafConnectedComp);
typedef boost::shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
diff --git a/include/hpp/manipulation/leaf-connected-comp.hh b/include/hpp/manipulation/leaf-connected-comp.hh
index b3f932e320224241f9f45d3c129711e83e4f08b3..40debeda3bcf6c0a0059a42febeac6763f6eb71f 100644
--- a/include/hpp/manipulation/leaf-connected-comp.hh
+++ b/include/hpp/manipulation/leaf-connected-comp.hh
@@ -34,22 +34,26 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI LeafConnectedComp
{
public:
- typedef std::set<LeafConnectedComp*> LeafConnectedComps_t;
-
+ typedef LeafConnectedComp* RawPtr_t;
+ typedef std::set <RawPtr_t> LeafConnectedComps_t;
/// return a shared pointer to new instance
static LeafConnectedCompPtr_t create (const RoadmapPtr_t& roadmap);
- /// Merge two symbolic components
+ /// Merge two connected components
/// \param other manipulation symbolic component to merge into this one.
/// \note other will be empty after calling this method.
- virtual void merge (LeafConnectedCompPtr_t otherCC);
+ virtual void merge (const LeafConnectedCompPtr_t& otherCC);
- bool canMerge (const LeafConnectedCompPtr_t& otherCC) const;
+ /// Whether this connected component can reach cc
+ /// \param cc a connected component
+ bool canReach (const LeafConnectedCompPtr_t& cc);
- /// Add otherCC to the list of reachable components
- ///
- /// This also add this object to the list of ancestor of otherCC.
- void canReach (const LeafConnectedCompPtr_t& otherCC);
+ /// Whether this connected component can reach cc
+ /// \param cc a connected component
+ /// \retval cc2Tocc1 list of connected components between cc2 and cc1
+ /// that should be merged.
+ bool canReach (const LeafConnectedCompPtr_t& cc,
+ LeafConnectedComp::LeafConnectedComps_t& cc2Tocc1);
/// Add roadmap node to connected component
/// \param roadmap node to be added
@@ -68,7 +72,21 @@ namespace hpp {
return nodes_;
}
- protected:
+ LeafConnectedCompPtr_t self ()
+ {
+ return weak_.lock ();
+ }
+
+ const LeafConnectedComp::LeafConnectedComps_t& from () const
+ {
+ return from_;
+ }
+
+ const LeafConnectedComp::LeafConnectedComps_t& to () const
+ {
+ return to_;
+ }
+ protected:
LeafConnectedComp(const RoadmapPtr_t& r)
: roadmap_(r) {}
@@ -81,16 +99,20 @@ namespace hpp {
RoadmapNodes_t nodes_;
private:
+ static void clean (LeafConnectedComps_t& set);
+ // status variable to indicate whether or not CC has been visited
+ mutable bool explored_;
RoadmapWkPtr_t roadmap_;
LeafConnectedComps_t to_, from_;
LeafConnectedCompWkPtr_t weak_;
+ friend class Roadmap;
}; // class LeafConnectedComp
class HPP_MANIPULATION_DLLAPI WeighedLeafConnectedComp :
public LeafConnectedComp
{
public:
- void merge (LeafConnectedCompPtr_t otherCC);
+ void merge (const LeafConnectedCompPtr_t& otherCC);
void setFirstNode (const RoadmapNodePtr_t& node);
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index abe2aa814b4e2739886afdf28afd03aef0b572af..0971c1dad4cd8d70904a57ea6cb70952cafca03c 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -23,6 +23,7 @@
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
# include <hpp/manipulation/deprecated.hh>
+# include <hpp/manipulation/leaf-connected-comp.hh>
namespace hpp {
namespace manipulation {
@@ -63,6 +64,18 @@ namespace hpp {
/// \deprecated use getState instead
graph::StatePtr_t getNode(RoadmapNodePtr_t node) HPP_MANIPULATION_DEPRECATED;
+ /// Update the graph of connected components after new connection
+ /// \param cc1, cc2 the two connected components that have just been
+ /// connected.
+ void connect (const LeafConnectedCompPtr_t& cc1,
+ const LeafConnectedCompPtr_t& cc2);
+
+ /// Merge two connected components
+ /// \param cc1 the connected component to merge into
+ /// \param the connected components to merge into cc1.
+ void merge (const LeafConnectedCompPtr_t& cc1,
+ LeafConnectedComp::LeafConnectedComps_t& ccs);
+
/// Get graph state corresponding to given roadmap node
graph::StatePtr_t getState(RoadmapNodePtr_t node);
diff --git a/src/leaf-connected-comp.cc b/src/leaf-connected-comp.cc
index bbf5fd56b8f1c7745869118d0a43ee2d18d13e2a..d3a69fc5899903ab9e6201e7129f19bdeaf3d461 100644
--- a/src/leaf-connected-comp.cc
+++ b/src/leaf-connected-comp.cc
@@ -21,6 +21,14 @@
namespace hpp {
namespace manipulation {
+ void LeafConnectedComp::clean (LeafConnectedComps_t& set)
+ {
+ for (LeafConnectedComps_t::iterator it = set.begin ();
+ it != set.end (); ++it) {
+ (*it)->explored_ = false;
+ }
+ }
+
LeafConnectedCompPtr_t LeafConnectedComp::create (const RoadmapPtr_t& roadmap)
{
LeafConnectedCompPtr_t shPtr =
@@ -48,28 +56,105 @@ namespace hpp {
nodes_.push_back(node);
}
- bool LeafConnectedComp::canMerge (const LeafConnectedCompPtr_t& otherCC) const
+ bool LeafConnectedComp::canReach (const LeafConnectedCompPtr_t& cc)
{
- if (otherCC->state_ != state_) return false;
- LeafConnectedComps_t::const_iterator it = std::find
- (to_.begin(), to_.end(), otherCC.get());
- if (it == to_.end()) return false;
- it = std::find
- (from_.begin(), from_.end(), otherCC.get());
- if (it == from_.end()) return false;
- return true;
+ // Store visited connected components for further cleaning.
+ LeafConnectedComp::LeafConnectedComps_t explored;
+ std::deque <RawPtr_t> queue;
+ queue.push_back (this);
+ explored_ = true;
+ explored.insert (this);
+ while (!queue.empty ()) {
+ RawPtr_t current = queue.front ();
+ queue.pop_front ();
+ if (current == cc.get()) {
+ clean (explored);
+ return true;
+ }
+ for (LeafConnectedComp::LeafConnectedComps_t::iterator itChild =
+ current->to_.begin ();
+ itChild != current->to_.end (); ++itChild) {
+ RawPtr_t child = *itChild;
+ if (!child->explored_) {
+ child->explored_ = true;
+ explored.insert (child);
+ queue.push_back (child);
+ }
+ }
+ }
+ clean (explored);
+ return false;
}
- void LeafConnectedComp::canReach (const LeafConnectedCompPtr_t& otherCC)
+ bool LeafConnectedComp::canReach
+ (const LeafConnectedCompPtr_t& cc,
+ LeafConnectedComp::LeafConnectedComps_t& ccToThis)
{
- to_.insert(otherCC.get());
- otherCC->from_.insert(this);
+ bool reachable = false;
+ // Store visited connected components
+ LeafConnectedComp::LeafConnectedComps_t exploredForward;
+ std::deque <RawPtr_t> queue;
+ queue.push_back (this);
+ explored_ = true;
+ exploredForward.insert (this);
+ while (!queue.empty ()) {
+ RawPtr_t current = queue.front ();
+ queue.pop_front ();
+ if (current == cc.get()) {
+ reachable = true;
+ exploredForward.insert (current);
+ } else {
+ for (LeafConnectedComp::LeafConnectedComps_t::iterator itChild =
+ current->to_.begin ();
+ itChild != current->to_.end (); ++itChild) {
+ RawPtr_t child = *itChild;
+ if (!child->explored_) {
+ child->explored_ = true;
+ exploredForward.insert (child);
+ queue.push_back (child);
+ }
+ }
+ }
+ }
+ // Set visited connected components to unexplored
+ clean (exploredForward);
+ if (!reachable) return false;
+
+ // Store visited connected components
+ LeafConnectedComps_t exploredBackward;
+ queue.push_back (cc.get());
+ cc->explored_ = true;
+ exploredBackward.insert (cc.get());
+ while (!queue.empty ()) {
+ RawPtr_t current = queue.front ();
+ queue.pop_front ();
+ if (current == this) {
+ exploredBackward.insert (current);
+ } else {
+ for (LeafConnectedComps_t::iterator itChild =
+ current->from_.begin ();
+ itChild != current->from_.end (); ++itChild) {
+ RawPtr_t child = *itChild;
+ if (!child->explored_) {
+ child->explored_ = true;
+ exploredBackward.insert (child);
+ queue.push_back (child);
+ }
+ }
+ }
+ }
+ // Set visited connected components to unexplored
+ clean (exploredBackward);
+ std::set_intersection (exploredForward.begin (), exploredForward.end (),
+ exploredBackward.begin (), exploredBackward.end (),
+ std::inserter (ccToThis, ccToThis.begin ()));
+ return true;
}
- void LeafConnectedComp::merge (LeafConnectedCompPtr_t other)
+ void LeafConnectedComp::merge (const LeafConnectedCompPtr_t& other)
{
- assert (canMerge(other));
- if (other == weak_.lock()) return;
+ assert(other);
+ assert(weak_.lock().get() == this);
// Tell other's nodes that they now belong to this connected component
for (RoadmapNodes_t::iterator itNode = other->nodes_.begin ();
@@ -79,12 +164,34 @@ namespace hpp {
// Add other's nodes to this list.
nodes_.insert (nodes_.end (), other->nodes_.begin(), other->nodes_.end());
- from_.erase (other.get());
- other->from_.erase (this);
- from_.insert (other->from_.begin(), other->from_.end());
+ // Tell other's reachableTo's that other has been replaced by this
+ for (LeafConnectedComps_t::iterator itcc = other->to_.begin ();
+ itcc != other->to_.end (); ++itcc) {
+ (*itcc)->from_.erase (other.get());
+ (*itcc)->from_.insert (this);
+ }
+
+ // Tell other's reachableFrom's that other has been replaced by this
+ for (LeafConnectedComps_t::iterator itcc=other->from_.begin ();
+ itcc != other->from_.end (); ++itcc) {
+ (*itcc)->to_.erase (other.get());
+ (*itcc)->to_.insert (this);
+ }
+
+ LeafConnectedComps_t tmp;
+ std::set_union (to_.begin (), to_.end (),
+ other->to_.begin (), other->to_.end (),
+ std::inserter (tmp, tmp.begin ()));
+ to_ = tmp; tmp.clear ();
to_.erase (other.get());
- other->to_.erase (this);
- to_.insert (other->to_.begin(), other->to_.end());
+ to_.erase (this);
+ std::set_union (from_.begin (), from_.end (),
+ other->from_.begin (),
+ other->from_.end (),
+ std::inserter (tmp, tmp.begin()));
+ from_ = tmp; tmp.clear ();
+ from_.erase (other.get());
+ from_.erase (this);
}
WeighedLeafConnectedCompPtr_t WeighedLeafConnectedComp::create (const RoadmapPtr_t& roadmap)
@@ -95,7 +202,7 @@ namespace hpp {
return shPtr;
}
- void WeighedLeafConnectedComp::merge (LeafConnectedCompPtr_t otherCC)
+ void WeighedLeafConnectedComp::merge (const LeafConnectedCompPtr_t& otherCC)
{
WeighedLeafConnectedCompPtr_t other =
HPP_DYNAMIC_PTR_CAST(WeighedLeafConnectedComp, otherCC);
diff --git a/src/roadmap.cc b/src/roadmap.cc
index cfec8b7711649bf36bf146cef5e0b1e410f85880..175fa747e99f1e59baed17e58537cb87cb51b1bb 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -37,7 +37,7 @@ namespace hpp {
Roadmap* ptr = new Roadmap (distance, robot);
RoadmapPtr_t shPtr (ptr);
ptr->init(shPtr);
- return shPtr;
+ return shPtr;
}
void Roadmap::clear ()
@@ -57,7 +57,7 @@ namespace hpp {
void Roadmap::push_node (const core::NodePtr_t& n)
{
Parent::push_node (n);
- const RoadmapNodePtr_t& node =
+ const RoadmapNodePtr_t& node =
static_cast <const RoadmapNodePtr_t> (n);
statInsert (node);
leafCCs_.insert(node->leafConnectedComponent());
@@ -129,22 +129,46 @@ namespace hpp {
return graph_->getState(node);
}
+ void Roadmap::connect (const LeafConnectedCompPtr_t& cc1,
+ const LeafConnectedCompPtr_t& cc2)
+ {
+ if (cc1->canReach (cc2)) return;
+ LeafConnectedComp::LeafConnectedComps_t cc2Tocc1;
+ if (cc2->canReach (cc1, cc2Tocc1)) {
+ merge (cc1, cc2Tocc1);
+ } else {
+ cc1->to_.insert (cc2.get());
+ cc2->from_.insert (cc1.get());
+ }
+ }
+
+ void Roadmap::merge (const LeafConnectedCompPtr_t& cc1,
+ LeafConnectedComp::LeafConnectedComps_t& ccs)
+ {
+ for (LeafConnectedComp::LeafConnectedComps_t::iterator itcc =
+ ccs.begin ();
+ itcc != ccs.end (); ++itcc) {
+ if (*itcc != cc1.get()) {
+ cc1->merge ((*itcc)->self());
+#ifndef NDEBUG
+ std::size_t nb =
+#endif
+ leafCCs_.erase ((*itcc)->self());
+ assert (nb == 1);
+ }
+ }
+ }
+
void Roadmap::addEdge (const core::EdgePtr_t& edge)
{
Parent::addEdge(edge);
const RoadmapNodePtr_t& f = static_cast <const RoadmapNodePtr_t> (edge->from());
const RoadmapNodePtr_t& t = static_cast <const RoadmapNodePtr_t> (edge->to());
- LeafConnectedCompPtr_t scf = f->leafConnectedComponent();
- LeafConnectedCompPtr_t sct = t->leafConnectedComponent();
- scf->canReach(sct);
- if (scf->canMerge(sct)) {
- if (scf->nodes().size() > sct->nodes().size()) {
- scf->merge(sct);
- leafCCs_.erase(sct);
- } else {
- sct->merge(scf);
- leafCCs_.erase(scf);
- }
+ if (f->graphState () == t->graphState ()) {
+ LeafConnectedCompPtr_t cc1 (f->leafConnectedComponent());
+ LeafConnectedCompPtr_t cc2 (t->leafConnectedComponent());
+
+ connect (cc1, cc2);
}
}
} // namespace manipulation