diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 70d716d7f..fa94b79fd 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -103,6 +103,7 @@ if (PYTHONLIBS_FOUND) Matrix4_TEST MovingWindowFilter_TEST PID_TEST + Plane_TEST Pose3_TEST python_TEST Quaternion_TEST diff --git a/src/python/Plane.i b/src/python/Plane.i new file mode 100644 index 000000000..0c35aa481 --- /dev/null +++ b/src/python/Plane.i @@ -0,0 +1,116 @@ +/* + * 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 plane +%{ +#include +#include +#include +#include +#include +#include +#include +#include +%} + +%include "typemaps.i" +%typemap(out) (std::optional< ignition::math::Vector3< double > >) %{ + if((*(&result)).has_value()) { + $result = SWIG_NewPointerObj( + (new ignition::math::Vector3< double >(static_cast< const ignition::math::Vector3< double >& >((*(&result)).value()))), + SWIGTYPE_p_ignition__math__Vector3T_double_t, + SWIG_POINTER_OWN | 0 ); + } else { + $result = Py_None; + Py_INCREF(Py_None); + } +%} + + +namespace ignition +{ + namespace math + { + template + class Plane + { + public: enum PlaneSide + { + /// \brief Negative side of the plane. This is the side that is + /// opposite the normal. + NEGATIVE_SIDE = 0, + + /// \brief Positive side of the plane. This is the side that has the + /// normal vector. + POSITIVE_SIDE = 1, + + /// \brief On the plane. + NO_SIDE = 2, + + /// \brief On both sides of the plane. + BOTH_SIDE = 3 + }; + + public: Plane(); + + public: Plane(const Vector3 &_normal, T _offset = 0.0); + + public: Plane(const Vector3 &_normal, const Vector2 &_size, + T _offset); + public: Plane(const Plane &_plane); + + public: virtual ~Plane(); + + public: void Set(const Vector3 &_normal, T _offset); + + public: void Set(const Vector3 &_normal, const Vector2 &_size, + T _offset); + + public: T Distance(const Vector3 &_point) const; + + public: std::optional> Intersection( + const Vector3 &_point, + const Vector3 &_gradient, + const double &_tolerance = 1e-6) const; + + public: PlaneSide Side(const Vector3 &_point) const; + + public: PlaneSide Side(const math::AxisAlignedBox &_box) const; + + public: T Distance(const Vector3 &_origin, + const Vector3 &_dir) const; + + public: inline const Vector2 &Size() const; + + public: inline Vector2 &Size(); + + public: inline const Vector3 &Normal() const; + + public: inline Vector3 &Normal(); + + public: inline T Offset() const; + + private: Vector3 normal; + + private: Vector2 size; + + private: T d; + }; + + %template(Planed) Plane; + } +} diff --git a/src/python/Plane_TEST.py b/src/python/Plane_TEST.py new file mode 100644 index 000000000..c1e1a6d25 --- /dev/null +++ b/src/python/Plane_TEST.py @@ -0,0 +1,165 @@ +# 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 AxisAlignedBox, Planed, Vector2d, Vector3d + + +class TestPlane(unittest.TestCase): + + def test_plane_constructor(self): + plane = Planed(Vector3d(1, 0, 0), 0.1) + self.assertEqual(plane.normal(), Vector3d(1, 0, 0)) + self.assertAlmostEqual(plane.offset(), 0.1, 1e-6) + + planeCopy = Planed(plane) + self.assertEqual(plane.normal(), planeCopy.normal()) + self.assertEqual(plane.offset(), planeCopy.offset()) + self.assertEqual(plane.size(), planeCopy.size()) + + def test_plane_distance(self): + plane = Planed(Vector3d(0, 0, 1), 0.1) + self.assertAlmostEqual(plane.distance( + Vector3d(0, 0, 0), + Vector3d(0, 0, 1)), 0.1, delta=1e-6) + + self.assertAlmostEqual(plane.distance( + Vector3d(0, 0, 0.1), + Vector3d(0, 0, 1)), 0, delta=1e-6) + + self.assertAlmostEqual(plane.distance( + Vector3d(0, 0, 0.2), + Vector3d(0, 0, 1)), -0.1, delta=1e-6) + self.assertAlmostEqual(plane.distance( + Vector3d(0, 0, 0.1), + Vector3d(1, 0, 0)), 0, delta=1e-6) + + def test_plane(self): + plane = Planed() + self.assertEqual(plane.offset(), 0.0) + self.assertEqual(plane.normal(), Vector3d()) + self.assertEqual(plane.size(), Vector2d(0, 0)) + + plane = Planed(Vector3d(0, 0, 1), Vector2d(2, 3), 2.0) + self.assertEqual(plane.offset(), 2.0) + self.assertEqual(plane.normal(), Vector3d(0, 0, 1)) + self.assertEqual(plane.size(), Vector2d(2, 3)) + + self.assertEqual(-1, plane.distance( + Vector3d(0, 0, 1), + Vector3d(0, 0, -1))) + + plane.set(Vector3d(1, 0, 0), Vector2d(1, 1), 1.0) + self.assertEqual(plane.offset(), 1.0) + self.assertEqual(plane.normal(), Vector3d(1, 0, 0)) + self.assertEqual(plane.size(), Vector2d(1, 1)) + + plane = Planed(Vector3d(0, 1, 0), Vector2d(4, 4), 5.0) + self.assertEqual(plane.offset(), 5.0) + self.assertEqual(plane.normal(), Vector3d(0, 1, 0)) + self.assertEqual(plane.size(), Vector2d(4, 4)) + + def test_side_point(self): + plane = Planed(Vector3d(0, 0, 1), 1) + + # On the negative side of the plane (below the plane) + point = Vector3d(0, 0, 0) + self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) + + # Still on the negative side of the plane (below the plane) + point.set(1, 1, 0) + self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) + + # Above the plane (positive side) + point.set(1, 1, 2) + self.assertEqual(plane.side(point), Planed.POSITIVE_SIDE) + + # On the plane + point.set(0, 0, 1) + self.assertEqual(plane.side(point), Planed.NO_SIDE) + + # Change the plane, but the point is still on the negative side + plane.set(Vector3d(1, 0, 0), 4) + self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) + + # Point is now on the positive side + point.set(4.1, 0, 1) + self.assertEqual(plane.side(point), Planed.POSITIVE_SIDE) + + def test_side__axis_aligned_box(self): + plane = Planed(Vector3d(0, 0, 1), 1) + + # On the negative side of the plane (below the plane) + box = AxisAlignedBox(Vector3d(-.5, -.5, -.5), Vector3d(.5, .5, .5)) + self.assertEqual(plane.side(box), Planed.NEGATIVE_SIDE) + + # Still on the negative side of the plane (below the plane) + box = AxisAlignedBox(Vector3d(-10, -10, -10), Vector3d(.9, .9, .9)) + self.assertEqual(plane.side(box), Planed.NEGATIVE_SIDE) + + # Above the plane (positive side) + box = AxisAlignedBox(Vector3d(2, 2, 2), Vector3d(3, 3, 3)) + self.assertEqual(plane.side(box), Planed.POSITIVE_SIDE) + + # On both sides the plane + box = AxisAlignedBox(Vector3d(0, 0, 0), Vector3d(3, 3, 3)) + self.assertEqual(plane.side(box), Planed.BOTH_SIDE) + + def test_intersection(self): + plane = Planed(Vector3d(0.5, 0, 1), 1) + intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 1)) + self.assertTrue(intersect is not None) + self.assertAlmostEqual(intersect.dot(plane.normal()), plane.offset(), 1e-6) + + plane.set(Vector3d(1, 0, 0), 2) + intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 0)) + self.assertTrue(intersect is not None) + self.assertEqual(intersect, Vector3d(2, 0, 0)) + + intersect = plane.intersection(Vector3d(1, 1, 0), Vector3d(-1, -1, 0)) + self.assertTrue(intersect is not None) + self.assertEqual(intersect, Vector3d(2, 2, 0)) + + # Lines on plane + intersect = plane.intersection(Vector3d(2, 0, 0), Vector3d(0, 1, 0)) + self.assertTrue(intersect is None) + + intersect = plane.intersection(Vector3d(2, 0, 0), Vector3d(0, 0, 1)) + self.assertTrue(intersect is None) + + intersect = plane.intersection(Vector3d(2, 0, 0), Vector3d(0, 1, 1)) + self.assertTrue(intersect is None) + + # Lines parallel to plane + intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 0)) + self.assertTrue(intersect is None) + + intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(0, 0, 1)) + self.assertTrue(intersect is None) + + intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 1)) + self.assertTrue(intersect is None) + + # Bounded plane + planeBounded = Planed(Vector3d(0, 0, 1), Vector2d(0.5, 0.5), 0) + intersect1 = planeBounded.intersection(Vector3d(0, 0, 0), Vector3d(0, 0, 1)) + self.assertTrue(intersect1 is not None) + self.assertEqual(intersect1, Vector3d(0, 0, 0)) + intersect2 = planeBounded.intersection(Vector3d(20, 20, 0), Vector3d(0, 0, 1)) + self.assertFalse(intersect2 is not None) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/python.i b/src/python/python.i index bb1c1d73e..1dec3721e 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -31,6 +31,7 @@ %include Kmeans.i %include Vector3Stats.i %include AxisAlignedBox.i +%include Plane.i %include MassMatrix3.i %include Cylinder.i %include Sphere.i