diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index c478b506b..fa94b79fd 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -112,6 +112,7 @@ if (PYTHONLIBS_FOUND) RotationSpline_TEST SemanticVersion_TEST SignalStats_TEST + Sphere_TEST SphericalCoordinates_TEST Spline_TEST StopWatch_TEST diff --git a/src/python/Sphere.i b/src/python/Sphere.i new file mode 100644 index 000000000..c2fa4b1a4 --- /dev/null +++ b/src/python/Sphere.i @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2018 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 sphere +%{ +#include "ignition/math/Sphere.hh" +#include "ignition/math/MassMatrix3.hh" +#include "ignition/math/Material.hh" +#include "ignition/math/Quaternion.hh" +#include "ignition/math/Plane.hh" +%} + +%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 Sphere + { + public: Sphere() = default; + + public: explicit Sphere(const Precision _radius); + + public: Sphere(const Precision _radius, const ignition::math::Material &_mat); + + public: ~Sphere() = default; + + public: Precision Radius() const; + + public: void SetRadius(const Precision _radius); + + public: const ignition::math::Material &Material() const; + + public: void SetMaterial(const ignition::math::Material &_mat); + + public: bool MassMatrix(ignition::math::MassMatrix3 &_massMat) const; + + public: bool operator==(const Sphere &_sphere) const; + + public: bool operator!=(const Sphere &_sphere) const; + + public: Precision Volume() const; + + public: Precision VolumeBelow(const ignition::math::Plane &_plane) const; + + public: std::optional> + CenterOfVolumeBelow(const ignition::math::Plane &_plane) const; + + public: Precision DensityFromMass(const Precision _mass) const; + + public: bool SetDensityFromMass(const Precision _mass); + }; + %template(Sphered) Sphere; + } +} diff --git a/src/python/Sphere_TEST.py b/src/python/Sphere_TEST.py new file mode 100644 index 000000000..7c44d5de2 --- /dev/null +++ b/src/python/Sphere_TEST.py @@ -0,0 +1,202 @@ +# 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 + +import ignition +from ignition.math import IGN_PI, MassMatrix3d, Material, Planed, Sphered, Vector2d, Vector3d + + +class TestSphere(unittest.TestCase): + + def test_constructor(self): + # Default constructor + sphere = Sphered() + self.assertEqual(0.0, sphere.radius()) + self.assertEqual(Material(), sphere.material()) + + sphere2 = Sphered() + self.assertEqual(sphere, sphere2) + + # Radius constructor + sphere = Sphered(1.0) + self.assertEqual(1.0, sphere.radius()) + self.assertEqual(Material(), sphere.material()) + + sphere2 = Sphered(1.0) + self.assertEqual(sphere, sphere2) + + # Radius and mat + sphere = Sphered(1.0, Material(ignition.math.MaterialType_WOOD)) + self.assertEqual(1.0, sphere.radius()) + self.assertEqual(Material(ignition.math.MaterialType_WOOD), sphere.material()) + + sphere2 = Sphered(1.0, Material(ignition.math.MaterialType_WOOD)) + self.assertEqual(sphere, sphere2) + + def test_comparison(self): + wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) + + modified = wood + self.assertEqual(wood, modified) + + modified.set_radius(1.0) + wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) + self.assertNotEqual(wood, modified) + + modified = wood + wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) + self.assertEqual(wood, modified) + + modified.set_material(Material(ignition.math.MaterialType_PINE)) + self.assertNotEqual(wood, modified) + + def test_mutators(self): + sphere = Sphered() + self.assertEqual(0.0, sphere.radius()) + self.assertEqual(Material(), sphere.material()) + + sphere.set_radius(.123) + sphere.set_material(Material(ignition.math.MaterialType_PINE)) + + self.assertEqual(.123, sphere.radius()) + self.assertEqual(Material(ignition.math.MaterialType_PINE), sphere.material()) + + def test_volume_and_density(self): + mass = 1.0 + sphere = Sphered(0.001) + expectedVolume = (4.0/3.0) * IGN_PI * math.pow(0.001, 3) + self.assertEqual(expectedVolume, sphere.volume()) + + expectedDensity = mass / expectedVolume + self.assertEqual(expectedDensity, sphere.density_from_mass(mass)) + + # Bad density + sphere2 = Sphered() + self.assertGreater(0.0, sphere2.density_from_mass(mass)) + sphere2.set_radius(1.0) + self.assertGreater(0.0, sphere2.density_from_mass(0.0)) + self.assertFalse(sphere.set_density_from_mass(0.0)) + + def test_mass(self): + mass = 2.0 + r = 0.1 + sphere = Sphered(r) + self.assertTrue(sphere.set_density_from_mass(mass)) + + massMat = MassMatrix3d() + ixxIyyIzz = 0.4 * mass * r * r + + expectedMassMat = MassMatrix3d() + expectedMassMat.set_inertia_matrix(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz, + 0.0, 0.0, 0.0) + expectedMassMat.set_mass(mass) + + sphere.mass_matrix(massMat) + self.assertEqual(expectedMassMat, massMat) + self.assertEqual(expectedMassMat.mass(), massMat.mass()) + + def test_volume_below(self): + + r = 2 + sphere = Sphered(r) + + # Fully below + plane = Planed(Vector3d(0, 0, 1), Vector2d(4, 4), 2*r) + self.assertAlmostEqual(sphere.volume(), sphere.volume_below(plane), delta=1e-3) + + # Fully below (because plane is rotated down) + plane = Planed(Vector3d(0, 0, -1), Vector2d(4, 4), 2*r) + self.assertAlmostEqual(sphere.volume(), sphere.volume_below(plane), delta=1e-3) + + # Fully above + plane = Planed(Vector3d(0, 0, 1), Vector2d(4, 4), -2*r) + self.assertAlmostEqual(sphere.volume_below(plane), 0, 1e-3) + + # Hemisphere + plane = Planed(Vector3d(0, 0, 1), 0) + self.assertAlmostEqual(sphere.volume() / 2, sphere.volume_below(plane), delta=1e-3) + + # Vertical plane + plane = Planed(Vector3d(1, 0, 0), 0) + self.assertAlmostEqual(sphere.volume() / 2, sphere.volume_below(plane), delta=1e-3) + + # Expectations from https:#planetcalc.com/283/ + plane = Planed(Vector3d(0, 0, 1), 0.5) + self.assertAlmostEqual(22.90745, sphere.volume_below(plane), delta=1e-3) + + plane = Planed(Vector3d(0, 0, 1), -0.5) + self.assertAlmostEqual(10.60288, sphere.volume_below(plane), delta=1e-3) + + def test_center_of_volume_below(self): + r = 2 + sphere = Sphered(r) + + # Entire sphere below plane + plane = Planed(Vector3d(0, 0, 1), Vector2d(0, 0), 2 * r) + self.assertEqual(Vector3d(0, 0, 0), sphere.center_of_volume_below(plane)) + + # Entire sphere above plane + plane = Planed(Vector3d(0, 0, 1), Vector2d(0, 0), -2 * r) + self.assertFalse(sphere.center_of_volume_below(plane) is not None) + + # Halfway point is a good spot to test. Center of Volume for a hemisphere + # is 3/8 its radius. In this case the point should fall below the y-plane + plane = Planed(Vector3d(0, 1, 0), Vector2d(0, 0), 0) + self.assertEqual(Vector3d(0, -0.75, 0), sphere.center_of_volume_below(plane)) + + # Halfway point is a good spot to test. Center of Volume for a hemisphere + # is 3/8 its radius. In this case the point should fall above the y-plane + # thanks to flipped normal + plane = Planed(Vector3d(0, -1, 0), Vector2d(0, 0), 0) + self.assertEqual(Vector3d(0, 0.75, 0), sphere.center_of_volume_below(plane)) + + # Handcalculated value. + # Plane at y = 0.8 pointing upwards + # Cap height is 2.8 + # Centroid should be at 0.3375. However, keep in mind this assumes an + # inverted cap. + # Center of volume below should be at -0.3375 + plane = Planed(Vector3d(0, 1, 0), Vector2d(0, 0), 0.4 * r) + self.assertEqual(Vector3d(0, -0.3375, 0), sphere.center_of_volume_below(plane)) + + # Handcalculated value. + plane = Planed(Vector3d(0, 1, 0), Vector2d(0, 0), -0.4 * r) + + self.assertEqual(Vector3d(0, -1.225, 0), sphere.center_of_volume_below(plane)) + + # Handcalculated value. + plane = Planed(Vector3d(0, -1, 0), Vector2d(0, 0), -0.4 * r) + + self.assertEqual(Vector3d(0, 1.225, 0), sphere.center_of_volume_below(plane)) + + # Handcalculated value. + plane = Planed(Vector3d(0, -1, 0), Vector2d(0, 0), 0.4 * r) + + self.assertEqual(Vector3d(0, 0.3375, 0), sphere.center_of_volume_below(plane)) + + # Weighted sums of the center of volume results in (0,0,0). + plane1 = Planed(Vector3d(0, 0, 1), -0.5) + # Flip plane1 axis + plane2 = Planed(Vector3d(0, 0, -1), -0.5) + self.assertEqual( + sphere.center_of_volume_below(plane1) * sphere.volume_below(plane1) + + sphere.center_of_volume_below(plane2) * + sphere.volume_below(plane2), + Vector3d(0, 0, 0)) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/python.i b/src/python/python.i index 71fbcc44f..1dec3721e 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -34,3 +34,4 @@ %include Plane.i %include MassMatrix3.i %include Cylinder.i +%include Sphere.i