From b7d5899ae6a251075c0b3564ce8dd75164af49d0 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 25 Oct 2017 17:44:54 +0200
Subject: [PATCH] Partially fix helper to build graph.
* The behaviour is exactly the same as before the change in class
* WaypointEdge.
* In a near future, the TODO in src/graph/helper.cc will be fixed.
---
src/graph/helper.cc | 14 +++++++++-----
1 file changed, 9 insertions(+), 5 deletions(-)
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 83be209..c1178e6 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -190,7 +190,7 @@ namespace hpp {
WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
gls = linkWaypoint <LevelSetEdge> (n, T-1, T, name, "ls");
- for (std::size_t i = 0; i < Nedges - 1; ++i)
+ for (std::size_t i = 0; i < Nedges; ++i)
we->setWaypoint (i, e[i], n[i]);
we->setWaypoint (T-1, gls, n[T]);
gls->state (n.front());
@@ -217,8 +217,11 @@ namespace hpp {
WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
pls = linkWaypoint <LevelSetEdge> (n, T+1, T, name, "ls");
- for (std::size_t i = Nedges - 1; i != 0; --i)
+ // for (std::size_t i = Nedges - 1; i != 0; --i)
+ for (std::size_t k = 0; k < Nedges; ++k) {
+ std::size_t i = Nedges - 1 - k;
we->setWaypoint (Nedges - 1 - i, e[i], n[i]);
+ }
we->setWaypoint (Nedges - 1 - T, pls, n[T]);
pls->state (n.back ());
pls->setShort (preplace);
@@ -255,16 +258,17 @@ namespace hpp {
EdgeArray e;
WaypointEdgePtr_t we = HPP_DYNAMIC_PTR_CAST(WaypointEdge, edge);
if (forward)
- for (std::size_t i = 0; i < Nedges - 1; ++i) {
+ for (std::size_t i = 0; i < Nedges; ++i) {
e[i] = linkWaypoint <Edge> (states, i, i + 1, name);
we->setWaypoint (i, e[i], states[i+1]);
}
else
- for (std::size_t i = Nedges - 1; i != 0; --i) {
+ // for (std::size_t i = Nedges - 1; i != 0; --i) {
+ for (std::size_t k = 0; k < Nedges; ++k) {
+ std::size_t i = Nedges - 1 - k;
e[i] = linkWaypoint <Edge> (states, i + 1, i, name);
we->setWaypoint (Nedges - 1 - i, e[i], states[i]);
}
- e[(forward?Nedges - 1:0)] = we;
return e;
}
--
GitLab