diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh
index 57670f4f7..09a55ec59 100644
--- a/include/ignition/math/Box.hh
+++ b/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;
 
diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh
index f8d73b53f..e927817b8 100644
--- a/include/ignition/math/Line3.hh
+++ b/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
diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh
index b7ae61a7a..4994f8128 100644
--- a/include/ignition/math/Plane.hh
+++ b/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
diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh
index d9bfe89aa..537bd109f 100644
--- a/include/ignition/math/Sphere.hh
+++ b/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
diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh
index af1dc71a0..6a3b5b1e9 100644
--- a/include/ignition/math/Vector3.hh
+++ b/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
diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh
index de1a521da..2ce5f0f9b 100644
--- a/include/ignition/math/detail/Box.hh
+++ b/include/ignition/math/detail/Box.hh
@@ -16,6 +16,14 @@
 */
 #ifndef IGNITION_MATH_DETAIL_BOX_HH_
 #define IGNITION_MATH_DETAIL_BOX_HH_
+
+#include "ignition/math/Triangle3.hh"
+
+#include <algorithm>
+#include <set>
+#include <utility>
+#include <vector>
+
 namespace ignition
 {
 namespace math
@@ -113,6 +121,168 @@ T Box<T>::Volume() const
   return this->size.X() * this->size.Y() * this->size.Z();
 }
 
