From 0b3797215345a1d37903634095361233d190b2e6 Mon Sep 17 00:00:00 2001
From: Kazfyx <1073716894@qq.com>
Date: Wed, 21 Feb 2024 03:33:37 +0800
Subject: [PATCH] fix iterator bugs in path optimizers (#77)

---
 src/rl/plan/AdvancedOptimizer.cpp | 17 +++++++++++++++--
 src/rl/plan/Optimizer.cpp         | 11 ++++++++---
 src/rl/plan/SimpleOptimizer.cpp   |  8 +++++++-
 3 files changed, 30 insertions(+), 6 deletions(-)

diff --git a/src/rl/plan/AdvancedOptimizer.cpp b/src/rl/plan/AdvancedOptimizer.cpp
index fa301b66..b3a9363d 100644
--- a/src/rl/plan/AdvancedOptimizer.cpp
+++ b/src/rl/plan/AdvancedOptimizer.cpp
@@ -68,7 +68,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);
 						
@@ -76,7 +76,8 @@ namespace rl
 						{
 							::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
index 404c562e..c05ac13a 100644
--- 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
index b4c6533a..9f033513 100644
--- 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;
+					}
 				}
 			}
 		}