Add Edge::canConnect

include/hpp/manipulation/graph/edge.hh

@@ -73,6 +73,8 @@ namespace hpp {
 
           virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
 
+          virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
           virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
               ConfigurationIn_t q2) const;
 
@@ -204,6 +206,8 @@ namespace hpp {
 
           virtual bool direction (const core::PathPtr_t& path) const;
 
+          virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
           virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
 
           virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;

src/graph/edge.cc

@@ -256,6 +256,17 @@ namespace hpp {
         return constraint;
       }
 
+      bool Edge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2)
+	const
+      {
+        ConstraintSetPtr_t constraints = pathConstraint ();
+        constraints->configProjector ()->rightHandSideFromConfig(q1);
+        if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
+          return false;
+        }
+        return true;
+      }
+
       bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
 			ConfigurationIn_t q2)
 	const
@@ -311,6 +322,11 @@ namespace hpp {
         wkPtr_ = weak;
       }
 
+      bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const
+      {
+        return waypoint_.first->canConnect (q1, q2) && Edge::canConnect (q1, q2);
+      }
+
       bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
           ConfigurationIn_t q2) const
       {