+//////////////////////////////////////////////////
+/// \brief Given a *convex* polygon described by the verices in a given plane,
+/// compute the list of triangles which form this polygon.
+/// \param[in] _plane The plane in which the vertices exist.
+/// \param[in] _vertices The vertices of the polygon.
+/// \return A vector of triangles and their sign, or an empty vector
+/// if _vertices in the _plane are less than 3. The sign will be +1 if the
+/// triangle is outward facing, -1 otherwise.
+template <typename T>
+std::vector<std::pair<Triangle3<T>, T>> TrianglesInPlane(
+    const Plane<T> &_plane, IntersectionPoints<T> &_vertices)
+{
+  std::vector<std::pair<Triangle3<T>, T>> triangles;
+  std::vector<Vector3<T>> pointsInPlane;
+
+  Vector3<T> centroid;
+  for (const auto &pt : _vertices)
+  {
+    if (_plane.Side(pt) == Plane<T>::NO_SIDE)
+    {
+      pointsInPlane.push_back(pt);
+      centroid += pt;
+    }
+  }
+  centroid /= T(pointsInPlane.size());
+
+  if (pointsInPlane.size() < 3)
+    return {};
+
+  // Choose a basis in the plane of the triangle
+  auto axis1 = (pointsInPlane[0] - centroid).Normalize();
+  auto axis2 = axis1.Cross(_plane.Normal()).Normalize();
+
+  // Since the polygon is always convex, we can try to create a fan of triangles
+  // by sorting the points by their angle in the plane basis.
+  std::sort(pointsInPlane.begin(), pointsInPlane.end(),
+    [centroid, axis1, axis2] (const Vector3<T> &_a, const Vector3<T> &_b)
+    {
+      auto aDisplacement = _a - centroid;
+      auto bDisplacement = _b - centroid;
+
+      // project line onto the new basis vectors
+      // The axis length will never be zero as we have three different points
+      // and the centroid will be different.
+      auto aX = axis1.Dot(aDisplacement) / axis1.Length();
+      auto aY = axis2.Dot(aDisplacement) / axis2.Length();
+
+      auto bX = axis1.Dot(bDisplacement) / axis1.Length();
+      auto bY = axis2.Dot(bDisplacement) / axis2.Length();
+
+      return atan2(aY, aX) < atan2(bY, bX);
+    });
+  for (std::size_t i = 0; i < pointsInPlane.size(); ++i)
+  {
+    triangles.emplace_back(
+      Triangle3<T>(pointsInPlane[i],
+        pointsInPlane[(i + 1) % pointsInPlane.size()], centroid),
+      (_plane.Side({0, 0, 0}) == Plane<T>::POSITIVE_SIDE) ? -1 : 1);
+  }
+
+  return triangles;
+}
+
+/////////////////////////////////////////////////
+template<typename T>
+T Box<T>::VolumeBelow(const Plane<T> &_plane) const
+{
+  auto verticesBelow = this->VerticesBelow(_plane);
+  if (verticesBelow.empty())
+    return 0;
+
+  auto intersections = this->Intersections(_plane);
+  // TODO(arjo): investigate the use of _epsilon tolerance as this method
+  // implicitly uses Vector3<T>::operator==()
+  verticesBelow.merge(intersections);
+
+  // Reconstruct the cut-box as a triangle mesh by attempting to fit planes.
+  std::vector<std::pair<Triangle3<T>, T>> triangles;
+
+  std::vector<Plane<T>> planes
+  {
+    Plane<T>{Vector3<T>{0, 0, 1}, this->Size().Z()/2},
+    Plane<T>{Vector3<T>{0, 0, -1}, this->Size().Z()/2},
+    Plane<T>{Vector3<T>{1, 0, 0}, this->Size().X()/2},
+    Plane<T>{Vector3<T>{-1, 0, 0}, this->Size().X()/2},
+    Plane<T>{Vector3<T>{0, 1, 0}, this->Size().Y()/2},
+    Plane<T>{Vector3<T>{0, -1, 0}, this->Size().Y()/2},
+    _plane
+  };
+
+  for (const auto &p : planes)
+  {
+    auto newTriangles = TrianglesInPlane(p, verticesBelow);
+    triangles.insert(triangles.end(),
+      newTriangles.begin(),
+      newTriangles.end());
+  }
+
+  // Calculate the volume of the triangles
+  // https://n-e-r-v-o-u-s.com/blog/?p=4415
+  T volume = 0;
+  for (const auto &triangle : triangles)
+  {
+    auto crossProduct = (triangle.first[2]).Cross(triangle.first[1]);
+    auto meshVolume = std::abs(crossProduct.Dot(triangle.first[0]));
+    volume += triangle.second * meshVolume;
+  }
+
+  return std::abs(volume)/6;
+}
+
+/////////////////////////////////////////////////
+template<typename T>
+std::optional<Vector3<T>>
+  Box<T>::CenterOfVolumeBelow(const Plane<T> &_plane) const
+{
+  auto verticesBelow = this->VerticesBelow(_plane);
+  if (verticesBelow.empty())
+    return std::nullopt;
+
+  auto intersections = this->Intersections(_plane);
+  verticesBelow.merge(intersections);
+
+  Vector3<T> centroid;
+  for (const auto &v : verticesBelow)
+  {
+    centroid += v;
+  }
+
+  return centroid / static_cast<T>(verticesBelow.size());
+}
+
+/////////////////////////////////////////////////
+template<typename T>
+IntersectionPoints<T> Box<T>::VerticesBelow(const Plane<T> &_plane) const
+{
+  // Get coordinates of all vertice of box
+  // TODO(arjo): Cache this for performance
+  IntersectionPoints<T> vertices
+  {
+    Vector3<T>{this->size.X()/2, this->size.Y()/2, this->size.Z()/2},
+    Vector3<T>{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2},
+    Vector3<T>{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2},
+    Vector3<T>{-this->size.X()/2, -this->size.Y()/2, this->size.Z()/2},
+    Vector3<T>{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2},
+    Vector3<T>{-this->size.X()/2, this->size.Y()/2, -this->size.Z()/2},
+    Vector3<T>{this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2},
+    Vector3<T>{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}
+  };
+
+  IntersectionPoints<T> verticesBelow;
+  for (const auto &v : vertices)
+  {
+    if (_plane.Distance(v) <= 0)
+    {
+      verticesBelow.insert(v);
+    }
+  }
+
+  return verticesBelow;
+}
+
 /////////////////////////////////////////////////
 template<typename T>
 T Box<T>::DensityFromMass(const T _mass) const
@@ -139,6 +309,55 @@ bool Box<T>::MassMatrix(MassMatrix3<T> &_massMat) const
 {
   return _massMat.SetFromBox(this->material, this->size);
 }
