Volume below a plane for spheres and boxes (#219)

Signed-off-by: Arjo Chakravarty <[email protected]>
Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
Co-authored-by: Steve Peters <[email protected]>

Author: 3 people

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.
     ///
@@ -128,6 +137,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
@@ -161,6 +192,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

@@ -229,6 +229,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
 {
@@ -119,6 +122,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/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

include/ignition/math/Vector3.hh

@@ -264,7 +264,7 @@ namespace ignition
         return n.Normalize();
       }
 
-      /// \brief Get distance to a line
+      /// \brief Get distance to an infinite line defined by 2 points.
       /// \param[in] _pt1 first point on the line
       /// \param[in] _pt2 second point on the line
       /// \return the minimum distance from this point to the line