Adds scripting interface to Spline and a python test.

Signed-off-by: LolaSegura <[email protected]>

src/CMakeLists.txt

@@ -131,6 +131,7 @@ if (PYTHONLIBS_FOUND)
     python_TEST
     Rand_TEST
     RollingMean_TEST
+    Spline_TEST
     Vector2_TEST
     Vector3_TEST
     Vector4_TEST

src/Spline.i

@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2016 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+%module spline
+%{
+#include <ignition/math/Spline.hh>
+#include <ignition/math/Helpers.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/config.hh>
+%}
+
+namespace ignition
+{
+  namespace math
+  {
+    class ControlPoint;
+    class SplinePrivate;
+
+    class Spline
+    {
+      public: Spline();
+      public: ~Spline();
+      public: void Tension(double _t);
+      public: double Tension() const;
+      public: double ArcLength() const;
+      public: double ArcLength(const double _t) const;
+      public: double ArcLength(const unsigned int _index,
+                               const double _t) const;
+      public: void AddPoint(const Vector3<double> &_p);
+      public: void AddPoint(const Vector3<double> &_p, const Vector3<double> &_t);
+      private: void AddPoint(const ControlPoint &_cp, const bool _fixed);
+      public: Vector3<double> Point(const unsigned int _index) const;
+      public: Vector3<double> Tangent(const unsigned int _index) const;
+      public: Vector3<double> MthDerivative(const unsigned int _index,
+                                     const unsigned int _mth) const;
+      public: size_t PointCount() const;
+      public: void Clear();
+      public: bool UpdatePoint(const unsigned int _index,
+                               const Vector3<double> &_p);
+      public: bool UpdatePoint(const unsigned int _index,
+                               const Vector3<double> &_p,
+                               const Vector3<double> &_t);
+      private: bool UpdatePoint(const unsigned int _index,
+                                const ControlPoint &_cp,
+                                const bool _fixed);
+      public: Vector3<double> Interpolate(const double _t) const;
+      public: Vector3<double> Interpolate(const unsigned int _fromIndex,
+                                   const double _t) const;
+      public: Vector3<double> InterpolateTangent(const double _t) const;
+      public: Vector3<double> InterpolateTangent(const unsigned int _fromIndex,
+                                          const double _t) const;
+      public: Vector3<double> InterpolateMthDerivative(const unsigned int _mth,
+                                                const double _1) const;
+      public: Vector3<double> InterpolateMthDerivative(const unsigned int _fromIndex,
+                                                const unsigned int _mth,
+                                                const double _s) const;
+      public: void AutoCalculate(bool _autoCalc);
+      public: void RecalcTangents();
+      private: void Rebuild();
+      private: bool MapToSegment(const double _t,
+                                 unsigned int &_index,
+                                 double &_fraction) const;
+      private: SplinePrivate *dataPtr;
+    };
+  }
+}

src/Spline_TEST.py

