From 895eb1ef98d89d937714e875ab108ff4e61061b7 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Fri, 6 Aug 2021 14:57:35 -0300 Subject: [PATCH] Adds scripting interface to Spline and a python test. Signed-off-by: LolaSegura --- src/CMakeLists.txt | 1 + src/Spline.i | 80 +++++++++++++++++++++ src/Spline_TEST.py | 166 ++++++++++++++++++++++++++++++++++++++++++++ src/python/python.i | 1 + 4 files changed, 248 insertions(+) create mode 100644 src/Spline.i create mode 100644 src/Spline_TEST.py diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e27364099..8b5366055 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -131,6 +131,7 @@ if (PYTHONLIBS_FOUND) python_TEST Rand_TEST RollingMean_TEST + Spline_TEST Vector2_TEST Vector3_TEST Vector4_TEST diff --git a/src/Spline.i b/src/Spline.i new file mode 100644 index 000000000..ccb6c396e --- /dev/null +++ b/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 +#include +#include +#include +%} + +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 &_p); + public: void AddPoint(const Vector3 &_p, const Vector3 &_t); + private: void AddPoint(const ControlPoint &_cp, const bool _fixed); + public: Vector3 Point(const unsigned int _index) const; + public: Vector3 Tangent(const unsigned int _index) const; + public: Vector3 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 &_p); + public: bool UpdatePoint(const unsigned int _index, + const Vector3 &_p, + const Vector3 &_t); + private: bool UpdatePoint(const unsigned int _index, + const ControlPoint &_cp, + const bool _fixed); + public: Vector3 Interpolate(const double _t) const; + public: Vector3 Interpolate(const unsigned int _fromIndex, + const double _t) const; + public: Vector3 InterpolateTangent(const double _t) const; + public: Vector3 InterpolateTangent(const unsigned int _fromIndex, + const double _t) const; + public: Vector3 InterpolateMthDerivative(const unsigned int _mth, + const double _1) const; + public: Vector3 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; + }; + } +} \ No newline at end of file diff --git a/src/Spline_TEST.py b/src/Spline_TEST.py new file mode 100644 index 000000000..efdd2c1d6 --- /dev/null +++ b/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() diff --git a/src/python/python.i b/src/python/python.i index 9314cc616..01c9ad3a8 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -7,3 +7,4 @@ %include ../Vector3.i %include ../Vector4.i %include ../Color.i +%include ../Spline.i