fix iterator bugs in path optimizers (#77)

roboticslibrary · Feb 20, 2024 · 0b37972 · 0b37972
1 parent c2f8621
commit 0b37972
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 6 deletions.
diff --git a/src/rl/plan/AdvancedOptimizer.cpp b/src/rl/plan/AdvancedOptimizer.cpp
@@ -68,15 +68,16 @@ namespace rl
  {
  changed = false;
 
- for (VectorList::iterator i = path.begin(), j = ::std::next(i), k = ::std::next(j); i != path.end() && j != path.end() && k != path.end(); ++i, ++j, ++k)
+ for (VectorList::iterator i = path.begin(), j = ::std::next(i), k = ::std::next(j); i != path.end() && j != path.end() && k != path.end();)
  {
  ::rl::math::Real ik = this->getModel()->distance(*i, *k);
 
  if (!this->getVerifier()->isColliding(*i, *k, ik))
  {
  ::rl::math::Real ij = this->getModel()->distance(*i, *j);
  ::rl::math::Real jk = this->getModel()->distance(*j, *k);
- this->getModel()->interpolate(*i, *k, ij / (ij + jk), inter);
+ ::rl::math::Real alpha = ij / (ij + jk);
+ this->getModel()->interpolate(*i, *k, alpha, inter);
  ::rl::math::Real ratio = this->getModel()->distance(*j, inter) / ik;
 
  if (ratio > this->ratio)
@@ -93,6 +94,18 @@ namespace rl
 
  changed = true;
  }
+ else
+ {
+ ++i;
+ ++j;
+ ++k;
+ }
+ }
+ else
+ {
+ ++i;
+ ++j;
+ ++k;
  }
  }
  }

diff --git a/src/rl/plan/Optimizer.cpp b/src/rl/plan/Optimizer.cpp
@@ -85,12 +85,12 @@ namespace rl
  bool changed = false;
  ::rl::math::Vector inter(this->getModel()->getDofPosition());
 
- for (VectorList::iterator i = path.begin(), j = ::std::next(i); i != path.end() && j != path.end(); ++i, ++j)
+ for (VectorList::iterator i = path.begin(), j = ::std::next(i); i != path.end() && j != path.end();)
  {
- if (0 == length || this->getModel()->distance(*i, *j) > length)
+ if (length > 0 && this->getModel()->distance(*i, *j) > length)
  {
  this->getModel()->interpolate(*i, *j, static_cast<::rl::math::Real>(0.5), inter);
- i = path.insert(j, inter);
+ j = path.insert(j, inter);
 
  if (nullptr != this->getViewer())
  {
@@ -99,6 +99,11 @@ namespace rl
 
  changed = true;
  }
+ else
+ {
+ ++i;
+ ++j;
+ }
  }
 
  return changed;

diff --git a/src/rl/plan/SimpleOptimizer.cpp b/src/rl/plan/SimpleOptimizer.cpp
@@ -51,7 +51,7 @@ namespace rl
  {
  changed = false;
 
- for (VectorList::iterator i = path.begin(), j = ::std::next(i), k = ::std::next(j); i != path.end() && j != path.end() && k != path.end(); ++i, ++j, ++k)
+ for (VectorList::iterator i = path.begin(), j = ::std::next(i), k = ::std::next(j); i != path.end() && j != path.end() && k != path.end();)
  {
  ::rl::math::Real ik = this->getModel()->distance(*i, *k);
 
@@ -69,6 +69,12 @@ namespace rl
 
  changed = true;
  }
+ else
+ {
+ ++i;
+ ++j;
+ ++k;
+ }
  }
  }
  }