Refactor subdivide

rickertm · rickertm · commit aa6eb334b527 · 2022-05-31T19:25:31.000+02:00
diff --git a/src/rl/plan/AdvancedOptimizer.cpp b/src/rl/plan/AdvancedOptimizer.cpp
@@ -97,20 +97,9 @@ namespace rl
 					}
 				}
 				
-				for (VectorList::iterator i = path.begin(), j = ::std::next(i); i != path.end() && j != path.end(); ++i, ++j)
+				if (this->subdivide(path, this->length))
 				{
-					if (this->getModel()->distance(*i, *j) > length)
-					{
-						this->getModel()->interpolate(*i, *j, static_cast<::rl::math::Real>(0.5), inter);
-						i = path.insert(j, inter);
-						
-						if (nullptr != this->getViewer())
-						{
-							this->getViewer()->drawConfigurationPath(path);
-						}
-						
-						changed = true;
-					}
+					changed = true;
 				}
 			}
 		}
diff --git a/src/rl/plan/Optimizer.cpp b/src/rl/plan/Optimizer.cpp
@@ -25,6 +25,8 @@
 //
 
 #include "Optimizer.h"
+#include "SimpleModel.h"
+#include "Viewer.h"
 
 namespace rl
 {
@@ -76,5 +78,30 @@ namespace rl
 		{
 			this->viewer = viewer;
 		}
+		
+		bool
+		Optimizer::subdivide(VectorList& path, const ::rl::math::Real& length)
+		{
+			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)
+			{
+				if (0 == length || this->getModel()->distance(*i, *j) > length)
+				{
+					this->getModel()->interpolate(*i, *j, static_cast<::rl::math::Real>(0.5), inter);
+					i = path.insert(j, inter);
+					
+					if (nullptr != this->getViewer())
+					{
+						this->getViewer()->drawConfigurationPath(path);
+					}
+					
+					changed = true;
+				}
+			}
+			
+			return changed;
+		}
 	}
 }
diff --git a/src/rl/plan/Optimizer.h b/src/rl/plan/Optimizer.h
@@ -60,6 +60,8 @@ namespace rl
 			
 			void setViewer(Viewer* viewer);
 			
+			bool subdivide(VectorList& path, const ::rl::math::Real& length = 0);
+			
 			SimpleModel* model;
 			
 			Verifier* verifier;

Original file line number	Diff line number	Diff line change
`@@ -97,20 +97,9 @@ namespace rl`
`97`	`97`	`}`
`98`	`98`	`}`
`99`	`99`
`100`		`- for (VectorList::iterator i = path.begin(), j = ::std::next(i); i != path.end() && j != path.end(); ++i, ++j)`
	`100`	`+ if (this->subdivide(path, this->length))`
`101`	`101`	`{`
`102`		`- if (this->getModel()->distance(i, j) > length)`
`103`		`- {`
`104`		`- this->getModel()->interpolate(i, j, static_cast<::rl::math::Real>(0.5), inter);`
`105`		`- i = path.insert(j, inter);`
`106`		`-`
`107`		`- if (nullptr != this->getViewer())`
`108`		`- {`
`109`		`- this->getViewer()->drawConfigurationPath(path);`
`110`		`- }`
`111`		`-`
`112`		`- changed = true;`
`113`		`- }`
	`102`	`+ changed = true;`
`114`	`103`	`}`
`115`	`104`	`}`
`116`	`105`	`}`