diff --git a/src/rl/plan/AdvancedOptimizer.cpp b/src/rl/plan/AdvancedOptimizer.cpp index f66f25f..b3a9363 100644 --- a/src/rl/plan/AdvancedOptimizer.cpp +++ b/src/rl/plan/AdvancedOptimizer.cpp @@ -61,7 +61,7 @@ namespace rl { bool changed = true; ::rl::math::Vector inter(this->getModel()->getDofPosition()); - + while (changed && path.size() > 2) { while (changed && path.size() > 2) @@ -76,11 +76,8 @@ namespace rl { ::rl::math::Real ij = this->getModel()->distance(*i, *j); ::rl::math::Real jk = this->getModel()->distance(*j, *k); - ::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) diff --git a/src/rl/plan/Optimizer.cpp b/src/rl/plan/Optimizer.cpp index 59bff2e..c05ac13 100644 --- a/src/rl/plan/Optimizer.cpp +++ b/src/rl/plan/Optimizer.cpp @@ -84,13 +84,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();) { if (length > 0 && this->getModel()->distance(*i, *j) > length) { this->getModel()->interpolate(*i, *j, static_cast<::rl::math::Real>(0.5), inter); - j = path.insert(j, inter); if (nullptr != this->getViewer())