diff --git a/include/ignition/math/Line2.hh b/include/ignition/math/Line2.hh index fa7b5de74..0b669f868 100644 --- a/include/ignition/math/Line2.hh +++ b/include/ignition/math/Line2.hh @@ -110,7 +110,7 @@ namespace ignition double _epsilon = 1e-6) const { return math::equal(this->CrossProduct(_pt), - static_cast(0), _epsilon); + 0., _epsilon); } /// \brief Check if the given line is parallel with this line. @@ -124,7 +124,7 @@ namespace ignition double _epsilon = 1e-6) const { return math::equal(this->CrossProduct(_line), - static_cast(0), _epsilon); + 0., _epsilon); } /// \brief Check if the given line is collinear with this line. This diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index c4825881b..0be042f58 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -60,9 +60,9 @@ if (PYTHONLIBS_FOUND) # Suppress warnings on SWIG-generated files target_compile_options(${SWIG_PY_LIB} PRIVATE - $<$:-Wno-pedantic -Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers> - $<$:-Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers> - $<$:-Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers> + $<$:-Wno-pedantic -Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers -Wno-class-memaccess> + $<$:-Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers -Wno-class-memaccess> + $<$:-Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers -Wno-class-memaccess> ) install(TARGETS ${SWIG_PY_LIB} DESTINATION ${IGN_LIB_INSTALL_DIR}/python/ignition) install(FILES ${CMAKE_BINARY_DIR}/lib/python/math.py DESTINATION ${IGN_LIB_INSTALL_DIR}/python/ignition) @@ -72,13 +72,18 @@ if (PYTHONLIBS_FOUND) set(python_tests Angle_TEST GaussMarkovProcess_TEST + Line2_TEST + Line3_TEST PID_TEST python_TEST Rand_TEST SemanticVersion_TEST + SignalStats_TEST Vector2_TEST Vector3_TEST Vector4_TEST + Temperature_TEST + Triangle_TEST ) foreach (test ${python_tests}) diff --git a/src/python/Line2.i b/src/python/Line2.i new file mode 100644 index 000000000..658c98ab4 --- /dev/null +++ b/src/python/Line2.i @@ -0,0 +1,83 @@ +/* + * 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. + * +*/ + +%module line2 +%{ +#include +#include +#include +%} + +%include "std_string.i" + +namespace ignition +{ + namespace math + { + template + class Line2 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: Line2(const math::Vector2 &_ptA, const math::Vector2 &_ptB); + public: Line2(double _x1, double _y1, double _x2, double _y2); + public: void Set(const math::Vector2 &_ptA, + const math::Vector2 &_ptB); + public: void Set(double _x1, double _y1, double _x2, double _y2); + public: double CrossProduct(const Line2 &_line) const; + public: double CrossProduct(const Vector2 &_pt) const; + public: bool Collinear(const math::Vector2 &_pt, + double _epsilon = 1e-6) const; + public: bool Parallel(const math::Line2 &_line, + double _epsilon = 1e-6) const; + public: bool Collinear(const math::Line2 &_line, + double _epsilon = 1e-6) const; + public: bool OnSegment(const math::Vector2 &_pt, + double _epsilon = 1e-6) const; + public: bool Within(const math::Vector2 &_pt, + double _epsilon = 1e-6) const; + public: bool Intersect(const Line2 &_line, + double _epsilon = 1e-6) const; + public: bool Intersect(const Line2 &_line, math::Vector2 &_pt, + double _epsilon = 1e-6) const; + public: T Length() const; + public: double Slope() const; + public: bool operator==(const Line2 &_line) const; + public: bool operator!=(const Line2 &_line) const; + }; + + %extend Line2 + { + ignition::math::Vector2 __getitem__(unsigned int i) const + { + return (*$self)[i]; + } + } + + %extend Line2 + { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Line2i) Line2; + %template(Line2d) Line2; + %template(Line2f) Line2; + } +} diff --git a/src/python/Line2_TEST.py b/src/python/Line2_TEST.py new file mode 100644 index 000000000..0c05a2e74 --- /dev/null +++ b/src/python/Line2_TEST.py @@ -0,0 +1,215 @@ +# 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 math +import unittest +from ignition.math import Line2d +from ignition.math import Vector2d + + +class TestLine2d(unittest.TestCase): + + def test_construction(self): + line_a = Line2d(0, 0, 10, 10) + self.assertAlmostEqual(line_a[0].x(), 0.0) + self.assertAlmostEqual(line_a[0].y(), 0.0) + self.assertAlmostEqual(line_a[1].x(), 10.0) + self.assertAlmostEqual(line_a[1].y(), 10.0) + + line_b = Line2d(Vector2d(1, 2), Vector2d(3, 4)) + self.assertAlmostEqual(line_b[0].x(), 1.0) + self.assertAlmostEqual(line_b[0].y(), 2.0) + self.assertAlmostEqual(line_b[1].x(), 3.0) + self.assertAlmostEqual(line_b[1].y(), 4.0) + + self.assertAlmostEqual(line_b[2].x(), line_b[1].x()) + + def test_length(self): + line_a = Line2d(0, 0, 10, 10) + self.assertAlmostEqual(line_a.length(), math.sqrt(200), delta=1e-10) + + def test_slope(self): + line = Line2d(0, 0, 10, 10) + self.assertAlmostEqual(line.slope(), 1.0, delta=1e-10) + + line = Line2d(0, 0, 0, 10) + self.assertTrue(math.isnan(line.slope())) + + line = Line2d(-10, 0, 100, 0) + self.assertAlmostEqual(line.slope(), 0.0) + + def test_parallel_line(self): + # Line is always parallel with itself + line = Line2d(0, 0, 10, 0) + self.assertTrue(line.parallel(line, 1e-10)) + + # Degenerate line segment + # Still expect Line is parallel with itself + line = Line2d(0, 0, 0, 0) + self.assertTrue(line.parallel(line, 1e-10)) + + line_a = Line2d(0, 0, 10, 0) + line_b = Line2d(0, 0, 10, 0) + self.assertTrue(line_a.parallel(line_b, 1e-10)) + + line_b.set(0, 0, 0, 10) + self.assertFalse(line_a.parallel(line_b)) + + line_b.set(0, 10, 10, 10) + self.assertTrue(line_a.parallel(line_b)) + + line_b.set(0, 10, 10, 10.00001) + self.assertFalse(line_a.parallel(line_b, 1e-10)) + self.assertFalse(line_a.parallel(line_b)) + self.assertTrue(line_a.parallel(line_b, 1e-3)) + + def test_collinear_line(self): + # Line is always collinear with itself + line = Line2d(0, 0, 10, 0) + self.assertTrue(line.collinear(line, 1e-10)) + + line_a = Line2d(0, 0, 10, 0) + line_b = Line2d(0, 0, 10, 0) + self.assertTrue(line_a.collinear(line_b, 1e-10)) + + line_b.set(0, 10, 10, 10) + self.assertFalse(line_a.collinear(line_b)) + + line_b.set(9, 0, 10, 0.00001) + self.assertFalse(line_a.collinear(line_b, 1e-10)) + self.assertFalse(line_a.collinear(line_b)) + self.assertTrue(line_a.collinear(line_b, 1e-3)) + + def test_collinear_point(self): + line_a = Line2d(0, 0, 10, 0) + pt = Vector2d(0, 0) + self.assertTrue(line_a.collinear(pt)) + + pt_line = Line2d(pt, pt) + self.assertTrue(line_a.collinear(pt_line)) + + pt.set(1000, 0) + self.assertTrue(line_a.collinear(pt, 1e-10)) + + pt_line = Line2d(pt, pt) + self.assertTrue(line_a.parallel(pt_line)) + self.assertFalse(line_a.intersect(pt_line)) + self.assertFalse(line_a.collinear(pt_line, 1e-10)) + + pt.set(10, 0) + pt_line.set(pt, pt) + self.assertTrue(line_a.collinear(pt_line, 1e-10)) + + pt.set(0, 0.00001) + self.assertFalse(line_a.collinear(pt)) + self.assertTrue(line_a.collinear(pt, 1e-3)) + + pt_line = Line2d(pt, pt) + self.assertFalse(line_a.collinear(pt_line)) + self.assertTrue(line_a.parallel(pt_line)) + self.assertFalse(line_a.intersect(pt_line)) + self.assertTrue(line_a.intersect(pt_line, 1e-2)) + self.assertTrue(line_a.collinear(pt_line, 1e-3)) + + pt.set(0, -0.00001) + self.assertFalse(line_a.collinear(pt)) + self.assertTrue(line_a.collinear(pt, 1e-3)) + + pt_line = Line2d(pt, pt) + self.assertFalse(line_a.collinear(pt_line)) + self.assertTrue(line_a.collinear(pt_line, 1e-4)) + + def test_intersect(self): + pt = Vector2d() + + # parallel horizontal lines + line_a = Line2d(1, 1, 2, 1) + line_b = Line2d(1, 2, 2, 2) + self.assertFalse(line_a.intersect(line_b, pt)) + + # parallel vertical lines + line_a.set(1, 1, 1, 10) + line_b.set(2, 1, 2, 10) + self.assertFalse(line_a.intersect(line_b, pt)) + + # Two lines that form an inverted T with a gap + line_a.set(1, 1, 1, 10) + line_b.set(0, 0, 2, 0) + self.assertFalse(line_a.intersect(line_b, pt)) + + # Two lines that form a T with a gap + line_a.set(1, 1, 1, 10) + line_b.set(0, 10.1, 2, 10.1) + self.assertFalse(line_a.intersect(line_b, pt)) + + # Two lines that form an inverted T with a gap + line_a.set(0, -10, 0, 10) + line_b.set(1, 0, 10, 0) + self.assertFalse(line_a.intersect(line_b, pt)) + + # Two lines that form a T with a gap + line_a.set(0, -10, 0, 10) + line_b.set(-1, 0, -10, 0) + self.assertFalse(line_a.intersect(line_b, pt)) + + # Two collinear lines, one starts where the other stopped + line_a.set(1, 1, 1, 10) + line_b.set(1, 10, 1, 11) + self.assertTrue(line_a.intersect(line_b, pt)) + self.assertEqual(pt, Vector2d(1, 10)) + + # Two collinear lines, one overlaps the other + line_a.set(0, 0, 0, 10) + line_b.set(0, 9, 0, 11) + self.assertTrue(line_a.intersect(line_b, pt)) + self.assertEqual(pt, Vector2d(0, 9)) + + # Two collinear lines, one overlaps the other + line_a.set(0, 0, 0, 10) + line_b.set(0, -10, 0, 1) + self.assertTrue(line_a.intersect(line_b, pt)) + self.assertEqual(pt, Vector2d(0, 1)) + + # Two intersecting lines + line_a.set(0, 0, 10, 10) + line_b.set(0, 10, 10, 0) + self.assertTrue(line_a.intersect(line_b, pt)) + self.assertEqual(pt, Vector2d(5, 5)) + + def test_equality(self): + line_a = Line2d(1, 1, 2, 1) + line_b = Line2d(1, 2, 2, 2) + + self.assertTrue(line_a != line_b) + self.assertTrue(line_a == line_a) + + line_b.set(1, 1, 2, 1.1) + self.assertFalse(line_a == line_b) + + line_b.set(1, 1, 2.1, 1) + self.assertFalse(line_a == line_b) + + line_b.set(1, 1.1, 2, 1) + self.assertFalse(line_a == line_b) + + line_b.set(1.1, 1, 2, 1) + self.assertFalse(line_a == line_b) + + def test_serialization(self): + line = Line2d(0, 1, 2, 3) + self.assertEqual(str(line), "0 1 2 3") + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/Line3.i b/src/python/Line3.i new file mode 100644 index 000000000..476abfced --- /dev/null +++ b/src/python/Line3.i @@ -0,0 +1,92 @@ +/* + * 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. + * +*/ + +%module line3 +%{ +#include +#include +#include +%} + +%include "std_string.i" + +namespace ignition +{ + namespace math + { + template + class Line3 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: Line3() = default; + public: Line3(const Line3 &_line); + public: Line3(const math::Vector3 &_ptA, const math::Vector3 &_ptB); + public: Line3(const double _x1, const double _y1, + const double _x2, const double _y2); + public: Line3(const double _x1, const double _y1, + const double _z1, const double _x2, + const double _y2, const double _z2); + public: void Set(const math::Vector3 &_ptA, + const math::Vector3 &_ptB); + public: void SetA(const math::Vector3 &_ptA); + public: void SetB(const math::Vector3 &_ptB); + public: void Set(const double _x1, const double _y1, + const double _x2, const double _y2, + const double _z = 0); + public: void Set(const double _x1, const double _y1, + const double _z1, const double _x2, + const double _y2, const double _z2); + public: math::Vector3 Direction() const; + public: T Length() const; + public: bool Distance(const Line3 &_line, Line3 &_result, + const double _epsilon = 1e-6) const; + public: bool Intersect(const Line3 &_line, + double _epsilon = 1e-6) const; + public: bool Coplanar(const Line3 &_line, + const double _epsilon = 1e-6) const; + public: bool Parallel(const Line3 &_line, + const double _epsilon = 1e-6) const; + public: bool Intersect(const Line3 &_line, math::Vector3 &_pt, + double _epsilon = 1e-6) const; + public: bool Within(const math::Vector3 &_pt, + double _epsilon = 1e-6) const; + public: bool operator==(const Line3 &_line) const; + public: bool operator!=(const Line3 &_line) const; + }; + + %extend Line3 + { + ignition::math::Vector3 __getitem__(const unsigned int i) const + { + return (*$self)[i]; + } + } + + %extend Line3 + { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Line3i) Line3; + %template(Line3d) Line3; + %template(Line3f) Line3; + } +} diff --git a/src/python/Line3_TEST.py b/src/python/Line3_TEST.py new file mode 100644 index 000000000..7d5cc11f7 --- /dev/null +++ b/src/python/Line3_TEST.py @@ -0,0 +1,250 @@ +# 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 math +import unittest +from ignition.math import Line3d +from ignition.math import Vector3d + + +class TestLine3d(unittest.TestCase): + + def test_construction(self): + line_a = Line3d(0, 0, 10, 10) + self.assertAlmostEqual(line_a[0].x(), 0.0) + self.assertAlmostEqual(line_a[0].y(), 0.0) + self.assertAlmostEqual(line_a[0].z(), 0.0) + self.assertAlmostEqual(line_a[1].x(), 10.0) + self.assertAlmostEqual(line_a[1].y(), 10.0) + self.assertAlmostEqual(line_a[1].z(), 0.0) + line_b = Line3d(Vector3d(1, 2, 3), Vector3d(4, 5, 6)) + self.assertAlmostEqual(line_b[0].x(), 1.0) + self.assertAlmostEqual(line_b[0].y(), 2.0) + self.assertAlmostEqual(line_b[0].z(), 3.0) + self.assertAlmostEqual(line_b[1].x(), 4.0) + self.assertAlmostEqual(line_b[1].y(), 5.0) + self.assertAlmostEqual(line_b[1].z(), 6.0) + + line_c = Line3d(0, 0, 5, 10, 10, 6) + self.assertAlmostEqual(line_c[0].x(), 0.0) + self.assertAlmostEqual(line_c[0].y(), 0.0) + self.assertAlmostEqual(line_c[0].z(), 5.0) + self.assertAlmostEqual(line_c[1].x(), 10.0) + self.assertAlmostEqual(line_c[1].y(), 10.0) + self.assertAlmostEqual(line_c[1].z(), 6.0) + + self.assertAlmostEqual(line_b[2].x(), line_b[1].x()) + + def test_set(self): + line_a = Line3d() + line_a.set(1, 1, 2, 2) + self.assertAlmostEqual(line_a[0].x(), 1.0) + self.assertAlmostEqual(line_a[0].y(), 1.0) + self.assertAlmostEqual(line_a[0].z(), 0.0) + self.assertAlmostEqual(line_a[1].x(), 2.0) + self.assertAlmostEqual(line_a[1].y(), 2.0) + self.assertAlmostEqual(line_a[1].z(), 0.0) + + line_a.set(10, 11, 12, 13, 14, 15) + self.assertAlmostEqual(line_a[0].x(), 10.0) + self.assertAlmostEqual(line_a[0].y(), 11.0) + self.assertAlmostEqual(line_a[0].z(), 12.0) + self.assertAlmostEqual(line_a[1].x(), 13.0) + self.assertAlmostEqual(line_a[1].y(), 14.0) + self.assertAlmostEqual(line_a[1].z(), 15.0) + + line_a.set_a(Vector3d(0, -1, -2)) + self.assertAlmostEqual(line_a[0].x(), 0.0) + self.assertAlmostEqual(line_a[0].y(), -1.0) + self.assertAlmostEqual(line_a[0].z(), -2.0) + self.assertAlmostEqual(line_a[1].x(), 13.0) + self.assertAlmostEqual(line_a[1].y(), 14.0) + self.assertAlmostEqual(line_a[1].z(), 15.0) + + line_a.set_b(Vector3d(5, 6, 7)) + self.assertAlmostEqual(line_a[0].x(), 0.0) + self.assertAlmostEqual(line_a[0].y(), -1.0) + self.assertAlmostEqual(line_a[0].z(), -2.0) + self.assertAlmostEqual(line_a[1].x(), 5.0) + self.assertAlmostEqual(line_a[1].y(), 6.0) + self.assertAlmostEqual(line_a[1].z(), 7.0) + + def test_length(self): + line_a = Line3d(0, 0, 0, 10, 10, 10) + self.assertAlmostEqual(line_a.length(), math.sqrt(300), delta=1e-10) + + def test_equality(self): + line_a = Line3d(1, 1, 1, 2, 1, 2) + line_b = Line3d(1, 2, 3, 2, 2, 4) + + self.assertTrue(line_a != line_b) + self.assertTrue(line_a == line_a) + + line_b.set(1, 1, 1, 2, 1.1, 2) + self.assertFalse(line_a == line_b) + + line_b.set(1, 1, 1, 2.1, 1, 2) + self.assertFalse(line_a == line_b) + + line_b.set(1, 1, 1.1, 2, 1, 2) + self.assertFalse(line_a == line_b) + + line_b.set(1.1, 1, 1, 2, 1, 2) + self.assertFalse(line_a == line_b) + + def test_serialization(self): + line = Line3d(0, 1, 4, 2, 3, 7) + self.assertEqual(str(line), "0 1 4 2 3 7") + + def test_copy_constructor(self): + line_a = Line3d(0, 1, 4, 2, 3, 7) + line_b = Line3d(line_a) + + self.assertEqual(line_a, line_b) + + def test_direction(self): + line_a = Line3d(1, 1, 1, 0, 0, 0) + line_b = Line3d(2, 2, 2, 0, 0, 0) + line_c = Line3d(0, 0, 0, 1, 1, 1) + self.assertTrue(line_a.direction() == + (line_a[1] - line_a[0]).normalize()) + self.assertTrue(line_a.direction() == line_b.direction()) + self.assertFalse(line_a.direction() == line_c.direction()) + + line_a.set(1, 1, 2, 1, 1, 10) + self.assertTrue(line_a.direction() == Vector3d.UNIT_Z) + + line_a.set(1, 5, 1, 1, 1, 1) + self.assertTrue(line_a.direction() == -Vector3d.UNIT_Y) + + line_a.set(1, 1, 1, 7, 1, 1) + self.assertTrue(line_a.direction() == Vector3d.UNIT_X) + + def test_within(self): + line = Line3d(0, 0, 0, 1, 1, 1) + self.assertTrue(line.within(Vector3d(0, 0, 0))) + self.assertTrue(line.within(Vector3d(1, 1, 1))) + self.assertTrue(line.within(Vector3d(0.5, 0.5, 0.5))) + + self.assertFalse(line.within(Vector3d(-0.5, 0.5, 0.5))) + self.assertFalse(line.within(Vector3d(0.5, -0.5, 0.5))) + self.assertFalse(line.within(Vector3d(0.5, 0.5, -0.5))) + + def test_distance(self): + line = Line3d(0, 0, 0, 0, 1, 0) + result = Line3d() + + self.assertTrue(line.distance(Line3d(1, 0.5, 0, -1, 0.5, 0), result)) + self.assertAlmostEqual(result.length(), 0) + self.assertEqual(result, Line3d(0, 0.5, 0, 0, 0.5, 0)) + + self.assertTrue(line.distance(Line3d(1, 0, 0, -1, 0, 0), result)) + self.assertAlmostEqual(result.length(), 0) + self.assertEqual(result, Line3d(0, 0, 0, 0, 0, 0)) + + self.assertTrue(line.distance(Line3d(1, 1.1, 0, -1, 1.1, 0), result)) + self.assertAlmostEqual(result.length(), 0.1, delta=1e-4) + self.assertEqual(result, Line3d(0, 1, 0, 0, 1.1, 0)) + + self.assertTrue(line.distance(Line3d(1, 0.5, 0.4, -1, 0.5, 0.4), + result)) + self.assertAlmostEqual(result.length(), 0.4, delta=1e-4) + self.assertEqual(result, Line3d(0, 0.5, 0, 0, 0.5, 0.4)) + + self.assertTrue(line.distance(Line3d(0, 0.5, 1, 1, 0.5, 0), + result)) + self.assertAlmostEqual(result.length(), math.sin(math.pi / 4), + delta=1e-4) + self.assertEqual(result, Line3d(0, 0.5, 0, 0.5, 0.5, 0.5)) + + # Expect true when lines are parallel + self.assertTrue(line.distance(Line3d(2, 0, 0, 2, 1, 0), result)) + self.assertEqual(result[0], line[0]) + self.assertEqual(result[1], Vector3d(2, 0, 0)) + + self.assertTrue(line.distance(Line3d(2, 1, 0, 2, 0, 0), result)) + self.assertEqual(result[0], line[0]) + self.assertEqual(result[1], Vector3d(2, 0, 0)) + + self.assertTrue(line.distance(Line3d(1, 1, 0, 1, 2, 0), result)) + self.assertEqual(result[0], line[1]) + self.assertEqual(result[1], Vector3d(1, 1, 0)) + + self.assertTrue(line.distance(Line3d(1, 2, 0, 1, 1, 0), result)) + self.assertEqual(result[0], line[1]) + self.assertEqual(result[1], Vector3d(1, 1, 0)) + + # Expect false when the passed in line is a point + self.assertFalse(line.distance(Line3d(2, 0, 0, 2, 0, 0), result)) + + # Expect false when the first line is a point. + line.set(0, 0, 0, 0, 0, 0) + self.assertFalse(line.distance(Line3d(2, 0, 0, 2, 1, 0), result)) + + def test_interesct(self): + line = Line3d(0, 0, 0, 0, 1, 0) + pt = Vector3d() + + self.assertTrue(line.intersect(Line3d(1, 0.5, 0, -1, 0.5, 0))) + self.assertTrue(line.intersect(Line3d(1, 0.5, 0, -1, 0.5, 0), pt)) + self.assertEqual(pt, Vector3d(0, 0.5, 0)) + + self.assertTrue(line.intersect(Line3d(1, 0, 0, -1, 0, 0))) + self.assertTrue(line.intersect(Line3d(1, 0, 0, -1, 0, 0), pt)) + self.assertEqual(pt, Vector3d(0, 0, 0)) + + self.assertTrue(line.intersect(Line3d(1, 1, 0, -1, 1, 0))) + self.assertTrue(line.intersect(Line3d(1, 1, 0, -1, 1, 0), pt)) + self.assertEqual(pt, Vector3d(0, 1, 0)) + + self.assertTrue(line.intersect(Line3d(0, 0.5, -1, 0, 0.5, 1))) + self.assertTrue(line.intersect(Line3d(0, 0.5, -1, 0, 0.5, 1), pt)) + self.assertEqual(pt, Vector3d(0, 0.5, 0)) + + self.assertTrue(line.intersect(Line3d(-1, 0.5, -1, 1, 0.5, 1))) + self.assertTrue(line.intersect(Line3d(-1, 0.5, -1, 1, 0.5, 1), pt)) + self.assertEqual(pt, Vector3d(0, 0.5, 0)) + + self.assertFalse(line.intersect(Line3d(1, 1.1, 0, -1, 1.1, 0))) + self.assertFalse(line.intersect(Line3d(1, -0.1, 0, -1, -0.1, 0))) + + self.assertFalse(line.intersect(Line3d(0.1, 0.1, 0, 0.6, 0.6, 0))) + self.assertFalse(line.intersect(Line3d(-0.1, 0, 0, -0.1, 1, 0))) + + self.assertTrue(line.intersect(Line3d(0, -1, 0, 0, 0.1, 0))) + self.assertTrue(line.intersect(Line3d(0, 1, 0, 0, 1.1, 0))) + + def test_parallel(self): + line = Line3d(0, 0, 0, 0, 1, 0) + self.assertTrue(line.parallel(Line3d(1, 0, 0, 1, 1, 0))) + self.assertTrue(line.parallel(Line3d(1, 1, 0, 1, 0, 0))) + self.assertTrue(line.parallel(Line3d(0, 0, 0, 0, 10, 0))) + self.assertTrue(line.parallel(Line3d(-100, 100, 20, -100, 200, 20))) + + self.assertFalse(line.parallel(Line3d(1, 0, 0, 1, 1, 1))) + self.assertFalse(line.parallel(Line3d(1, 0, 0, 2, 0, 0))) + self.assertFalse(line.parallel(Line3d(1, 0, 1, 2, 0, 1))) + + def test_coplanar(self): + line = Line3d(0, 0, 0, 0, 1, 0) + self.assertTrue(line.coplanar(Line3d(1, 0, 0, 1, 1, 0))) + self.assertTrue(line.coplanar(Line3d(0, 0, 0, 0, 10, 0))) + self.assertTrue(line.coplanar(Line3d(-100, 100, 20, -100, 200, 20))) + + self.assertFalse(line.coplanar(Line3d(1, 0, 0, 1, 1, 1))) + self.assertFalse(line.coplanar(Line3d(1, 0, 1, 2, 0, 0))) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/SignalStats.i b/src/python/SignalStats.i new file mode 100644 index 000000000..b918d3f35 --- /dev/null +++ b/src/python/SignalStats.i @@ -0,0 +1,107 @@ +/* + * 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. + * +*/ + +%module signalStats + +%{ +#include +%} + +%include "std_string.i" +%include "std_map.i" +%template(map_string_double) std::map; + +namespace ignition +{ + namespace math + { + class SignalMaximum + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual void Reset(); + public: virtual size_t Count() const; + public: virtual double Value() const; + public: virtual std::string ShortName() const; + public: virtual void InsertData(const double _data); + }; + + class SignalMean + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual void Reset(); + public: virtual size_t Count() const; + public: virtual double Value() const; + public: virtual std::string ShortName() const; + public: virtual void InsertData(const double _data); + }; + + class SignalMinimum + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual void Reset(); + public: virtual size_t Count() const; + public: virtual double Value() const; + public: virtual std::string ShortName() const; + public: virtual void InsertData(const double _data); + }; + + class SignalRootMeanSquare + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual void Reset(); + public: virtual size_t Count() const; + public: virtual double Value() const; + public: virtual std::string ShortName() const; + public: virtual void InsertData(const double _data); + }; + + class SignalMaxAbsoluteValue + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual void Reset(); + public: virtual size_t Count() const; + public: virtual double Value() const; + public: virtual std::string ShortName() const; + public: virtual void InsertData(const double _data); + }; + + class SignalVariance + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual void Reset(); + public: virtual size_t Count() const; + public: virtual double Value() const; + public: virtual std::string ShortName() const; + public: virtual void InsertData(const double _data); + }; + + class SignalStats + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: SignalStats(); + public: ~SignalStats(); + public: SignalStats(const SignalStats &_ss); + public: size_t Count() const; + public: std::map Map() const; + public: void InsertData(const double _data); + public: bool InsertStatistic(const std::string &_name); + public: bool InsertStatistics(const std::string &_names); + public: void Reset(); + }; + + } +} diff --git a/src/python/SignalStats_TEST.py b/src/python/SignalStats_TEST.py new file mode 100644 index 000000000..e8497e6d2 --- /dev/null +++ b/src/python/SignalStats_TEST.py @@ -0,0 +1,527 @@ +# 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 math +import unittest +from ignition.math import Rand +from ignition.math import SignalMaxAbsoluteValue +from ignition.math import SignalMaximum +from ignition.math import SignalMean +from ignition.math import SignalMinimum +from ignition.math import SignalRootMeanSquare +from ignition.math import SignalStats +from ignition.math import SignalVariance + + +class TestSignalStats(unittest.TestCase): + + def test_signal_maximum_constructor(self): + # Constructor + max = SignalMaximum() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + self.assertEqual(max.short_name(), "max") + + # reset + max.reset() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + def test_signal_maximum_constant_values(self): + # Constant values, max should match + max = SignalMaximum() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + max.insert_data(value) + self.assertAlmostEqual(max.value(), value) + self.assertEqual(max.count(), i) + + # reset + max.reset() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + def test_signal_maximum_alternating_values(self): + # Values with alternating sign, increasing magnitude + # Should always match positive value + max = SignalMaximum() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + + for i in range(1, 11): + max.insert_data(value * i) + self.assertAlmostEqual(max.value(), value * i) + max.insert_data(-value * i) + self.assertAlmostEqual(max.value(), value * i) + self.assertEqual(max.count(), i*2) + + # reset + max.reset() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + def test_signal_mean_constructor(self): + # Constructor + mean = SignalMean() + self.assertAlmostEqual(mean.value(), 0.0) + self.assertEqual(mean.count(), 0) + self.assertEqual(mean.short_name(), "mean") + + # reset + mean.reset() + self.assertAlmostEqual(mean.value(), 0.0) + self.assertEqual(mean.count(), 0) + + def test_signal_mean_constant_values(self): + # Constant values, mean should match + mean = SignalMean() + self.assertAlmostEqual(mean.value(), 0.0) + self.assertEqual(mean.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + mean.insert_data(value) + self.assertAlmostEqual(mean.value(), value) + self.assertEqual(mean.count(), i) + + # reset + mean.reset() + self.assertAlmostEqual(mean.value(), 0.0) + self.assertEqual(mean.count(), 0) + + def test_signal_mean_alternating_values(self): + # Values with alternating sign, increasing magnitude + # Should be zero every other time + mean = SignalMean() + self.assertAlmostEqual(mean.value(), 0.0) + self.assertEqual(mean.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + mean.insert_data(value * i) + mean.insert_data(-value * i) + self.assertAlmostEqual(mean.value(), 0.0) + self.assertEqual(mean.count(), i*2) + + # reset + mean.reset() + self.assertAlmostEqual(mean.value(), 0.0) + self.assertEqual(mean.count(), 0) + + def test_signal_minimum_constructor(self): + # Constructor + min = SignalMinimum() + self.assertAlmostEqual(min.value(), 0.0) + self.assertEqual(min.count(), 0) + self.assertEqual(min.short_name(), "min") + + # reset + min.reset() + self.assertAlmostEqual(min.value(), 0.0) + self.assertEqual(min.count(), 0) + + def test_signal_minimum_constant_values(self): + # Constant values, min should match + min = SignalMinimum() + self.assertAlmostEqual(min.value(), 0.0) + self.assertEqual(min.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + min.insert_data(value) + self.assertAlmostEqual(min.value(), value) + self.assertEqual(min.count(), i) + + # reset + min.reset() + self.assertAlmostEqual(min.value(), 0.0) + self.assertEqual(min.count(), 0) + + def test_signal_minimum_alternating_values(self): + # Values with alternating sign, increasing magnitude + # Should always match negative value + min = SignalMinimum() + self.assertAlmostEqual(min.value(), 0.0) + self.assertEqual(min.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + min.insert_data(value * i) + min.insert_data(-value * i) + self.assertAlmostEqual(min.value(), -value * i) + self.assertEqual(min.count(), i*2) + + # reset + min.reset() + self.assertAlmostEqual(min.value(), 0.0) + self.assertEqual(min.count(), 0) + + def test_signal_root_mean_square(self): + # Constructor + rms = SignalRootMeanSquare() + self.assertAlmostEqual(rms.value(), 0.0) + self.assertEqual(rms.count(), 0) + self.assertEqual(rms.short_name(), "rms") + + # reset + rms.reset() + self.assertAlmostEqual(rms.value(), 0.0) + self.assertEqual(rms.count(), 0) + + def test_signal_root_mean_square_constant_values(self): + # Constant values, rms should match + rms = SignalRootMeanSquare() + self.assertAlmostEqual(rms.value(), 0.0) + self.assertEqual(rms.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + rms.insert_data(value) + self.assertAlmostEqual(rms.value(), value) + self.assertEqual(rms.count(), i) + + # reset + rms.reset() + self.assertAlmostEqual(rms.value(), 0.0) + self.assertEqual(rms.count(), 0) + + def test_signal_root_mean_square_alternating_values(self): + # Values with alternating sign, same magnitude + # rms should match absolute value every time + rms = SignalRootMeanSquare() + self.assertAlmostEqual(rms.value(), 0.0) + self.assertEqual(rms.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + rms.insert_data(value) + self.assertAlmostEqual(rms.value(), value) + self.assertEqual(rms.count(), i*2-1) + + rms.insert_data(-value) + self.assertAlmostEqual(rms.value(), value) + self.assertEqual(rms.count(), i*2) + + # reset + rms.reset() + self.assertAlmostEqual(rms.value(), 0.0) + self.assertEqual(rms.count(), 0) + + def test_signal_max_absolute_value_constructor(self): + # Constructor + max = SignalMaxAbsoluteValue() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + self.assertEqual(max.short_name(), "maxAbs") + + # reset + max.reset() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + def test_signal_max_absolute_value_constant_values(self): + # Constant values, max should match + max = SignalMaxAbsoluteValue() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + max.insert_data(value) + self.assertAlmostEqual(max.value(), value) + self.assertEqual(max.count(), i) + + # reset + max.reset() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + def test_signal_max_absolute_value_alternating_values(self): + # Values with alternating sign, increasing magnitude + # max should match absolute value every time + max = SignalMaxAbsoluteValue() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + max.insert_data(value * i) + self.assertAlmostEqual(max.value(), value * i) + self.assertEqual(max.count(), i*2-1) + + max.insert_data(-value * i) + self.assertAlmostEqual(max.value(), value * i) + self.assertEqual(max.count(), i*2) + + # reset + max.reset() + self.assertAlmostEqual(max.value(), 0.0) + self.assertEqual(max.count(), 0) + + def test_signal_variance_constructor(self): + var = SignalVariance() + self.assertAlmostEqual(var.value(), 0.0) + self.assertEqual(var.count(), 0) + self.assertEqual(var.short_name(), "var") + + # reset + var.reset() + self.assertAlmostEqual(var.value(), 0.0) + self.assertEqual(var.count(), 0) + + def test_signal_variance_one_value(self): + # Add one value, expect 0.0 variance + values = {0, 1.0, 10.0, -100.0} + for value in values: + var = SignalVariance() + var.insert_data(value) + self.assertEqual(var.count(), 1) + self.assertAlmostEqual(0.0, var.value()) + + # reset + var.reset() + self.assertAlmostEqual(0.0, var.value()) + self.assertEqual(var.count(), 0) + + def test_signal_variance_constant_values(self): + # Constant values, expect 0.0 variance + var = SignalVariance() + value = 3.14159 + + # Loop two times to verify reset + for j in range(2): + for i in range(1, 11): + var.insert_data(value) + self.assertAlmostEqual(0.0, var.value()) + self.assertEqual(var.count(), i) + + # reset + var.reset() + self.assertAlmostEqual(var.value(), 0.0) + self.assertEqual(var.count(), 0) + + def test_signal_variance_random_values(self): + # Random normally distributed values + # The sample variance has the following variance: + # 2 variance^2 / (count - 1) + # en.wikipedia.org/wiki/Variance#Distribution_of_the_sample_variance + # We will use 5 sigma (4e-5 chance of failure) + var = SignalVariance() + std_dev = 3.14159 + count = 10000 + sigma = 5.0 + for i in range(count): + var.insert_data(Rand.dbl_normal(0.0, std_dev)) + + variance = std_dev*std_dev + sampleVariance2 = 2 * variance*variance / (count - 1) + self.assertAlmostEqual(var.value(), variance, + delta=sigma*math.sqrt(sampleVariance2)) + + # reset + var.reset() + self.assertAlmostEqual(var.value(), 0.0) + self.assertEqual(var.count(), 0) + + def test_signal_stats_constructor(self): + # Constructor + stats = SignalStats() + self.assertTrue(stats.map().empty()) + self.assertEqual(stats.count(), 0) + + stats2 = SignalStats(stats) + self.assertEqual(stats.count(), stats2.count()) + + # reset + stats.reset() + self.assertTrue(stats.map().empty()) + self.assertEqual(stats.count(), 0) + + def test_01_signal_stats_intern_statistic(self): + # insert static + stats = SignalStats() + self.assertTrue(stats.map().empty()) + + self.assertTrue(stats.insert_statistic("max")) + self.assertFalse(stats.insert_statistic("max")) + self.assertFalse(stats.map().empty()) + + self.assertTrue(stats.insert_statistic("maxAbs")) + self.assertFalse(stats.insert_statistic("maxAbs")) + self.assertFalse(stats.map().empty()) + + self.assertTrue(stats.insert_statistic("mean")) + self.assertFalse(stats.insert_statistic("mean")) + self.assertFalse(stats.map().empty()) + + self.assertTrue(stats.insert_statistic("min")) + self.assertFalse(stats.insert_statistic("min")) + self.assertFalse(stats.map().empty()) + + self.assertTrue(stats.insert_statistic("rms")) + self.assertFalse(stats.insert_statistic("rms")) + self.assertFalse(stats.map().empty()) + + self.assertTrue(stats.insert_statistic("var")) + self.assertFalse(stats.insert_statistic("var")) + self.assertFalse(stats.map().empty()) + + self.assertFalse(stats.insert_statistic("FakeStatistic")) + + # map with no data + map = stats.map() + self.assertFalse(map.empty()) + self.assertEqual(map.size(), 6) + self.assertEqual(map.count("max"), 1) + self.assertEqual(map.count("maxAbs"), 1) + self.assertEqual(map.count("mean"), 1) + self.assertEqual(map.count("min"), 1) + self.assertEqual(map.count("rms"), 1) + self.assertEqual(map.count("var"), 1) + self.assertEqual(map.count("FakeStatistic"), 0) + + stats2 = SignalStats(stats) + map2 = stats2.map() + self.assertFalse(map2.empty()) + self.assertEqual(map.size(), map2.size()) + self.assertEqual(map.count("max"), map2.count("max")) + self.assertEqual(map.count("maxAbs"), map2.count("maxAbs")) + self.assertEqual(map.count("mean"), map2.count("mean")) + self.assertEqual(map.count("min"), map2.count("min")) + self.assertEqual(map.count("rms"), map2.count("rms")) + self.assertEqual(map.count("var"), map2.count("var")) + self.assertEqual(map.count("FakeStatistic"), + map2.count("FakeStatistic")) + + def test_02_signal_stats_intern_statistic(self): + # insert statics + stats = SignalStats() + self.assertFalse(stats.insert_statistics("")) + self.assertTrue(stats.map().empty()) + + self.assertTrue(stats.insert_statistics("maxAbs,rms")) + self.assertEqual(stats.map().size(), 2) + self.assertFalse(stats.insert_statistics("maxAbs,rms")) + self.assertFalse(stats.insert_statistics("maxAbs")) + self.assertFalse(stats.insert_statistics("rms")) + self.assertEqual(stats.map().size(), 2) + + self.assertFalse(stats.insert_statistics("mean,FakeStatistic")) + self.assertEqual(stats.map().size(), 3) + + self.assertFalse(stats.insert_statistics("var,FakeStatistic")) + self.assertEqual(stats.map().size(), 4) + + self.assertFalse(stats.insert_statistics("max,FakeStatistic")) + self.assertEqual(stats.map().size(), 5) + + self.assertFalse(stats.insert_statistics("min,FakeStatistic")) + self.assertEqual(stats.map().size(), 6) + + self.assertFalse(stats.insert_statistics("FakeStatistic")) + self.assertEqual(stats.map().size(), 6) + + # map with no data + map = stats.map() + self.assertFalse(map.empty()) + self.assertEqual(map.size(), 6) + self.assertEqual(map.count("max"), 1) + self.assertEqual(map.count("maxAbs"), 1) + self.assertEqual(map.count("mean"), 1) + self.assertEqual(map.count("min"), 1) + self.assertEqual(map.count("rms"), 1) + self.assertEqual(map.count("var"), 1) + self.assertEqual(map.count("FakeStatistic"), 0) + + def test_signal_stats_alternating_values(self): + # Add some statistics + stats = SignalStats() + self.assertTrue(stats.insert_statistics("max,maxAbs,mean,min,rms")) + self.assertEqual(stats.map().size(), 5) + + # No data yet + self.assertEqual(stats.count(), 0) + + # Insert data with alternating signs + value = 3.14159 + stats.insert_data(value) + stats.insert_data(-value) + self.assertEqual(stats.count(), 2) + + map = stats.map() + self.assertAlmostEqual(map["max"], value) + self.assertAlmostEqual(map["maxAbs"], value) + self.assertAlmostEqual(map["min"], -value) + self.assertAlmostEqual(map["rms"], value) + self.assertAlmostEqual(map["mean"], 0.0) + + # test operator= + copy = SignalStats(stats) + self.assertEqual(copy.count(), 2) + map = stats.map() + self.assertEqual(map.size(), 5) + self.assertAlmostEqual(map["max"], value) + self.assertAlmostEqual(map["maxAbs"], value) + self.assertAlmostEqual(map["min"], -value) + self.assertAlmostEqual(map["rms"], value) + self.assertAlmostEqual(map["mean"], 0.0) + + stats.reset() + self.assertEqual(stats.map().size(), 5) + self.assertEqual(stats.count(), 0) + map = stats.map() + self.assertAlmostEqual(map["max"], 0.0) + self.assertAlmostEqual(map["maxAbs"], 0.0) + self.assertAlmostEqual(map["min"], 0.0) + self.assertAlmostEqual(map["rms"], 0.0) + self.assertAlmostEqual(map["mean"], 0.0) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/Temperature.i b/src/python/Temperature.i new file mode 100644 index 000000000..27043ea62 --- /dev/null +++ b/src/python/Temperature.i @@ -0,0 +1,88 @@ +/* + * 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. + * +*/ + +%module temperature +%{ +#include +%} + +%include "std_string.i" + +namespace ignition +{ + namespace math + { + class Temperature + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: Temperature(); + public: Temperature(const double _temp); + public: Temperature(const Temperature &_temp); + public: virtual ~Temperature(); + public: static double KelvinToCelsius(const double _temp); + public: static double KelvinToFahrenheit(const double _temp); + public: static double CelsiusToFahrenheit(const double _temp); + public: static double CelsiusToKelvin(const double _temp); + public: static double FahrenheitToCelsius(const double _temp); + public: static double FahrenheitToKelvin(const double _temp); + public: void SetKelvin(const double _temp); + public: void SetCelsius(const double _temp); + public: void SetFahrenheit(const double _temp); + public: double Kelvin() const; + public: double Celsius() const; + public: double Fahrenheit() const; + public: double operator()() const; + public: Temperature operator+(const double _temp); + public: Temperature operator+(const Temperature &_temp); + public: const Temperature &operator+=(const double _temp); + public: const Temperature &operator+=(const Temperature &_temp); + public: Temperature operator-(const double _temp); + public: Temperature operator-(const Temperature &_temp); + public: const Temperature &operator-=(const double _temp); + public: const Temperature &operator-=(const Temperature &_temp); + public: Temperature operator*(const double _temp); + public: Temperature operator*(const Temperature &_temp); + public: const Temperature &operator*=(const double _temp); + public: const Temperature &operator*=(const Temperature &_temp); + public: Temperature operator/(const double _temp); + public: Temperature operator/(const Temperature &_temp); + public: const Temperature &operator/=(const double _temp); + public: const Temperature &operator/=(const Temperature &_temp); + public: bool operator==(const Temperature &_temp) const; + public: bool operator==(const double _temp) const; + public: bool operator!=(const Temperature &_temp) const; + public: bool operator!=(const double _temp) const; + public: bool operator<(const Temperature &_temp) const; + public: bool operator<(const double _temp) const; + public: bool operator<=(const Temperature &_temp) const; + public: bool operator<=(const double _temp) const; + public: bool operator>(const Temperature &_temp) const; + public: bool operator>(const double _temp) const; + public: bool operator>=(const Temperature &_temp) const; + public: bool operator>=(const double _temp) const; + }; + + %extend Temperature + { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + } +} diff --git a/src/python/Temperature_TEST.py b/src/python/Temperature_TEST.py new file mode 100644 index 000000000..4955804e5 --- /dev/null +++ b/src/python/Temperature_TEST.py @@ -0,0 +1,130 @@ +# 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 +from ignition.math import Temperature + + +class TestTemperature(unittest.TestCase): + + def test_temperature_constructor(self): + temp = Temperature() + self.assertAlmostEqual(temp.kelvin(), 0.0, 1e-6) + + temp2 = Temperature(1.1) + self.assertAlmostEqual(temp2.kelvin(), 1.1, 1e-6) + + temp3 = Temperature(temp2) + self.assertAlmostEqual(temp3.kelvin(), 1.1, delta=1e-6) + self.assertAlmostEqual(temp3.celsius(), -272.05, delta=1e-6) + + self.assertTrue(temp2 == temp3) + self.assertTrue(temp2 == 1.1) + self.assertTrue(temp2 != temp) + self.assertTrue(temp2 != 1.2) + + self.assertTrue(temp < temp2) + self.assertTrue(temp < 10.0) + self.assertTrue(temp <= temp2) + self.assertTrue(temp <= 0.0) + self.assertTrue(temp <= 0.1) + + self.assertFalse(temp > temp2) + self.assertFalse(temp > 80.0) + self.assertFalse(temp >= temp2) + self.assertFalse(temp >= 0.1) + self.assertTrue(temp >= 0.0) + + def test_temperature_conversions(self): + self.assertAlmostEqual(Temperature.kelvin_to_celsius(0), -273.15, + delta=1e-6) + self.assertAlmostEqual(Temperature.kelvin_to_fahrenheit(300), 80.33, + delta=1e-6) + self.assertAlmostEqual(Temperature.celsius_to_fahrenheit(20.0), 68.0, + delta=1e-6) + self.assertAlmostEqual(Temperature.celsius_to_kelvin(10.0), 283.15, + delta=1e-6) + self.assertAlmostEqual(Temperature.fahrenheit_to_celsius(-40.0), + Temperature.celsius_to_fahrenheit(-40.0), 1e-6) + self.assertAlmostEqual(Temperature.fahrenheit_to_kelvin(60.0), + 288.7055, delta=1e-3) + + def test_temperature_mutators_accessors(self): + temp = Temperature() + self.assertAlmostEqual(temp.kelvin(), 0.0, delta=1e-6) + + temp.set_kelvin(10) + self.assertAlmostEqual(temp.kelvin(), 10.0, delta=1e-6) + + temp.set_celsius(20) + self.assertAlmostEqual(temp(), 293.15, delta=1e-6) + + temp.set_fahrenheit(30) + self.assertAlmostEqual(temp.fahrenheit(), 30.0, delta=1e-6) + self.assertAlmostEqual(temp(), 272.0388889, delta=1e-6) + + def test_temperature_operators(self): + temp = Temperature(20) + self.assertAlmostEqual(temp(), 20, delta=1e-6) + + temp = Temperature(30) + + self.assertAlmostEqual(temp(), 30, delta=1e-6) + + temp2 = Temperature(temp) + self.assertTrue(temp == temp2) + + self.assertAlmostEqual((temp + temp2).kelvin(), 60, delta=1e-6) + self.assertAlmostEqual((temp + 40).kelvin(), 70, delta=1e-6) + + self.assertAlmostEqual((temp - temp2).kelvin(), 0, delta=1e-6) + self.assertAlmostEqual((temp - 20).kelvin(), 10.0, delta=1e-6) + + self.assertAlmostEqual((temp * temp2).kelvin(), 900, delta=1e-6) + self.assertAlmostEqual((temp * 2).kelvin(), 60.0, delta=1e-6) + + self.assertAlmostEqual((temp / temp2).kelvin(), 1.0, delta=1e-6) + self.assertAlmostEqual((temp / 2).kelvin(), 15.0, delta=1e-6) + + temp += temp2 + self.assertAlmostEqual(temp.kelvin(), 60.0, delta=1e-6) + temp -= temp2 + self.assertAlmostEqual(temp.kelvin(), 30.0, delta=1e-6) + + temp += 5.0 + self.assertAlmostEqual(temp.kelvin(), 35.0, delta=1e-6) + temp -= 5.0 + self.assertAlmostEqual(temp.kelvin(), 30.0, delta=1e-6) + + temp *= temp2 + self.assertAlmostEqual(temp.kelvin(), 900, delta=1e-6) + temp /= temp2 + self.assertAlmostEqual(temp.kelvin(), 30, delta=1e-6) + + temp *= 4.0 + self.assertAlmostEqual(temp.kelvin(), 120, delta=1e-6) + temp /= 4.0 + self.assertAlmostEqual(temp.kelvin(), 30, delta=1e-6) + + temp3 = Temperature(temp) + self.assertTrue(temp3 == temp) + self.assertTrue(temp3 == temp2) + + def test_serialization(self): + temp = Temperature(55.45) + self.assertEqual(str(temp), "55.45") + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/Triangle.i b/src/python/Triangle.i new file mode 100644 index 000000000..08c5f5370 --- /dev/null +++ b/src/python/Triangle.i @@ -0,0 +1,67 @@ +/* + * 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. + * +*/ + +%module triangle +%{ +#include +#include +#include +#include +#include +#include +%} + +namespace ignition +{ +namespace math +{ + template + class Triangle + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: Triangle() = default; + public: Triangle(const math::Vector2 &_pt1, + const math::Vector2 &_pt2, + const math::Vector2 &_pt3); + public: void Set(const unsigned int _index, const math::Vector2 &_pt); + public: void Set(const math::Vector2 &_pt1, + const math::Vector2 &_pt2, + const math::Vector2 &_pt3); + public: bool Valid() const; + public: Line2 Side(const unsigned int _index) const; + public: bool Contains(const Line2 &_line) const; + public: bool Contains(const math::Vector2 &_pt) const; + public: bool Intersects(const Line2 &_line, + math::Vector2 &_ipt1, + math::Vector2 &_ipt2) const; + public: T Perimeter() const; + public: double Area() const; + }; + + %extend Triangle + { + ignition::math::Vector2 __getitem__(const unsigned int i) const + { + return (*$self)[i]; + } + } + + %template(Trianglei) Triangle; + %template(Triangled) Triangle; + %template(Trianglef) Triangle; +} +} diff --git a/src/python/Triangle_TEST.py b/src/python/Triangle_TEST.py new file mode 100644 index 000000000..4973b7d0b --- /dev/null +++ b/src/python/Triangle_TEST.py @@ -0,0 +1,162 @@ +# 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 math +import unittest +from ignition.math import Line2d +from ignition.math import Triangled +from ignition.math import Vector2d + + +class TestTriangle(unittest.TestCase): + + def test_constructor(self): + # Constructor + tri = Triangled() + self.assertAlmostEqual(tri[0], Vector2d(0, 0)) + self.assertAlmostEqual(tri[1], Vector2d(0, 0)) + self.assertAlmostEqual(tri[2], Vector2d(0, 0)) + + # Construct from three points + tri = Triangled(Vector2d(0, 0), + Vector2d(0, 1), + Vector2d(1, 0)) + + self.assertTrue(tri.valid()) + + self.assertAlmostEqual(tri[0], Vector2d(0, 0)) + self.assertAlmostEqual(tri[1], Vector2d(0, 1)) + self.assertAlmostEqual(tri[2], Vector2d(1, 0)) + self.assertAlmostEqual(tri[3], tri[2]) + + # Construct degenerate from 3 collinear points + tri = Triangled(Vector2d(0, 0), + Vector2d(0, 1), + Vector2d(0, 2)) + + # Expect not valid + self.assertFalse(tri.valid()) + + def test_set(self): + tri = Triangled() + + tri.set(0, Vector2d(3, 4)) + tri.set(1, Vector2d(5, 6)) + tri.set(2, Vector2d(7, 8)) + self.assertAlmostEqual(tri[0], Vector2d(3, 4)) + self.assertAlmostEqual(tri[1], Vector2d(5, 6)) + self.assertAlmostEqual(tri[2], Vector2d(7, 8)) + + tri.set(Vector2d(0.1, 0.2), + Vector2d(0.3, 0.4), + Vector2d(1.5, 2.6)) + self.assertAlmostEqual(tri[0], Vector2d(0.1, 0.2)) + self.assertAlmostEqual(tri[1], Vector2d(0.3, 0.4)) + self.assertAlmostEqual(tri[2], Vector2d(1.5, 2.6)) + + def test_side(self): + tri = Triangled(Vector2d(0, 0), + Vector2d(0, 1), + Vector2d(1, 0)) + + self.assertTrue(tri.side(0) == Line2d(0, 0, 0, 1)) + self.assertTrue(tri.side(1) == Line2d(0, 1, 1, 0)) + self.assertTrue(tri.side(2) == Line2d(1, 0, 0, 0)) + + def test_contains_line(self): + tri = Triangled(Vector2d(0, 0), + Vector2d(0, 1), + Vector2d(1, 0)) + + self.assertTrue(tri.contains(tri.side(0))) + self.assertTrue(tri.contains(tri.side(1))) + self.assertTrue(tri.contains(tri.side(2))) + + self.assertTrue(tri.contains(Line2d(0.1, 0.1, 0.5, 0.5))) + + self.assertFalse(tri.contains(Line2d(0.1, 0.1, 0.6, 0.6))) + self.assertFalse(tri.contains(Line2d(-0.1, -0.1, 0.5, 0.5))) + + def test_intersects(self): + pt1 = Vector2d() + pt2 = Vector2d() + tri = Triangled(Vector2d(0, 0), + Vector2d(0, 1), + Vector2d(1, 0)) + + self.assertTrue(tri.intersects(tri.side(0), pt1, pt2)) + self.assertAlmostEqual(pt1, Vector2d(0, 0)) + self.assertAlmostEqual(pt2, Vector2d(0, 1)) + + self.assertTrue(tri.intersects(tri.side(1), pt1, pt2)) + self.assertAlmostEqual(pt1, Vector2d(0, 1)) + self.assertAlmostEqual(pt2, Vector2d(1, 0)) + + self.assertTrue(tri.intersects(tri.side(2), pt1, pt2)) + self.assertAlmostEqual(pt1, Vector2d(1, 0)) + self.assertAlmostEqual(pt2, Vector2d(0, 0)) + + self.assertTrue(tri.intersects(Line2d(0.1, 0.1, 0.5, 0.5), pt1, pt2)) + self.assertAlmostEqual(pt1, Vector2d(0.1, 0.1)) + self.assertAlmostEqual(pt2, Vector2d(0.5, 0.5)) + + self.assertTrue(tri.intersects(Line2d(0.1, 0.1, 0.6, 0.6), pt1, pt2)) + self.assertAlmostEqual(pt1, Vector2d(0.5, 0.5)) + self.assertAlmostEqual(pt2, Vector2d(0.1, 0.1)) + + self.assertTrue(tri.intersects(Line2d(-0.1, -0.1, 0.5, 0.5), pt1, pt2)) + self.assertAlmostEqual(pt1, Vector2d(0.0, 0.0)) + self.assertAlmostEqual(pt2, Vector2d(0.5, 0.5)) + + self.assertTrue(tri.intersects(Line2d(-2, -2, 0.2, 0.2), pt1, pt2)) + self.assertAlmostEqual(pt1, Vector2d(0.0, 0.0)) + self.assertAlmostEqual(pt2, Vector2d(0.2, 0.2)) + + self.assertFalse(tri.intersects(Line2d(-0.1, 0, -0.1, 1), pt1, pt2)) + + def test_contains_pt(self): + tri = Triangled(Vector2d(0, 0), + Vector2d(0, 1), + Vector2d(1, 0)) + + self.assertTrue(tri.contains(tri[0])) + self.assertTrue(tri.contains(tri[1])) + self.assertTrue(tri.contains(tri[2])) + + self.assertTrue(tri.contains(Vector2d(0.1, 0.1))) + self.assertTrue(tri.contains(Vector2d(0, 0.5))) + self.assertTrue(tri.contains(Vector2d(0.5, 0))) + self.assertTrue(tri.contains(Vector2d(0.5, 0.5))) + + self.assertFalse(tri.contains(Vector2d(-0.01, -0.01))) + self.assertFalse(tri.contains(Vector2d(1.01, 0))) + self.assertFalse(tri.contains(Vector2d(0, 1.01))) + + def test_perimeter(self): + tri = Triangled(Vector2d(0, 0), + Vector2d(0, 1), + Vector2d(1, 0)) + + self.assertAlmostEqual(tri.perimeter(), 2.0 + math.sqrt(2.0)) + + def test_area(self): + tri = Triangled(Vector2d(0, 0), + Vector2d(0, 1), + Vector2d(1, 0)) + + self.assertAlmostEqual(tri.area(), 0.499999, delta=1e-6) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/python.i b/src/python/python.i index 256b64df5..b461e5967 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -5,5 +5,10 @@ %include Vector2.i %include Vector3.i %include Vector4.i +%include Line2.i +%include Line3.i %include PID.i %include SemanticVersion.i +%include SignalStats.i +%include Temperature.i +%include Triangle.i