+
+
+//////////////////////////////////////////////////
+template<typename T>
+IntersectionPoints<T> Box<T>::Intersections(
+        const Plane<T> &_plane) const
+{
+  IntersectionPoints<T> intersections;
+  // These are vertices via which we can describe edges. We only need 4 such
+  // vertices
+  std::vector<Vector3<T> > vertices
+  {
+    Vector3<T>{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2},
+    Vector3<T>{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2},
+    Vector3<T>{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2},
+    Vector3<T>{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}
+  };
+
+  // Axes
+  std::vector<Vector3<T>> axes
+  {
+    Vector3<T>{1, 0, 0},
+    Vector3<T>{0, 1, 0},
+    Vector3<T>{0, 0, 1}
+  };
+
+  // There are 12 edges, which are checked along 3 axes from 4 box corner
+  // points.
+  for (auto &v : vertices)
+  {
+    for (auto &a : axes)
+    {
+      auto intersection = _plane.Intersection(v, a);
+      if (intersection.has_value() &&
+          intersection->X() >= -this->size.X()/2 &&
+          intersection->X() <= this->size.X()/2 &&
+          intersection->Y() >= -this->size.Y()/2 &&
+          intersection->Y() <= this->size.Y()/2 &&
+          intersection->Z() >= -this->size.Z()/2 &&
+          intersection->Z() <= this->size.Z()/2)
+      {
+        intersections.insert(intersection.value());
+      }
+    }
+  }
+
+  return intersections;
+}
+
 }
 }
 #endif
diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh
index 6c85c197e..740ee6741 100644
--- a/include/ignition/math/detail/Sphere.hh
+++ b/include/ignition/math/detail/Sphere.hh
@@ -95,6 +95,60 @@ T Sphere<T>::Volume() const
   return (4.0/3.0) * IGN_PI * std::pow(this->radius, 3);
 }
 
+//////////////////////////////////////////////////
+template<typename T>
+T Sphere<T>::VolumeBelow(const Plane<T> &_plane) const
+{
+  auto r = this->radius;
+  // get nearest point to center on plane
+  auto dist = _plane.Distance(Vector3d(0, 0, 0));
+
+  if (dist < -r)
+  {
+    // sphere is below plane.
+    return Volume();
+  }
+  else if (dist > r)
+  {
+    // sphere is completely above plane
+    return 0.0;
+  }
+
+  auto h = r - dist;
+  return IGN_PI * h * h * (3 * r - h) / 3;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+std::optional<Vector3<T>>
+ Sphere<T>::CenterOfVolumeBelow(const Plane<T> &_plane) const
+{
+  auto r = this->radius;
+  // get nearest point to center on plane
+  auto dist = _plane.Distance(Vector3d(0, 0, 0));
+
+  if (dist < -r)
+  {
+    // sphere is completely below plane
+    return Vector3<T>{0, 0, 0};
+  }
+  else if (dist > r)
+  {
+    // sphere is completely above plane
+    return std::nullopt;
+  }
+
+  // Get the height of the spherical cap
+  auto h = r - dist;
+
+  // Formula for geometric centorid:
+  // https://mathworld.wolfram.com/SphericalCap.html
+  auto numerator = 2 * r - h;
+
+  auto z = 3 * numerator * numerator / (4 * (3 * r - h));
+  return - z * _plane.Normal().Normalized();
+}
+
 //////////////////////////////////////////////////
 template<typename T>
 bool Sphere<T>::SetDensityFromMass(const T _mass)
diff --git a/include/ignition/math/detail/WellOrderedVector.hh b/include/ignition/math/detail/WellOrderedVector.hh
new file mode 100644
index 000000000..1040a9bf5
--- /dev/null
+++ b/include/ignition/math/detail/WellOrderedVector.hh
@@ -0,0 +1,63 @@
+/*
+ * 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.
+ *
+*/
+#ifndef IGNITION_MATH_DETAIL_WELLORDERED_VECTOR_HH_
+#define IGNITION_MATH_DETAIL_WELLORDERED_VECTOR_HH_
+#include <ignition/math/Vector3.hh>
+
+namespace ignition
+{
+  namespace math
+  {
+    /// \brief Comparator for well-ordering vectors.
+    template<typename T>
+    struct WellOrderedVectors
+    {
+      /// \brief The normal Vector3::operator< is not actually properly ordered.
+      /// It does not form an ordinal set. This leads to various complications.
+      /// To solve this we introduce this function which orders vector3's by
+      /// their X value first, then their Y values and lastly their Z-values.
+      /// \param[in] _a - first vector
+      /// \param[in] _b - second vector
+      /// \return true if _a comes before _b.
+      bool operator() (const Vector3<T>& _a, const Vector3<T>& _b) const
+      {
+        if (_a[0] < _b[0])
+        {
+          return true;
+        }
+        else if (equal<T>(_a[0], _b[0], 1e-3))
+        {
+          if (_a[1] < _b[1])
+          {
+            return true;
+          }
+          else if (equal<T>(_a[1], _b[1], 1e-3))
+          {
+            return _a[2] < _b[2];
+          }
+          else
+          {
+            return false;
+          }
+        }
+        return false;
+      }
+    };
+  }
+}
+
+#endif
diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc
index c15452f7c..454abf4cb 100644
--- a/src/Box_TEST.cc
+++ b/src/Box_TEST.cc
@@ -113,6 +113,351 @@ TEST(BoxTest, VolumeAndDensity)
   EXPECT_GT(0.0, box2.DensityFromMass(mass));
 }
 
