From 9c11c0c1317b8262aa10f7773e8bfcb9136b6961 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 27 Nov 2015 15:32:23 +0100
Subject: [PATCH] Do not loop on all the nodes when trying to connect new
nodes.
---
src/manipulation-planner.cc | 18 ++++--------------
1 file changed, 4 insertions(+), 14 deletions(-)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index d9aa4ed..bbae157 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -267,16 +267,10 @@ namespace hpp {
graph::GraphPtr_t graph = problem_.constraintGraph ();
bool connectSucceed = false;
for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
- itn1 != nodes.end (); ++itn1) {
- ConfigurationPtr_t q1 ((*itn1)->configuration ());
- connectSucceed = false;
- for (core::ConnectedComponents_t::const_iterator itcc =
- roadmap ()->connectedComponents ().begin ();
- itcc != roadmap ()->connectedComponents ().end (); ++itcc) {
- if (*itcc == (*itn1)->connectedComponent ())
- continue;
- for (core::Nodes_t::const_iterator itn2 = (*itcc)->nodes().begin ();
- itn2 != (*itcc)->nodes ().end (); ++itn2) {
+ itn1 != nodes.end (); ++itn1) {
+ for (core::Nodes_t::const_iterator itn2 = boost::next (itn1);
+ itn2 != nodes.end (); ++itn2) {
+ ConfigurationPtr_t q1 ((*itn1)->configuration ());
ConfigurationPtr_t q2 ((*itn2)->configuration ());
assert (*q1 != *q2);
path = (*sm) (*q1, *q2);
@@ -291,11 +285,7 @@ namespace hpp {
roadmap ()->addEdge (*itn2, *itn1, projPath->extract
(core::interval_t (timeRange.second,
timeRange.first)));
- connectSucceed = true;
- break;
}
- }
- if (connectSucceed) break;
}
}
}
--
GitLab