Merge branch 'ign-math6' into ahcorde/python/plane

src/python/CMakeLists.txt

@@ -90,6 +90,7 @@ if (PYTHONLIBS_FOUND)
       Angle_TEST
       AxisAlignedBox_TEST
       Color_TEST
+      Cylinder_TEST
       Filter_TEST
       GaussMarkovProcess_TEST
       Helpers_TEST
@@ -111,6 +112,7 @@ if (PYTHONLIBS_FOUND)
       RotationSpline_TEST
       SemanticVersion_TEST
       SignalStats_TEST
+      SphericalCoordinates_TEST
       Spline_TEST
       StopWatch_TEST
       Temperature_TEST

src/python/Cylinder.i

@@ -0,0 +1,78 @@
+/*
+ * 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 cylinder
+%{
+#include <ignition/math/Cylinder.hh>
+#include <ignition/math/config.hh>
+#include <ignition/math/MassMatrix3.hh>
+#include <ignition/math/Material.hh>
+#include <ignition/math/Quaternion.hh>
+%}
+
+namespace ignition
+{
+  namespace math
+  {
+    template<typename Precision>
+    class Cylinder
+    {
+      %rename("%(undercase)s", %$isfunction, notregexmatch$name="^[A-Z]*$") "";
+
+      public: Cylinder() = default;
+
+      public: Cylinder(const Precision _length, const Precision _radius,
+                  const ignition::math::Quaternion<Precision> &_rotOffset =
+                  ignition::math::Quaternion<Precision>::Identity);
+
+      public: Cylinder(const Precision _length, const Precision _radius,
+                  const ignition::math::Material &_mat,
+                  const ignition::math::Quaternion<Precision> &_rotOffset =
+                  ignition::math::Quaternion<Precision>::Identity);
+
+      public: ~Cylinder() = default;
+
+      public: Precision Radius() const;
+
+      public: void SetRadius(const Precision _radius);
+
+      public: Precision Length() const;
+
+      public: void SetLength(const Precision _length);
+
+      public: ignition::math::Quaternion<Precision> RotationalOffset() const;
+
+      public: void SetRotationalOffset(
+                  const ignition::math::Quaternion<Precision> &_rotOffset);
+
+      public: const ignition::math::Material &Mat() const;
+
+      public: void SetMat(const ignition::math::Material &_mat);
+
+      public: bool MassMatrix(ignition::math::MassMatrix3<double> &_massMat) const;
+
+      public: bool operator==(const Cylinder &_cylinder) const;
+
+      public: Precision Volume() const;
+
+      public: Precision DensityFromMass(const Precision _mass) const;
+
+      public: bool SetDensityFromMass(const Precision _mass);
+    };
+    %template(Cylinderd) Cylinder<double>;
+  }
+}

src/python/Cylinder_TEST.py

@@ -0,0 +1,119 @@
+# 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 Cylinderd, IGN_PI, MassMatrix3d, Material, Quaterniond
+
+
+class TestCylinder(unittest.TestCase):
+
+    def test_constructor(self):
+        # Default constructor
+        cylinder = Cylinderd()
+        self.assertEqual(0.0, cylinder.length())
+        self.assertEqual(0.0, cylinder.radius())
+        self.assertEqual(Quaterniond.IDENTITY, cylinder.rotational_offset())
+        self.assertEqual(Material(), cylinder.mat())
+
+        cylinder2 = Cylinderd()
+        self.assertEqual(cylinder, cylinder2)
+
+        # Length and radius constructor
+        cylinder = Cylinderd(1.0, 2.0)
+        self.assertEqual(1.0, cylinder.length())
+        self.assertEqual(2.0, cylinder.radius())
+        self.assertEqual(Quaterniond.IDENTITY, cylinder.rotational_offset())
+        self.assertEqual(Material(), cylinder.mat())
+
+        cylinder2 = Cylinderd(1.0, 2.0)
+        self.assertEqual(cylinder, cylinder2)
+
+        # Length, radius, and rot constructor
+        cylinder = Cylinderd(1.0, 2.0, Quaterniond(0.1, 0.2, 0.3))
+        self.assertEqual(1.0, cylinder.length())
+        self.assertEqual(2.0, cylinder.radius())
+        self.assertEqual(Quaterniond(0.1, 0.2, 0.3),
+                         cylinder.rotational_offset())
+        self.assertEqual(Material(), cylinder.mat())
+
+        cylinder2 = Cylinderd(1.0, 2.0, Quaterniond(0.1, 0.2, 0.3))
+        self.assertEqual(cylinder, cylinder2)
+
+        # Length, radius, mat and rot constructor
+        cylinder = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType_WOOD),
+                             Quaterniond(0.1, 0.2, 0.3))
+        self.assertEqual(1.0, cylinder.length())
+        self.assertEqual(2.0, cylinder.radius())
+        self.assertEqual(Quaterniond(0.1, 0.2, 0.3), cylinder.rotational_offset())
+        self.assertEqual(Material(ignition.math.MaterialType_WOOD), cylinder.mat())
+
+        cylinder2 = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType_WOOD),
+                              Quaterniond(0.1, 0.2, 0.3))
+        self.assertEqual(cylinder, cylinder2)
+
+    def test_mutators(self):
+        cylinder = Cylinderd()
+        self.assertEqual(0.0, cylinder.length())
+        self.assertEqual(0.0, cylinder.radius())
+        self.assertEqual(Quaterniond.IDENTITY, cylinder.rotational_offset())
+        self.assertEqual(Material(), cylinder.mat())
+
+        cylinder.set_length(100.1)
+        cylinder.set_radius(.123)
+        cylinder.set_rotational_offset(Quaterniond(1.2, 2.3, 3.4))
+        cylinder.set_mat(Material(ignition.math.MaterialType_PINE))
+
+        self.assertEqual(100.1, cylinder.length())
+        self.assertEqual(.123, cylinder.radius())
+        self.assertEqual(Quaterniond(1.2, 2.3, 3.4), cylinder.rotational_offset())
+        self.assertEqual(Material(ignition.math.MaterialType_PINE), cylinder.mat())
+
+    def test_volume_and_density(self):
+        mass = 1.0
+        cylinder = Cylinderd(1.0, 0.001)
+        expectedVolume = (IGN_PI * math.pow(0.001, 2) * 1.0)
+        self.assertEqual(expectedVolume, cylinder.volume())
+
+        expectedDensity = mass / expectedVolume
+        self.assertEqual(expectedDensity, cylinder.density_from_mass(mass))
+
+        # Bad density
+        cylinder2 = Cylinderd()
+        self.assertGreater(0.0, cylinder2.density_from_mass(mass))
+
+    def test_mass(self):
+        mass = 2.0
+        length = 2.0
+        r = 0.1
+        cylinder = Cylinderd(length, r)
+        cylinder.set_density_from_mass(mass)
+
+        massMat = MassMatrix3d()
+        ixxIyy = (1/12.0) * mass * (3*r*r + length*length)
+        izz = 0.5 * mass * r * r
+
+        expectedMassMat = MassMatrix3d()
+        expectedMassMat.set_inertia_matrix(ixxIyy, ixxIyy, izz, 0.0, 0.0, 0.0)
+        expectedMassMat.set_mass(mass)
+
+        cylinder.mass_matrix(massMat)
+        self.assertEqual(expectedMassMat, massMat)
+        self.assertEqual(expectedMassMat.mass(), massMat.mass())
+
+
+if __name__ == '__main__':
+    unittest.main()

src/python/SphericalCoordinates.i

@@ -0,0 +1,134 @@
+/*
+ * 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 sphericalcoordinates
+%{
+#include <memory>
+#include <string>
+
+#include <ignition/math/SphericalCoordinates.hh>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/config.hh>
+%}
+
+namespace ignition
+{
+  namespace math
+  {
+    class SphericalCoordinates
+    {
+      public: enum SurfaceType
+              {
+                /// \brief Model of reference ellipsoid for earth, based on
+                /// WGS 84 standard. see wikipedia: World_Geodetic_System
+                EARTH_WGS84 = 1
+              };
+
+      public: enum CoordinateType
+              {
+                /// \brief Latitude, Longitude and Altitude by SurfaceType
+                SPHERICAL = 1,
+
+                /// \brief Earth centered, earth fixed Cartesian
+                ECEF = 2,
+
+                /// \brief Local tangent plane (East, North, Up)
+                GLOBAL = 3,
+
+                /// \brief Heading-adjusted tangent plane (X, Y, Z)
+                /// This has kept a bug for backwards compatibility, use
+                /// LOCAL2 for the correct behaviour.
+                LOCAL = 4,
+
+                /// \brief Heading-adjusted tangent plane (X, Y, Z)
+                LOCAL2 = 5
+              };
+
+      public: SphericalCoordinates();
+
+      public: explicit SphericalCoordinates(const SurfaceType _type);
+
+      public: SphericalCoordinates(const SurfaceType _type,
+                                   const ignition::math::Angle &_latitude,
+                                   const ignition::math::Angle &_longitude,
+                                   const double _elevation,
+                                   const ignition::math::Angle &_heading);
+
+      public: SphericalCoordinates(const SphericalCoordinates &_sc);
+
+      /// \brief Destructor.
+      public: ~SphericalCoordinates();
+
+      public: ignition::math::Vector3<double> SphericalFromLocalPosition(
+                  const ignition::math::Vector3<double> &_xyz) const;
+
+      public: ignition::math::Vector3<double> GlobalFromLocalVelocity(
+                  const ignition::math::Vector3<double> &_xyz) const;
+
+      public: static SurfaceType Convert(const std::string &_str);
+
+      public: static std::string Convert(SurfaceType _type);
+
+      public: static double Distance(const ignition::math::Angle &_latA,
+                                     const ignition::math::Angle &_lonA,
+                                     const ignition::math::Angle &_latB,
+                                     const ignition::math::Angle &_lonB);
+
+      public: SurfaceType Surface() const;
+
+      public: ignition::math::Angle LatitudeReference() const;
+
+      public: ignition::math::Angle LongitudeReference() const;
+
+      public: double ElevationReference() const;
+
+      public: ignition::math::Angle HeadingOffset() const;
+
+      public: void SetSurface(const SurfaceType &_type);
+
+      public: void SetLatitudeReference(const ignition::math::Angle &_angle);
+
+      public: void SetLongitudeReference(const ignition::math::Angle &_angle);
+
+      public: void SetElevationReference(const double _elevation);
+
+      public: void SetHeadingOffset(const ignition::math::Angle &_angle);
+
+      public: ignition::math::Vector3<double> LocalFromSphericalPosition(
+                  const ignition::math::Vector3<double> &_latLonEle) const;
+
+      public: ignition::math::Vector3<double> LocalFromGlobalVelocity(
+                  const ignition::math::Vector3<double> &_xyz) const;
+
+      public: void UpdateTransformationMatrix();
+
+      public: ignition::math::Vector3<double>
+              PositionTransform(const ignition::math::Vector3<double> &_pos,
+                  const CoordinateType &_in, const CoordinateType &_out) const;
+
+      /// \return Transformed velocity vector
+      public: ignition::math::Vector3<double> VelocityTransform(
+                  const ignition::math::Vector3<double> &_vel,
+                  const CoordinateType &_in, const CoordinateType &_out) const;
+
+      public: bool operator==(const SphericalCoordinates &_sc) const;
+
+      public: bool operator!=(const SphericalCoordinates &_sc) const;
+    };
+  }
+}