+//////////////////////////////////////////////////
+TEST(BoxTest, Intersections)
+{
+  // No intersections
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0);
+    EXPECT_EQ(box.Intersections(plane).size(), 0UL);
+  }
+
+  // Plane crosses 4 edges
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 0);
+
+    auto intersections = box.Intersections(plane);
+    ASSERT_EQ(4UL, intersections.size());
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, -1.0, 0.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 0.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 0.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, 1.0, 0.0)), 1UL);
+  }
+
+  // Plane coincides with box's face
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 1.0);
+
+    auto intersections = box.Intersections(plane);
+    ASSERT_EQ(4UL, intersections.size());
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, -1.0, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, 1.0, 1.0)), 1UL);
+  }
+
+  // 3 intersections
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(1.0, 1.0, 1.0), 1.0);
+
+    auto intersections = box.Intersections(plane);
+    ASSERT_EQ(3UL, intersections.size());
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, 1.0, -1.0)), 1UL);
+  }
+
+  // 6 intersections
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(1.0, 1.0, 1.0), 0.5);
+
+    auto intersections = box.Intersections(plane);
+    ASSERT_EQ(6UL, intersections.size());
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 0.5)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 0.5, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 0.5)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(0.5, -1.0, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, 0.5, -1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(0.5, 1.0, -1.0)), 1UL);
+  }
+
+  // 5 intersections
+  // This is the plane above tilted further up
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(1.0, 1.0, 2.0), 0.5);
+
+    auto intersections = box.Intersections(plane);
+    ASSERT_EQ(5UL, intersections.size());
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 0.25)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(-1.0, -0.5, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 0.25)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(-0.5, -1.0, 1.0)), 1UL);
+    EXPECT_EQ(intersections.count(math::Vector3d(1.0, 1.0, -0.75)), 1UL);
+  }
+}
+
+//////////////////////////////////////////////////
+TEST(BoxTest, VolumeBelow)
+{
+  // Fully above
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0);
+    EXPECT_DOUBLE_EQ(0.0, box.VolumeBelow(plane));
+  }
+  // Fully below
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 20.0);
+    EXPECT_DOUBLE_EQ(box.Volume(), box.VolumeBelow(plane));
+  }
+  // Fully below (because plane is rotated down)
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, -1.0), 20.0);
+    EXPECT_DOUBLE_EQ(box.Volume(), box.VolumeBelow(plane));
+  }
+  // Cut in half
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0, 0, 1.0), 0);
+
+    EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane));
+  }
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0, 1, 0), 0);
+
+    EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane));
+  }
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(-1, 0, 0), 0);
+
+    EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane));
+  }
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(-1, -1, 0), 0);
+
+    EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane));
+  }
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0, 1, 1), 0);
+
+    EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane));
+  }
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(1, 1, 1), 0);
+
+    EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane));
+  }
+  // Cut in 3/4
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0, 0, 1.0), 0.5);
+
+    EXPECT_DOUBLE_EQ(3*box.Volume()/4, box.VolumeBelow(plane));
+  }
+  // Opposites add to the total volume
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed planeA(math::Vector3d(0, 0, 1.0), 0.5);
+    math::Planed planeB(math::Vector3d(0, 0, 1.0), -0.5);
+
+    EXPECT_DOUBLE_EQ(box.Volume(),
+        box.VolumeBelow(planeA) + box.VolumeBelow(planeB));
+  }
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed planeA(math::Vector3d(0, 1.0, 1.0), 0.5);
+    math::Planed planeB(math::Vector3d(0, 1.0, 1.0), -0.5);
+
+    EXPECT_DOUBLE_EQ(box.Volume(),
+        box.VolumeBelow(planeA) + box.VolumeBelow(planeB));
+  }
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed planeA(math::Vector3d(-1, 1.0, 1.0), 0.5);
+    math::Planed planeB(math::Vector3d(-1, 1.0, 1.0), -0.5);
+
+    EXPECT_DOUBLE_EQ(box.Volume(),
+        box.VolumeBelow(planeA) + box.VolumeBelow(planeB));
+  }
+}
+
+//////////////////////////////////////////////////
+TEST(BoxTest, CenterOfVolumeBelow)
+{
+  // Fully above
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0);
+    EXPECT_FALSE(box.CenterOfVolumeBelow(plane).has_value());
+  }
+
+  // Fully below
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 5.0);
+    EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value());
+    EXPECT_EQ(box.CenterOfVolumeBelow(plane), math::Vector3d(0, 0, 0));
+  }
+
+  // Cut in half
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 0);
+    EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value());
+    EXPECT_EQ(box.CenterOfVolumeBelow(plane).value(),
+      math::Vector3d(0, 0, -0.5));
+  }
+
+  {
+    math::Boxd box(2.0, 2.0, 2.0);
+    math::Planed plane(math::Vector3d(0.0, 0.0, -1.0), 0);
+    EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value());
+    EXPECT_EQ(box.CenterOfVolumeBelow(plane).value(),
+      math::Vector3d(0, 0, 0.5));
+  }
+}
+
+//////////////////////////////////////////////////
+TEST(BoxTest, VerticesBelow)
+{
+  math::Boxd box(2.0, 2.0, 2.0);
+  auto size = box.Size();
+
+  math::Vector3d pXpYpZ{ size.X()/2,  size.Y()/2,  size.Z()/2};
+  math::Vector3d nXpYpZ{-size.X()/2,  size.Y()/2,  size.Z()/2};
+  math::Vector3d pXnYpZ{ size.X()/2, -size.Y()/2,  size.Z()/2};
+  math::Vector3d nXnYpZ{-size.X()/2, -size.Y()/2,  size.Z()/2};
+  math::Vector3d pXpYnZ{ size.X()/2,  size.Y()/2, -size.Z()/2};
+  math::Vector3d nXpYnZ{-size.X()/2,  size.Y()/2, -size.Z()/2};
+  math::Vector3d pXnYnZ{ size.X()/2, -size.Y()/2, -size.Z()/2};
+  math::Vector3d nXnYnZ{-size.X()/2, -size.Y()/2, -size.Z()/2};
+
+  // Fully above
+  {
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0);
+    EXPECT_TRUE(box.VerticesBelow(plane).empty());
+  }
+  // Fully below
+  {
+    math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 20.0);
+    EXPECT_EQ(8u, box.VerticesBelow(plane).size());
+  }
+  // Fully below (because plane is rotated down)
+  {
+    math::Planed plane(math::Vector3d(0.0, 0.0, -1.0), 20.0);
+    EXPECT_EQ(8u, box.VerticesBelow(plane).size());
+  }
+  // 4 vertices
+  {
+    math::Planed plane(math::Vector3d(0, 0, 1.0), 0);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(4u, vertices.size());
+
+    EXPECT_EQ(vertices.count(nXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(nXpYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXpYnZ), 1UL);
+  }
+  {
+    math::Planed plane(math::Vector3d(0, 1, 0), 0.5);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(4u, vertices.size());
+
+    EXPECT_EQ(vertices.count(nXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(nXnYpZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYpZ), 1UL);
+  }
+  {
+    math::Planed plane(math::Vector3d(-1, 0, 0), -0.5);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(4u, vertices.size());
+
+    EXPECT_EQ(vertices.count(pXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYpZ), 1UL);
+    EXPECT_EQ(vertices.count(pXpYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXpYpZ), 1UL);
+  }
+  {
+    math::Planed plane(math::Vector3d(1, 1, 1), 0.0);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(4u, vertices.size());
+
+    EXPECT_EQ(vertices.count(nXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(nXnYpZ), 1UL);
+    EXPECT_EQ(vertices.count(nXpYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYnZ), 1UL);
+  }
+  // 6 vertices
+  {
+    math::Planed plane(math::Vector3d(-1, -1, 0), 0.3);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(6u, vertices.size());
+
+    EXPECT_EQ(vertices.count(nXpYnZ), 1UL);
+    EXPECT_EQ(vertices.count(nXpYpZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYpZ), 1UL);
+    EXPECT_EQ(vertices.count(pXpYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXpYpZ), 1UL);
+  }
+  {
+    math::Planed plane(math::Vector3d(0, 1, 1), 0.9);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(6u, vertices.size());
+
+    EXPECT_EQ(vertices.count(nXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(nXnYpZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYpZ), 1UL);
+    EXPECT_EQ(vertices.count(nXpYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXpYnZ), 1UL);
+  }
+  // 2 vertices
+  {
+    math::Planed plane(math::Vector3d(-1, -1, 0), -0.5);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(2u, vertices.size());
+
+    EXPECT_EQ(vertices.count(pXpYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXpYpZ), 1UL);
+  }
+  // 7 vertices
+  {
+    math::Planed plane(math::Vector3d(1, 1, 1), 1.0);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(7u, vertices.size());
+
+    EXPECT_EQ(vertices.count(nXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(nXnYpZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYpZ), 1UL);
+    EXPECT_EQ(vertices.count(nXpYnZ), 1UL);
+    EXPECT_EQ(vertices.count(nXpYpZ), 1UL);
+    EXPECT_EQ(vertices.count(pXnYnZ), 1UL);
+    EXPECT_EQ(vertices.count(pXpYnZ), 1UL);
+  }
+  // 1 vertex
+  {
+    math::Planed plane(math::Vector3d(1, 1, 1), -1.2);
+
+    auto vertices = box.VerticesBelow(plane);
+    ASSERT_EQ(1u, vertices.size());
+
+    EXPECT_EQ(vertices.count(nXnYnZ), 1UL);
+  }
+}
+
 //////////////////////////////////////////////////
 TEST(BoxTest, Mass)
 {
diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc
index 05a060573..db50be5b6 100644
--- a/src/Line3_TEST.cc
+++ b/src/Line3_TEST.cc
@@ -230,6 +230,86 @@ TEST(Line3Test, Distance)
   EXPECT_FALSE(line.Distance(math::Line3d(2, 0, 0, 2, 1, 0), result));
 }
 
+/////////////////////////////////////////////////
+TEST(Line3Test, DistanceToPoint)
+{
+  // Line on horizontal plane
+  math::Vector3d pointA{0, -1, 0};
+  math::Vector3d pointB{0, 1, 0};
+  math::Line3d line{pointA, pointB};
+
+  // Point on the line
+  {
+    math::Vector3d point(0, 0.5, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 0.0);
+  }
+
+  // Points projected onto the line
+  {
+    math::Vector3d point(5, 0, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 5);
+  }
+  {
+    math::Vector3d point(-1, -1, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 1);
+  }
+
+  // Points projected beyond the line's ends
+  {
+    math::Vector3d point(0, 2, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 1);
+  }
+  {
+    math::Vector3d point(2, -3, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), sqrt(8));
+  }
+
+  // 3D line
+  pointA.Set(1, 1, 1);
+  pointB.Set(-1, -1, -1);
+  line.Set(pointA, pointB);
+
+  // Point on the line
+  {
+    math::Vector3d point(-0.5, -0.5, -0.5);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 0.0);
+  }
+
+  // Point projected onto the line
+  {
+    math::Vector3d point(1, -1, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), point.Length());
+  }
+
+  // Points projected beyond the line's ends
+  {
+    math::Vector3d point(2, 2, 3);
+    EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointA));
+  }
+  {
+    math::Vector3d point(-5, -3, -8);
+    EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointB));
+  }
+  pointA.Set(0, 0, 0);
+  line.Set(pointA, pointA);
+
+  // Point on the line
+  {
+    math::Vector3d point(0, 0, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 0.0);
+  }
+
+  // Points projected beyond the line's ends
+  {
+    math::Vector3d point(2, 2, 3);
+    EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointA));
+  }
+  {
+    math::Vector3d point(-5, -3, -8);
+    EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointA));
+  }
+}
+
 /////////////////////////////////////////////////
 TEST(Line3Test, Intersect)
 {
diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc
index 4defeb839..e082d2c26 100644
--- a/src/Plane_TEST.cc
+++ b/src/Plane_TEST.cc
@@ -150,3 +150,64 @@ TEST(PlaneTest, SideAxisAlignedBox)
     EXPECT_EQ(plane.Side(box), Planed::BOTH_SIDE);
   }
 }
