Add Edge::direction and Edge::intersectionConstraint

include/hpp/manipulation/graph/edge.hh

@@ -100,6 +100,15 @@ namespace hpp {
 	    return steeringMethod_;
 	  }
 
+          /// Get direction of the path compare to the edge
+          /// \return true is reverse
+          virtual bool direction (const core::PathPtr_t& path) const;
+
+          /// Populate a ConfigProjector with constraints required to generate
+          /// a path at the intersection of two edges.
+          virtual bool intersectionConstraint (const EdgePtr_t& other,
+              ConfigProjectorPtr_t projector) const;
+
           /// Print the object in a stream.
           virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
 
@@ -188,6 +197,8 @@ namespace hpp {
 	   const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
 	   const NodeWkPtr_t& to);
 
+          virtual bool direction (const core::PathPtr_t& path) const;
+
           virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
 
           virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;

src/graph/edge.cc

@@ -61,6 +61,60 @@ namespace hpp {
         else return to ();
       }
 
+      bool Edge::direction (const core::PathPtr_t& path) const
+      {
+        Configuration_t q0 = path->initial (),
+                        q1 = path->end ();
+        const bool src_contains_q0 = from()->contains (q0);
+        const bool dst_contains_q0 = to  ()->contains (q0);
+        const bool src_contains_q1 = from()->contains (q1);
+        const bool dst_contains_q1 = to  ()->contains (q1);
+        assert ((src_contains_q0 && dst_contains_q1)
+             || (src_contains_q1 && dst_contains_q0));
+        /// true if reverse
+        return !dst_contains_q1;
+      }
+
+      bool WaypointEdge::direction (const core::PathPtr_t& path) const
+      {
+        Configuration_t q0 = path->initial (),
+                        q1 = path->end ();
+        const bool src_contains_q0 = waypoint_.second->contains (q0);
+        const bool dst_contains_q0 = to  ()->contains (q0);
+        const bool src_contains_q1 = waypoint_.second->contains (q1);
+        const bool dst_contains_q1 = to  ()->contains (q1);
+        assert ((src_contains_q0 && dst_contains_q1)
+             || (src_contains_q1 && dst_contains_q0));
+        /// true if reverse
+        return !dst_contains_q1;
+      }
+
+      bool Edge::intersectionConstraint (const EdgePtr_t& other,
+          ConfigProjectorPtr_t proj) const
+      {
+        GraphPtr_t g = graph_.lock ();
+        
+        g->insertNumericalConstraints (proj);
+        insertNumericalConstraints (proj);
+        node ()->insertNumericalConstraints (proj);
+
+        g->insertLockedJoints (proj);
+        insertLockedJoints (proj);
+        node ()->insertLockedJoints (proj);
+
+        if (wkPtr_.lock() == other) // No intersection to be computed.
+          return false;
+
+        bool nodeB_Eq_nodeA = (node() == other->node());
+
+        other->insertNumericalConstraints (proj);
+        if (!nodeB_Eq_nodeA) other->node()->insertNumericalConstraints (proj);
+        other->insertLockedJoints (proj);
+        if (!nodeB_Eq_nodeA) other->node()->insertLockedJoints (proj);
+
+        return true;
+      }
+
       EdgePtr_t Edge::create (const std::string& name,
 			      const core::SteeringMethodPtr_t& steeringMethod,
 			      const GraphWkPtr_t& graph,

src/path-optimization/config-optimization.cc

@@ -37,23 +37,13 @@ namespace hpp {
           HPP_STATIC_PTR_CAST (ConstraintSet, after->constraints ());
         assert (setA->edge () && setB->edge());
         graph::GraphPtr_t graph = setA->edge()->parentGraph ();
-        graph::NodePtr_t n0 = graph->getNode (before->initial ()),
-                         n1 = graph->getNode (before->end ()),
-                         n2 = graph->getNode (after->initial ()),
-                         n3 = graph->getNode (after->end ());
         /// Find if path were computed from init or goal config
         /// There is a nasty case: when build a path fails partially, then you
         /// end up where the edge->to() do not correspond to the node of
         /// path->end(). In that case, path->end () should be in edge->node().
         /// (obviously, in this case, we have edge->isInNodeFrom_ = true)
-        assert ((n0 == setB->edge()->from () && n1 == setB->edge()->to ())
-                || (n1 == setB->edge()->from () && n0 == setB->edge()->to ()));
-        assert ((n2 == setA->edge()->from () && n3 == setA->edge()->to ())
-                || (n3 == setA->edge()->from () && n2 == setA->edge()->to ()));
-        bool reverseB = (n0 != n1) &&
-          (n1 == setB->edge()->from () && n0 == setB->edge()->to ());
-        bool reverseA = (n2 != n3) &&
-          (n3 == setA->edge()->from () && n2 == setA->edge()->to ());
+        const bool reverseB = setB->edge ()->direction(before);
+        const bool reverseA = setA->edge ()->direction(after);
 
         reverse = reverseB;
 
@@ -61,25 +51,7 @@ namespace hpp {
             "intersect_" + setB->edge()->name() + "_" + setA->edge()->name(),
             graph->errorThreshold (), graph->maxIterations ());
 
-        graph->insertNumericalConstraints (p);
-        // TODO: Is reverse case different ?
-        bool nodeB_Eq_nodeA = (setB->edge()->node() == setA->edge()->node());
-
-        setB->edge()->insertNumericalConstraints (p);
-        setB->edge()->node ()->insertNumericalConstraints (p);
-
-        graph->insertLockedJoints (p);
-        setB->edge()->insertLockedJoints (p);
-        setB->edge()->node ()->insertLockedJoints (p);
-
-        vector_t rhsB = p->rightHandSideFromConfig (before->initial ());
-
-        setA->edge()->insertNumericalConstraints (p);
-        if (!nodeB_Eq_nodeA)
-          setA->edge()->node()->insertNumericalConstraints (p);
-        setA->edge()->insertLockedJoints (p);
-        if (!nodeB_Eq_nodeA)
-          setA->edge()->node()->insertLockedJoints (p);
+        setB->edge()->intersectionConstraint (setA->edge (), p);
 
         p->rightHandSideFromConfig (before->end ());
         return p;