Rewrite implementation of class LeafConnectedComp

  - copy paste and adapt code of hpp::core::ConnectedComponent.

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;

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);
 

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);
 

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);

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