+
+/////////////////////////////////////////////////
+TEST(PlaneTest, Intersection)
+{
+  Planed plane(Vector3d(0.5, 0, 1), 1);
+  {
+    auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 1));
+    EXPECT_TRUE(intersect.has_value());
+    EXPECT_NEAR(intersect->Dot(plane.Normal()), plane.Offset(), 1e-6);
+  }
+
+  plane.Set(Vector3d(1, 0, 0), 2);
+  {
+    auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 0));
+    EXPECT_TRUE(intersect.has_value());
+    EXPECT_EQ(intersect.value(), Vector3d(2, 0, 0));
+  }
+  {
+    auto intersect = plane.Intersection(Vector3d(1, 1, 0), Vector3d(-1, -1, 0));
+    EXPECT_TRUE(intersect.has_value());
+    EXPECT_EQ(intersect.value(), Vector3d(2, 2, 0));
+  }
+  // Lines on plane
+  {
+    auto intersect = plane.Intersection(Vector3d(2, 0, 0), Vector3d(0, 1, 0));
+    EXPECT_FALSE(intersect.has_value());
+  }
+  {
+    auto intersect = plane.Intersection(Vector3d(2, 0, 0), Vector3d(0, 0, 1));
+    EXPECT_FALSE(intersect.has_value());
+  }
+  {
+    auto intersect = plane.Intersection(Vector3d(2, 0, 0), Vector3d(0, 1, 1));
+    EXPECT_FALSE(intersect.has_value());
+  }
+  // Lines parallel to plane
+  {
+    auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 0));
+    EXPECT_FALSE(intersect.has_value());
+  }
+  {
+    auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(0, 0, 1));
+    EXPECT_FALSE(intersect.has_value());
+  }
+  {
+    auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 1));
+    EXPECT_FALSE(intersect.has_value());
+  }
+
+  // Bounded plane
+  {
+    Planed planeBounded(Vector3d(0, 0, 1), Vector2d(0.5, 0.5), 0);
+    auto intersect1 =
+      planeBounded.Intersection(Vector3d(0, 0, 0), Vector3d(0, 0, 1));
+    EXPECT_TRUE(intersect1.has_value());
+    EXPECT_EQ(intersect1.value(), Vector3d(0, 0, 0));
+    auto intersect2 =
+      planeBounded.Intersection(Vector3d(20, 20, 0), Vector3d(0, 0, 1));
+    EXPECT_FALSE(intersect2.has_value());
+  }
+}
diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc
index aa9143eee..cfb069c87 100644
--- a/src/Sphere_TEST.cc
+++ b/src/Sphere_TEST.cc
@@ -131,3 +131,139 @@ TEST(SphereTest, Mass)
   EXPECT_EQ(expectedMassMat, massMat);
   EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass());
 }
