Volume below a plane

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