@@ -0,0 +1,166 @@
+# Copyright (C) 2021 Open Source Robotics Foundation
+
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+
+#       http://www.apache.org/licenses/LICENSE-2.0
+
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import unittest
+import math
+from ignition.math import Vector3d
+from ignition.math import Spline
+
+
+class TestSpline(unittest.TestCase):
+
+    def test_spline(self):
+        s = Spline()
+
+        s.AddPoint(Vector3d(0, 0, 0))
+        self.assertEqual(1, s.PointCount())
+
+        s.Clear()
+        self.assertEqual(0, s.PointCount())
+
+        s.AddPoint(Vector3d(0, 0, 0))
+        self.assertTrue(s.Point(0) == Vector3d(0, 0, 0))
+        s.AddPoint(Vector3d(1, 1, 1))
+        self.assertTrue(s.Point(1) == Vector3d(1, 1, 1))
+
+        # UpdatePoint
+        self.assertFalse(s.UpdatePoint(2, Vector3d(2, 2, 2)))
+
+        self.assertTrue(s.UpdatePoint(1, Vector3d(2, 2, 2)))
+        s.AutoCalculate(False)
+        self.assertTrue(s.UpdatePoint(0, Vector3d(-1, -1, -1)))
+        s.AutoCalculate(True)
+
+        # Interpolate
+        self.assertAlmostEqual(s.Interpolate(0.0), Vector3d(-1, -1, -1))
+        self.assertAlmostEqual(s.Interpolate(0.5), Vector3d(0.5, 0.5, 0.5))
+        self.assertAlmostEqual(s.Interpolate(1.0), Vector3d(2, 2, 2))
+
+        # Interpolate
+        s.AddPoint(Vector3d(4, 4, 4))
+        self.assertAlmostEqual(s.Interpolate(1, 0.2),
+                               Vector3d(2.496, 2.496, 2.496))
+
+    def test_fixed_tangent(self):
+        s = Spline()
+
+        # AddPoint
+        s.AutoCalculate(False)
+        s.AddPoint(Vector3d(0, 0, 0))
+        s.AddPoint(Vector3d(0, 0.5, 0), Vector3d(0, 1, 0))
+        s.AddPoint(Vector3d(0.5, 1, 0), Vector3d(1, 0, 0))
+        s.AddPoint(Vector3d(1, 1, 0), Vector3d(1, 0, 0))
+
+        # UpdatePoint
+        s.UpdatePoint(0, Vector3d(0, 0, 0), Vector3d(0, 1, 0))
+
+        s.AutoCalculate(True)
+
+        s.RecalcTangents()
+
+        # Interpolate
+        self.assertAlmostEqual(s.Interpolate(0, 0.5), Vector3d(0, 0.25, 0))
+        self.assertAlmostEqual(s.InterpolateTangent(0, 0.5),
+                               Vector3d(0, 0.25, 0))
+        self.assertAlmostEqual(s.Interpolate(1, 0.5),
+                               Vector3d(0.125, 0.875, 0))
+        self.assertAlmostEqual(s.Interpolate(2, 0.5), Vector3d(0.75, 1, 0))
+        self.assertAlmostEqual(s.InterpolateTangent(2, 0.5),
+                               Vector3d(0.25, 0, 0))
+
+    def test_arc_length(self):
+        s = Spline()
+        self.assertFalse(math.isfinite(s.ArcLength()))
+        s.AddPoint(Vector3d(1, 1, 1), Vector3d(1, 1, 1))
+        self.assertFalse(math.isfinite(s.ArcLength(0)))
+        s.AddPoint(Vector3d(3, 3, 3), Vector3d(1, 1, 1))
+        s.AddPoint(Vector3d(4, 4, 4), Vector3d(1, 1, 1))
+        self.assertAlmostEqual(s.ArcLength(0, 1.0),
+                               3.46410161513775, delta=1e-14)
+        self.assertAlmostEqual(s.ArcLength(), 5.19615242270663, delta=1e-14)
+        self.assertAlmostEqual(s.ArcLength(), s.ArcLength(1.0))
+        self.assertFalse(math.isfinite(s.ArcLength(-1.0)))
+        self.assertFalse(math.isfinite(s.ArcLength(4, 0.0)))
+
+    def test_tension(self):
+        s = Spline()
+
+        s.Tension(0.1)
+        self.assertAlmostEqual(s.Tension(), 0.1)
+
+    def test_interpolate(self):
+        s = Spline()
+        self.assertFalse(s.Interpolate(0, 0.1).IsFinite())
+
+        s.AddPoint(Vector3d(0, 0, 0))
+        self.assertAlmostEqual(s.Interpolate(0, 0.1), Vector3d(0, 0, 0))
+        self.assertFalse(s.InterpolateTangent(0.1).IsFinite())
+
+        s.AddPoint(Vector3d(1, 2, 3))
+        self.assertAlmostEqual(s.Interpolate(0, 0.0), s.Point(0))
+        self.assertFalse(s.Interpolate(0, -0.1).IsFinite())
+        self.assertAlmostEqual(s.InterpolateTangent(0, 0.0), s.Tangent(0))
+
+        # Fast and slow call variations
+        self.assertAlmostEqual(s.Interpolate(0, 0.5), Vector3d(0.5, 1.0, 1.5))
+        self.assertAlmostEqual(s.Interpolate(0, 1.0), s.Point(1))
+        self.assertAlmostEqual(s.InterpolateTangent(0, 0.5),
+                               Vector3d(1.25, 2.5, 3.75))
+        self.assertAlmostEqual(s.InterpolateTangent(0, 1.0), s.Tangent(1))
+        self.assertAlmostEqual(s.InterpolateMthDerivative(2, 0.5),
+                               Vector3d(0, 0, 0))
+        self.assertAlmostEqual(s.InterpolateMthDerivative(2, 1.0),
+                               Vector3d(-3, -6, -9))
+        self.assertAlmostEqual(s.InterpolateMthDerivative(3, 0.5),
+                               Vector3d(-6, -12, -18))
+        self.assertAlmostEqual(s.InterpolateMthDerivative(3, 1.0),
+                               Vector3d(-6, -12, -18))
+        self.assertAlmostEqual(s.InterpolateMthDerivative(4, 0.5),
+                               Vector3d(0, 0, 0))
+        self.assertAlmostEqual(s.InterpolateMthDerivative(4, 1.0),
+                               Vector3d(0, 0, 0))
+
+    def test_point(self):
+        s = Spline()
+        self.assertFalse(s.Point(0).IsFinite())
+
+    def test_tangent(self):
+        s = Spline()
+        self.assertFalse(s.Tangent(0).IsFinite())
+
+        s.AddPoint(Vector3d(0, 0, 0))
+        self.assertFalse(s.Tangent(0).IsFinite())
+
+        s.AddPoint(Vector3d(1, 0, 0))
+        self.assertAlmostEqual(s.Tangent(0), Vector3d(0.5, 0, 0))
+
+        s.AddPoint(Vector3d(1, 1, 0), Vector3d(-1, 1, 0))
+        self.assertAlmostEqual(s.Tangent(1), Vector3d(0.5, 0.5, 0))
+        self.assertAlmostEqual(s.Tangent(2), Vector3d(-1, 1, 0))
+
+    def test_recalc_tangents(self):
+        s = Spline()
+
+        s.AddPoint(Vector3d(0, 0, 0))
+        s.AddPoint(Vector3d(.4, .4, .4))
+        s.AddPoint(Vector3d(0, 0, 0))
+
+        s.RecalcTangents()
+
+        self.assertAlmostEqual(s.Interpolate(0, 0.5), Vector3d(0.2, 0.2, 0.2))
+        self.assertAlmostEqual(s.Interpolate(1, 0.5), Vector3d(0.2, 0.2, 0.2))
+
+
+if __name__ == '__main__':
+    unittest.main()

src/python/python.i

@@ -7,3 +7,4 @@
 %include ../Vector3.i
 %include ../Vector4.i
 %include ../Color.i
+%include ../Spline.i