+
+//////////////////////////////////////////////////
+TEST(SphereTest, VolumeBelow)
+{
+  double r = 2;
+  math::Sphered sphere(r);
+
+  // Fully below
+  {
+    math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), 2*r);
+    EXPECT_NEAR(sphere.Volume(), sphere.VolumeBelow(_plane), 1e-3);
+  }
+
+  // Fully below (because plane is rotated down)
+  {
+    math::Planed _plane(math::Vector3d{0, 0, -1}, math::Vector2d(4, 4), 2*r);
+    EXPECT_NEAR(sphere.Volume(), sphere.VolumeBelow(_plane), 1e-3);
+  }
+
+  // Fully above
+  {
+    math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), -2*r);
+    EXPECT_NEAR(sphere.VolumeBelow(_plane), 0, 1e-3);
+  }
+
+  // Hemisphere
+  {
+    math::Planed _plane(math::Vector3d{0, 0, 1}, 0);
+    EXPECT_NEAR(sphere.Volume() / 2, sphere.VolumeBelow(_plane), 1e-3);
+  }
+
+  // Vertical plane
+  {
+    math::Planed _plane(math::Vector3d{1, 0, 0}, 0);
+    EXPECT_NEAR(sphere.Volume() / 2, sphere.VolumeBelow(_plane), 1e-3);
+  }
+
+  // Expectations from https://planetcalc.com/283/
+  {
+    math::Planed _plane(math::Vector3d{0, 0, 1}, 0.5);
+    EXPECT_NEAR(22.90745, sphere.VolumeBelow(_plane), 1e-3);
+  }
+
+  {
+    math::Planed _plane(math::Vector3d{0, 0, 1}, -0.5);
+    EXPECT_NEAR(10.60288, sphere.VolumeBelow(_plane), 1e-3);
+  }
+}
+
+//////////////////////////////////////////////////
+TEST(SphereTest, CenterOfVolumeBelow)
+{
+  double r = 2;
+  math::Sphered sphere(r);
+
+  // Entire sphere below plane
+  {
+    math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(0, 0), 2 * r);
+    EXPECT_EQ(Vector3d(0, 0, 0), sphere.CenterOfVolumeBelow(_plane));
+  }
+
+  // Entire sphere above plane
+  {
+    math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(0, 0), -2 * r);
+    EXPECT_FALSE(sphere.CenterOfVolumeBelow(_plane).has_value());
+  }
+
+  {
+    // 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
+    math::Planed _plane(math::Vector3d{0, 1, 0}, math::Vector2d(0, 0), 0);
+    EXPECT_EQ(
+      Vector3d(0, -0.75, 0), sphere.CenterOfVolumeBelow(_plane).value());
+  }
+
+  {
+    // 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
+    math::Planed _plane(math::Vector3d{0, -1, 0}, math::Vector2d(0, 0), 0);
+    EXPECT_EQ(
+      Vector3d(0, 0.75, 0), sphere.CenterOfVolumeBelow(_plane).value());
+  }
+
+  {
+    // 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
+    math::Planed _plane(math::Vector3d{0, 1, 0}, math::Vector2d(0, 0), 0.4 * r);
+    EXPECT_EQ(
+      Vector3d(0, -0.3375, 0), sphere.CenterOfVolumeBelow(_plane).value());
+  }
+
+  {
+    // Handcalculated value.
+    math::Planed _plane(math::Vector3d{0, 1, 0},
+      math::Vector2d(0, 0), -0.4 * r);
+
+    EXPECT_EQ(
+      Vector3d(0, -1.225, 0), sphere.CenterOfVolumeBelow(_plane).value());
+  }
+
+  {
+    // Handcalculated value.
+    math::Planed _plane(math::Vector3d{0, -1, 0},
+      math::Vector2d(0, 0), -0.4 * r);
+
+    EXPECT_EQ(
+      Vector3d(0, 1.225, 0), sphere.CenterOfVolumeBelow(_plane).value());
+  }
+
+  {
+    // Handcalculated value.
+    math::Planed _plane(math::Vector3d{0, -1, 0},
+      math::Vector2d(0, 0), 0.4 * r);
+
+    EXPECT_EQ(
+      Vector3d(0, 0.3375, 0), sphere.CenterOfVolumeBelow(_plane).value());
+  }
+
+  {
+    // Weighted sums of the center of volume results in (0,0,0).
+    math::Planed _plane1(math::Vector3d{0, 0, 1}, -0.5);
+    // Flip plane1 axis
+    math::Planed _plane2(math::Vector3d{0, 0, -1}, -0.5);
+    EXPECT_EQ(
+      sphere.CenterOfVolumeBelow(_plane1).value() * sphere.VolumeBelow(_plane1)
+      + sphere.CenterOfVolumeBelow(_plane2).value()
+        * sphere.VolumeBelow(_plane2),
+      math::Vector3d(0, 0, 0)
+    );
+  }
+}
diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc
index 9076b1099..9a4615aa1 100644
--- a/src/Vector3_TEST.cc
+++ b/src/Vector3_TEST.cc
@@ -504,3 +504,33 @@ TEST(Vector3dTest, NaN)
   EXPECT_EQ(math::Vector3f::Zero, nanVecF);
   EXPECT_TRUE(nanVecF.IsFinite());
 }
+
+/////////////////////////////////////////////////
+TEST(Vector3dTest, DistToLine)
+{
+  // Line on horizontal plane
+  math::Vector3d pointA{0, -1, 0};
+  math::Vector3d pointB{0, 1, 0};
+
+  // Point on the line
+  {
+    math::Vector3d point(0, 0.5, 0);
+    EXPECT_DOUBLE_EQ(point.DistToLine(pointA, pointB), 0.0);
+  }
+
+  // Points projected onto the line
+  {
+    math::Vector3d point(5, 0, 0);
+    EXPECT_DOUBLE_EQ(point.DistToLine(pointA, pointB), 5);
+  }
+  {
+    math::Vector3d point(-1, -1, 0);
+    EXPECT_DOUBLE_EQ(point.DistToLine(pointA, pointB), 1);
+  }
+
+  // Point projected beyond the segment's ends
+  {
+    math::Vector3d point(0, 2, 0);
+    EXPECT_DOUBLE_EQ(point.DistToLine(pointA, pointB), 0);
+  }
+}