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