Added new functions to spline

2 files changed, 37 insertions, 3 deletions

diff --git a/geometry/spline.cpp b/geometry/spline.cpp
index 7125aa3ffa..1a2df1b5d3 100644
--- a/geometry/spline.cpp
+++ b/geometry/spline.cpp
@@ -90,14 +90,21 @@ void Spline::iterator::Attach(Spline const & spl)
 void Spline::iterator::Step(double speed)
 {
   m_dist += speed;
-  while(m_dist > m_spl->m_length[m_index])
+  if (m_checker)
+  {
+    m_pos = m_spl->m_position[m_index] + m_dir * m_dist;
+    return;
+  }
+  while (m_dist > m_spl->m_length[m_index])
   {
     m_dist -= m_spl->m_length[m_index];
     m_index++;
-    if(m_index >= m_spl->m_direction.size())
+    if (m_index >= m_spl->m_direction.size())
     {
-      m_index = 0;
+      m_index--;
+      m_dist += m_spl->m_length[m_index];
       m_checker = true;
+      break;
     }
   }
   m_dir = m_spl->m_direction[m_index];
@@ -106,6 +113,30 @@ void Spline::iterator::Step(double speed)
   m_avrDir += m_pos;
 }
 
+void Spline::iterator::StepBack(double speed)
+{
+  m_dist -= speed;
+  while(m_dist < 0.0f)
+  {
+    m_index--;
+    m_dist += m_spl->m_length[m_index];
+  }
+  m_dir = m_spl->m_direction[m_index];
+  m_avrDir = -m_pos;
+  m_pos = m_spl->m_position[m_index] + m_dir * m_dist;
+  m_avrDir += m_pos;
+}
+
+double Spline::iterator::GetLength() const
+{
+  return accumulate(m_spl->m_length.begin(), m_spl->m_length.begin() + m_index, m_dist);
+}
+
+double Spline::iterator::GetFullLength() const
+{
+  return m_spl->GetLength();
+}
+
 bool Spline::iterator::BeginAgain() const
 {
   return m_checker;
diff --git a/geometry/spline.hpp b/geometry/spline.hpp
index d87ad385b8..d313968ae3 100644
--- a/geometry/spline.hpp
+++ b/geometry/spline.hpp
@@ -20,7 +20,10 @@ public:
     iterator();
     void Attach(Spline const & spl);
     void Step(double speed);
+    void StepBack(double speed);
     bool BeginAgain() const;
+    double GetLength() const;
+    double GetFullLength() const;
 
   private:
     friend class Spline;