Fix bug in GraphPathValidation

src/graph-path-validation.cc

@@ -34,40 +34,40 @@ namespace hpp {
           const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
     {
       assert (path);
-      PathPtr_t pathGraphValid;
-      bool graphValid = impl_validate (path, reverse, pathGraphValid);
-      bool collisionValid = pathValidation_->validate (pathGraphValid, reverse, validPart);
-      return graphValid && collisionValid;
+      return impl_validate (path, reverse, validPart);
     }
 
     bool GraphPathValidation::impl_validate (
         const PathVectorPtr_t& path, bool reverse, PathPtr_t& validPart)
     {
-      size_t start = 0,
-             end = path->numberPaths ();
-      int inc = 1;
-      value_type timeOffset = path->timeRange().first;
-      if (reverse) {
-        std::swap (start, end);
-        start--;end--;
-        inc = -1;
-        timeOffset = path->timeRange ().second;
-      }
       PathPtr_t validSubPart;
-      for (size_t index = start; index != end; index += inc) {
-        // We should stop at the first non valid subpath.
-        if (!impl_validate (path->pathAtRank (index), reverse, validSubPart)) {
-          if (reverse)
-            validPart = path->extract (
-                std::make_pair (timeOffset - validSubPart->timeRange().second,
-                                path->timeRange().second));
-          else
-            validPart = path->extract (
-                std::make_pair (path->timeRange().first,
-                                timeOffset + validSubPart->timeRange().second));
-          return false;
+      if (reverse) {
+        // TODO: This has never been tested.
+        for (int i = path->numberPaths () - 1; i >= 0; i--) {
+          // We should stop at the first non valid subpath.
+          if (!impl_validate (path->pathAtRank (i), true, validSubPart)) {
+            PathVectorPtr_t p = PathVector::create (path->outputSize());
+            for (int v = path->numberPaths () - 1; v > i; v--)
+              p->appendPath (path->pathAtRank(i)->copy());
+            // TODO: Make sure this subpart is generated by the steering method.
+            p->appendPath (validSubPart);
+            validPart = p;
+            return false;
+          }
+        }
+      } else {
+        for (size_t i = 0; i != path->numberPaths (); i++) {
+          // We should stop at the first non valid subpath.
+          if (!impl_validate (path->pathAtRank (i), false, validSubPart)) {
+            PathVectorPtr_t p = PathVector::create (path->outputSize());
+            for (size_t v = 0; v < i; v++)
+              p->appendPath (path->pathAtRank(i)->copy());
+            // TODO: Make sure this subpart is generated by the steering method.
+            p->appendPath (validSubPart);
+            validPart = p;
+            return false;
+          }
         }
-        timeOffset += inc * path->pathAtRank (index)->length();
       }
       // Here, every subpath is valid.
       validPart = path;
@@ -81,28 +81,53 @@ namespace hpp {
       if (pathVector)
         return impl_validate (pathVector, reverse, validPart);
 
-      value_type tmin = path->timeRange ().first;
-      value_type tmax = path->timeRange ().second;
-      if (reverse)
-        std::swap (tmin, tmax);
-      const Path& configAt (*path);
-      graph::Nodes_t origNodes = constraintGraph_->getNode (configAt (tmin));
-      graph::Nodes_t destNodes = constraintGraph_->getNode (configAt (tmax));
+      PathPtr_t pathNoCollision;
+      if (pathValidation_->validate (path, reverse, pathNoCollision)) {
+        validPart = path;
+        return true;
+      }
+      const Path& newPath (*pathNoCollision);
+      value_type newTmin = pathNoCollision->timeRange ().first,
+                 newTmax = pathNoCollision->timeRange ().second,
+                 oldTmin = path->timeRange ().first,
+                 oldTmax = path->timeRange ().second;
+      graph::Nodes_t origNodes, destNodes;
+      try {
+        origNodes = constraintGraph_->getNode (newPath (newTmin));
+        destNodes = constraintGraph_->getNode (newPath (newTmax));
+
+        if (origNodes == constraintGraph_->getNode ((*path) (oldTmin))
+            && destNodes == constraintGraph_->getNode ((*path) (oldTmax))) {
+          validPart = pathNoCollision;
+          return false;
+        }
+      } catch (std::logic_error& e) {
+        /// A configuration has no node, which may be because the path could not be projected.
+        /// Path should be considered invalid.
+        validPart = path->extract (std::make_pair (oldTmin,oldTmin));
+        return false;
+      }
+
+      // Here, the full path is not valid. Moreover, it does not correspond to the same
+      // edge of the constraint graph. Two option are possible:
+      // - Use the steering method to create a new path and validate it.
+      // - Return a null path.
+      assert (!reverse && "This has never been tested with reverse path");
       std::vector <graph::Edges_t> possibleEdges (constraintGraph_->getEdge (origNodes, destNodes));
       // We check for all of them if both nodes are on the same leaf.
       ConstraintSetPtr_t constraints;
       while (!possibleEdges.empty ()) {
         constraints = constraintGraph_->pathConstraint (possibleEdges.back());
-        constraints->offsetFromConfig(configAt (tmin));
-        assert (constraints->isSatisfied (configAt (tmin)));
-        if (constraints == path->constraints ()
-            && constraints->isSatisfied (configAt (tmax))) {
-          validPart = path;
-          return true;
+        constraints->offsetFromConfig(newPath (newTmin));
+        assert (constraints->isSatisfied (newPath (newTmin)));
+        if (constraints->isSatisfied (newPath (newTmax))) {
+          pathNoCollision->constraints (constraints);
+          impl_validate (pathNoCollision, reverse, validPart);
+          return false;
         }
         possibleEdges.pop_back();
       }
-      validPart = PathPtr_t ();
+      validPart = path->extract (std::make_pair (oldTmin,oldTmin));
       return false;
     }
   } // namespace manipulation