Merge 6.9.0 to main

CMakeLists.txt

@@ -69,6 +69,7 @@ if (SWIG_FOUND)
 
   ########################################
   # Include python
+  find_package(PythonInterp 3 REQUIRED) # change to Python3 when Bionic is EOL
   find_package(PythonLibs QUIET)
   if (NOT PYTHONLIBS_FOUND)
     message (STATUS "Searching for Python - not found.")

Changelog.md

@@ -46,6 +46,71 @@
 
 ### Ignition Math 6.x.x
 
+## Ignition Math 6.9.0 (2021-09-28)
+
+1. Volume below a plane for spheres and boxes
+    * [Pull request #219](https://github.com/ignitionrobotics/ign-math/pull/219)
+
+1. 🌐 Spherical coordinates: bug fix, docs and sanity checks
+    * [Pull request #235](https://github.com/ignitionrobotics/ign-math/pull/235)
+
+1. Add Vector(2|3|4)<T>::NaN to easily create invalid vectors
+    * [Pull request #222](https://github.com/ignitionrobotics/ign-math/pull/222)
+
+1. Add options to install python/ruby in system standard paths
+    * [Pull request #236](https://github.com/ignitionrobotics/ign-math/pull/236)
+
+1. Add eigen utils to convert mesh 3d vertices to oriented box
+    * [Pull request #224](https://github.com/ignitionrobotics/ign-math/pull/224)
+
+1. Python interface
+
+    1. Adds python interface to RollingMean, Color and Spline
+        * [Pull request #218](https://github.com/ignitionrobotics/ign-math/pull/218)
+
+    1. Adds python interface for Kmeans and Vector3Stats
+        * [Pull request #232](https://github.com/ignitionrobotics/ign-math/pull/232)
+
+    1. Adds python interface to PID and SemanticVersion.
+        * [Pull request #229](https://github.com/ignitionrobotics/ign-math/pull/229)
+
+    1. Adds python interface to triangle.
+        * [Pull request #231](https://github.com/ignitionrobotics/ign-math/pull/231)
+
+    1. Adds Line2, Line3, SignalStats, Temperature python interface
+        * [Pull request #220](https://github.com/ignitionrobotics/ign-math/pull/220)
+
+    1. Python interface: Renames methods to match PEP8 style
+        * [Pull request #226](https://github.com/ignitionrobotics/ign-math/pull/226)
+
+    1. Adds python interface to Filter, MovingWindowFilter, RotationSpline.
+        * [Pull request #230](https://github.com/ignitionrobotics/ign-math/pull/230)
+
+    1. Adds python interface to Quaternion, Pose3, Matrix3 and Matrix4
+        * [Pull request #221](https://github.com/ignitionrobotics/ign-math/pull/221)
+
+    1. Basic setup for Python interface using SWIG
+        * [Pull request #216](https://github.com/ignitionrobotics/ign-math/pull/216)
+        * [Pull request #223](https://github.com/ignitionrobotics/ign-math/pull/223)
+        * [Pull request #208](https://github.com/ignitionrobotics/ign-math/pull/208)
+        * [Pull request #239](https://github.com/ignitionrobotics/ign-math/pull/239)
+
+1. 👩‍🌾 Don't use std::pow with integers in Vectors and handle sqrt
+    * [Pull request #207](https://github.com/ignitionrobotics/ign-math/pull/207)
+
+1. Relax expectations about zero in SpeedLimiter_TEST to make ARM happy
+    * [Pull request #204](https://github.com/ignitionrobotics/ign-math/pull/204)
+
+1. Infrastructure
+    * [Pull request #242](https://github.com/ignitionrobotics/ign-math/pull/242)
+    * [Pull request #217](https://github.com/ignitionrobotics/ign-math/pull/217)
+    * [Pull request #211](https://github.com/ignitionrobotics/ign-math/pull/211)
+    * [Pull request #209](https://github.com/ignitionrobotics/ign-math/pull/209)
+    * [Pull request #227](https://github.com/ignitionrobotics/ign-math/pull/227)
+    * [Pull request #225](https://github.com/ignitionrobotics/ign-math/pull/225)
+    * [Pull request #252](https://github.com/ignitionrobotics/ign-math/pull/252)
+    * [Pull request #253](https://github.com/ignitionrobotics/ign-math/pull/253)
+
 ## Ignition Math 6.8.0 (2021-03-30)
 
 1. Add speed limiter class

Migration.md

@@ -58,6 +58,13 @@ release will remove the deprecated code.
 1. The out stream operator is guaranteed to return always plain 0 and not to
    return -0, 0.0 or other instances of zero value.
 
+## Ignition Math 6.8 to 6.9
+
+1. **SphericalCoordinates**: A bug related to the LOCAL frame was fixed. To
+   preserve behaviour, the `LOCAL` frame was left with the bug, and a new
+   `LOCAL2` frame was introduced, which can be used to get the correct
+   calculations.
+
 ## Ignition Math 4.X to 5.X
 
 ### Additions

README.md

@@ -64,6 +64,8 @@ ign-math
 ├── include/ignition/math    Header files.
 ├── src                      Source files and unit tests.
 │   └── graph                Source files for the graph classes.
+│   └── python               SWIG Python interfaces.
+│   └── ruby                 SWIG Ruby interfaces.
 ├── eigen3                   Files for Eigen component.
 ├── test
 │    ├── integration         Integration tests.

eigen3/BUILD.bazel

@@ -10,9 +10,9 @@ package(
 
 licenses(["notice"])
 
-public_headers = [
-    "include/ignition/math/eigen3/Conversions.hh",
-]
+public_headers = glob([
+    "include/ignition/math/eigen3/*.hh",
+])
 
 cc_library(
     name = "eigen3",

include/ignition/math/Box.hh

@@ -20,15 +20,24 @@
 #include <ignition/math/config.hh>
 #include <ignition/math/MassMatrix3.hh>
 #include <ignition/math/Material.hh>
+#include <ignition/math/Plane.hh>
 #include <ignition/math/Vector3.hh>
 
+#include "ignition/math/detail/WellOrderedVector.hh"
+
+#include <set>
+
 namespace ignition
 {
   namespace math
   {
     // Inline bracket to help doxygen filtering.
     inline namespace IGNITION_MATH_VERSION_NAMESPACE {
-    //
+    /// \brief This is the type used for deduplicating and returning the set of
+    /// intersections.
+    template<typename T>
+    using IntersectionPoints = std::set<Vector3<T>, WellOrderedVectors<T>>;
+
     /// \class Box Box.hh ignition/math/Box.hh
     /// \brief A representation of a box. All units are in meters.
     ///
@@ -132,6 +141,28 @@ namespace ignition
       /// \return Volume of the box in m^3.
       public: Precision Volume() const;
 
+      /// \brief Get the volume of the box below a plane.
+      /// \param[in] _plane The plane which cuts the box, expressed in the box's
+      /// frame.
+      /// \return Volume below the plane in m^3.
+      public: Precision VolumeBelow(const Plane<Precision> &_plane) const;
+
+      /// \brief Center of volume below the plane. This is useful when
+      /// calculating where buoyancy should be applied, for example.
+      /// \param[in] _plane The plane which cuts the box, expressed in the box's
+      /// frame.
+      /// \return Center of volume, in box's frame.
+      public: std::optional<Vector3<Precision>>
+        CenterOfVolumeBelow(const Plane<Precision> &_plane) const;
+
+      /// \brief All the vertices which are on or below the plane.
+      /// \param[in] _plane The plane which cuts the box, expressed in the box's
+      /// frame.
+      /// \return Box vertices which are below the plane, expressed in the box's
+      /// frame.
+      public: IntersectionPoints<Precision>
+        VerticesBelow(const Plane<Precision> &_plane) const;
+
       /// \brief Compute the box's density given a mass value. The
       /// box is assumed to be solid with uniform density. This
       /// function requires the box's size to be set to
@@ -165,6 +196,14 @@ namespace ignition
       /// could be due to an invalid size (<=0) or density (<=0).
       public: bool MassMatrix(MassMatrix3<Precision> &_massMat) const;
 
+      /// \brief Get intersection between a plane and the box's edges.
+      /// Edges contained on the plane are ignored.
+      /// \param[in] _plane The plane against which we are testing intersection.
+      /// \returns A list of points along the edges of the box where the
+      /// intersection occurs.
+      public: IntersectionPoints<Precision> Intersections(
+        const Plane<Precision> &_plane) const;
+
       /// \brief Size of the box.
       private: Vector3<Precision> size = Vector3<Precision>::Zero;
 

include/ignition/math/Line3.hh

@@ -225,6 +225,36 @@ namespace ignition
         return true;
       }
 
+      /// \brief Calculate shortest distance between line and point
+      /// \param[in] _pt Point which we are measuring distance to.
+      /// \returns Distance from point to line.
+      public: T Distance(const Vector3<T> &_pt)
+      {
+        auto line = this->pts[1] - this->pts[0];
+        auto ptTo0 = _pt - this->pts[0];
+        auto ptTo1 = _pt - this->pts[1];
+
+        // Point is projected beyond pt0 or the line has length 0
+        if (ptTo0.Dot(line) <= 0.0)
+        {
+          return ptTo0.Length();
+        }
+
+        // Point is projected beyond pt1
+        if (ptTo1.Dot(line) >= 0.0)
+        {
+          return ptTo1.Length();
+        }
+
+        // Distance to point projected onto line
+        // line.Length() will have to be > 0 at this point otherwise it would
+        // return at line 244.
+        auto d = ptTo0.Cross(line);
+        auto lineLength = line.Length();
+        assert(lineLength > 0);
+        return d.Length() / lineLength;
+      }
+
       /// \brief Check if this line intersects the given line segment.
       /// \param[in] _line The line to check for intersection.
       /// \param[in] _epsilon The error bounds within which the intersection

include/ignition/math/Plane.hh

@@ -21,6 +21,9 @@
 #include <ignition/math/Vector2.hh>
 #include <ignition/math/Vector3.hh>
 #include <ignition/math/config.hh>
+#include <ignition/math/Line2.hh>
+#include <ignition/math/Quaternion.hh>
+#include <optional>
 
 namespace ignition
 {
@@ -117,6 +120,52 @@ namespace ignition
         return this->normal.Dot(_point) - this->d;
       }
 
+      /// \brief Get the intersection of an infinite line with the plane,
+      /// given the line's gradient and a point in parametrized space.
+      /// \param[in] _point A point that lies on the line.
+      /// \param[in] _gradient The gradient of the line.
+      /// \param[in] _tolerance The tolerance for determining a line is
+      /// parallel to the plane. Optional, default=10^-16
+      /// \return The point of intersection. std::nullopt if the line is
+      /// parallel to the plane (including lines on the plane).
+      public: std::optional<Vector3<T>> Intersection(
+        const Vector3<T> &_point,
+        const Vector3<T> &_gradient,
+        const double &_tolerance = 1e-6) const
+      {
+        if (std::abs(this->Normal().Dot(_gradient)) < _tolerance)
+        {
+          return std::nullopt;
+        }
+        auto constant = this->Offset() - this->Normal().Dot(_point);
+        auto param = constant / this->Normal().Dot(_gradient);
+        auto intersection = _point + _gradient*param;
+
+        if (this->Size() == Vector2<T>(0, 0))
+          return intersection;
+
+        // Check if the point is within the size bounds
+        // To do this we create a Quaternion using Angle, Axis constructor and
+        // rotate the Y and X axis the same amount as the normal.
+        auto dotProduct = Vector3<T>::UnitZ.Dot(this->Normal());
+        auto angle = acos(dotProduct / this->Normal().Length());
+        auto axis = Vector3<T>::UnitZ.Cross(this->Normal().Normalized());
+        Quaternion<T> rotation(axis, angle);
+
+        Vector3<T> rotatedXAxis = rotation * Vector3<T>::UnitX;
+        Vector3<T> rotatedYAxis = rotation * Vector3<T>::UnitY;
+
+        auto xBasis = rotatedXAxis.Dot(intersection);
+        auto yBasis = rotatedYAxis.Dot(intersection);
+
+        if (std::abs(xBasis) < this->Size().X() / 2 &&
+            std::abs(yBasis) < this->Size().Y() / 2)
+        {
+          return intersection;
+        }
+        return std::nullopt;
+      }
+
       /// \brief The side of the plane a point is on.
       /// \param[in] _point The 3D point to check.
       /// \return Plane::NEGATIVE_SIDE if the distance from the point to the

include/ignition/math/Pose3.hh

@@ -366,7 +366,7 @@ namespace ignition
       {
         // set the position to zero
         this->p.Set();
-        this->q.Set(1, 0, 0, 0);
+        this->q = Quaternion<T>::Identity;
       }
 
       /// \brief Rotate the vector part of a pose about the origin.

include/ignition/math/Sphere.hh

@@ -20,6 +20,7 @@
 #include "ignition/math/MassMatrix3.hh"
 #include "ignition/math/Material.hh"
 #include "ignition/math/Quaternion.hh"
+#include "ignition/math/Plane.hh"
 
 namespace ignition
 {
@@ -91,6 +92,23 @@ namespace ignition
       /// \return Volume of the sphere in m^3.
       public: Precision Volume() const;
 
+      /// \brief Get the volume of sphere below a given plane in m^3.
+      /// It is assumed that the center of the sphere is on the origin
+      /// \param[in] _plane The plane which slices this sphere, expressed
+      /// in the sphere's reference frame.
+      /// \return Volume below the sphere in m^3.
+      public: Precision VolumeBelow(const Plane<Precision> &_plane) const;
+
+      /// \brief Center of volume below the plane. This is useful for example
+      /// when calculating where buoyancy should be applied.
+      /// \param[in] _plane The plane which slices this sphere, expressed
+      /// in the sphere's reference frame.
+      /// \return The center of volume if there is anything under the plane,
+      /// otherwise return a std::nullopt. Expressed in the sphere's reference
+      /// frame.
+      public: std::optional<Vector3<Precision>>
+        CenterOfVolumeBelow(const Plane<Precision> &_plane) const;
+
       /// \brief Compute the sphere's density given a mass value. The
       /// sphere is assumed to be solid with uniform density. This
       /// function requires the sphere's radius to be set to a