diff --git a/eigen3/include/gz/math/eigen3/Conversions.hh b/eigen3/include/gz/math/eigen3/Conversions.hh
index 636f51606..1240c6b0d 100644
--- a/eigen3/include/gz/math/eigen3/Conversions.hh
+++ b/eigen3/include/gz/math/eigen3/Conversions.hh
@@ -26,185 +26,178 @@
 #include <gz/math/Quaternion.hh>
 #include <gz/math/Vector3.hh>
 
-namespace gz
+namespace gz::math::eigen3
 {
-  namespace math
+  /// \brief Convert from gz::math::Vector3d to Eigen::Vector3d.
+  /// \param[in] _v gz::math::Vector3d to convert
+  /// \return The equivalent Eigen::Vector3d.
+  inline Eigen::Vector3d convert(const gz::math::Vector3d &_v)
   {
-    namespace eigen3
+    return Eigen::Vector3d(_v[0], _v[1], _v[2]);
+  }
+
+  /// \brief Convert from gz::math::AxisAlignedBox to
+  /// Eigen::AlignedBox3d.
+  /// \param[in] _b gz::math::AxisAlignedBox to convert
+  /// \return The equivalent Eigen::AlignedBox3d.
+  inline Eigen::AlignedBox3d convert(
+      const gz::math::AxisAlignedBox &_b)
+  {
+    return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max()));
+  }
+
+  /// \brief Convert from gz::math::Matrix3d to Eigen::Matrix3d.
+  /// \param[in] _m gz::math::Matrix3d to convert.
+  /// \return The equivalent Eigen::Matrix3d.
+  inline Eigen::Matrix3d convert(const gz::math::Matrix3d &_m)
+  {
+    Eigen::Matrix3d matrix;
+    for (std::size_t i=0; i < 3; ++i)
     {
-      /// \brief Convert from gz::math::Vector3d to Eigen::Vector3d.
-      /// \param[in] _v gz::math::Vector3d to convert
-      /// \return The equivalent Eigen::Vector3d.
-      inline Eigen::Vector3d convert(const gz::math::Vector3d &_v)
+      for (std::size_t j=0; j < 3; ++j)
       {
-        return Eigen::Vector3d(_v[0], _v[1], _v[2]);
+        matrix(i, j) = _m(i, j);
       }
+    }
 
-      /// \brief Convert from gz::math::AxisAlignedBox to
-      /// Eigen::AlignedBox3d.
-      /// \param[in] _b gz::math::AxisAlignedBox to convert
-      /// \return The equivalent Eigen::AlignedBox3d.
-      inline Eigen::AlignedBox3d convert(
-          const gz::math::AxisAlignedBox &_b)
-      {
-        return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max()));
-      }
+    return matrix;
+  }
 
-      /// \brief Convert from gz::math::Matrix3d to Eigen::Matrix3d.
-      /// \param[in] _m gz::math::Matrix3d to convert.
-      /// \return The equivalent Eigen::Matrix3d.
-      inline Eigen::Matrix3d convert(const gz::math::Matrix3d &_m)
+  /// \brief Convert from gz::math::Matrix6d to
+  /// Eigen::Matrix<Precision, 6, 6>.
+  /// \param[in] _m gz::math::Matrix6d to convert.
+  /// \return The equivalent Eigen::Matrix<Precision, 6, 6>.
+  /// \tparam Precision Precision such as int, double or float.
+  template<typename Precision>
+  inline
+  Eigen::Matrix<Precision, 6, 6> convert(const Matrix6<Precision> &_m)
+  {
+    Eigen::Matrix<Precision, 6, 6> matrix;
+    for (std::size_t i = 0; i < 6; ++i)
+    {
+      for (std::size_t j = 0; j < 6; ++j)
       {
-        Eigen::Matrix3d matrix;
-        for (std::size_t i=0; i < 3; ++i)
-        {
-          for (std::size_t j=0; j < 3; ++j)
-          {
-            matrix(i, j) = _m(i, j);
-          }
-        }
-
-        return matrix;
+        matrix(i, j) = _m(i, j);
       }
+    }
 
-      /// \brief Convert from gz::math::Matrix6d to
-      /// Eigen::Matrix<Precision, 6, 6>.
-      /// \param[in] _m gz::math::Matrix6d to convert.
-      /// \return The equivalent Eigen::Matrix<Precision, 6, 6>.
-      /// \tparam Precision Precision such as int, double or float.
-      template<typename Precision>
-      inline
-      Eigen::Matrix<Precision, 6, 6> convert(const Matrix6<Precision> &_m)
-      {
-        Eigen::Matrix<Precision, 6, 6> matrix;
-        for (std::size_t i = 0; i < 6; ++i)
-        {
-          for (std::size_t j = 0; j < 6; ++j)
-          {
-            matrix(i, j) = _m(i, j);
-          }
-        }
-
-        return matrix;
-      }
+    return matrix;
+  }
 
-      /// \brief Convert gz::math::Quaterniond to Eigen::Quaterniond.
-      /// \param[in] _q gz::math::Quaterniond to convert.
-      /// \return The equivalent Eigen::Quaterniond.
-      inline Eigen::Quaterniond convert(const gz::math::Quaterniond &_q)
-      {
-        Eigen::Quaterniond quat;
-        quat.w() = _q.W();
-        quat.x() = _q.X();
-        quat.y() = _q.Y();
-        quat.z() = _q.Z();
+  /// \brief Convert gz::math::Quaterniond to Eigen::Quaterniond.
+  /// \param[in] _q gz::math::Quaterniond to convert.
+  /// \return The equivalent Eigen::Quaterniond.
+  inline Eigen::Quaterniond convert(const gz::math::Quaterniond &_q)
+  {
+    Eigen::Quaterniond quat;
+    quat.w() = _q.W();
+    quat.x() = _q.X();
+    quat.y() = _q.Y();
+    quat.z() = _q.Z();
 
-        return quat;
-      }
+    return quat;
+  }
 
-      /// \brief Convert gz::math::Pose3d to Eigen::Isometry3d.
-      /// \param[in] _pose gz::math::Pose3d to convert.
-      /// \return The equivalent Eigen::Isometry3d.
-      inline Eigen::Isometry3d convert(const gz::math::Pose3d &_pose)
-      {
-        Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
-        tf.translation() = convert(_pose.Pos());
-        tf.linear() = Eigen::Matrix3d(convert(_pose.Rot()));
+  /// \brief Convert gz::math::Pose3d to Eigen::Isometry3d.
+  /// \param[in] _pose gz::math::Pose3d to convert.
+  /// \return The equivalent Eigen::Isometry3d.
+  inline Eigen::Isometry3d convert(const gz::math::Pose3d &_pose)
+  {
+    Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
+    tf.translation() = convert(_pose.Pos());
+    tf.linear() = Eigen::Matrix3d(convert(_pose.Rot()));
 
-        return tf;
-      }
+    return tf;
+  }
 
-      /// \brief Convert Eigen::Vector3d to gz::math::Vector3d.
-      /// \param[in] _v Eigen::Vector3d to convert.
-      /// \return The equivalent gz::math::Vector3d.
-      inline gz::math::Vector3d convert(const Eigen::Vector3d &_v)
-      {
-        gz::math::Vector3d vec;
-        vec.X() = _v[0];
-        vec.Y() = _v[1];
-        vec.Z() = _v[2];
+  /// \brief Convert Eigen::Vector3d to gz::math::Vector3d.
+  /// \param[in] _v Eigen::Vector3d to convert.
+  /// \return The equivalent gz::math::Vector3d.
+  inline gz::math::Vector3d convert(const Eigen::Vector3d &_v)
+  {
+    gz::math::Vector3d vec;
+    vec.X() = _v[0];
+    vec.Y() = _v[1];
+    vec.Z() = _v[2];
 
-        return vec;
-      }
+    return vec;
+  }
 
-      /// \brief Convert Eigen::AlignedBox3d to gz::math::AxisAlignedBox.
-      /// \param[in] _b Eigen::AlignedBox3d to convert.
-      /// \return The equivalent gz::math::AxisAlignedBox.
-      inline gz::math::AxisAlignedBox convert(
-          const Eigen::AlignedBox3d &_b)
-      {
-        gz::math::AxisAlignedBox box;
-        box.Min() = convert(_b.min());
-        box.Max() = convert(_b.max());
+  /// \brief Convert Eigen::AlignedBox3d to gz::math::AxisAlignedBox.
+  /// \param[in] _b Eigen::AlignedBox3d to convert.
+  /// \return The equivalent gz::math::AxisAlignedBox.
+  inline gz::math::AxisAlignedBox convert(
+      const Eigen::AlignedBox3d &_b)
+  {
+    gz::math::AxisAlignedBox box;
+    box.Min() = convert(_b.min());
+    box.Max() = convert(_b.max());
 
-        return box;
-      }
+    return box;
+  }
 
-      /// \brief Convert Eigen::Matrix3d to gz::math::Matrix3d.
-      /// \param[in] _m Eigen::Matrix3d to convert.
-      /// \return The equivalent gz::math::Matrix3d.
-      inline gz::math::Matrix3d convert(const Eigen::Matrix3d &_m)
+  /// \brief Convert Eigen::Matrix3d to gz::math::Matrix3d.
+  /// \param[in] _m Eigen::Matrix3d to convert.
+  /// \return The equivalent gz::math::Matrix3d.
+  inline gz::math::Matrix3d convert(const Eigen::Matrix3d &_m)
+  {
+    gz::math::Matrix3d matrix;
+    for (std::size_t i=0; i < 3; ++i)
+    {
+      for (std::size_t j=0; j < 3; ++j)
       {
-        gz::math::Matrix3d matrix;
-        for (std::size_t i=0; i < 3; ++i)
-        {
-          for (std::size_t j=0; j < 3; ++j)
-          {
-            matrix(i, j) = _m(i, j);
-          }
-        }
-
-        return matrix;
+        matrix(i, j) = _m(i, j);
       }
+    }
 
-      /// \brief Convert Eigen::Matrix<Precision, 6, 6> to
-      /// gz::math::Matrix6d.
-      /// \param[in] _m Eigen::Matrix<Precision, 6, 6> to convert.
-      /// \return The equivalent gz::math::Matrix6d.
-      /// \tparam Precision Precision such as int, double or float.
-      template<typename Precision>
-      inline
-      Matrix6<Precision> convert(const Eigen::Matrix<Precision, 6, 6> &_m)
-      {
-        Matrix6<Precision> matrix;
-        for (std::size_t i = 0; i < 6; ++i)
-        {
-          for (std::size_t j = 0; j < 6; ++j)
-          {
-            matrix(i, j) = _m(i, j);
-          }
-        }
-
-        return matrix;
-      }
+    return matrix;
+  }
 
-      /// \brief Convert Eigen::Quaterniond to gz::math::Quaterniond.
-      /// \param[in] _q Eigen::Quaterniond to convert.
-      /// \return The equivalent gz::math::Quaterniond.
-      inline gz::math::Quaterniond convert(const Eigen::Quaterniond &_q)
+  /// \brief Convert Eigen::Matrix<Precision, 6, 6> to
+  /// gz::math::Matrix6d.
+  /// \param[in] _m Eigen::Matrix<Precision, 6, 6> to convert.
+  /// \return The equivalent gz::math::Matrix6d.
+  /// \tparam Precision Precision such as int, double or float.
+  template<typename Precision>
+  inline
+  Matrix6<Precision> convert(const Eigen::Matrix<Precision, 6, 6> &_m)
+  {
+    Matrix6<Precision> matrix;
+    for (std::size_t i = 0; i < 6; ++i)
+    {
+      for (std::size_t j = 0; j < 6; ++j)
       {
-        gz::math::Quaterniond quat;
-        quat.W() = _q.w();
-        quat.X() = _q.x();
-        quat.Y() = _q.y();
-        quat.Z() = _q.z();
-
-        return quat;
+        matrix(i, j) = _m(i, j);
       }
+    }
 
-      /// \brief Convert Eigen::Isometry3d to gz::math::Pose3d.
-      /// \param[in] _tf Eigen::Isometry3d to convert.
-      /// \return The equivalent gz::math::Pose3d.
-      inline gz::math::Pose3d convert(const Eigen::Isometry3d &_tf)
-      {
-        gz::math::Pose3d pose;
-        pose.Pos() = convert(Eigen::Vector3d(_tf.translation()));
-        pose.Rot() = convert(Eigen::Quaterniond(_tf.linear()));
+    return matrix;
+  }
 
-        return pose;
-      }
-    }
+  /// \brief Convert Eigen::Quaterniond to gz::math::Quaterniond.
+  /// \param[in] _q Eigen::Quaterniond to convert.
+  /// \return The equivalent gz::math::Quaterniond.
+  inline gz::math::Quaterniond convert(const Eigen::Quaterniond &_q)
+  {
+    gz::math::Quaterniond quat;
+    quat.W() = _q.w();
+    quat.X() = _q.x();
+    quat.Y() = _q.y();
+    quat.Z() = _q.z();
+
+    return quat;
   }
-}
 
-#endif
+  /// \brief Convert Eigen::Isometry3d to gz::math::Pose3d.
+  /// \param[in] _tf Eigen::Isometry3d to convert.
+  /// \return The equivalent gz::math::Pose3d.
+  inline gz::math::Pose3d convert(const Eigen::Isometry3d &_tf)
+  {
+    gz::math::Pose3d pose;
+    pose.Pos() = convert(Eigen::Vector3d(_tf.translation()));
+    pose.Rot() = convert(Eigen::Quaterniond(_tf.linear()));
+
+    return pose;
+  }
+}  // namespace gz::math::eigen3
+#endif  // GZ_MATH_EIGEN3_CONVERSIONS_HH_
diff --git a/eigen3/include/gz/math/eigen3/Util.hh b/eigen3/include/gz/math/eigen3/Util.hh
index e4b25642f..0b772ab83 100644
--- a/eigen3/include/gz/math/eigen3/Util.hh
+++ b/eigen3/include/gz/math/eigen3/Util.hh
@@ -31,131 +31,124 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/eigen3/Conversions.hh>
 
-namespace gz
+namespace gz::math::eigen3
 {
-  namespace math
+  /// \brief Get covariance matrix from a set of 3d vertices
+  /// https://github.com/isl-org/Open3D/blob/76c2baf9debd460900f056a9b51e9a80de9c0e64/cpp/open3d/utility/Eigen.cpp#L305
+  /// \param[in] _vertices a vector of 3d vertices
+  /// \return Covariance matrix
+  inline Eigen::Matrix3d covarianceMatrix(
+    const std::vector<math::Vector3d> &_vertices)
   {
-    namespace eigen3
+    if (_vertices.empty())
+      return Eigen::Matrix3d::Identity();
+
+    Eigen::Matrix<double, 9, 1> cumulants;
+    cumulants.setZero();
+    for (const auto &vertex : _vertices)
     {
-      /// \brief Get covariance matrix from a set of 3d vertices
-      /// https://github.com/isl-org/Open3D/blob/76c2baf9debd460900f056a9b51e9a80de9c0e64/cpp/open3d/utility/Eigen.cpp#L305
-      /// \param[in] _vertices a vector of 3d vertices
-      /// \return Covariance matrix
-      inline Eigen::Matrix3d covarianceMatrix(
-        const std::vector<math::Vector3d> &_vertices)
-      {
-        if (_vertices.empty())
-          return Eigen::Matrix3d::Identity();
-
-        Eigen::Matrix<double, 9, 1> cumulants;
-        cumulants.setZero();
-        for (const auto &vertex : _vertices)
-        {
-          const Eigen::Vector3d &point = math::eigen3::convert(vertex);
-          cumulants(0) += point(0);
-          cumulants(1) += point(1);
-          cumulants(2) += point(2);
-          cumulants(3) += point(0) * point(0);
-          cumulants(4) += point(0) * point(1);
-          cumulants(5) += point(0) * point(2);
-          cumulants(6) += point(1) * point(1);
-          cumulants(7) += point(1) * point(2);
-          cumulants(8) += point(2) * point(2);
-        }
-
-        Eigen::Matrix3d covariance;
-
-        cumulants /= static_cast<double>(_vertices.size());
-
-        covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
-        covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
-        covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
-        covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
-        covariance(1, 0) = covariance(0, 1);
-        covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
-        covariance(2, 0) = covariance(0, 2);
-        covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
-        covariance(2, 1) = covariance(1, 2);
-        return covariance;
-      }
-
-      /// \brief Get the oriented 3d bounding box of a set of 3d
-      /// vertices using PCA
-      /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html
-      /// \param[in] _vertices a vector of 3d vertices
-      /// \return Oriented 3D box
-      inline gz::math::OrientedBoxd verticesToOrientedBox(
-        const std::vector<math::Vector3d> &_vertices)
-      {
-        math::OrientedBoxd box;
-
-        // Return an empty box if there are no vertices
-        if (_vertices.empty())
-          return box;
-
-        math::Vector3d mean;
-        for (const auto &point : _vertices)
-          mean += point;
-        mean /= static_cast<double>(_vertices.size());
-
-        Eigen::Vector3d centroid = math::eigen3::convert(mean);
-        Eigen::Matrix3d covariance = covarianceMatrix(_vertices);
-
-        // Eigen Vectors
-        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
-          eigenSolver(covariance, Eigen::ComputeEigenvectors);
-        Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors();
-
-        // This line is necessary for proper orientation in some cases.
-        // The numbers come out the same without it, but the signs are
-        // different and the box doesn't get correctly oriented in some cases.
-        eigenVectorsPCA.col(2) =
-          eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
-
-        // Transform the original cloud to the origin where the principal
-        // components correspond to the axes.
-        Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity());
-        projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
-        projectionTransform.block<3, 1>(0, 3) =
-          -1.0f * (projectionTransform.block<3, 3>(0, 0) * centroid);
-
-        Eigen::Vector3d minPoint(INF_I32, INF_I32, INF_I32);
-        Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32);
-
-        // Get the minimum and maximum points of the transformed cloud.
-        for (const auto &point : _vertices)
-        {
-          Eigen::Vector4d pt(0, 0, 0, 1);
-          pt.head<3>() = math::eigen3::convert(point);
-          Eigen::Vector4d tfPoint = projectionTransform * pt;
-          minPoint = minPoint.cwiseMin(tfPoint.head<3>());
-          maxPoint = maxPoint.cwiseMax(tfPoint.head<3>());
-        }
-
-        const Eigen::Vector3d meanDiagonal = 0.5f * (maxPoint + minPoint);
-
-        // quaternion is calculated using the eigenvectors (which determines
-        // how the final box gets rotated), and the transform to put the box
-        // in correct location is calculated
-        const Eigen::Quaterniond bboxQuaternion(eigenVectorsPCA);
-        const Eigen::Vector3d bboxTransform =
-          eigenVectorsPCA * meanDiagonal + centroid;
-
-        math::Vector3d size(
-            maxPoint.x() - minPoint.x(),
-            maxPoint.y() - minPoint.y(),
-            maxPoint.z() - minPoint.z()
-        );
-        math::Pose3d pose;
-        pose.Rot() = math::eigen3::convert(bboxQuaternion);
-        pose.Pos() = math::eigen3::convert(bboxTransform);
-
-        box.Size(size);
-        box.Pose(pose);
-        return box;
-      }
+      const Eigen::Vector3d &point = math::eigen3::convert(vertex);
+      cumulants(0) += point(0);
+      cumulants(1) += point(1);
+      cumulants(2) += point(2);
+      cumulants(3) += point(0) * point(0);
+      cumulants(4) += point(0) * point(1);
+      cumulants(5) += point(0) * point(2);
+      cumulants(6) += point(1) * point(1);
+      cumulants(7) += point(1) * point(2);
+      cumulants(8) += point(2) * point(2);
     }
+
+    Eigen::Matrix3d covariance;
+
+    cumulants /= static_cast<double>(_vertices.size());
+
+    covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
+    covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
+    covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
+    covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
+    covariance(1, 0) = covariance(0, 1);
+    covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
+    covariance(2, 0) = covariance(0, 2);
+    covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
+    covariance(2, 1) = covariance(1, 2);
+    return covariance;
   }
-}
 
-#endif
+  /// \brief Get the oriented 3d bounding box of a set of 3d
+  /// vertices using PCA
+  /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html
+  /// \param[in] _vertices a vector of 3d vertices
+  /// \return Oriented 3D box
+  inline gz::math::OrientedBoxd verticesToOrientedBox(
+    const std::vector<math::Vector3d> &_vertices)
+  {
+    math::OrientedBoxd box;
+
+    // Return an empty box if there are no vertices
+    if (_vertices.empty())
+      return box;
+
+    math::Vector3d mean;
+    for (const auto &point : _vertices)
+      mean += point;
+    mean /= static_cast<double>(_vertices.size());
+
+    Eigen::Vector3d centroid = math::eigen3::convert(mean);
+    Eigen::Matrix3d covariance = covarianceMatrix(_vertices);
+
+    // Eigen Vectors
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
+      eigenSolver(covariance, Eigen::ComputeEigenvectors);
+    Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors();
+
+    // This line is necessary for proper orientation in some cases.
+    // The numbers come out the same without it, but the signs are
+    // different and the box doesn't get correctly oriented in some cases.
+    eigenVectorsPCA.col(2) =
+      eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
+
+    // Transform the original cloud to the origin where the principal
+    // components correspond to the axes.
+    Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity());
+    projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
+    projectionTransform.block<3, 1>(0, 3) =
+      -1.0f * (projectionTransform.block<3, 3>(0, 0) * centroid);
+
+    Eigen::Vector3d minPoint(INF_I32, INF_I32, INF_I32);
+    Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32);
+
+    // Get the minimum and maximum points of the transformed cloud.
+    for (const auto &point : _vertices)
+    {
+      Eigen::Vector4d pt(0, 0, 0, 1);
+      pt.head<3>() = math::eigen3::convert(point);
+      Eigen::Vector4d tfPoint = projectionTransform * pt;
+      minPoint = minPoint.cwiseMin(tfPoint.head<3>());
+      maxPoint = maxPoint.cwiseMax(tfPoint.head<3>());
+    }
+
+    const Eigen::Vector3d meanDiagonal = 0.5f * (maxPoint + minPoint);
+
+    // quaternion is calculated using the eigenvectors (which determines
+    // how the final box gets rotated), and the transform to put the box
+    // in correct location is calculated
+    const Eigen::Quaterniond bboxQuaternion(eigenVectorsPCA);
+    const Eigen::Vector3d bboxTransform =
+      eigenVectorsPCA * meanDiagonal + centroid;
+
+    math::Vector3d size(
+        maxPoint.x() - minPoint.x(),
+        maxPoint.y() - minPoint.y(),
+        maxPoint.z() - minPoint.z()
+    );
+    math::Pose3d pose;
+    pose.Rot() = math::eigen3::convert(bboxQuaternion);
+    pose.Pos() = math::eigen3::convert(bboxTransform);
+
+    box.Size(size);
+    box.Pose(pose);
+    return box;
+  }
+}  // namespace gz::math::eigen3
+#endif  // GZ_MATH_EIGEN3_UTIL_HH_
diff --git a/include/gz/math/AdditivelySeparableScalarField3.hh b/include/gz/math/AdditivelySeparableScalarField3.hh
index e3c31bd3f..7733b03ee 100644
--- a/include/gz/math/AdditivelySeparableScalarField3.hh
+++ b/include/gz/math/AdditivelySeparableScalarField3.hh
@@ -24,174 +24,171 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /** \class AdditivelySeparableScalarField3\
+   * AdditivelySeparableScalarField3.hh\
+   * gz/math/AdditivelySeparableScalarField3.hh
+   */
+  /// \brief The AdditivelySeparableScalarField3 class constructs
+  /// a scalar field F in R^3 as a sum of scalar functions i.e.
+  /// F(x, y, z) = k (p(x) + q(y) + r(z)).
+  ///
+  /// \tparam ScalarFunctionT a callable type that taking a single ScalarT
+  ///   value as argument and returning another ScalarT value. Additionally:
+  ///   - for AdditivelySeparableScalarField3T to have a stream operator
+  ///     overload, ScalarFunctionT must implement a
+  ///     void Print(std::ostream &, const std::string &) method that streams
+  ///     a representation of it using the given string as argument variable
+  ///     name;
+  ///   - for AdditivelySeparableScalarField3T::Minimum to be callable,
+  ///     ScalarFunctionT must implement a
+  ///     ScalarT Minimum(const Interval<ScalarT> &, ScalarT &) method that
+  ///     computes its minimum in the given interval and returns an argument
+  ///     value that yields said minimum.
+  /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits
+  ///   have been specialized.
+  ///
+  /// ## Example
+  ///
+  /// \snippet examples/additively_separable_scalar_field3_example.cc complete
+  template<typename ScalarFunctionT, typename ScalarT>
+  class AdditivelySeparableScalarField3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /** \class AdditivelySeparableScalarField3\
-     * AdditivelySeparableScalarField3.hh\
-     * gz/math/AdditivelySeparableScalarField3.hh
-     */
-    /// \brief The AdditivelySeparableScalarField3 class constructs
-    /// a scalar field F in R^3 as a sum of scalar functions i.e.
-    /// F(x, y, z) = k (p(x) + q(y) + r(z)).
-    ///
-    /// \tparam ScalarFunctionT a callable type that taking a single ScalarT
-    ///   value as argument and returning another ScalarT value. Additionally:
-    ///   - for AdditivelySeparableScalarField3T to have a stream operator
-    ///     overload, ScalarFunctionT must implement a
-    ///     void Print(std::ostream &, const std::string &) method that streams
-    ///     a representation of it using the given string as argument variable
-    ///     name;
-    ///   - for AdditivelySeparableScalarField3T::Minimum to be callable,
-    ///     ScalarFunctionT must implement a
-    ///     ScalarT Minimum(const Interval<ScalarT> &, ScalarT &) method that
-    ///     computes its minimum in the given interval and returns an argument
-    ///     value that yields said minimum.
-    /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits
-    ///   have been specialized.
-    ///
-    /// ## Example
-    ///
-    /// \snippet examples/additively_separable_scalar_field3_example.cc complete
-    template<typename ScalarFunctionT, typename ScalarT>
-    class AdditivelySeparableScalarField3
+    /// \brief Constructor
+    /// \param[in] _k scalar constant
+    /// \param[in] _p scalar function of x
+    /// \param[in] _q scalar function of y
+    /// \param[in] _r scalar function of z
+    public: AdditivelySeparableScalarField3(
+        ScalarT _k, ScalarFunctionT _p,
+        ScalarFunctionT _q, ScalarFunctionT _r)
+    : k(_k), p(std::move(_p)), q(std::move(_q)), r(std::move(_r))
     {
-      /// \brief Constructor
-      /// \param[in] _k scalar constant
-      /// \param[in] _p scalar function of x
-      /// \param[in] _q scalar function of y
-      /// \param[in] _r scalar function of z
-      public: AdditivelySeparableScalarField3(
-          ScalarT _k, ScalarFunctionT _p,
-          ScalarFunctionT _q, ScalarFunctionT _r)
-      : k(_k), p(std::move(_p)), q(std::move(_q)), r(std::move(_r))
-      {
-      }
+    }
 
-      /// \brief Evaluate the scalar field at `_point`
-      /// \param[in] _point scalar field argument
-      /// \return the result of evaluating `F(_point)`
-      public: ScalarT Evaluate(const Vector3<ScalarT> &_point) const
-      {
-        return this->k * (
-            this->p(_point.X()) +
-            this->q(_point.Y()) +
-            this->r(_point.Z()));
-      }
+    /// \brief Evaluate the scalar field at `_point`
+    /// \param[in] _point scalar field argument
+    /// \return the result of evaluating `F(_point)`
+    public: ScalarT Evaluate(const Vector3<ScalarT> &_point) const
+    {
+      return this->k * (
+          this->p(_point.X()) +
+          this->q(_point.Y()) +
+          this->r(_point.Z()));
+    }
 
-      /// \brief Call operator overload
-      /// \see SeparableScalarField3::Evaluate()
-      /// \param[in] _point scalar field argument
-      /// \return the result of evaluating `F(_point)`
-      public: ScalarT operator()(const Vector3<ScalarT> &_point) const
-      {
-        return this->Evaluate(_point);
-      }
+    /// \brief Call operator overload
+    /// \see SeparableScalarField3::Evaluate()
+    /// \param[in] _point scalar field argument
+    /// \return the result of evaluating `F(_point)`
+    public: ScalarT operator()(const Vector3<ScalarT> &_point) const
+    {
+      return this->Evaluate(_point);
+    }
 
-      /// \brief Compute scalar field minimum in a `_region`
-      /// \param[in] _region scalar field argument set to check
-      /// \param[out] _pMin scalar field argument that yields
-      ///   the minimum, or NaN if `_region` is empty
-      /// \return the scalar field minimum in the given `_region`,
-      ///   or NaN if `_region` is empty
-      public: ScalarT Minimum(const Region3<ScalarT> &_region,
-                              Vector3<ScalarT> &_pMin) const
+    /// \brief Compute scalar field minimum in a `_region`
+    /// \param[in] _region scalar field argument set to check
+    /// \param[out] _pMin scalar field argument that yields
+    ///   the minimum, or NaN if `_region` is empty
+    /// \return the scalar field minimum in the given `_region`,
+    ///   or NaN if `_region` is empty
+    public: ScalarT Minimum(const Region3<ScalarT> &_region,
+                            Vector3<ScalarT> &_pMin) const
+    {
+      if (_region.Empty())
       {
-        if (_region.Empty())
-        {
-          _pMin = Vector3<ScalarT>::NaN;
-          return std::numeric_limits<ScalarT>::quiet_NaN();
-        }
-        return this->k * (
-            this->p.Minimum(_region.Ix(), _pMin.X()) +
-            this->q.Minimum(_region.Iy(), _pMin.Y()) +
-            this->r.Minimum(_region.Iz(), _pMin.Z()));
+        _pMin = Vector3<ScalarT>::NaN;
+        return std::numeric_limits<ScalarT>::quiet_NaN();
       }
+      return this->k * (
+          this->p.Minimum(_region.Ix(), _pMin.X()) +
+          this->q.Minimum(_region.Iy(), _pMin.Y()) +
+          this->r.Minimum(_region.Iz(), _pMin.Z()));
+    }
 
-      /// \brief Compute scalar field minimum in a `_region`
-      /// \param[in] _region scalar field argument set to check
-      /// \return the scalar field minimum in the given `_region`,
-      ///   or NaN if `_region` is empty
-      public: ScalarT Minimum(const Region3<ScalarT> &_region) const
-      {
-        Vector3<ScalarT> pMin;
-        return this->Minimum(_region, pMin);
-      }
+    /// \brief Compute scalar field minimum in a `_region`
+    /// \param[in] _region scalar field argument set to check
+    /// \return the scalar field minimum in the given `_region`,
+    ///   or NaN if `_region` is empty
+    public: ScalarT Minimum(const Region3<ScalarT> &_region) const
+    {
+      Vector3<ScalarT> pMin;
+      return this->Minimum(_region, pMin);
+    }
 
-      /// \brief Compute scalar field minimum
-      /// \param[out] _pMin scalar field argument that yields
-      ///   the minimum, or NaN if `_region` is empty
-      /// \return the scalar field minimum
-      public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
-      {
-        return this->Minimum(Region3<ScalarT>::Unbounded, _pMin);
-      }
+    /// \brief Compute scalar field minimum
+    /// \param[out] _pMin scalar field argument that yields
+    ///   the minimum, or NaN if `_region` is empty
+    /// \return the scalar field minimum
+    public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
+    {
+      return this->Minimum(Region3<ScalarT>::Unbounded, _pMin);
+    }
 
-      /// \brief Compute scalar field minimum
-      /// \return the scalar field minimum
-      public: ScalarT Minimum() const
-      {
-        Vector3<ScalarT> pMin;
-        return this->Minimum(Region3<ScalarT>::Unbounded, pMin);
-      }
+    /// \brief Compute scalar field minimum
+    /// \return the scalar field minimum
+    public: ScalarT Minimum() const
+    {
+      Vector3<ScalarT> pMin;
+      return this->Minimum(Region3<ScalarT>::Unbounded, pMin);
+    }
 
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _field SeparableScalarField3 to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-          std::ostream &_out,
-          const gz::math::AdditivelySeparableScalarField3<
-          ScalarFunctionT, ScalarT> &_field)
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _field SeparableScalarField3 to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+        std::ostream &_out,
+        const gz::math::AdditivelySeparableScalarField3<
+        ScalarFunctionT, ScalarT> &_field)
+    {
+      using std::abs;  // enable ADL
+      constexpr ScalarT epsilon =
+          std::numeric_limits<ScalarT>::epsilon();
+      if ((abs(_field.k) - ScalarT(1)) < epsilon)
       {
-        using std::abs;  // enable ADL
-        constexpr ScalarT epsilon =
-            std::numeric_limits<ScalarT>::epsilon();
-        if ((abs(_field.k) - ScalarT(1)) < epsilon)
-        {
-          if (_field.k < ScalarT(0))
-          {
-            _out << "-";
-          }
-        }
-        else
+        if (_field.k < ScalarT(0))
         {
-          _out << _field.k << " ";
+          _out << "-";
         }
-        _out << "[(";
-        _field.p.Print(_out, "x");
-        _out << ") + (";
-        _field.q.Print(_out, "y");
-        _out << ") + (";
-        _field.r.Print(_out, "z");
-        return _out << ")]";
       }
+      else
+      {
+        _out << _field.k << " ";
+      }
+      _out << "[(";
+      _field.p.Print(_out, "x");
+      _out << ") + (";
+      _field.q.Print(_out, "y");
+      _out << ") + (";
+      _field.r.Print(_out, "z");
+      return _out << ")]";
+    }
 
-      /// \brief Scalar constant
-      private: ScalarT k;
+    /// \brief Scalar constant
+    private: ScalarT k;
 
-      /// \brief Scalar function of x
-      private: ScalarFunctionT p;
+    /// \brief Scalar function of x
+    private: ScalarFunctionT p;
 
-      /// \brief Scalar function of y
-      private: ScalarFunctionT q;
+    /// \brief Scalar function of y
+    private: ScalarFunctionT q;
 
-      /// \brief Scalar function of z
-      private: ScalarFunctionT r;
-    };
+    /// \brief Scalar function of z
+    private: ScalarFunctionT r;
+  };
 
-    template<typename ScalarFunctionT>
-    using AdditivelySeparableScalarField3f =
-        AdditivelySeparableScalarField3<ScalarFunctionT, float>;
+  template<typename ScalarFunctionT>
+  using AdditivelySeparableScalarField3f =
+      AdditivelySeparableScalarField3<ScalarFunctionT, float>;
 
-    template<typename ScalarFunctionT>
-    using AdditivelySeparableScalarField3d =
-        AdditivelySeparableScalarField3<ScalarFunctionT, double>;
-    }
-  }
-}
-#endif
+  template<typename ScalarFunctionT>
+  using AdditivelySeparableScalarField3d =
+      AdditivelySeparableScalarField3<ScalarFunctionT, double>;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_SEPARABLE_SCALAR_FIELD3_HH_
diff --git a/include/gz/math/Angle.hh b/include/gz/math/Angle.hh
index 114e1e870..de508778e 100644
--- a/include/gz/math/Angle.hh
+++ b/include/gz/math/Angle.hh
@@ -40,198 +40,195 @@
 /// \return the angle, in range
 #define GZ_NORMALIZE(a) (atan2(sin(a), cos(a)))
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Angle Angle.hh gz/math/Angle.hh
+  /// \brief The Angle class is used to simplify and clarify the use of
+  /// radians and degrees measurements. A default constructed Angle instance
+  /// has a value of zero radians/degrees.
+  ///
+  /// Unless otherwise specified, the Angle class assumes units are in
+  /// radians. An example of this are the stream insertion (<<) and
+  /// extraction (>>) operators.
+  ///
+  /// ## Example
+  ///
+  /// \snippet examples/angle_example.cc complete
+  class GZ_MATH_VISIBLE Angle
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Angle Angle.hh gz/math/Angle.hh
-    /// \brief The Angle class is used to simplify and clarify the use of
-    /// radians and degrees measurements. A default constructed Angle instance
-    /// has a value of zero radians/degrees.
-    ///
-    /// Unless otherwise specified, the Angle class assumes units are in
-    /// radians. An example of this are the stream insertion (<<) and
-    /// extraction (>>) operators.
-    ///
-    /// ## Example
+    /// \brief An angle with a value of zero.
+    /// Equivalent to math::Angle(0).
+    public: static const Angle &Zero;
+
+    /// \brief An angle with a value of Pi.
+    /// Equivalent to math::Angle(GZ_PI).
+    public: static const Angle &Pi;
+
+    /// \brief An angle with a value of Pi * 0.5.
+    /// Equivalent to math::Angle(GZ_PI * 0.5).
+    public: static const Angle &HalfPi;
+
+    /// \brief An angle with a value of Pi * 2.
+    /// Equivalent to math::Angle(GZ_PI * 2).
+    public: static const Angle &TwoPi;
+
+    /// \brief Default constructor that initializes an Angle to zero
+    /// radians/degrees.
+    public: Angle() = default;
+
+    /// \brief Conversion constructor that initializes an Angle to the
+    /// specified radians. This constructor supports implicit conversion
+    /// of a double to an Angle. For example:
     ///
-    /// \snippet examples/angle_example.cc complete
-    class GZ_MATH_VISIBLE Angle
+    /// \code
+    /// Angle a = 3.14;
+    /// \endcode
+    //
+    /// \param[in] _radian The radians used to initialize this Angle.
+    // cppcheck-suppress noExplicitConstructor
+    public: constexpr Angle(double _radian)
+    : value(_radian)
+    {
+    }
+
+    /// \brief Set the value from an angle in radians.
+    /// \param[in] _radian Radian value.
+    public: void SetRadian(double _radian);
+
+    /// \brief Set the value from an angle in degrees
+    /// \param[in] _degree Degree value
+    public: void SetDegree(double _degree);
+
+    /// \brief Get the angle in radians.
+    /// \return Double containing the angle's radian value.
+    public: double Radian() const;
+
+    /// \brief Get the angle in degrees.
+    /// \return Double containing the angle's degree value.
+    public: double Degree() const;
+
+    /// \brief Normalize the angle in the range -Pi to Pi. This
+    /// modifies the value contained in this Angle instance.
+    /// \sa Normalized()
+    public: void Normalize();
+
+    /// \brief Return the normalized angle in the range -Pi to Pi. This
+    /// does not modify the value contained in this Angle instance.
+    /// \return The normalized value of this Angle.
+    public: Angle Normalized() const;
+
+    /// \brief Return the angle's radian value
+    /// \return double containing the angle's radian value
+    public: double operator()() const;
+
+    /// \brief Dereference operator
+    /// \return Double containing the angle's radian value
+    public: inline double operator*() const
+            {
+              return value;
+            }
+
+    /// \brief Subtraction operator, result = this - _angle.
+    /// \param[in] _angle Angle for subtraction.
+    /// \return The new angle.
+    public: Angle operator-(const Angle &_angle) const;
+
+    /// \brief Addition operator, result = this + _angle.
+    /// \param[in] _angle Angle for addition.
+    /// \return The new angle.
+    public: Angle operator+(const Angle &_angle) const;
+
+    /// \brief Multiplication operator, result = this * _angle.
+    /// \param[in] _angle Angle for multiplication.
+    /// \return The new angle
+    public: Angle operator*(const Angle &_angle) const;
+
+    /// \brief Division operator, result = this / _angle.
+    /// \param[in] _angle Angle for division.
+    /// \return The new angle.
+    public: Angle operator/(const Angle &_angle) const;
+
+    /// \brief Subtraction set operator, this = this - _angle.
+    /// \param[in] _angle Angle for subtraction.
+    /// \return The new angle.
+    public: Angle operator-=(const Angle &_angle);
+
+    /// \brief Addition set operator, this = this + _angle.
+    /// \param[in] _angle Angle for addition.
+    /// \return The new angle.
+    public: Angle operator+=(const Angle &_angle);
+
+    /// \brief Multiplication set operator, this = this * _angle.
+    /// \param[in] _angle Angle for multiplication.
+    /// \return The new angle.
+    public: Angle operator*=(const Angle &_angle);
+
+    /// \brief Division set operator, this = this / _angle.
+    /// \param[in] _angle Angle for division.
+    /// \return The new angle.
+    public: Angle operator/=(const Angle &_angle);
+
+    /// \brief Equality operator, result = this == _angle.
+    /// \param[in] _angle Angle to check for equality.
+    /// \return True if this == _angle.
+    public: bool operator==(const Angle &_angle) const;
+
+    /// \brief Inequality operator
+    /// \param[in] _angle Angle to check for inequality.
+    /// \return True if this != _angle.
+    public: bool operator!=(const Angle &_angle) const;
+
+    /// \brief Less than operator.
+    /// \param[in] _angle Angle to check.
+    /// \return True if this < _angle.
+    public: bool operator<(const Angle &_angle) const;
+
+    /// \brief Less than or equal operator.
+    /// \param[in] _angle Angle to check.
+    /// \return True if this <= _angle.
+    public: bool operator<=(const Angle &_angle) const;
+
+    /// \brief Greater than operator.
+    /// \param[in] _angle Angle to check.
+    /// \return True if this > _angle.
+    public: bool operator>(const Angle &_angle) const;
+
+    /// \brief Greater than or equal operator.
+    /// \param[in] _angle Angle to check.
+    /// \return True if this >= _angle.
+    public: bool operator>=(const Angle &_angle) const;
+
+    /// \brief Stream insertion operator. Outputs in radians.
+    /// \param[in] _out Output stream.
+    /// \param[in] _a Angle to output.
+    /// \return The output stream.
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                                            const gz::math::Angle &_a)
     {
-      /// \brief An angle with a value of zero.
-      /// Equivalent to math::Angle(0).
-      public: static const Angle &Zero;
-
-      /// \brief An angle with a value of Pi.
-      /// Equivalent to math::Angle(GZ_PI).
-      public: static const Angle &Pi;
-
-      /// \brief An angle with a value of Pi * 0.5.
-      /// Equivalent to math::Angle(GZ_PI * 0.5).
-      public: static const Angle &HalfPi;
-
-      /// \brief An angle with a value of Pi * 2.
-      /// Equivalent to math::Angle(GZ_PI * 2).
-      public: static const Angle &TwoPi;
-
-      /// \brief Default constructor that initializes an Angle to zero
-      /// radians/degrees.
-      public: Angle() = default;
-
-      /// \brief Conversion constructor that initializes an Angle to the
-      /// specified radians. This constructor supports implicit conversion
-      /// of a double to an Angle. For example:
-      ///
-      /// \code
-      /// Angle a = 3.14;
-      /// \endcode
-      //
-      /// \param[in] _radian The radians used to initialize this Angle.
-      // cppcheck-suppress noExplicitConstructor
-      public: constexpr Angle(double _radian)
-      : value(_radian)
-      {
-      }
-
-      /// \brief Set the value from an angle in radians.
-      /// \param[in] _radian Radian value.
-      public: void SetRadian(double _radian);
-
-      /// \brief Set the value from an angle in degrees
-      /// \param[in] _degree Degree value
-      public: void SetDegree(double _degree);
-
-      /// \brief Get the angle in radians.
-      /// \return Double containing the angle's radian value.
-      public: double Radian() const;
-
-      /// \brief Get the angle in degrees.
-      /// \return Double containing the angle's degree value.
-      public: double Degree() const;
-
-      /// \brief Normalize the angle in the range -Pi to Pi. This
-      /// modifies the value contained in this Angle instance.
-      /// \sa Normalized()
-      public: void Normalize();
-
-      /// \brief Return the normalized angle in the range -Pi to Pi. This
-      /// does not modify the value contained in this Angle instance.
-      /// \return The normalized value of this Angle.
-      public: Angle Normalized() const;
-
-      /// \brief Return the angle's radian value
-      /// \return double containing the angle's radian value
-      public: double operator()() const;
-
-      /// \brief Dereference operator
-      /// \return Double containing the angle's radian value
-      public: inline double operator*() const
-              {
-                return value;
-              }
-
-      /// \brief Subtraction operator, result = this - _angle.
-      /// \param[in] _angle Angle for subtraction.
-      /// \return The new angle.
-      public: Angle operator-(const Angle &_angle) const;
-
-      /// \brief Addition operator, result = this + _angle.
-      /// \param[in] _angle Angle for addition.
-      /// \return The new angle.
-      public: Angle operator+(const Angle &_angle) const;
-
-      /// \brief Multiplication operator, result = this * _angle.
-      /// \param[in] _angle Angle for multiplication.
-      /// \return The new angle
-      public: Angle operator*(const Angle &_angle) const;
-
-      /// \brief Division operator, result = this / _angle.
-      /// \param[in] _angle Angle for division.
-      /// \return The new angle.
-      public: Angle operator/(const Angle &_angle) const;
-
-      /// \brief Subtraction set operator, this = this - _angle.
-      /// \param[in] _angle Angle for subtraction.
-      /// \return The new angle.
-      public: Angle operator-=(const Angle &_angle);
-
-      /// \brief Addition set operator, this = this + _angle.
-      /// \param[in] _angle Angle for addition.
-      /// \return The new angle.
-      public: Angle operator+=(const Angle &_angle);
-
-      /// \brief Multiplication set operator, this = this * _angle.
-      /// \param[in] _angle Angle for multiplication.
-      /// \return The new angle.
-      public: Angle operator*=(const Angle &_angle);
-
-      /// \brief Division set operator, this = this / _angle.
-      /// \param[in] _angle Angle for division.
-      /// \return The new angle.
-      public: Angle operator/=(const Angle &_angle);
-
-      /// \brief Equality operator, result = this == _angle.
-      /// \param[in] _angle Angle to check for equality.
-      /// \return True if this == _angle.
-      public: bool operator==(const Angle &_angle) const;
-
-      /// \brief Inequality operator
-      /// \param[in] _angle Angle to check for inequality.
-      /// \return True if this != _angle.
-      public: bool operator!=(const Angle &_angle) const;
-
-      /// \brief Less than operator.
-      /// \param[in] _angle Angle to check.
-      /// \return True if this < _angle.
-      public: bool operator<(const Angle &_angle) const;
-
-      /// \brief Less than or equal operator.
-      /// \param[in] _angle Angle to check.
-      /// \return True if this <= _angle.
-      public: bool operator<=(const Angle &_angle) const;
-
-      /// \brief Greater than operator.
-      /// \param[in] _angle Angle to check.
-      /// \return True if this > _angle.
-      public: bool operator>(const Angle &_angle) const;
-
-      /// \brief Greater than or equal operator.
-      /// \param[in] _angle Angle to check.
-      /// \return True if this >= _angle.
-      public: bool operator>=(const Angle &_angle) const;
-
-      /// \brief Stream insertion operator. Outputs in radians.
-      /// \param[in] _out Output stream.
-      /// \param[in] _a Angle to output.
-      /// \return The output stream.
-      public: friend std::ostream &operator<<(std::ostream &_out,
-                                              const gz::math::Angle &_a)
-      {
-        _out << _a.Radian();
-        return _out;
-      }
-
-      /// \brief Stream extraction operator. Assumes input is in radians.
-      /// \param[in,out] _in Input stream.
-      /// \param[out] _a Angle to read value into.
-      /// \return The input stream.
-      public: friend std::istream &operator>>(std::istream &_in,
-                                              gz::math::Angle &_a)
-      {
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        _in >> _a.value;
-        return _in;
-      }
-
-      /// The angle in radians, with a default value of zero.
-      private: double value{0};
-    };
+      _out << _a.Radian();
+      return _out;
     }
-  }
-}
 
-#endif
+    /// \brief Stream extraction operator. Assumes input is in radians.
+    /// \param[in,out] _in Input stream.
+    /// \param[out] _a Angle to read value into.
+    /// \return The input stream.
+    public: friend std::istream &operator>>(std::istream &_in,
+                                            gz::math::Angle &_a)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      _in >> _a.value;
+      return _in;
+    }
+
+    /// The angle in radians, with a default value of zero.
+    private: double value{0};
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+
+#endif  // GZ_MATH_ANGLE_HH_
diff --git a/include/gz/math/AxisAlignedBox.hh b/include/gz/math/AxisAlignedBox.hh
index d27e48a99..6dfaa5053 100644
--- a/include/gz/math/AxisAlignedBox.hh
+++ b/include/gz/math/AxisAlignedBox.hh
@@ -27,223 +27,220 @@
 #include <gz/math/Vector3.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \class AxisAlignedBox AxisAlignedBox.hh gz/math/AxisAlignedBox.hh
+  /// \brief Mathematical representation of a box that is aligned along
+  /// an X,Y,Z axis.
+  class GZ_MATH_VISIBLE AxisAlignedBox
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \class AxisAlignedBox AxisAlignedBox.hh gz/math/AxisAlignedBox.hh
-    /// \brief Mathematical representation of a box that is aligned along
-    /// an X,Y,Z axis.
-    class GZ_MATH_VISIBLE AxisAlignedBox
+    /// \brief Default constructor. This constructor will set the box's
+    /// minimum and maximum corners to the highest (max) and lowest
+    /// floating point values available to indicate that it is uninitialized.
+    /// The default box does not intersect any other boxes or contain any
+    /// points since it has no extent. Its center is the origin and its side
+    /// lengths are 0.
+    public: AxisAlignedBox();
+
+    /// \brief Constructor. This constructor will compute the box's
+    /// minimum and maximum corners based on the two arguments.
+    /// \param[in] _vec1 One corner of the box
+    /// \param[in] _vec2 Another corner of the box
+    public: AxisAlignedBox(const Vector3d &_vec1, const Vector3d &_vec2);
+
+    /// \brief Constructor. This constructor will compute the box's
+    /// minimum and maximum corners based on the arguments.
+    /// \param[in] _vec1X One corner's X position
+    /// \param[in] _vec1Y One corner's Y position
+    /// \param[in] _vec1Z One corner's Z position
+    /// \param[in] _vec2X Other corner's X position
+    /// \param[in] _vec2Y Other corner's Y position
+    /// \param[in] _vec2Z Other corner's Z position
+    public: AxisAlignedBox(double _vec1X, double _vec1Y, double _vec1Z,
+                double _vec2X, double _vec2Y, double _vec2Z);
+
+    /// \brief Get the length along the x dimension
+    /// \return Double value of the length in the x dimension
+    public: double XLength() const;
+
+    /// \brief Get the length along the y dimension
+    /// \return Double value of the length in the y dimension
+    public: double YLength() const;
+
+    /// \brief Get the length along the z dimension
+    /// \return Double value of the length in the z dimension
+    public: double ZLength() const;
+
+    /// \brief Get the size of the box
+    /// \return Size of the box
+    public: math::Vector3d Size() const;
+
+    /// \brief Get the box center
+    /// \return The center position of the box
+    public: math::Vector3d Center() const;
+
+    /// \brief Merge a box with this box
+    /// \param[in]  _box AxisAlignedBox to add to this box
+    public: void Merge(const AxisAlignedBox &_box);
+
+    /// \brief Addition operator. result = this + _b
+    /// \param[in] _b AxisAlignedBox to add
+    /// \return The new box
+    public: AxisAlignedBox operator+(const AxisAlignedBox &_b) const;
+
+    /// \brief Addition set operator. this = this + _b
+    /// \param[in] _b AxisAlignedBox to add
+    /// \return This new box
+    public: const AxisAlignedBox &operator+=(const AxisAlignedBox &_b);
+
+    /// \brief Equality test operator
+    /// \param[in] _b AxisAlignedBox to test
+    /// \return True if equal
+    public: bool operator==(const AxisAlignedBox &_b) const;
+
+    /// \brief Inequality test operator
+    /// \param[in] _b AxisAlignedBox to test
+    /// \return True if not equal
+    public: bool operator!=(const AxisAlignedBox &_b) const;
+
+    /// \brief Subtract a vector from the min and max values
+    /// \param _v The vector to use during subtraction
+    /// \return The new box
+    public: AxisAlignedBox operator-(const Vector3d &_v);
+
+    /// \brief Add a vector to the min and max values
+    /// \param _v The vector to use during addition
+    /// \return The new box
+    public: AxisAlignedBox operator+(const Vector3d &_v);
+
+    /// \brief Subtract a vector from the min and max values
+    /// \param _v The vector to use during subtraction
+    /// \return The new box
+    public: AxisAlignedBox operator-(const Vector3d &_v) const;
+
+    /// \brief Add a vector to the min and max values
+    /// \param _v The vector to use during addition
+    /// \return The new box
+    public: AxisAlignedBox operator+(const Vector3d &_v) const;
+
+    /// \brief Output operator
+    /// \param[in] _out Output stream
+    /// \param[in] _b AxisAlignedBox to output to the stream
+    /// \return The stream
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                const gz::math::AxisAlignedBox &_b)
     {
-      /// \brief Default constructor. This constructor will set the box's
-      /// minimum and maximum corners to the highest (max) and lowest
-      /// floating point values available to indicate that it is uninitialized.
-      /// The default box does not intersect any other boxes or contain any
-      /// points since it has no extent. Its center is the origin and its side
-      /// lengths are 0.
-      public: AxisAlignedBox();
-
-      /// \brief Constructor. This constructor will compute the box's
-      /// minimum and maximum corners based on the two arguments.
-      /// \param[in] _vec1 One corner of the box
-      /// \param[in] _vec2 Another corner of the box
-      public: AxisAlignedBox(const Vector3d &_vec1, const Vector3d &_vec2);
-
-      /// \brief Constructor. This constructor will compute the box's
-      /// minimum and maximum corners based on the arguments.
-      /// \param[in] _vec1X One corner's X position
-      /// \param[in] _vec1Y One corner's Y position
-      /// \param[in] _vec1Z One corner's Z position
-      /// \param[in] _vec2X Other corner's X position
-      /// \param[in] _vec2Y Other corner's Y position
-      /// \param[in] _vec2Z Other corner's Z position
-      public: AxisAlignedBox(double _vec1X, double _vec1Y, double _vec1Z,
-                  double _vec2X, double _vec2Y, double _vec2Z);
-
-      /// \brief Get the length along the x dimension
-      /// \return Double value of the length in the x dimension
-      public: double XLength() const;
-
-      /// \brief Get the length along the y dimension
-      /// \return Double value of the length in the y dimension
-      public: double YLength() const;
-
-      /// \brief Get the length along the z dimension
-      /// \return Double value of the length in the z dimension
-      public: double ZLength() const;
-
-      /// \brief Get the size of the box
-      /// \return Size of the box
-      public: math::Vector3d Size() const;
-
-      /// \brief Get the box center
-      /// \return The center position of the box
-      public: math::Vector3d Center() const;
-
-      /// \brief Merge a box with this box
-      /// \param[in]  _box AxisAlignedBox to add to this box
-      public: void Merge(const AxisAlignedBox &_box);
-
-      /// \brief Addition operator. result = this + _b
-      /// \param[in] _b AxisAlignedBox to add
-      /// \return The new box
-      public: AxisAlignedBox operator+(const AxisAlignedBox &_b) const;
-
-      /// \brief Addition set operator. this = this + _b
-      /// \param[in] _b AxisAlignedBox to add
-      /// \return This new box
-      public: const AxisAlignedBox &operator+=(const AxisAlignedBox &_b);
-
-      /// \brief Equality test operator
-      /// \param[in] _b AxisAlignedBox to test
-      /// \return True if equal
-      public: bool operator==(const AxisAlignedBox &_b) const;
-
-      /// \brief Inequality test operator
-      /// \param[in] _b AxisAlignedBox to test
-      /// \return True if not equal
-      public: bool operator!=(const AxisAlignedBox &_b) const;
-
-      /// \brief Subtract a vector from the min and max values
-      /// \param _v The vector to use during subtraction
-      /// \return The new box
-      public: AxisAlignedBox operator-(const Vector3d &_v);
-
-      /// \brief Add a vector to the min and max values
-      /// \param _v The vector to use during addition
-      /// \return The new box
-      public: AxisAlignedBox operator+(const Vector3d &_v);
-
-      /// \brief Subtract a vector from the min and max values
-      /// \param _v The vector to use during subtraction
-      /// \return The new box
-      public: AxisAlignedBox operator-(const Vector3d &_v) const;
-
-      /// \brief Add a vector to the min and max values
-      /// \param _v The vector to use during addition
-      /// \return The new box
-      public: AxisAlignedBox operator+(const Vector3d &_v) const;
-
-      /// \brief Output operator
-      /// \param[in] _out Output stream
-      /// \param[in] _b AxisAlignedBox to output to the stream
-      /// \return The stream
-      public: friend std::ostream &operator<<(std::ostream &_out,
-                  const gz::math::AxisAlignedBox &_b)
-      {
-        _out << "Min[" << _b.Min() << "] Max[" << _b.Max() << "]";
-        return _out;
-      }
-
-      /// \brief Get the minimum corner.
-      /// \return The Vector3d that is the minimum corner of the box.
-      public: const Vector3d &Min() const;
-
-      /// \brief Get the maximum corner.
-      /// \return The Vector3d that is the maximum corner of the box.
-      public: const Vector3d &Max() const;
-
-      /// \brief Get a mutable version of the minimum corner.
-      /// \return The Vector3d that is the minimum corner of the box.
-      public: Vector3d &Min();
-
-      /// \brief Get a mutable version of the maximum corner.
-      /// \return The Vector3d that is the maximum corner of the box.
-      public: Vector3d &Max();
-
-      /// \brief Test box intersection. This test will only work if
-      /// both box's minimum corner is less than or equal to their
-      /// maximum corner.
-      /// \param[in] _box AxisAlignedBox to check for intersection with
-      /// this box.
-      /// \return True if this box intersects _box.
-      public: bool Intersects(const AxisAlignedBox &_box) const;
-
-      /// \brief Check if a point lies inside the box.
-      /// \param[in] _p Point to check.
-      /// \return True if the point is inside the box.
-      public: bool Contains(const Vector3d &_p) const;
-
-      /// \brief Check if a ray (origin, direction) intersects the box.
-      /// \param[in] _origin Origin of the ray.
-      /// \param[in] _dir Direction of the ray. This ray will be normalized.
-      /// \param[in] _min Minimum allowed distance.
-      /// \param[in] _max Maximum allowed distance.
-      /// \return A boolean
-      public: bool IntersectCheck(const Vector3d &_origin, const Vector3d &_dir,
-                  const double _min, const double _max) const;
-
-      /// \brief Check if a ray (origin, direction) intersects the box.
-      /// \param[in] _origin Origin of the ray.
-      /// \param[in] _dir Direction of the ray. This ray will be normalized.
-      /// \param[in] _min Minimum allowed distance.
-      /// \param[in] _max Maximum allowed distance.
-      /// \return A boolean and double tuple. The boolean value is true
-      /// if the line intersects the box.
-      ///
-      /// The double is the distance from
-      /// the ray's start  to the closest intersection point on the box,
-      /// minus the _min distance. For example, if _min == 0.5 and the
-      /// intersection happens at a distance of 2.0 from _origin then returned
-      /// distance is 1.5.
-      ///
-      /// The double value is zero when the boolean value is false.
-      public: std::tuple<bool, double> IntersectDist(
-                  const Vector3d &_origin, const Vector3d &_dir,
-                  const double _min, const double _max) const;
-
-      /// \brief Check if a ray (origin, direction) intersects the box.
-      /// \param[in] _origin Origin of the ray.
-      /// \param[in] _dir Direction of the ray. This ray will be normalized.
-      /// \param[in] _min Minimum allowed distance.
-      /// \param[in] _max Maximum allowed distance.
-      /// \return A boolean, double, Vector3d tuple. The boolean value is true
-      /// if the line intersects the box.
-      ///
-      /// The double is the distance from the ray's start to the closest
-      /// intersection point on the box,
-      /// minus the _min distance. For example, if _min == 0.5 and the
-      /// intersection happens at a distance of 2.0 from _origin then returned
-      /// distance is 1.5.
-      /// The double value is zero when the boolean value is false. The
-      ///
-      /// Vector3d is the intersection point on the box. The Vector3d value
-      /// is zero if the boolean value is false.
-      public: std::tuple<bool, double, Vector3d> Intersect(
-                  const Vector3d &_origin, const Vector3d &_dir,
-                  const double _min, const double _max) const;
-
-      /// \brief Check if a line intersects the box.
-      /// \param[in] _line The line to check against this box.
-      /// \return A boolean, double, Vector3d tuple. The boolean value is true
-      /// if the line intersects the box. The double is the distance from
-      /// the line's start to the closest intersection point on the box.
-      /// The double value is zero when the boolean value is false. The
-      /// Vector3d is the intersection point on the box. The Vector3d value
-      /// is zero if the boolean value is false.
-      public: std::tuple<bool, double, Vector3d> Intersect(
-                  const Line3d &_line) const;
-
-      /// \brief Get the volume of the box in m^3.
-      /// \return Volume of the box in m^3.
-      public: double Volume() const;
-
-      /// \brief Clip a line to a dimension of the box.
-      /// This is a helper function to Intersects
-      /// \param[in] _d Dimension of the box(0, 1, or 2).
-      /// \param[in] _line Line to clip
-      /// \param[in,out] _low Close distance
-      /// \param[in,out] _high Far distance
-      private: bool ClipLine(const int _d, const Line3d &_line,
-                   double &_low, double &_high) const;
-
-      /// \brief Private data pointer
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
+      _out << "Min[" << _b.Min() << "] Max[" << _b.Max() << "]";
+      return _out;
     }
-  }
-}
-#endif
+
+    /// \brief Get the minimum corner.
+    /// \return The Vector3d that is the minimum corner of the box.
+    public: const Vector3d &Min() const;
+
+    /// \brief Get the maximum corner.
+    /// \return The Vector3d that is the maximum corner of the box.
+    public: const Vector3d &Max() const;
+
+    /// \brief Get a mutable version of the minimum corner.
+    /// \return The Vector3d that is the minimum corner of the box.
+    public: Vector3d &Min();
+
+    /// \brief Get a mutable version of the maximum corner.
+    /// \return The Vector3d that is the maximum corner of the box.
+    public: Vector3d &Max();
+
+    /// \brief Test box intersection. This test will only work if
+    /// both box's minimum corner is less than or equal to their
+    /// maximum corner.
+    /// \param[in] _box AxisAlignedBox to check for intersection with
+    /// this box.
+    /// \return True if this box intersects _box.
+    public: bool Intersects(const AxisAlignedBox &_box) const;
+
+    /// \brief Check if a point lies inside the box.
+    /// \param[in] _p Point to check.
+    /// \return True if the point is inside the box.
+    public: bool Contains(const Vector3d &_p) const;
+
+    /// \brief Check if a ray (origin, direction) intersects the box.
+    /// \param[in] _origin Origin of the ray.
+    /// \param[in] _dir Direction of the ray. This ray will be normalized.
+    /// \param[in] _min Minimum allowed distance.
+    /// \param[in] _max Maximum allowed distance.
+    /// \return A boolean
+    public: bool IntersectCheck(const Vector3d &_origin, const Vector3d &_dir,
+                const double _min, const double _max) const;
+
+    /// \brief Check if a ray (origin, direction) intersects the box.
+    /// \param[in] _origin Origin of the ray.
+    /// \param[in] _dir Direction of the ray. This ray will be normalized.
+    /// \param[in] _min Minimum allowed distance.
+    /// \param[in] _max Maximum allowed distance.
+    /// \return A boolean and double tuple. The boolean value is true
+    /// if the line intersects the box.
+    ///
+    /// The double is the distance from
+    /// the ray's start  to the closest intersection point on the box,
+    /// minus the _min distance. For example, if _min == 0.5 and the
+    /// intersection happens at a distance of 2.0 from _origin then returned
+    /// distance is 1.5.
+    ///
+    /// The double value is zero when the boolean value is false.
+    public: std::tuple<bool, double> IntersectDist(
+                const Vector3d &_origin, const Vector3d &_dir,
+                const double _min, const double _max) const;
+
+    /// \brief Check if a ray (origin, direction) intersects the box.
+    /// \param[in] _origin Origin of the ray.
+    /// \param[in] _dir Direction of the ray. This ray will be normalized.
+    /// \param[in] _min Minimum allowed distance.
+    /// \param[in] _max Maximum allowed distance.
+    /// \return A boolean, double, Vector3d tuple. The boolean value is true
+    /// if the line intersects the box.
+    ///
+    /// The double is the distance from the ray's start to the closest
+    /// intersection point on the box,
+    /// minus the _min distance. For example, if _min == 0.5 and the
+    /// intersection happens at a distance of 2.0 from _origin then returned
+    /// distance is 1.5.
+    /// The double value is zero when the boolean value is false. The
+    ///
+    /// Vector3d is the intersection point on the box. The Vector3d value
+    /// is zero if the boolean value is false.
+    public: std::tuple<bool, double, Vector3d> Intersect(
+                const Vector3d &_origin, const Vector3d &_dir,
+                const double _min, const double _max) const;
+
+    /// \brief Check if a line intersects the box.
+    /// \param[in] _line The line to check against this box.
+    /// \return A boolean, double, Vector3d tuple. The boolean value is true
+    /// if the line intersects the box. The double is the distance from
+    /// the line's start to the closest intersection point on the box.
+    /// The double value is zero when the boolean value is false. The
+    /// Vector3d is the intersection point on the box. The Vector3d value
+    /// is zero if the boolean value is false.
+    public: std::tuple<bool, double, Vector3d> Intersect(
+                const Line3d &_line) const;
+
+    /// \brief Get the volume of the box in m^3.
+    /// \return Volume of the box in m^3.
+    public: double Volume() const;
+
+    /// \brief Clip a line to a dimension of the box.
+    /// This is a helper function to Intersects
+    /// \param[in] _d Dimension of the box(0, 1, or 2).
+    /// \param[in] _line Line to clip
+    /// \param[in,out] _low Close distance
+    /// \param[in,out] _high Far distance
+    private: bool ClipLine(const int _d, const Line3d &_line,
+                 double &_low, double &_high) const;
+
+    /// \brief Private data pointer
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_AXISALIGNEDBOX_HH_
diff --git a/include/gz/math/Box.hh b/include/gz/math/Box.hh
index 33551c17d..870585cec 100644
--- a/include/gz/math/Box.hh
+++ b/include/gz/math/Box.hh
@@ -28,204 +28,201 @@
 
 #include <set>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_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 gz/math/Box.hh
+  /// \brief A representation of a box. All units are in meters.
+  ///
+  /// The box class supports defining a size and material properties.
+  /// See Material for more on material properties.
+  ///
+  /// By default, a box's size (length, width, and height)  is zero.
+  ///
+  /// See AxisAlignedBox for an axis aligned box implementation.
+  template<typename Precision>
+  class Box
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_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 gz/math/Box.hh
-    /// \brief A representation of a box. All units are in meters.
+    /// \brief Default constructor.
+    public: Box() = default;
+
+    /// \brief Construct a box with specified dimensions.
+    /// \param[in] _length Length of the box in meters.
+    /// \param[in] _width Width of the box in meters.
+    /// \param[in] _height Height of the box in meters.
+    public: Box(const Precision _length,
+                const Precision _width,
+                const Precision _height);
+
+    /// \brief Construct a box with specified dimensions and a material.
+    /// \param[in] _length Length of the box in meters.
+    /// \param[in] _width Width of the box in meters.
+    /// \param[in] _height Height of the box.
+    /// \param[in] _mat Material property for the box.
+    public: Box(const Precision _length, const Precision _width,
+                const Precision _height,
+                const gz::math::Material &_mat);
+
+    /// \brief Construct a box with specified dimensions, in vector form.
+    /// \param[in] _size Size of the box. The vector _size has the following
+    /// mapping:
     ///
-    /// The box class supports defining a size and material properties.
-    /// See Material for more on material properties.
+    /// * _size[0] == length in meters
+    /// * _size[1] == width in meters
+    /// * _size[2] == height in meters
+    public: explicit Box(const Vector3<Precision> &_size);
+
+    /// \brief Construct a box with specified dimensions, in vector form
+    /// and a material.
+    /// \param[in] _size Size of the box. The vector _size has the following
+    /// mapping:
     ///
-    /// By default, a box's size (length, width, and height)  is zero.
+    /// * _size[0] == length in meters
+    /// * _size[1] == width in meters
+    /// * _size[2] == height in meters
+    /// \param[in] _mat Material property for the box.
+    public: Box(const Vector3<Precision> &_size,
+                const gz::math::Material &_mat);
+
+    /// \brief Get the size of the box.
+    /// \return Size of the box in meters.
+    public: math::Vector3<Precision> Size() const;
+
+    /// \brief Set the size of the box.
+    /// \param[in] _size Size of the box. The vector _size has the following
+    /// mapping:
     ///
-    /// See AxisAlignedBox for an axis aligned box implementation.
-    template<typename Precision>
-    class Box
-    {
-      /// \brief Default constructor.
-      public: Box() = default;
-
-      /// \brief Construct a box with specified dimensions.
-      /// \param[in] _length Length of the box in meters.
-      /// \param[in] _width Width of the box in meters.
-      /// \param[in] _height Height of the box in meters.
-      public: Box(const Precision _length,
-                  const Precision _width,
-                  const Precision _height);
-
-      /// \brief Construct a box with specified dimensions and a material.
-      /// \param[in] _length Length of the box in meters.
-      /// \param[in] _width Width of the box in meters.
-      /// \param[in] _height Height of the box.
-      /// \param[in] _mat Material property for the box.
-      public: Box(const Precision _length, const Precision _width,
-                  const Precision _height,
-                  const gz::math::Material &_mat);
-
-      /// \brief Construct a box with specified dimensions, in vector form.
-      /// \param[in] _size Size of the box. The vector _size has the following
-      /// mapping:
-      ///
-      /// * _size[0] == length in meters
-      /// * _size[1] == width in meters
-      /// * _size[2] == height in meters
-      public: explicit Box(const Vector3<Precision> &_size);
-
-      /// \brief Construct a box with specified dimensions, in vector form
-      /// and a material.
-      /// \param[in] _size Size of the box. The vector _size has the following
-      /// mapping:
-      ///
-      /// * _size[0] == length in meters
-      /// * _size[1] == width in meters
-      /// * _size[2] == height in meters
-      /// \param[in] _mat Material property for the box.
-      public: Box(const Vector3<Precision> &_size,
-                  const gz::math::Material &_mat);
-
-      /// \brief Get the size of the box.
-      /// \return Size of the box in meters.
-      public: math::Vector3<Precision> Size() const;
-
-      /// \brief Set the size of the box.
-      /// \param[in] _size Size of the box. The vector _size has the following
-      /// mapping:
-      ///
-      /// * _size[0] == lengt in metersh
-      /// * _size[1] == widt in metersh
-      /// * _size[2] == heigh in meterst
-      public: void SetSize(const math::Vector3<Precision> &_size);
-
-      /// \brief Set the size of the box.
-      /// \param[in] _length Length of the box in meters.
-      /// \param[in] _width Width of the box in meters.
-      /// \param[in] _height Height of the box in meters.
-      public: void SetSize(const Precision _length,
-                           const Precision _width,
-                           const Precision _height);
-
-      /// \brief Equality test operator.
-      /// \param[in] _b Box to test.
-      /// \return True if equal.
-      public: bool operator==(const Box<Precision> &_b) const;
-
-      /// \brief Inequality test operator.
-      /// \param[in] _b Box to test.
-      /// \return True if not equal.
-      public: bool operator!=(const Box<Precision> &_b) const;
-
-      /// \brief Get the material associated with this box.
-      /// \return The material assigned to this box.
-      public: const gz::math::Material &Material() const;
-
-      /// \brief Set the material associated with this box.
-      /// \param[in] _mat The material assigned to this box.
-      public: void SetMaterial(const gz::math::Material &_mat);
-
-      /// \brief Get the volume of the box in m^3.
-      /// \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
-      /// values greater than zero. The Material of the box is ignored.
-      /// \param[in] _mass Mass of the box, in kg. This value should be
-      /// greater than zero.
-      /// \return Density of the box in kg/m^3. A negative value is
-      /// returned if the size or _mass is <= 0.
-      public: Precision DensityFromMass(const Precision _mass) const;
-
-      /// \brief Set the density of this box based on a mass value.
-      /// Density is computed using
-      /// double DensityFromMass(const double _mass) const. The
-      /// box is assumed to be solid with uniform density. This
-      /// function requires the box's size to be set to
-      /// values greater than zero. The existing Material density value is
-      /// overwritten only if the return value from this true.
-      /// \param[in] _mass Mass of the box, in kg. This value should be
-      /// greater than zero.
-      /// \return True if the density was set. False is returned if the
-      /// box's size or the _mass value are <= 0.
-      /// \sa double DensityFromMass(const double _mass) const
-      public: bool SetDensityFromMass(const Precision _mass);
-
-      /// \brief Get the mass matrix for this box. This function
-      /// is only meaningful if the box's size and material
-      /// have been set.
-      /// \param[out] _massMat The computed mass matrix will be stored
-      /// here.
-      /// \return False if computation of the mass matrix failed, which
-      /// could be due to an invalid size (<=0) or density (<=0).
-      public: bool MassMatrix(MassMatrix3<Precision> &_massMat) const;
-
-      /// \brief Get the mass matrix for this box. This function
-      /// is only meaningful if the box's size and material
-      /// have been set.
-      /// \return The computed mass matrix if parameters are valid
-      /// (radius > 0), (length > 0), and (density > 0). Otherwise
-      /// std::nullopt is returned.
-      public: std::optional< MassMatrix3<Precision> > MassMatrix() 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;
-
-      /// \brief The box's material.
-      private: gz::math::Material material;
-    };
-
-    /// \typedef Box<int> Boxi
-    /// \brief Box with integer precision.
-    typedef Box<int> Boxi;
-
-    /// \typedef Box<double> Boxd
-    /// \brief Box with double precision.
-    typedef Box<double> Boxd;
-
-    /// \typedef Box<float> Boxf
-    /// \brief Box with float precision.
-    typedef Box<float> Boxf;
-    }
-  }
-}
+    /// * _size[0] == lengt in metersh
+    /// * _size[1] == widt in metersh
+    /// * _size[2] == heigh in meterst
+    public: void SetSize(const math::Vector3<Precision> &_size);
+
+    /// \brief Set the size of the box.
+    /// \param[in] _length Length of the box in meters.
+    /// \param[in] _width Width of the box in meters.
+    /// \param[in] _height Height of the box in meters.
+    public: void SetSize(const Precision _length,
+                         const Precision _width,
+                         const Precision _height);
+
+    /// \brief Equality test operator.
+    /// \param[in] _b Box to test.
+    /// \return True if equal.
+    public: bool operator==(const Box<Precision> &_b) const;
+
+    /// \brief Inequality test operator.
+    /// \param[in] _b Box to test.
+    /// \return True if not equal.
+    public: bool operator!=(const Box<Precision> &_b) const;
+
+    /// \brief Get the material associated with this box.
+    /// \return The material assigned to this box.
+    public: const gz::math::Material &Material() const;
+
+    /// \brief Set the material associated with this box.
+    /// \param[in] _mat The material assigned to this box.
+    public: void SetMaterial(const gz::math::Material &_mat);
+
+    /// \brief Get the volume of the box in m^3.
+    /// \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
+    /// values greater than zero. The Material of the box is ignored.
+    /// \param[in] _mass Mass of the box, in kg. This value should be
+    /// greater than zero.
+    /// \return Density of the box in kg/m^3. A negative value is
+    /// returned if the size or _mass is <= 0.
+    public: Precision DensityFromMass(const Precision _mass) const;
+
+    /// \brief Set the density of this box based on a mass value.
+    /// Density is computed using
+    /// double DensityFromMass(const double _mass) const. The
+    /// box is assumed to be solid with uniform density. This
+    /// function requires the box's size to be set to
+    /// values greater than zero. The existing Material density value is
+    /// overwritten only if the return value from this true.
+    /// \param[in] _mass Mass of the box, in kg. This value should be
+    /// greater than zero.
+    /// \return True if the density was set. False is returned if the
+    /// box's size or the _mass value are <= 0.
+    /// \sa double DensityFromMass(const double _mass) const
+    public: bool SetDensityFromMass(const Precision _mass);
+
+    /// \brief Get the mass matrix for this box. This function
+    /// is only meaningful if the box's size and material
+    /// have been set.
+    /// \param[out] _massMat The computed mass matrix will be stored
+    /// here.
+    /// \return False if computation of the mass matrix failed, which
+    /// could be due to an invalid size (<=0) or density (<=0).
+    public: bool MassMatrix(MassMatrix3<Precision> &_massMat) const;
+
+    /// \brief Get the mass matrix for this box. This function
+    /// is only meaningful if the box's size and material
+    /// have been set.
+    /// \return The computed mass matrix if parameters are valid
+    /// (radius > 0), (length > 0), and (density > 0). Otherwise
+    /// std::nullopt is returned.
+    public: std::optional< MassMatrix3<Precision> > MassMatrix() 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;
+
+    /// \brief The box's material.
+    private: gz::math::Material material;
+  };
+
+  /// \typedef Box<int> Boxi
+  /// \brief Box with integer precision.
+  typedef Box<int> Boxi;
+
+  /// \typedef Box<double> Boxd
+  /// \brief Box with double precision.
+  typedef Box<double> Boxd;
+
+  /// \typedef Box<float> Boxf
+  /// \brief Box with float precision.
+  typedef Box<float> Boxf;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
 #include "gz/math/detail/Box.hh"
-#endif
+#endif  // GZ_MATH_BOX_HH_
diff --git a/include/gz/math/Capsule.hh b/include/gz/math/Capsule.hh
index bf445bac0..b081701cd 100644
--- a/include/gz/math/Capsule.hh
+++ b/include/gz/math/Capsule.hh
@@ -21,128 +21,124 @@
 #include "gz/math/MassMatrix3.hh"
 #include "gz/math/Material.hh"
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Capsule Capsule.hh gz/math/Capsule.hh
+  /// \brief A representation of a capsule or sphere-capped cylinder.
+  ///
+  /// The capsule class supports defining a capsule with a radius,
+  /// length, and material properties. The shape is equivalent to a cylinder
+  /// aligned with the Z-axis and capped with hemispheres. Radius and
+  /// length are in meters. See Material for more on material properties.
+  /// \tparam Precision Scalar numeric type.
+  template<typename Precision>
+  class Capsule
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Capsule Capsule.hh gz/math/Capsule.hh
-    /// \brief A representation of a capsule or sphere-capped cylinder.
-    ///
-    /// The capsule class supports defining a capsule with a radius,
-    /// length, and material properties. The shape is equivalent to a cylinder
-    /// aligned with the Z-axis and capped with hemispheres. Radius and
-    /// length are in meters. See Material for more on material properties.
-    /// \tparam Precision Scalar numeric type.
-    template<typename Precision>
-    class Capsule
-    {
-      /// \brief Default constructor. The default radius and length are both
-      /// zero.
-      public: Capsule() = default;
-
-      /// \brief Construct a capsule with a length and radius.
-      /// \param[in] _length Length of the capsule.
-      /// \param[in] _radius Radius of the capsule.
-      public: Capsule(const Precision _length, const Precision _radius);
-
-      /// \brief Construct a capsule with a length, radius, and material.
-      /// \param[in] _length Length of the capsule.
-      /// \param[in] _radius Radius of the capsule.
-      /// \param[in] _mat Material property for the capsule.
-      public: Capsule(const Precision _length, const Precision _radius,
-                  const Material &_mat);
-
-      /// \brief Get the radius in meters.
-      /// \return The radius of the capsule in meters.
-      public: Precision Radius() const;
-
-      /// \brief Set the radius in meters.
-      /// \param[in] _radius The radius of the capsule in meters.
-      public: void SetRadius(const Precision _radius);
-
-      /// \brief Get the length in meters.
-      /// \return The length of the capsule in meters.
-      public: Precision Length() const;
-
-      /// \brief Set the length in meters.
-      /// \param[in] _length The length of the capsule in meters.
-      public: void SetLength(const Precision _length);
-
-      /// \brief Get the material associated with this capsule.
-      /// \return The material assigned to this capsule
-      public: const Material &Mat() const;
-
-      /// \brief Set the material associated with this capsule.
-      /// \param[in] _mat The material assigned to this capsule
-      public: void SetMat(const Material &_mat);
-
-      /// \brief Get the mass matrix for this capsule. This function
-      /// is only meaningful if the capsule's radius, length, and material
-      /// have been set.
-      /// \return The computed mass matrix if parameters are valid
-      /// (radius > 0), (length > 0), and (density > 0). Otherwise
-      /// std::nullopt is returned.
-      public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
-
-      /// \brief Check if this capsule is equal to the provided capsule.
-      /// Radius, length, and material properties will be checked.
-      public: bool operator==(const Capsule &_capsule) const;
-
-      /// \brief Get the volume of the capsule in m^3.
-      /// \return Volume of the capsule in m^3.
-      public: Precision Volume() const;
-
-      /// \brief Compute the capsule's density given a mass value. The
-      /// capsule is assumed to be solid with uniform density. This
-      /// function requires the capsule's radius and length to be set to
-      /// values greater than zero. The Material of the capsule is ignored.
-      /// \param[in] _mass Mass of the capsule, in kg. This value should be
-      /// greater than zero.
-      /// \return Density of the capsule in kg/m^3. A NaN is returned
-      /// if radius, length or _mass is <= 0.
-      public: Precision DensityFromMass(const Precision _mass) const;
-
-      /// \brief Set the density of this capsule based on a mass value.
-      /// Density is computed using
-      /// Precision DensityFromMass(const Precision _mass) const. The
-      /// capsule is assumed to be solid with uniform density. This
-      /// function requires the capsule's radius and length to be set to
-      /// values greater than zero. The existing Material density value is
-      /// overwritten only if the return value from this true.
-      /// \param[in] _mass Mass of the capsule, in kg. This value should be
-      /// greater than zero.
-      /// \return True if the density was set. False is returned if the
-      /// capsule's radius, length, or the _mass value are <= 0.
-      /// \sa Precision DensityFromMass(const Precision _mass) const
-      public: bool SetDensityFromMass(const Precision _mass);
-
-      /// \brief Radius of the capsule.
-      private: Precision radius = 0.0;
-
-      /// \brief Length of the capsule.
-      private: Precision length = 0.0;
-
-      /// \brief the capsule's material.
-      private: Material material;
-    };
-
-    /// \typedef Capsule<int> Capsulei
-    /// \brief Capsule with integer precision.
-    typedef Capsule<int> Capsulei;
-
-    /// \typedef Capsule<double> Capsuled
-    /// \brief Capsule with double precision.
-    typedef Capsule<double> Capsuled;
-
-    /// \typedef Capsule<float> Capsulef
-    /// \brief Capsule with float precision.
-    typedef Capsule<float> Capsulef;
-    }
-  }
-}
+    /// \brief Default constructor. The default radius and length are both
+    /// zero.
+    public: Capsule() = default;
+
+    /// \brief Construct a capsule with a length and radius.
+    /// \param[in] _length Length of the capsule.
+    /// \param[in] _radius Radius of the capsule.
+    public: Capsule(const Precision _length, const Precision _radius);
+
+    /// \brief Construct a capsule with a length, radius, and material.
+    /// \param[in] _length Length of the capsule.
+    /// \param[in] _radius Radius of the capsule.
+    /// \param[in] _mat Material property for the capsule.
+    public: Capsule(const Precision _length, const Precision _radius,
+                const Material &_mat);
+
+    /// \brief Get the radius in meters.
+    /// \return The radius of the capsule in meters.
+    public: Precision Radius() const;
+
+    /// \brief Set the radius in meters.
+    /// \param[in] _radius The radius of the capsule in meters.
+    public: void SetRadius(const Precision _radius);
+
+    /// \brief Get the length in meters.
+    /// \return The length of the capsule in meters.
+    public: Precision Length() const;
+
+    /// \brief Set the length in meters.
+    /// \param[in] _length The length of the capsule in meters.
+    public: void SetLength(const Precision _length);
+
+    /// \brief Get the material associated with this capsule.
+    /// \return The material assigned to this capsule
+    public: const Material &Mat() const;
+
+    /// \brief Set the material associated with this capsule.
+    /// \param[in] _mat The material assigned to this capsule
+    public: void SetMat(const Material &_mat);
+
+    /// \brief Get the mass matrix for this capsule. This function
+    /// is only meaningful if the capsule's radius, length, and material
+    /// have been set.
+    /// \return The computed mass matrix if parameters are valid
+    /// (radius > 0), (length > 0), and (density > 0). Otherwise
+    /// std::nullopt is returned.
+    public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
+
+    /// \brief Check if this capsule is equal to the provided capsule.
+    /// Radius, length, and material properties will be checked.
+    public: bool operator==(const Capsule &_capsule) const;
+
+    /// \brief Get the volume of the capsule in m^3.
+    /// \return Volume of the capsule in m^3.
+    public: Precision Volume() const;
+
+    /// \brief Compute the capsule's density given a mass value. The
+    /// capsule is assumed to be solid with uniform density. This
+    /// function requires the capsule's radius and length to be set to
+    /// values greater than zero. The Material of the capsule is ignored.
+    /// \param[in] _mass Mass of the capsule, in kg. This value should be
+    /// greater than zero.
+    /// \return Density of the capsule in kg/m^3. A NaN is returned
+    /// if radius, length or _mass is <= 0.
+    public: Precision DensityFromMass(const Precision _mass) const;
+
+    /// \brief Set the density of this capsule based on a mass value.
+    /// Density is computed using
+    /// Precision DensityFromMass(const Precision _mass) const. The
+    /// capsule is assumed to be solid with uniform density. This
+    /// function requires the capsule's radius and length to be set to
+    /// values greater than zero. The existing Material density value is
+    /// overwritten only if the return value from this true.
+    /// \param[in] _mass Mass of the capsule, in kg. This value should be
+    /// greater than zero.
+    /// \return True if the density was set. False is returned if the
+    /// capsule's radius, length, or the _mass value are <= 0.
+    /// \sa Precision DensityFromMass(const Precision _mass) const
+    public: bool SetDensityFromMass(const Precision _mass);
+
+    /// \brief Radius of the capsule.
+    private: Precision radius = 0.0;
+
+    /// \brief Length of the capsule.
+    private: Precision length = 0.0;
+
+    /// \brief the capsule's material.
+    private: Material material;
+  };
+
+  /// \typedef Capsule<int> Capsulei
+  /// \brief Capsule with integer precision.
+  typedef Capsule<int> Capsulei;
+
+  /// \typedef Capsule<double> Capsuled
+  /// \brief Capsule with double precision.
+  typedef Capsule<double> Capsuled;
+
+  /// \typedef Capsule<float> Capsulef
+  /// \brief Capsule with float precision.
+  typedef Capsule<float> Capsulef;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
 #include "gz/math/detail/Capsule.hh"
-
-#endif
+#endif  // GZ_MATH_CAPSULE_HH_
diff --git a/include/gz/math/Color.hh b/include/gz/math/Color.hh
index 22d8c92fd..4e5f97b38 100644
--- a/include/gz/math/Color.hh
+++ b/include/gz/math/Color.hh
@@ -25,368 +25,365 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Color Color.hh gz/math/Color.hh
+  /// \brief Defines a color using a red (R), green (G), blue (B), and alpha
+  /// (A) component. Each color component is in the range [0..1].
+  ///
+  /// ## Example
+  ///
+  /// \snippet examples/color_example.cc complete
+  class GZ_MATH_VISIBLE Color
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Color Color.hh gz/math/Color.hh
-    /// \brief Defines a color using a red (R), green (G), blue (B), and alpha
-    /// (A) component. Each color component is in the range [0..1].
+    /// \brief (1, 1, 1)
+    public: static const Color &White;
+    /// \brief (0, 0, 0)
+    public: static const Color &Black;
+    /// \brief (1, 0, 0)
+    public: static const Color &Red;
+    /// \brief (0, 1, 0)
+    public: static const Color &Green;
+    /// \brief (0, 0, 1)
+    public: static const Color &Blue;
+    /// \brief (1, 1, 0)
+    public: static const Color &Yellow;
+    /// \brief (1, 0, 1)
+    public: static const Color &Magenta;
+    /// \brief (0, 1, 1)
+    public: static const Color &Cyan;
+
+    /// \typedef RGBA
+    /// \brief A RGBA packed value as an unsigned int
+    /// Each 8 bits corresponds to a channel.
     ///
-    /// ## Example
+    /// \code
+    /// RGBA a = 0xFF0000FF; // (1, 0, 0, 1) for RGBA, i.e. red.
+    /// \endcode
+    public: typedef unsigned int RGBA;
+
+    /// \typedef BGRA
+    /// \brief A BGRA packed value as an unsigned int
+    /// Each 8 bits corresponds to a channel.
     ///
-    /// \snippet examples/color_example.cc complete
-    class GZ_MATH_VISIBLE Color
+    /// \code
+    /// BGRA a = 0xFF0000FF; // (0, 0, 1, 1) for RGBA, i.e. blue.
+    /// \endcode
+    public: typedef unsigned int BGRA;
+
+    /// \typedef ARGB
+    /// \brief A ARGB packed value as an unsigned int
+    /// Each 8 bits corresponds to a channel.
+    ///
+    /// \code
+    /// ARGB a = 0xFF0000FF; // (0, 0, 1, 1) for RGBA, i.e. blue.
+    /// \endcode
+    public: typedef unsigned int ARGB;
+
+    /// \typedef ABGR
+    /// \brief A ABGR packed value as an unsigned int
+    /// Each 8 bits corresponds to a channel.
+    ///
+    /// \code
+    /// ABGR a = 0xFF0000FF; // (1, 0, 0, 1) for RGBA, i.e. red.
+    /// \endcode
+    public: typedef unsigned int ABGR;
+
+    /// \brief Constructor
+    public: Color() = default;
+
+    /// \brief Constructor
+    /// \param[in] _r Red value (range 0 to 1)
+    /// \param[in] _g Green value (range 0 to 1)
+    /// \param[in] _b Blue value (range 0 to 1)
+    /// \param[in] _a Alpha value (0=transparent, 1=opaque)
+    public: constexpr Color(const float _r, const float _g, const float _b,
+                            const float _a = 1.0)
+    : r(_r), g(_g), b(_b), a(_a)
+    {
+      this->Clamp();
+    }
+
+    /// \brief Copy Constructor
+    /// \param[in] _clr Color to copy
+    public: Color(const Color &_clr) = default;
+
+    /// \brief Destructor
+    public: ~Color() = default;
+
+    /// \brief Reset the color to default values to red=0, green=0,
+    /// blue=0, alpha=1.
+    public: void Reset();
+
+    /// \brief Set the contents of the vector
+    /// \param[in] _r Red value (range 0 to 1)
+    /// \param[in] _g Green value (range 0 to 1)
+    /// \param[in] _b Blue value (range 0 to 1)
+    /// \param[in] _a Alpha value (0=transparent, 1=opaque)
+    public: void Set(const float _r = 1, const float _g = 1,
+                const float _b = 1, const float _a = 1);
+
+    /// \brief Get the color in HSV colorspace
+    /// \return HSV values in a Vector3f format. A vector3f containing
+    /// {NAN_F, NAN_F, NAN_F} is returned on error.
+    public: Vector3f HSV() const;
+
+    /// \brief Set a color based on HSV values
+    /// \param[in] _h Hue(0..360)
+    /// \param[in] _s Saturation(0..1)
+    /// \param[in] _v Value(0..1)
+    public: void SetFromHSV(const float _h, const float _s, const float _v);
+
+    /// \brief Get the color in YUV colorspace
+    /// \return the YUV  color
+    public: Vector3f YUV() const;
+
+    /// \brief Set from yuv
+    /// \param[in] _y value
+    /// \param[in] _u value
+    /// \param[in] _v value
+    public: void SetFromYUV(const float _y, const float _u, const float _v);
+
+    /// \brief Equal operator
+    /// \param[in] _pt Color to copy
+    /// \return Reference to this color
+    public: Color &operator=(const Color &_pt) = default;
+
+    /// \brief Array index operator
+    /// \param[in] _index Color component index(0=red, 1=green, 2=blue,
+    /// 3=alpha)
+    /// \return r, g, b, or a when _index is 0, 1, 2 or 3. A NAN_F value is
+    /// returned if the _index is invalid
+    public: float operator[](const unsigned int _index);
+
+    /// \brief Array index operator, const version
+    /// \param[in] _index Color component index(0=red, 1=green, 2=blue,
+    /// 3=alpha)
+    /// \return r, g, b, or a when _index is 0, 1, 2 or 3. A NAN_F value is
+    /// returned if the _index is invalid
+    public: float operator[](const unsigned int _index) const;
+
+    /// \brief Get as uint32 RGBA packed value
+    /// \return the color
+    public: RGBA AsRGBA() const;
+
+    /// \brief Get as uint32 BGRA packed value
+    /// \return the color
+    public: BGRA AsBGRA() const;
+
+    /// \brief Get as uint32 ARGB packed value
+    /// \return the color
+    public: ARGB AsARGB() const;
+
+    /// \brief Get as uint32 ABGR packed value
+    /// \return the color
+    public: ABGR AsABGR() const;
+
+    /// \brief Set from uint32 RGBA packed value
+    /// \param[in] _v the new color
+    public: void SetFromRGBA(const RGBA _v);
+
+    /// \brief Set from uint32 BGRA packed value
+    /// \param[in] _v the new color
+    public: void SetFromBGRA(const BGRA _v);
+
+    /// \brief Set from uint32 ARGB packed value
+    /// \param[in] _v the new color
+    public: void SetFromARGB(const ARGB _v);
+
+    /// \brief Set from uint32 ABGR packed value
+    /// \param[in] _v the new color
+    public: void SetFromABGR(const ABGR _v);
+
+    /// \brief Addition operator (this + _pt)
+    /// \param[in] _pt Color to add
+    /// \return The resulting color
+    public: Color operator+(const Color &_pt) const;
+
+    /// \brief Add _v to all color components
+    /// \param[in] _v Value to add to each color component
+    /// \return The resulting color
+    public: Color operator+(const float &_v) const;
+
+    /// \brief Addition equal operator
+    /// \param[in] _pt Color to add
+    /// \return The resulting color
+    public: const Color &operator+=(const Color &_pt);
+
+    /// \brief Subtraction operator
+    /// \param[in] _pt The color to substract
+    /// \return The resulting color
+    public: Color operator-(const Color &_pt) const;
+
+    /// \brief Subtract _v from all color components
+    /// \param[in] _v Value to subtract
+    /// \return The resulting color
+    public: Color operator-(const float &_v) const;
+
+    /// \brief Subtraction equal operator
+    /// \param[in] _pt Color to subtract
+    /// \return The resulting color
+    public: const Color &operator-=(const Color &_pt);
+
+    /// \brief Division operator
+    /// \param[in] _pt Color to divide by
+    /// \return The resulting color
+    public: const Color operator/(const Color &_pt) const;
+
+    /// \brief Divide all color component by _v
+    /// \param[in] _v The value to divide by
+    /// \return The resulting color
+    public: const Color operator/(const float &_v) const;
+
+    /// \brief Division equal operator
+    /// \param[in] _pt Color to divide by
+    /// \return The resulting color
+    public: const Color &operator/=(const Color &_pt);
+
+    /// \brief Multiplication operator
+    /// \param[in] _pt The color to muliply by
+    /// \return The resulting color
+    public: const Color operator*(const Color &_pt) const;
+
+    /// \brief Multiply all color components by _v
+    /// \param[in] _v The value to multiply by
+    /// \return The resulting color
+    public: const Color operator*(const float &_v) const;
+
+    /// \brief Multiplication equal operator
+    /// \param[in] _pt The color to muliply by
+    /// \return The resulting color
+    public: const Color &operator*=(const Color &_pt);
+
+    /// \brief Equality operator
+    /// \param[in] _pt The color to check for equality
+    /// \return True if the this color equals _pt
+    public: bool operator==(const Color &_pt) const;
+
+    /// \brief Inequality operator
+    /// \param[in] _pt The color to check for inequality
+    /// \return True if the this color does not equal _pt
+    public: bool operator!=(const Color &_pt) const;
+
+    /// \brief Clamp the color values to valid ranges
+    private: constexpr void Clamp()
+    {
+      // These comparisons are carefully written to handle NaNs correctly.
+      if (!(this->r >= 0)) { this->r = 0; }
+      if (!(this->g >= 0)) { this->g = 0; }
+      if (!(this->b >= 0)) { this->b = 0; }
+      if (!(this->a >= 0)) { this->a = 0; }
+      if (this->r > 1) { this->r = this->r/255.0f; }
+      if (this->g > 1) { this->g = this->g/255.0f; }
+      if (this->b > 1) { this->b = this->b/255.0f; }
+      if (this->a > 1) { this->a = 1; }
+    }
+
+    /// \brief Stream insertion operator
+    /// \param[in] _out the output stream
+    /// \param[in] _color the color
+    /// \return the output stream
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                                            const Color &_color)
     {
-      /// \brief (1, 1, 1)
-      public: static const Color &White;
-      /// \brief (0, 0, 0)
-      public: static const Color &Black;
-      /// \brief (1, 0, 0)
-      public: static const Color &Red;
-      /// \brief (0, 1, 0)
-      public: static const Color &Green;
-      /// \brief (0, 0, 1)
-      public: static const Color &Blue;
-      /// \brief (1, 1, 0)
-      public: static const Color &Yellow;
-      /// \brief (1, 0, 1)
-      public: static const Color &Magenta;
-      /// \brief (0, 1, 1)
-      public: static const Color &Cyan;
-
-      /// \typedef RGBA
-      /// \brief A RGBA packed value as an unsigned int
-      /// Each 8 bits corresponds to a channel.
-      ///
-      /// \code
-      /// RGBA a = 0xFF0000FF; // (1, 0, 0, 1) for RGBA, i.e. red.
-      /// \endcode
-      public: typedef unsigned int RGBA;
-
-      /// \typedef BGRA
-      /// \brief A BGRA packed value as an unsigned int
-      /// Each 8 bits corresponds to a channel.
-      ///
-      /// \code
-      /// BGRA a = 0xFF0000FF; // (0, 0, 1, 1) for RGBA, i.e. blue.
-      /// \endcode
-      public: typedef unsigned int BGRA;
-
-      /// \typedef ARGB
-      /// \brief A ARGB packed value as an unsigned int
-      /// Each 8 bits corresponds to a channel.
-      ///
-      /// \code
-      /// ARGB a = 0xFF0000FF; // (0, 0, 1, 1) for RGBA, i.e. blue.
-      /// \endcode
-      public: typedef unsigned int ARGB;
-
-      /// \typedef ABGR
-      /// \brief A ABGR packed value as an unsigned int
-      /// Each 8 bits corresponds to a channel.
-      ///
-      /// \code
-      /// ABGR a = 0xFF0000FF; // (1, 0, 0, 1) for RGBA, i.e. red.
-      /// \endcode
-      public: typedef unsigned int ABGR;
-
-      /// \brief Constructor
-      public: Color() = default;
-
-      /// \brief Constructor
-      /// \param[in] _r Red value (range 0 to 1)
-      /// \param[in] _g Green value (range 0 to 1)
-      /// \param[in] _b Blue value (range 0 to 1)
-      /// \param[in] _a Alpha value (0=transparent, 1=opaque)
-      public: constexpr Color(const float _r, const float _g, const float _b,
-                              const float _a = 1.0)
-      : r(_r), g(_g), b(_b), a(_a)
+      for (auto i : {0, 1, 2, 3})
       {
-        this->Clamp();
+        if (i > 0)
+          _out << " ";
+
+        appendToStream(_out, _color[i]);
       }
+      return _out;
+    }
 
-      /// \brief Copy Constructor
-      /// \param[in] _clr Color to copy
-      public: Color(const Color &_clr) = default;
-
-      /// \brief Destructor
-      public: ~Color() = default;
-
-      /// \brief Reset the color to default values to red=0, green=0,
-      /// blue=0, alpha=1.
-      public: void Reset();
-
-      /// \brief Set the contents of the vector
-      /// \param[in] _r Red value (range 0 to 1)
-      /// \param[in] _g Green value (range 0 to 1)
-      /// \param[in] _b Blue value (range 0 to 1)
-      /// \param[in] _a Alpha value (0=transparent, 1=opaque)
-      public: void Set(const float _r = 1, const float _g = 1,
-                  const float _b = 1, const float _a = 1);
-
-      /// \brief Get the color in HSV colorspace
-      /// \return HSV values in a Vector3f format. A vector3f containing
-      /// {NAN_F, NAN_F, NAN_F} is returned on error.
-      public: Vector3f HSV() const;
-
-      /// \brief Set a color based on HSV values
-      /// \param[in] _h Hue(0..360)
-      /// \param[in] _s Saturation(0..1)
-      /// \param[in] _v Value(0..1)
-      public: void SetFromHSV(const float _h, const float _s, const float _v);
-
-      /// \brief Get the color in YUV colorspace
-      /// \return the YUV  color
-      public: Vector3f YUV() const;
-
-      /// \brief Set from yuv
-      /// \param[in] _y value
-      /// \param[in] _u value
-      /// \param[in] _v value
-      public: void SetFromYUV(const float _y, const float _u, const float _v);
-
-      /// \brief Equal operator
-      /// \param[in] _pt Color to copy
-      /// \return Reference to this color
-      public: Color &operator=(const Color &_pt) = default;
-
-      /// \brief Array index operator
-      /// \param[in] _index Color component index(0=red, 1=green, 2=blue,
-      /// 3=alpha)
-      /// \return r, g, b, or a when _index is 0, 1, 2 or 3. A NAN_F value is
-      /// returned if the _index is invalid
-      public: float operator[](const unsigned int _index);
-
-      /// \brief Array index operator, const version
-      /// \param[in] _index Color component index(0=red, 1=green, 2=blue,
-      /// 3=alpha)
-      /// \return r, g, b, or a when _index is 0, 1, 2 or 3. A NAN_F value is
-      /// returned if the _index is invalid
-      public: float operator[](const unsigned int _index) const;
-
-      /// \brief Get as uint32 RGBA packed value
-      /// \return the color
-      public: RGBA AsRGBA() const;
-
-      /// \brief Get as uint32 BGRA packed value
-      /// \return the color
-      public: BGRA AsBGRA() const;
-
-      /// \brief Get as uint32 ARGB packed value
-      /// \return the color
-      public: ARGB AsARGB() const;
-
-      /// \brief Get as uint32 ABGR packed value
-      /// \return the color
-      public: ABGR AsABGR() const;
-
-      /// \brief Set from uint32 RGBA packed value
-      /// \param[in] _v the new color
-      public: void SetFromRGBA(const RGBA _v);
-
-      /// \brief Set from uint32 BGRA packed value
-      /// \param[in] _v the new color
-      public: void SetFromBGRA(const BGRA _v);
-
-      /// \brief Set from uint32 ARGB packed value
-      /// \param[in] _v the new color
-      public: void SetFromARGB(const ARGB _v);
-
-      /// \brief Set from uint32 ABGR packed value
-      /// \param[in] _v the new color
-      public: void SetFromABGR(const ABGR _v);
-
-      /// \brief Addition operator (this + _pt)
-      /// \param[in] _pt Color to add
-      /// \return The resulting color
-      public: Color operator+(const Color &_pt) const;
-
-      /// \brief Add _v to all color components
-      /// \param[in] _v Value to add to each color component
-      /// \return The resulting color
-      public: Color operator+(const float &_v) const;
-
-      /// \brief Addition equal operator
-      /// \param[in] _pt Color to add
-      /// \return The resulting color
-      public: const Color &operator+=(const Color &_pt);
-
-      /// \brief Subtraction operator
-      /// \param[in] _pt The color to substract
-      /// \return The resulting color
-      public: Color operator-(const Color &_pt) const;
-
-      /// \brief Subtract _v from all color components
-      /// \param[in] _v Value to subtract
-      /// \return The resulting color
-      public: Color operator-(const float &_v) const;
-
-      /// \brief Subtraction equal operator
-      /// \param[in] _pt Color to subtract
-      /// \return The resulting color
-      public: const Color &operator-=(const Color &_pt);
-
-      /// \brief Division operator
-      /// \param[in] _pt Color to divide by
-      /// \return The resulting color
-      public: const Color operator/(const Color &_pt) const;
-
-      /// \brief Divide all color component by _v
-      /// \param[in] _v The value to divide by
-      /// \return The resulting color
-      public: const Color operator/(const float &_v) const;
-
-      /// \brief Division equal operator
-      /// \param[in] _pt Color to divide by
-      /// \return The resulting color
-      public: const Color &operator/=(const Color &_pt);
-
-      /// \brief Multiplication operator
-      /// \param[in] _pt The color to muliply by
-      /// \return The resulting color
-      public: const Color operator*(const Color &_pt) const;
-
-      /// \brief Multiply all color components by _v
-      /// \param[in] _v The value to multiply by
-      /// \return The resulting color
-      public: const Color operator*(const float &_v) const;
-
-      /// \brief Multiplication equal operator
-      /// \param[in] _pt The color to muliply by
-      /// \return The resulting color
-      public: const Color &operator*=(const Color &_pt);
-
-      /// \brief Equality operator
-      /// \param[in] _pt The color to check for equality
-      /// \return True if the this color equals _pt
-      public: bool operator==(const Color &_pt) const;
-
-      /// \brief Inequality operator
-      /// \param[in] _pt The color to check for inequality
-      /// \return True if the this color does not equal _pt
-      public: bool operator!=(const Color &_pt) const;
-
-      /// \brief Clamp the color values to valid ranges
-      private: constexpr void Clamp()
+    /// \brief Stream insertion operator
+    /// \param[in] _in the input stream. If the input stream does not include
+    /// an alpha value, a default alpha value of 1.0 will be used.
+    /// \param[in] _pt
+    public: friend std::istream &operator>> (std::istream &_in, Color &_pt)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      _in >> _pt.r >> _pt.g >> _pt.b;
+      // Since alpha is optional, check if it's there before parsing
+      while (_in.good() && std::isspace(_in.peek()))
       {
-        // These comparisons are carefully written to handle NaNs correctly.
-        if (!(this->r >= 0)) { this->r = 0; }
-        if (!(this->g >= 0)) { this->g = 0; }
-        if (!(this->b >= 0)) { this->b = 0; }
-        if (!(this->a >= 0)) { this->a = 0; }
-        if (this->r > 1) { this->r = this->r/255.0f; }
-        if (this->g > 1) { this->g = this->g/255.0f; }
-        if (this->b > 1) { this->b = this->b/255.0f; }
-        if (this->a > 1) { this->a = 1; }
+        _in.get();
       }
-
-      /// \brief Stream insertion operator
-      /// \param[in] _out the output stream
-      /// \param[in] _color the color
-      /// \return the output stream
-      public: friend std::ostream &operator<<(std::ostream &_out,
-                                              const Color &_color)
+      if (_in.good())
       {
-        for (auto i : {0, 1, 2, 3})
-        {
-          if (i > 0)
-            _out << " ";
-
-          appendToStream(_out, _color[i]);
-        }
-        return _out;
+        _in >> _pt.a;
       }
-
-      /// \brief Stream insertion operator
-      /// \param[in] _in the input stream. If the input stream does not include
-      /// an alpha value, a default alpha value of 1.0 will be used.
-      /// \param[in] _pt
-      public: friend std::istream &operator>> (std::istream &_in, Color &_pt)
+      else if (!_in.fail())
       {
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        _in >> _pt.r >> _pt.g >> _pt.b;
-        // Since alpha is optional, check if it's there before parsing
-        while (_in.good() && std::isspace(_in.peek()))
-        {
-          _in.get();
-        }
-        if (_in.good())
-        {
-          _in >> _pt.a;
-        }
-        else if (!_in.fail())
-        {
-          _pt.a = 1.0;
-        }
-        return _in;
+        _pt.a = 1.0;
       }
+      return _in;
+    }
 
-      /// \brief Get the red value
-      /// \return The red value
-      public: float R() const;
+    /// \brief Get the red value
+    /// \return The red value
+    public: float R() const;
 
-      /// \brief Get the green value
-      /// \return The green value
-      public: float G() const;
+    /// \brief Get the green value
+    /// \return The green value
+    public: float G() const;
 
-      /// \brief Get the blue value
-      /// \return The blue value
-      public: float B() const;
+    /// \brief Get the blue value
+    /// \return The blue value
+    public: float B() const;
 
-      /// \brief Get the alpha value
-      /// \return The alpha value
-      public: float A() const;
+    /// \brief Get the alpha value
+    /// \return The alpha value
+    public: float A() const;
 
-      /// \brief Get a mutable reference to the red value
-      /// \return The red value
-      public: float &R();
+    /// \brief Get a mutable reference to the red value
+    /// \return The red value
+    public: float &R();
 
-      /// \brief Get a mutable reference to the green value
-      /// \return The green value
-      public: float &G();
+    /// \brief Get a mutable reference to the green value
+    /// \return The green value
+    public: float &G();
 
-      /// \brief Get a mutable reference to the blue value
-      /// \return The blue value
-      public: float &B();
+    /// \brief Get a mutable reference to the blue value
+    /// \return The blue value
+    public: float &B();
 
-      /// \brief Get a mutable reference to the alpha value
-      /// \return The alpha value
-      public: float &A();
+    /// \brief Get a mutable reference to the alpha value
+    /// \return The alpha value
+    public: float &A();
 
-      /// \brief Set the red value
-      /// \param[in] _r New red value
-      public: void R(const float _r);
+    /// \brief Set the red value
+    /// \param[in] _r New red value
+    public: void R(const float _r);
 
-      /// \brief Set the green value
-      /// \param[in] _g New green value
-      public: void G(const float _g);
+    /// \brief Set the green value
+    /// \param[in] _g New green value
+    public: void G(const float _g);
 
-      /// \brief Set the blue value
-      /// \param[in] _b New blue value
-      public: void B(const float _b);
+    /// \brief Set the blue value
+    /// \param[in] _b New blue value
+    public: void B(const float _b);
 
-      /// \brief Set the alpha value
-      /// \param[in] _a New alpha value
-      public: void A(const float _a);
+    /// \brief Set the alpha value
+    /// \param[in] _a New alpha value
+    public: void A(const float _a);
 
-      /// \brief Red value
-      private: float r = 0;
+    /// \brief Red value
+    private: float r = 0;
 
-      /// \brief Green value
-      private: float g = 0;
+    /// \brief Green value
+    private: float g = 0;
 
-      /// \brief Blue value
-      private: float b = 0;
+    /// \brief Blue value
+    private: float b = 0;
 
-      /// \brief Alpha value
-      private: float a = 1;
-    };
-    }
-  }
-}
-#endif
+    /// \brief Alpha value
+    private: float a = 1;
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_COLOR_HH_
diff --git a/include/gz/math/Cone.hh b/include/gz/math/Cone.hh
index 716f2b6b1..920d5e434 100644
--- a/include/gz/math/Cone.hh
+++ b/include/gz/math/Cone.hh
@@ -24,170 +24,166 @@
 #include "gz/math/Material.hh"
 #include "gz/math/Quaternion.hh"
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Foward declarations
+  class ConePrivate;
+
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Cone Cone.hh gz/math/Cone.hh
+  /// \brief A representation of a cone.
+  ///
+  /// The cone class supports defining a cone with a radius,
+  /// length, rotational offset, and material properties. Radius and
+  /// length are in meters. See Material for more on material properties.
+  /// By default, a cone's length is aligned with the Z axis where the base
+  /// of the cone is proximal to the origin and vertex points in positive Z.
+  /// The rotational offset encodes a rotation from the z axis.
+  template<typename Precision>
+  class Cone
   {
-    // Foward declarations
-    class ConePrivate;
-
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Cone Cone.hh gz/math/Cone.hh
-    /// \brief A representation of a cone.
-    ///
-    /// The cone class supports defining a cone with a radius,
-    /// length, rotational offset, and material properties. Radius and
-    /// length are in meters. See Material for more on material properties.
-    /// By default, a cone's length is aligned with the Z axis where the base
-    /// of the cone is proximal to the origin and vertex points in positive Z.
-    /// The rotational offset encodes a rotation from the z axis.
-    template<typename Precision>
-    class Cone
-    {
-      /// \brief Default constructor. The default radius and length are both
-      /// zero. The default rotational offset is
-      /// Quaternion<Precision>::Identity.
-      public: Cone() = default;
-
-      /// \brief Construct a cone with a length, radius, and optionally
-      /// a rotational offset.
-      /// \param[in] _length Length of the cone.
-      /// \param[in] _radius Radius of the cone.
-      /// \param[in] _rotOffset Rotational offset of the cone.
-      public: Cone(const Precision _length, const Precision _radius,
-                   const Quaternion<Precision> &_rotOffset =
-                   Quaternion<Precision>::Identity);
-
-      /// \brief Construct a cone with a length, radius, material and
-      /// optionally a rotational offset.
-      /// \param[in] _length Length of the cone.
-      /// \param[in] _radius Radius of the cone.
-      /// \param[in] _mat Material property for the cone.
-      /// \param[in] _rotOffset Rotational offset of the cone.
-      public: Cone(const Precision _length, const Precision _radius,
-                   const Material &_mat,
-                   const Quaternion<Precision> &_rotOffset =
-                   Quaternion<Precision>::Identity);
-
-      /// \brief Get the radius in meters.
-      /// \return The radius of the cone in meters.
-      public: Precision Radius() const;
-
-      /// \brief Set the radius in meters.
-      /// \param[in] _radius The radius of the cone in meters.
-      public: void SetRadius(const Precision _radius);
-
-      /// \brief Get the length in meters.
-      /// \return The length of the cone in meters.
-      public: Precision Length() const;
-
-      /// \brief Set the length in meters.
-      /// \param[in] _length The length of the cone in meters.
-      public: void SetLength(const Precision _length);
-
-      /// \brief Get the rotational offset. By default, a cone's length
-      /// is aligned with the Z axis. The rotational offset encodes
-      /// a rotation from the z axis.
-      /// \return The cone's rotational offset.
-      /// \sa void SetRotationalOffset(const Quaternion<Precision> &_rot)
-      public: Quaternion<Precision> RotationalOffset() const;
-
-      /// \brief Set the rotation offset.
-      /// \param[in] _rotOffset rotational offset quaternion.
-      /// See Quaternion<Precision> RotationalOffset() for details on the
-      /// rotational offset.
-      /// \sa Quaternion<Precision> RotationalOffset() const
-      public: void SetRotationalOffset(
-                  const Quaternion<Precision> &_rotOffset);
-
-      /// \brief Get the material associated with this cone.
-      /// \return The material assigned to this cone
-      public: const Material &Mat() const;
-
-      /// \brief Set the material associated with this cone.
-      /// \param[in] _mat The material assigned to this cone
-      public: void SetMat(const Material &_mat);
-
-      /// \brief Get the mass matrix for this cone. This function
-      /// is only meaningful if the cone's radius, length, and material
-      /// have been set. Optionally, set the rotational offset.
-      /// \param[out] _massMat The computed mass matrix will be stored
-      /// here.
-      /// \return False if computation of the mass matrix failed, which
-      /// could be due to an invalid radius (<=0), length (<=0), or density
-      /// (<=0).
-      public: bool MassMatrix(MassMatrix3d &_massMat) const;
-
-      /// \brief Get the mass matrix for this cone. This function
-      /// is only meaningful if the cone's radius, length, and material
-      /// have been set. Optionally, set the rotational offset.
-      /// \return The computed mass matrix if parameters are valid
-      /// (radius > 0), (length > 0) and (density > 0). Otherwise
-      /// std::nullopt is returned.
-      public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
-
-      /// \brief Check if this cone is equal to the provided cone.
-      /// Radius, length, and material properties will be checked.
-      public: bool operator==(const Cone &_cone) const;
-
-      /// \brief Get the volume of the cone in m^3.
-      /// \return Volume of the cone in m^3.
-      public: Precision Volume() const;
-
-      /// \brief Compute the cone's density given a mass value. The
-      /// cone is assumed to be solid with uniform density. This
-      /// function requires the cone's radius and length to be set to
-      /// values greater than zero. The Material of the cone is ignored.
-      /// \param[in] _mass Mass of the cone, in kg. This value should be
-      /// greater than zero.
-      /// \return Density of the cone in kg/m^3. A negative value is
-      /// returned if radius, length or _mass is <= 0.
-      public: Precision DensityFromMass(const Precision _mass) const;
-
-      /// \brief Set the density of this cone based on a mass value.
-      /// Density is computed using
-      /// Precision DensityFromMass(const Precision _mass) const. The
-      /// cone is assumed to be solid with uniform density. This
-      /// function requires the cone's radius and length to be set to
-      /// values greater than zero. The existing Material density value is
-      /// overwritten only if the return value from this true.
-      /// \param[in] _mass Mass of the cone, in kg. This value should be
-      /// greater than zero.
-      /// \return True if the density was set. False is returned if the
-      /// cone's radius, length, or the _mass value are <= 0.
-      /// \sa Precision DensityFromMass(const Precision _mass) const
-      public: bool SetDensityFromMass(const Precision _mass);
-
-      /// \brief Radius of the cone.
-      private: Precision radius = 0.0;
-
-      /// \brief Length of the cone.
-      private: Precision length = 0.0;
-
-      /// \brief the cone's material.
-      private: Material material;
-
-      /// \brief Rotational offset.
-      private: Quaternion<Precision> rotOffset =
-               Quaternion<Precision>::Identity;
-    };
-
-    /// \typedef Cone<int> Conei
-    /// \brief Cone with integer precision.
-    typedef Cone<int> Conei;
-
-    /// \typedef Cone<double> Coned
-    /// \brief Cone with double precision.
-    typedef Cone<double> Coned;
-
-    /// \typedef Cone<float> Conef
-    /// \brief Cone with float precision.
-    typedef Cone<float> Conef;
-    }
-  }
-}
+    /// \brief Default constructor. The default radius and length are both
+    /// zero. The default rotational offset is
+    /// Quaternion<Precision>::Identity.
+    public: Cone() = default;
+
+    /// \brief Construct a cone with a length, radius, and optionally
+    /// a rotational offset.
+    /// \param[in] _length Length of the cone.
+    /// \param[in] _radius Radius of the cone.
+    /// \param[in] _rotOffset Rotational offset of the cone.
+    public: Cone(const Precision _length, const Precision _radius,
+                 const Quaternion<Precision> &_rotOffset =
+                 Quaternion<Precision>::Identity);
+
+    /// \brief Construct a cone with a length, radius, material and
+    /// optionally a rotational offset.
+    /// \param[in] _length Length of the cone.
+    /// \param[in] _radius Radius of the cone.
+    /// \param[in] _mat Material property for the cone.
+    /// \param[in] _rotOffset Rotational offset of the cone.
+    public: Cone(const Precision _length, const Precision _radius,
+                 const Material &_mat,
+                 const Quaternion<Precision> &_rotOffset =
+                 Quaternion<Precision>::Identity);
+
+    /// \brief Get the radius in meters.
+    /// \return The radius of the cone in meters.
+    public: Precision Radius() const;
+
+    /// \brief Set the radius in meters.
+    /// \param[in] _radius The radius of the cone in meters.
+    public: void SetRadius(const Precision _radius);
+
+    /// \brief Get the length in meters.
+    /// \return The length of the cone in meters.
+    public: Precision Length() const;
+
+    /// \brief Set the length in meters.
+    /// \param[in] _length The length of the cone in meters.
+    public: void SetLength(const Precision _length);
+
+    /// \brief Get the rotational offset. By default, a cone's length
+    /// is aligned with the Z axis. The rotational offset encodes
+    /// a rotation from the z axis.
+    /// \return The cone's rotational offset.
+    /// \sa void SetRotationalOffset(const Quaternion<Precision> &_rot)
+    public: Quaternion<Precision> RotationalOffset() const;
+
+    /// \brief Set the rotation offset.
+    /// \param[in] _rotOffset rotational offset quaternion.
+    /// See Quaternion<Precision> RotationalOffset() for details on the
+    /// rotational offset.
+    /// \sa Quaternion<Precision> RotationalOffset() const
+    public: void SetRotationalOffset(
+                const Quaternion<Precision> &_rotOffset);
+
+    /// \brief Get the material associated with this cone.
+    /// \return The material assigned to this cone
+    public: const Material &Mat() const;
+
+    /// \brief Set the material associated with this cone.
+    /// \param[in] _mat The material assigned to this cone
+    public: void SetMat(const Material &_mat);
+
+    /// \brief Get the mass matrix for this cone. This function
+    /// is only meaningful if the cone's radius, length, and material
+    /// have been set. Optionally, set the rotational offset.
+    /// \param[out] _massMat The computed mass matrix will be stored
+    /// here.
+    /// \return False if computation of the mass matrix failed, which
+    /// could be due to an invalid radius (<=0), length (<=0), or density
+    /// (<=0).
+    public: bool MassMatrix(MassMatrix3d &_massMat) const;
+
+    /// \brief Get the mass matrix for this cone. This function
+    /// is only meaningful if the cone's radius, length, and material
+    /// have been set. Optionally, set the rotational offset.
+    /// \return The computed mass matrix if parameters are valid
+    /// (radius > 0), (length > 0) and (density > 0). Otherwise
+    /// std::nullopt is returned.
+    public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
+
+    /// \brief Check if this cone is equal to the provided cone.
+    /// Radius, length, and material properties will be checked.
+    public: bool operator==(const Cone &_cone) const;
+
+    /// \brief Get the volume of the cone in m^3.
+    /// \return Volume of the cone in m^3.
+    public: Precision Volume() const;
+
+    /// \brief Compute the cone's density given a mass value. The
+    /// cone is assumed to be solid with uniform density. This
+    /// function requires the cone's radius and length to be set to
+    /// values greater than zero. The Material of the cone is ignored.
+    /// \param[in] _mass Mass of the cone, in kg. This value should be
+    /// greater than zero.
+    /// \return Density of the cone in kg/m^3. A negative value is
+    /// returned if radius, length or _mass is <= 0.
+    public: Precision DensityFromMass(const Precision _mass) const;
+
+    /// \brief Set the density of this cone based on a mass value.
+    /// Density is computed using
+    /// Precision DensityFromMass(const Precision _mass) const. The
+    /// cone is assumed to be solid with uniform density. This
+    /// function requires the cone's radius and length to be set to
+    /// values greater than zero. The existing Material density value is
+    /// overwritten only if the return value from this true.
+    /// \param[in] _mass Mass of the cone, in kg. This value should be
+    /// greater than zero.
+    /// \return True if the density was set. False is returned if the
+    /// cone's radius, length, or the _mass value are <= 0.
+    /// \sa Precision DensityFromMass(const Precision _mass) const
+    public: bool SetDensityFromMass(const Precision _mass);
+
+    /// \brief Radius of the cone.
+    private: Precision radius = 0.0;
+
+    /// \brief Length of the cone.
+    private: Precision length = 0.0;
+
+    /// \brief the cone's material.
+    private: Material material;
+
+    /// \brief Rotational offset.
+    private: Quaternion<Precision> rotOffset =
+             Quaternion<Precision>::Identity;
+  };
+
+  /// \typedef Cone<int> Conei
+  /// \brief Cone with integer precision.
+  typedef Cone<int> Conei;
+
+  /// \typedef Cone<double> Coned
+  /// \brief Cone with double precision.
+  typedef Cone<double> Coned;
+
+  /// \typedef Cone<float> Conef
+  /// \brief Cone with float precision.
+  typedef Cone<float> Conef;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
 #include "gz/math/detail/Cone.hh"
-
-#endif
+#endif  // GZ_MATH_CONE_HH_
diff --git a/include/gz/math/Cylinder.hh b/include/gz/math/Cylinder.hh
index 691fc993f..1fd9e2bc5 100644
--- a/include/gz/math/Cylinder.hh
+++ b/include/gz/math/Cylinder.hh
@@ -22,165 +22,161 @@
 #include "gz/math/Material.hh"
 #include "gz/math/Quaternion.hh"
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Cylinder Cylinder.hh gz/math/Cylinder.hh
+  /// \brief A representation of a cylinder.
+  ///
+  /// The cylinder class supports defining a cylinder with a radius,
+  /// length, rotational offset, and material properties. Radius and
+  /// length are in meters. See Material for more on material properties.
+  /// By default, a cylinder's length is aligned with the Z axis. The
+  /// rotational offset encodes a rotation from the z axis.
+  template<typename Precision>
+  class Cylinder
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Cylinder Cylinder.hh gz/math/Cylinder.hh
-    /// \brief A representation of a cylinder.
-    ///
-    /// The cylinder class supports defining a cylinder with a radius,
-    /// length, rotational offset, and material properties. Radius and
-    /// length are in meters. See Material for more on material properties.
-    /// By default, a cylinder's length is aligned with the Z axis. The
-    /// rotational offset encodes a rotation from the z axis.
-    template<typename Precision>
-    class Cylinder
-    {
-      /// \brief Default constructor. The default radius and length are both
-      /// zero. The default rotational offset is
-      /// Quaternion<Precision>::Identity.
-      public: Cylinder() = default;
-
-      /// \brief Construct a cylinder with a length, radius, and optionally
-      /// a rotational offset.
-      /// \param[in] _length Length of the cylinder.
-      /// \param[in] _radius Radius of the cylinder.
-      /// \param[in] _rotOffset Rotational offset of the cylinder.
-      public: Cylinder(const Precision _length, const Precision _radius,
-                  const Quaternion<Precision> &_rotOffset =
-                  Quaternion<Precision>::Identity);
-
-      /// \brief Construct a cylinder with a length, radius, material and
-      /// optionally a rotational offset.
-      /// \param[in] _length Length of the cylinder.
-      /// \param[in] _radius Radius of the cylinder.
-      /// \param[in] _mat Material property for the cylinder.
-      /// \param[in] _rotOffset Rotational offset of the cylinder.
-      public: Cylinder(const Precision _length, const Precision _radius,
-                  const Material &_mat,
-                  const Quaternion<Precision> &_rotOffset =
-                  Quaternion<Precision>::Identity);
-
-      /// \brief Get the radius in meters.
-      /// \return The radius of the cylinder in meters.
-      public: Precision Radius() const;
-
-      /// \brief Set the radius in meters.
-      /// \param[in] _radius The radius of the cylinder in meters.
-      public: void SetRadius(const Precision _radius);
-
-      /// \brief Get the length in meters.
-      /// \return The length of the cylinder in meters.
-      public: Precision Length() const;
-
-      /// \brief Set the length in meters.
-      /// \param[in] _length The length of the cylinder in meters.
-      public: void SetLength(const Precision _length);
-
-      /// \brief Get the rotational offset. By default, a cylinder's length
-      /// is aligned with the Z axis. The rotational offset encodes
-      /// a rotation from the z axis.
-      /// \return The cylinder's rotational offset.
-      /// \sa void SetRotationalOffset(const Quaternion<Precision> &_rot)
-      public: Quaternion<Precision> RotationalOffset() const;
-
-      /// \brief Set the rotation offset.
-      /// See Quaternion<Precision> RotationalOffset() for details on the
-      /// rotational offset.
-      /// \sa Quaternion<Precision> RotationalOffset() const
-      public: void SetRotationalOffset(
-                  const Quaternion<Precision> &_rotOffset);
-
-      /// \brief Get the material associated with this cylinder.
-      /// \return The material assigned to this cylinder
-      public: const Material &Mat() const;
-
-      /// \brief Set the material associated with this cylinder.
-      /// \param[in] _mat The material assigned to this cylinder
-      public: void SetMat(const Material &_mat);
-
-      /// \brief Get the mass matrix for this cylinder. This function
-      /// is only meaningful if the cylinder's radius, length, and material
-      /// have been set. Optionally, set the rotational offset.
-      /// \param[out] _massMat The computed mass matrix will be stored
-      /// here.
-      /// \return False if computation of the mass matrix failed, which
-      /// could be due to an invalid radius (<=0), length (<=0), or density
-      /// (<=0).
-      public: bool MassMatrix(MassMatrix3d &_massMat) const;
-
-      /// \brief Get the mass matrix for this cylinder. This function
-      /// is only meaningful if the cylinder's radius, length, and material
-      /// have been set. Optionally, set the rotational offset.
-      /// \return The computed mass matrix if parameters are valid
-      /// (radius > 0), (length > 0) and (density > 0). Otherwise
-      /// std::nullopt is returned.
-      public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
-
-      /// \brief Check if this cylinder is equal to the provided cylinder.
-      /// Radius, length, and material properties will be checked.
-      public: bool operator==(const Cylinder &_cylinder) const;
-
-      /// \brief Get the volume of the cylinder in m^3.
-      /// \return Volume of the cylinder in m^3.
-      public: Precision Volume() const;
-
-      /// \brief Compute the cylinder's density given a mass value. The
-      /// cylinder is assumed to be solid with uniform density. This
-      /// function requires the cylinder's radius and length to be set to
-      /// values greater than zero. The Material of the cylinder is ignored.
-      /// \param[in] _mass Mass of the cylinder, in kg. This value should be
-      /// greater than zero.
-      /// \return Density of the cylinder in kg/m^3. A negative value is
-      /// returned if radius, length or _mass is <= 0.
-      public: Precision DensityFromMass(const Precision _mass) const;
-
-      /// \brief Set the density of this cylinder based on a mass value.
-      /// Density is computed using
-      /// Precision DensityFromMass(const Precision _mass) const. The
-      /// cylinder is assumed to be solid with uniform density. This
-      /// function requires the cylinder's radius and length to be set to
-      /// values greater than zero. The existing Material density value is
-      /// overwritten only if the return value from this true.
-      /// \param[in] _mass Mass of the cylinder, in kg. This value should be
-      /// greater than zero.
-      /// \return True if the density was set. False is returned if the
-      /// cylinder's radius, length, or the _mass value are <= 0.
-      /// \sa Precision DensityFromMass(const Precision _mass) const
-      public: bool SetDensityFromMass(const Precision _mass);
-
-      /// \brief Radius of the cylinder.
-      private: Precision radius = 0.0;
-
-      /// \brief Length of the cylinder.
-      private: Precision length = 0.0;
-
-      /// \brief the cylinder's material.
-      private: Material material;
-
-      /// \brief Rotational offset.
-      private: Quaternion<Precision> rotOffset =
-               Quaternion<Precision>::Identity;
-    };
-
-    /// \typedef Cylinder<int> Cylinderi
-    /// \brief Cylinder with integer precision.
-    typedef Cylinder<int> Cylinderi;
-
-    /// \typedef Cylinder<double> Cylinderd
-    /// \brief Cylinder with double precision.
-    typedef Cylinder<double> Cylinderd;
-
-    /// \typedef Cylinder<float> Cylinderf
-    /// \brief Cylinder with float precision.
-    typedef Cylinder<float> Cylinderf;
-    }
-  }
-}
+    /// \brief Default constructor. The default radius and length are both
+    /// zero. The default rotational offset is
+    /// Quaternion<Precision>::Identity.
+    public: Cylinder() = default;
+
+    /// \brief Construct a cylinder with a length, radius, and optionally
+    /// a rotational offset.
+    /// \param[in] _length Length of the cylinder.
+    /// \param[in] _radius Radius of the cylinder.
+    /// \param[in] _rotOffset Rotational offset of the cylinder.
+    public: Cylinder(const Precision _length, const Precision _radius,
+                const Quaternion<Precision> &_rotOffset =
+                Quaternion<Precision>::Identity);
+
+    /// \brief Construct a cylinder with a length, radius, material and
+    /// optionally a rotational offset.
+    /// \param[in] _length Length of the cylinder.
+    /// \param[in] _radius Radius of the cylinder.
+    /// \param[in] _mat Material property for the cylinder.
+    /// \param[in] _rotOffset Rotational offset of the cylinder.
+    public: Cylinder(const Precision _length, const Precision _radius,
+                const Material &_mat,
+                const Quaternion<Precision> &_rotOffset =
+                Quaternion<Precision>::Identity);
+
+    /// \brief Get the radius in meters.
+    /// \return The radius of the cylinder in meters.
+    public: Precision Radius() const;
+
+    /// \brief Set the radius in meters.
+    /// \param[in] _radius The radius of the cylinder in meters.
+    public: void SetRadius(const Precision _radius);
+
+    /// \brief Get the length in meters.
+    /// \return The length of the cylinder in meters.
+    public: Precision Length() const;
+
+    /// \brief Set the length in meters.
+    /// \param[in] _length The length of the cylinder in meters.
+    public: void SetLength(const Precision _length);
+
+    /// \brief Get the rotational offset. By default, a cylinder's length
+    /// is aligned with the Z axis. The rotational offset encodes
+    /// a rotation from the z axis.
+    /// \return The cylinder's rotational offset.
+    /// \sa void SetRotationalOffset(const Quaternion<Precision> &_rot)
+    public: Quaternion<Precision> RotationalOffset() const;
+
+    /// \brief Set the rotation offset.
+    /// See Quaternion<Precision> RotationalOffset() for details on the
+    /// rotational offset.
+    /// \sa Quaternion<Precision> RotationalOffset() const
+    public: void SetRotationalOffset(
+                const Quaternion<Precision> &_rotOffset);
+
+    /// \brief Get the material associated with this cylinder.
+    /// \return The material assigned to this cylinder
+    public: const Material &Mat() const;
+
+    /// \brief Set the material associated with this cylinder.
+    /// \param[in] _mat The material assigned to this cylinder
+    public: void SetMat(const Material &_mat);
+
+    /// \brief Get the mass matrix for this cylinder. This function
+    /// is only meaningful if the cylinder's radius, length, and material
+    /// have been set. Optionally, set the rotational offset.
+    /// \param[out] _massMat The computed mass matrix will be stored
+    /// here.
+    /// \return False if computation of the mass matrix failed, which
+    /// could be due to an invalid radius (<=0), length (<=0), or density
+    /// (<=0).
+    public: bool MassMatrix(MassMatrix3d &_massMat) const;
+
+    /// \brief Get the mass matrix for this cylinder. This function
+    /// is only meaningful if the cylinder's radius, length, and material
+    /// have been set. Optionally, set the rotational offset.
+    /// \return The computed mass matrix if parameters are valid
+    /// (radius > 0), (length > 0) and (density > 0). Otherwise
+    /// std::nullopt is returned.
+    public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
+
+    /// \brief Check if this cylinder is equal to the provided cylinder.
+    /// Radius, length, and material properties will be checked.
+    public: bool operator==(const Cylinder &_cylinder) const;
+
+    /// \brief Get the volume of the cylinder in m^3.
+    /// \return Volume of the cylinder in m^3.
+    public: Precision Volume() const;
+
+    /// \brief Compute the cylinder's density given a mass value. The
+    /// cylinder is assumed to be solid with uniform density. This
+    /// function requires the cylinder's radius and length to be set to
+    /// values greater than zero. The Material of the cylinder is ignored.
+    /// \param[in] _mass Mass of the cylinder, in kg. This value should be
+    /// greater than zero.
+    /// \return Density of the cylinder in kg/m^3. A negative value is
+    /// returned if radius, length or _mass is <= 0.
+    public: Precision DensityFromMass(const Precision _mass) const;
+
+    /// \brief Set the density of this cylinder based on a mass value.
+    /// Density is computed using
+    /// Precision DensityFromMass(const Precision _mass) const. The
+    /// cylinder is assumed to be solid with uniform density. This
+    /// function requires the cylinder's radius and length to be set to
+    /// values greater than zero. The existing Material density value is
+    /// overwritten only if the return value from this true.
+    /// \param[in] _mass Mass of the cylinder, in kg. This value should be
+    /// greater than zero.
+    /// \return True if the density was set. False is returned if the
+    /// cylinder's radius, length, or the _mass value are <= 0.
+    /// \sa Precision DensityFromMass(const Precision _mass) const
+    public: bool SetDensityFromMass(const Precision _mass);
+
+    /// \brief Radius of the cylinder.
+    private: Precision radius = 0.0;
+
+    /// \brief Length of the cylinder.
+    private: Precision length = 0.0;
+
+    /// \brief the cylinder's material.
+    private: Material material;
+
+    /// \brief Rotational offset.
+    private: Quaternion<Precision> rotOffset =
+             Quaternion<Precision>::Identity;
+  };
+
+  /// \typedef Cylinder<int> Cylinderi
+  /// \brief Cylinder with integer precision.
+  typedef Cylinder<int> Cylinderi;
+
+  /// \typedef Cylinder<double> Cylinderd
+  /// \brief Cylinder with double precision.
+  typedef Cylinder<double> Cylinderd;
+
+  /// \typedef Cylinder<float> Cylinderf
+  /// \brief Cylinder with float precision.
+  typedef Cylinder<float> Cylinderf;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
 #include "gz/math/detail/Cylinder.hh"
-
-#endif
+#endif  // GZ_MATH_CYLINDER_HH_
diff --git a/include/gz/math/DiffDriveOdometry.hh b/include/gz/math/DiffDriveOdometry.hh
index ad43dd724..8a4d66929 100644
--- a/include/gz/math/DiffDriveOdometry.hh
+++ b/include/gz/math/DiffDriveOdometry.hh
@@ -23,118 +23,114 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Use a steady clock
-    using clock = std::chrono::steady_clock;
-
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /** \class DiffDriveOdometry DiffDriveOdometry.hh \
-     * gz/math/DiffDriveOdometry.hh
-     **/
-    /// \brief Computes odometry values based on a set of kinematic
-    /// properties and wheel speeds for a diff-drive vehicle.
-    ///
-    /// A vehicle with a heading of zero degrees has a local
-    /// reference frame according to the diagram below.
-    ///
-    ///       Y
-    ///       ^
-    ///       |
-    ///       |
-    ///       O--->X(forward)
-    ///
-    /// Rotating the right wheel while keeping the left wheel fixed will cause
-    /// the vehicle to rotate counter-clockwise. For example (excuse the
-    /// lack of precision with ASCII art):
-    ///
-    ///     Y     X(forward)
-    ///     ^     ^
-    ///      \   /
-    ///       \ /
-    ///        O
-    ///
-    /// **Example Usage**
-    ///
-    /// \code{.cpp}
-    /// gz::math::DiffDriveOdometry odom;
-    /// odom.SetWheelParams(2.0, 0.5, 0.5);
-    /// odom.Init(std::chrono::steady_clock::now());
-    ///
-    /// // ... Some time later
-    ///
-    /// // Both wheels have rotated the same amount
-    /// odom.Update(GZ_DTOR(2), GZ_DTOR(2), std::chrono::steady_clock::now());
-    ///
-    /// // ... Some time later
-    ///
-    /// // The left wheel has rotated, the right wheel did not rotate
-    /// odom.Update(GZ_DTOR(4), GZ_DTOR(2), std::chrono::steady_clock::now());
-    /// \endcode
-    class GZ_MATH_VISIBLE DiffDriveOdometry
-    {
-      /// \brief Constructor.
-      /// \param[in] _windowSize Rolling window size used to compute the
-      /// velocity mean
-      public: explicit DiffDriveOdometry(size_t _windowSize = 10);
+  // Use a steady clock
+  using clock = std::chrono::steady_clock;
 
-      /// \brief Initialize the odometry
-      /// \param[in] _time Current time.
-      public: void Init(const clock::time_point &_time);
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /** \class DiffDriveOdometry DiffDriveOdometry.hh \
+   * gz/math/DiffDriveOdometry.hh
+   **/
+  /// \brief Computes odometry values based on a set of kinematic
+  /// properties and wheel speeds for a diff-drive vehicle.
+  ///
+  /// A vehicle with a heading of zero degrees has a local
+  /// reference frame according to the diagram below.
+  ///
+  ///       Y
+  ///       ^
+  ///       |
+  ///       |
+  ///       O--->X(forward)
+  ///
+  /// Rotating the right wheel while keeping the left wheel fixed will cause
+  /// the vehicle to rotate counter-clockwise. For example (excuse the
+  /// lack of precision with ASCII art):
+  ///
+  ///     Y     X(forward)
+  ///     ^     ^
+  ///      \   /
+  ///       \ /
+  ///        O
+  ///
+  /// **Example Usage**
+  ///
+  /// \code{.cpp}
+  /// gz::math::DiffDriveOdometry odom;
+  /// odom.SetWheelParams(2.0, 0.5, 0.5);
+  /// odom.Init(std::chrono::steady_clock::now());
+  ///
+  /// // ... Some time later
+  ///
+  /// // Both wheels have rotated the same amount
+  /// odom.Update(GZ_DTOR(2), GZ_DTOR(2), std::chrono::steady_clock::now());
+  ///
+  /// // ... Some time later
+  ///
+  /// // The left wheel has rotated, the right wheel did not rotate
+  /// odom.Update(GZ_DTOR(4), GZ_DTOR(2), std::chrono::steady_clock::now());
+  /// \endcode
+  class GZ_MATH_VISIBLE DiffDriveOdometry
+  {
+    /// \brief Constructor.
+    /// \param[in] _windowSize Rolling window size used to compute the
+    /// velocity mean
+    public: explicit DiffDriveOdometry(size_t _windowSize = 10);
 
-      /// \brief Get whether Init has been called.
-      /// \return True if Init has been called, false otherwise.
-      public: bool Initialized() const;
+    /// \brief Initialize the odometry
+    /// \param[in] _time Current time.
+    public: void Init(const clock::time_point &_time);
 
-      /// \brief Updates the odometry class with latest wheels and
-      /// steerings position
-      /// \param[in] _leftPos Left wheel position in radians.
-      /// \param[in] _rightPos Right wheel postion in radians.
-      /// \param[in] _time Current time point.
-      /// \return True if the odometry is actually updated.
-      public: bool Update(const Angle &_leftPos, const Angle &_rightPos,
-                          const clock::time_point &_time);
+    /// \brief Get whether Init has been called.
+    /// \return True if Init has been called, false otherwise.
+    public: bool Initialized() const;
 
-      /// \brief Get the heading.
-      /// \return The heading in radians.
-      public: const Angle &Heading() const;
+    /// \brief Updates the odometry class with latest wheels and
+    /// steerings position
+    /// \param[in] _leftPos Left wheel position in radians.
+    /// \param[in] _rightPos Right wheel postion in radians.
+    /// \param[in] _time Current time point.
+    /// \return True if the odometry is actually updated.
+    public: bool Update(const Angle &_leftPos, const Angle &_rightPos,
+                        const clock::time_point &_time);
 
-      /// \brief Get the X position.
-      ///  \return The X position in meters
-      public: double X() const;
+    /// \brief Get the heading.
+    /// \return The heading in radians.
+    public: const Angle &Heading() const;
 
-      /// \brief Get the Y position.
-      /// \return The Y position in meters.
-      public: double Y() const;
+    /// \brief Get the X position.
+    ///  \return The X position in meters
+    public: double X() const;
 
-      /// \brief Get the linear velocity.
-      /// \return The linear velocity in meter/second.
-      public: double LinearVelocity() const;
+    /// \brief Get the Y position.
+    /// \return The Y position in meters.
+    public: double Y() const;
 
-      /// \brief Get the angular velocity.
-      /// \return The angular velocity in radian/second.
-      public: const Angle &AngularVelocity() const;
+    /// \brief Get the linear velocity.
+    /// \return The linear velocity in meter/second.
+    public: double LinearVelocity() const;
 
-      /// \brief Set the wheel parameters including the radius and separation.
-      /// \param[in] _wheelSeparation Distance between left and right wheels.
-      /// \param[in] _leftWheelRadius Radius of the left wheel.
-      /// \param[in] _rightWheelRadius Radius of the right wheel.
-      public: void SetWheelParams(double _wheelSeparation,
-                      double _leftWheelRadius,
-                      double _rightWheelRadius);
+    /// \brief Get the angular velocity.
+    /// \return The angular velocity in radian/second.
+    public: const Angle &AngularVelocity() const;
 
-      /// \brief Set the velocity rolling window size.
-      /// \param[in] _size The Velocity rolling window size.
-      public: void SetVelocityRollingWindowSize(size_t _size);
+    /// \brief Set the wheel parameters including the radius and separation.
+    /// \param[in] _wheelSeparation Distance between left and right wheels.
+    /// \param[in] _leftWheelRadius Radius of the left wheel.
+    /// \param[in] _rightWheelRadius Radius of the right wheel.
+    public: void SetWheelParams(double _wheelSeparation,
+                    double _leftWheelRadius,
+                    double _rightWheelRadius);
 
-      /// \brief Private data pointer.
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
+    /// \brief Set the velocity rolling window size.
+    /// \param[in] _size The Velocity rolling window size.
+    public: void SetVelocityRollingWindowSize(size_t _size);
 
-#endif
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_DIFFDRIVEODOMETRY_HH_
diff --git a/include/gz/math/Ellipsoid.hh b/include/gz/math/Ellipsoid.hh
index 867091ca3..4a2039316 100644
--- a/include/gz/math/Ellipsoid.hh
+++ b/include/gz/math/Ellipsoid.hh
@@ -21,114 +21,110 @@
 #include "gz/math/MassMatrix3.hh"
 #include "gz/math/Material.hh"
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Ellipsoid Ellipsoid.hh gz/math/Ellipsoid.hh
+  /// \brief A representation of a general ellipsoid.
+  ///
+  /// The ellipsoid class supports defining a ellipsoid with three radii and
+  /// material properties. Radii are in meters. See Material for more on
+  /// material properties.
+  /// \tparam Precision Scalar numeric type.
+  template<typename Precision>
+  class Ellipsoid
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Ellipsoid Ellipsoid.hh gz/math/Ellipsoid.hh
-    /// \brief A representation of a general ellipsoid.
-    ///
-    /// The ellipsoid class supports defining a ellipsoid with three radii and
-    /// material properties. Radii are in meters. See Material for more on
-    /// material properties.
-    /// \tparam Precision Scalar numeric type.
-    template<typename Precision>
-    class Ellipsoid
-    {
-      /// \brief Default constructor. The default radius and length are both
-      /// zero.
-      public: Ellipsoid() = default;
-
-      /// \brief Construct a ellipsoid with a Vector3 of three radii.
-      /// \param[in] _radii The three radii (x, y, z) defining this ellipsoid
-      public: explicit Ellipsoid(const Vector3<Precision> &_radii);
-
-      /// \brief Construct a ellipsoid with three radii and a material.
-      /// \param[in] _radii The three radii (x, y, z) defining this ellipsoid
-      /// \param[in] _mat Material property for the ellipsoid.
-      public: Ellipsoid(const Vector3<Precision> &_radii,
-                  const Material &_mat);
-
-      /// \brief Get the radius in meters.
-      /// \return The radius of the ellipsoid in meters.
-      public: Vector3<Precision> Radii() const;
-
-      /// \brief Set the radius in meters.
-      /// \param[in] _radii The radii of the ellipsoid in meters.
-      public: void SetRadii(const Vector3<Precision> &_radii);
-
-      /// \brief Get the material associated with this ellipsoid.
-      /// \return The material assigned to this ellipsoid
-      public: const Material &Mat() const;
-
-      /// \brief Set the material associated with this ellipsoid.
-      /// \param[in] _mat The material assigned to this ellipsoid
-      public: void SetMat(const Material &_mat);
-
-      /// \brief Get the mass matrix for this ellipsoid. This function
-      /// is only meaningful if the ellipsoid's radii and material
-      /// have been set.
-      /// \return The computed mass matrix if parameters are valid
-      /// (radius > 0), (length > 0), and (density > 0). Otherwise
-      /// std::nullopt is returned.
-      public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
-
-      /// \brief Check if this ellipsoid is equal to the provided ellipsoid.
-      /// Radius, length, and material properties will be checked.
-      public: bool operator==(const Ellipsoid &_ellipsoid) const;
-
-      /// \brief Get the volume of the ellipsoid in m^3.
-      /// \return Volume of the ellipsoid in m^3.
-      public: Precision Volume() const;
-
-      /// \brief Compute the ellipsoid's density given a mass value. The
-      /// ellipsoid is assumed to be solid with uniform density. This
-      /// function requires the ellipsoid's radius and length to be set to
-      /// values greater than zero. The Material of the ellipsoid is ignored.
-      /// \param[in] _mass Mass of the ellipsoid, in kg. This value should be
-      /// greater than zero.
-      /// \return Density of the ellipsoid in kg/m^3. A NaN is returned
-      /// if radius, length or _mass is <= 0.
-      public: Precision DensityFromMass(const Precision _mass) const;
-
-      /// \brief Set the density of this ellipsoid based on a mass value.
-      /// Density is computed using
-      /// Precision DensityFromMass(const Precision _mass) const. The
-      /// ellipsoid is assumed to be solid with uniform density. This
-      /// function requires the ellipsoid's radius and length to be set to
-      /// values greater than zero. The existing Material density value is
-      /// overwritten only if the return value from this true.
-      /// \param[in] _mass Mass of the ellipsoid, in kg. This value should be
-      /// greater than zero.
-      /// \return True if the density was set. False is returned if the
-      /// ellipsoid's radius, length, or the _mass value are <= 0.
-      /// \sa Precision DensityFromMass(const Precision _mass) const
-      public: bool SetDensityFromMass(const Precision _mass);
-
-      /// \brief Radius of the ellipsoid.
-      private: Vector3<Precision> radii = Vector3<Precision>::Zero;
-
-      /// \brief the ellipsoid's material.
-      private: Material material;
-    };
-
-    /// \typedef Ellipsoid<int> Ellipsoidi
-    /// \brief Ellipsoid with integer precision.
-    typedef Ellipsoid<int> Ellipsoidi;
-
-    /// \typedef Ellipsoid<double> Ellipsoidd
-    /// \brief Ellipsoid with double precision.
-    typedef Ellipsoid<double> Ellipsoidd;
-
-    /// \typedef Ellipsoid<float> Ellipsoidf
-    /// \brief Ellipsoid with float precision.
-    typedef Ellipsoid<float> Ellipsoidf;
-    }
-  }
-}
+    /// \brief Default constructor. The default radius and length are both
+    /// zero.
+    public: Ellipsoid() = default;
+
+    /// \brief Construct a ellipsoid with a Vector3 of three radii.
+    /// \param[in] _radii The three radii (x, y, z) defining this ellipsoid
+    public: explicit Ellipsoid(const Vector3<Precision> &_radii);
+
+    /// \brief Construct a ellipsoid with three radii and a material.
+    /// \param[in] _radii The three radii (x, y, z) defining this ellipsoid
+    /// \param[in] _mat Material property for the ellipsoid.
+    public: Ellipsoid(const Vector3<Precision> &_radii,
+                const Material &_mat);
+
+    /// \brief Get the radius in meters.
+    /// \return The radius of the ellipsoid in meters.
+    public: Vector3<Precision> Radii() const;
+
+    /// \brief Set the radius in meters.
+    /// \param[in] _radii The radii of the ellipsoid in meters.
+    public: void SetRadii(const Vector3<Precision> &_radii);
+
+    /// \brief Get the material associated with this ellipsoid.
+    /// \return The material assigned to this ellipsoid
+    public: const Material &Mat() const;
+
+    /// \brief Set the material associated with this ellipsoid.
+    /// \param[in] _mat The material assigned to this ellipsoid
+    public: void SetMat(const Material &_mat);
+
+    /// \brief Get the mass matrix for this ellipsoid. This function
+    /// is only meaningful if the ellipsoid's radii and material
+    /// have been set.
+    /// \return The computed mass matrix if parameters are valid
+    /// (radius > 0), (length > 0), and (density > 0). Otherwise
+    /// std::nullopt is returned.
+    public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
+
+    /// \brief Check if this ellipsoid is equal to the provided ellipsoid.
+    /// Radius, length, and material properties will be checked.
+    public: bool operator==(const Ellipsoid &_ellipsoid) const;
+
+    /// \brief Get the volume of the ellipsoid in m^3.
+    /// \return Volume of the ellipsoid in m^3.
+    public: Precision Volume() const;
+
+    /// \brief Compute the ellipsoid's density given a mass value. The
+    /// ellipsoid is assumed to be solid with uniform density. This
+    /// function requires the ellipsoid's radius and length to be set to
+    /// values greater than zero. The Material of the ellipsoid is ignored.
+    /// \param[in] _mass Mass of the ellipsoid, in kg. This value should be
+    /// greater than zero.
+    /// \return Density of the ellipsoid in kg/m^3. A NaN is returned
+    /// if radius, length or _mass is <= 0.
+    public: Precision DensityFromMass(const Precision _mass) const;
+
+    /// \brief Set the density of this ellipsoid based on a mass value.
+    /// Density is computed using
+    /// Precision DensityFromMass(const Precision _mass) const. The
+    /// ellipsoid is assumed to be solid with uniform density. This
+    /// function requires the ellipsoid's radius and length to be set to
+    /// values greater than zero. The existing Material density value is
+    /// overwritten only if the return value from this true.
+    /// \param[in] _mass Mass of the ellipsoid, in kg. This value should be
+    /// greater than zero.
+    /// \return True if the density was set. False is returned if the
+    /// ellipsoid's radius, length, or the _mass value are <= 0.
+    /// \sa Precision DensityFromMass(const Precision _mass) const
+    public: bool SetDensityFromMass(const Precision _mass);
+
+    /// \brief Radius of the ellipsoid.
+    private: Vector3<Precision> radii = Vector3<Precision>::Zero;
+
+    /// \brief the ellipsoid's material.
+    private: Material material;
+  };
+
+  /// \typedef Ellipsoid<int> Ellipsoidi
+  /// \brief Ellipsoid with integer precision.
+  typedef Ellipsoid<int> Ellipsoidi;
+
+  /// \typedef Ellipsoid<double> Ellipsoidd
+  /// \brief Ellipsoid with double precision.
+  typedef Ellipsoid<double> Ellipsoidd;
+
+  /// \typedef Ellipsoid<float> Ellipsoidf
+  /// \brief Ellipsoid with float precision.
+  typedef Ellipsoid<float> Ellipsoidf;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
 #include "gz/math/detail/Ellipsoid.hh"
-
-#endif
+#endif  // GZ_MATH_ELLIPSOID_HH_
diff --git a/include/gz/math/Filter.hh b/include/gz/math/Filter.hh
index ef53aef6d..5a1ddeb33 100644
--- a/include/gz/math/Filter.hh
+++ b/include/gz/math/Filter.hh
@@ -22,231 +22,227 @@
 #include <gz/math/Quaternion.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Filter Filter.hh gz/math/Filter.hh
+  /// \brief Filter base class
+  template <class T>
+  class Filter
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Filter Filter.hh gz/math/Filter.hh
-    /// \brief Filter base class
-    template <class T>
-    class Filter
-    {
-      /// \brief Destructor.
-      public: virtual ~Filter() {}
-
-      /// \brief Set the output of the filter.
-      /// \param[in] _val New value.
-      public: virtual void Set(const T &_val)
-      {
-        y0 = _val;
-      }
-
-      /// \brief Set the cutoff frequency and sample rate.
-      /// \param[in] _fc Cutoff frequency.
-      /// \param[in] _fs Sample rate.
-      public: virtual void Fc(double _fc, double _fs) = 0;
-
-      /// \brief Get the output of the filter.
-      /// \return Filter's output.
-      public: virtual const T &Value() const
-      {
-        return y0;
-      }
-
-      /// \brief Output.
-      protected: T y0{};
-    };
-
-    /// \class OnePole Filter.hh gz/math/Filter.hh
-    /// \brief A one-pole DSP filter.
-    /// \sa http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/
-    template <class T>
-    class OnePole : public Filter<T>
-    {
-      /// \brief Constructor.
-      public: OnePole() = default;
-
-      /// \brief Constructor.
-      /// \param[in] _fc Cutoff frequency.
-      /// \param[in] _fs Sample rate.
-      public: OnePole(double _fc, double _fs)
-      {
-        this->Fc(_fc, _fs);
-      }
-
-      // Documentation Inherited.
-      public: virtual void Fc(double _fc, double _fs) override
-      {
-        b1 = exp(-2.0 * GZ_PI * _fc / _fs);
-        a0 = 1.0 - b1;
-      }
-
-      /// \brief Update the filter's output.
-      /// \param[in] _x Input value.
-      /// \return The filter's current output.
-      public: const T& Process(const T &_x)
-      {
-        this->y0 = a0 * _x + b1 * this->y0;
-        return this->y0;
-      }
-
-      /// \brief Input gain control.
-      protected: double a0 = 0;
-
-      /// \brief Gain of the feedback.
-      protected: double b1 = 0;
-    };
-
-    /// \class OnePoleQuaternion Filter.hh gz/math/Filter.hh
-    /// \brief One-pole quaternion filter.
-    class OnePoleQuaternion : public OnePole<math::Quaterniond>
-    {
-      /// \brief Constructor.
-      public: OnePoleQuaternion()
-      {
-        this->Set(math::Quaterniond(1, 0, 0, 0));
-      }
-
-      /// \brief Constructor.
-      /// \param[in] _fc Cutoff frequency.
-      /// \param[in] _fs Sample rate.
-      public: OnePoleQuaternion(double _fc, double _fs)
-        : OnePole<math::Quaterniond>(_fc, _fs)
-      {
-        this->Set(math::Quaterniond(1, 0, 0, 0));
-      }
-
-      /// \brief Update the filter's output.
-      /// \param[in] _x Input value.
-      /// \return The filter's current output.
-      public: const math::Quaterniond& Process(
-                  const math::Quaterniond &_x)
-      {
-        y0 = math::Quaterniond::Slerp(a0, y0, _x);
-        return y0;
-      }
-    };
-
-    /// \class OnePoleVector3 Filter.hh gz/math/Filter.hh
-    /// \brief One-pole vector3 filter.
-    class OnePoleVector3 : public OnePole<math::Vector3d>
-    {
-      /// \brief Constructor.
-      public: OnePoleVector3()
-      {
-        this->Set(math::Vector3d(0, 0, 0));
-      }
-
-      /// \brief Constructor.
-      /// \param[in] _fc Cutoff frequency.
-      /// \param[in] _fs Sample rate.
-      public: OnePoleVector3(double _fc, double _fs)
-        : OnePole<math::Vector3d>(_fc, _fs)
-      {
-        this->Set(math::Vector3d(0, 0, 0));
-      }
-    };
-
-    /// \class BiQuad Filter.hh gz/math/Filter.hh
-    /// \brief Bi-quad filter base class.
-    /// \sa http://www.earlevel.com/main/2003/03/02/the-bilinear-z-transform/
-    template <class T>
-    class BiQuad : public Filter<T>
-    {
-      /// \brief Constructor.
-      public: BiQuad() = default;
-
-      /// \brief Constructor.
-      /// \param[in] _fc Cutoff frequency.
-      /// \param[in] _fs Sample rate.
-      public: BiQuad(double _fc, double _fs)
-      {
-        this->Fc(_fc, _fs);
-      }
-
-      // Documentation Inherited.
-      public: void Fc(double _fc, double _fs) override
-      {
-        this->Fc(_fc, _fs, 0.5);
-      }
-
-      /// \brief Set the cutoff frequency, sample rate and Q coefficient.
-      /// \param[in] _fc Cutoff frequency.
-      /// \param[in] _fs Sample rate.
-      /// \param[in] _q Q coefficient.
-      public: void Fc(double _fc, double _fs, double _q)
-      {
-        double k = tan(GZ_PI * _fc / _fs);
-        double kQuadDenom = k * k + k / _q + 1.0;
-        this->a0 = k * k/ kQuadDenom;
-        this->a1 = 2 * this->a0;
-        this->a2 = this->a0;
-        this->b0 = 1.0;
-        this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
-        this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
-      }
-
-      /// \brief Set the current filter's output.
-      /// \param[in] _val New filter's output.
-      public: virtual void Set(const T &_val) override
-      {
-        this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
-      }
-
-      /// \brief Update the filter's output.
-      /// \param[in] _x Input value.
-      /// \return The filter's current output.
-      public: virtual const T& Process(const T &_x)
-      {
-        this->y0 = this->a0 * _x +
-                   this->a1 * this->x1 +
-                   this->a2 * this->x2 -
-                   this->b1 * this->y1 -
-                   this->b2 * this->y2;
-
-        this->x2 = this->x1;
-        this->x1 = _x;
-        this->y2 = this->y1;
-        this->y1 = this->y0;
-        return this->y0;
-      }
-
-      /// \brief Input gain control coefficients.
-      protected: double a0 = 0,
-                        a1 = 0,
-                        a2 = 0,
-                        b0 = 0,
-                        b1 = 0,
-                        b2 = 0;
-
-      /// \brief Gain of the feedback coefficients.
-      protected: T x1{}, x2{}, y1{}, y2{};
-    };
-
-    /// \class BiQuadVector3 Filter.hh gz/math/Filter.hh
-    /// \brief BiQuad vector3 filter
-    class BiQuadVector3 : public BiQuad<math::Vector3d>
-    {
-      /// \brief Constructor.
-      public: BiQuadVector3()
-      {
-        this->Set(math::Vector3d(0, 0, 0));
-      }
-
-      /// \brief Constructor.
-      /// \param[in] _fc Cutoff frequency.
-      /// \param[in] _fs Sample rate.
-      public: BiQuadVector3(double _fc, double _fs)
-        : BiQuad<math::Vector3d>(_fc, _fs)
-      {
-        this->Set(math::Vector3d(0, 0, 0));
-      }
-    };
-    }
-  }
-}
-
-#endif
+    /// \brief Destructor.
+    public: virtual ~Filter() {}
+
+    /// \brief Set the output of the filter.
+    /// \param[in] _val New value.
+    public: virtual void Set(const T &_val)
+    {
+      y0 = _val;
+    }
+
+    /// \brief Set the cutoff frequency and sample rate.
+    /// \param[in] _fc Cutoff frequency.
+    /// \param[in] _fs Sample rate.
+    public: virtual void Fc(double _fc, double _fs) = 0;
+
+    /// \brief Get the output of the filter.
+    /// \return Filter's output.
+    public: virtual const T &Value() const
+    {
+      return y0;
+    }
+
+    /// \brief Output.
+    protected: T y0{};
+  };
+
+  /// \class OnePole Filter.hh gz/math/Filter.hh
+  /// \brief A one-pole DSP filter.
+  /// \sa http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/
+  template <class T>
+  class OnePole : public Filter<T>
+  {
+    /// \brief Constructor.
+    public: OnePole() = default;
+
+    /// \brief Constructor.
+    /// \param[in] _fc Cutoff frequency.
+    /// \param[in] _fs Sample rate.
+    public: OnePole(double _fc, double _fs)
+    {
+      this->Fc(_fc, _fs);
+    }
+
+    // Documentation Inherited.
+    public: virtual void Fc(double _fc, double _fs) override
+    {
+      b1 = exp(-2.0 * GZ_PI * _fc / _fs);
+      a0 = 1.0 - b1;
+    }
+
+    /// \brief Update the filter's output.
+    /// \param[in] _x Input value.
+    /// \return The filter's current output.
+    public: const T& Process(const T &_x)
+    {
+      this->y0 = a0 * _x + b1 * this->y0;
+      return this->y0;
+    }
+
+    /// \brief Input gain control.
+    protected: double a0 = 0;
+
+    /// \brief Gain of the feedback.
+    protected: double b1 = 0;
+  };
+
+  /// \class OnePoleQuaternion Filter.hh gz/math/Filter.hh
+  /// \brief One-pole quaternion filter.
+  class OnePoleQuaternion : public OnePole<math::Quaterniond>
+  {
+    /// \brief Constructor.
+    public: OnePoleQuaternion()
+    {
+      this->Set(math::Quaterniond(1, 0, 0, 0));
+    }
+
+    /// \brief Constructor.
+    /// \param[in] _fc Cutoff frequency.
+    /// \param[in] _fs Sample rate.
+    public: OnePoleQuaternion(double _fc, double _fs)
+      : OnePole<math::Quaterniond>(_fc, _fs)
+    {
+      this->Set(math::Quaterniond(1, 0, 0, 0));
+    }
+
+    /// \brief Update the filter's output.
+    /// \param[in] _x Input value.
+    /// \return The filter's current output.
+    public: const math::Quaterniond& Process(
+                const math::Quaterniond &_x)
+    {
+      y0 = math::Quaterniond::Slerp(a0, y0, _x);
+      return y0;
+    }
+  };
+
+  /// \class OnePoleVector3 Filter.hh gz/math/Filter.hh
+  /// \brief One-pole vector3 filter.
+  class OnePoleVector3 : public OnePole<math::Vector3d>
+  {
+    /// \brief Constructor.
+    public: OnePoleVector3()
+    {
+      this->Set(math::Vector3d(0, 0, 0));
+    }
+
+    /// \brief Constructor.
+    /// \param[in] _fc Cutoff frequency.
+    /// \param[in] _fs Sample rate.
+    public: OnePoleVector3(double _fc, double _fs)
+      : OnePole<math::Vector3d>(_fc, _fs)
+    {
+      this->Set(math::Vector3d(0, 0, 0));
+    }
+  };
+
+  /// \class BiQuad Filter.hh gz/math/Filter.hh
+  /// \brief Bi-quad filter base class.
+  /// \sa http://www.earlevel.com/main/2003/03/02/the-bilinear-z-transform/
+  template <class T>
+  class BiQuad : public Filter<T>
+  {
+    /// \brief Constructor.
+    public: BiQuad() = default;
+
+    /// \brief Constructor.
+    /// \param[in] _fc Cutoff frequency.
+    /// \param[in] _fs Sample rate.
+    public: BiQuad(double _fc, double _fs)
+    {
+      this->Fc(_fc, _fs);
+    }
+
+    // Documentation Inherited.
+    public: void Fc(double _fc, double _fs) override
+    {
+      this->Fc(_fc, _fs, 0.5);
+    }
+
+    /// \brief Set the cutoff frequency, sample rate and Q coefficient.
+    /// \param[in] _fc Cutoff frequency.
+    /// \param[in] _fs Sample rate.
+    /// \param[in] _q Q coefficient.
+    public: void Fc(double _fc, double _fs, double _q)
+    {
+      double k = tan(GZ_PI * _fc / _fs);
+      double kQuadDenom = k * k + k / _q + 1.0;
+      this->a0 = k * k/ kQuadDenom;
+      this->a1 = 2 * this->a0;
+      this->a2 = this->a0;
+      this->b0 = 1.0;
+      this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
+      this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
+    }
+
+    /// \brief Set the current filter's output.
+    /// \param[in] _val New filter's output.
+    public: virtual void Set(const T &_val) override
+    {
+      this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
+    }
+
+    /// \brief Update the filter's output.
+    /// \param[in] _x Input value.
+    /// \return The filter's current output.
+    public: virtual const T& Process(const T &_x)
+    {
+      this->y0 = this->a0 * _x +
+                 this->a1 * this->x1 +
+                 this->a2 * this->x2 -
+                 this->b1 * this->y1 -
+                 this->b2 * this->y2;
+
+      this->x2 = this->x1;
+      this->x1 = _x;
+      this->y2 = this->y1;
+      this->y1 = this->y0;
+      return this->y0;
+    }
+
+    /// \brief Input gain control coefficients.
+    protected: double a0 = 0,
+                      a1 = 0,
+                      a2 = 0,
+                      b0 = 0,
+                      b1 = 0,
+                      b2 = 0;
+
+    /// \brief Gain of the feedback coefficients.
+    protected: T x1{}, x2{}, y1{}, y2{};
+  };
+
+  /// \class BiQuadVector3 Filter.hh gz/math/Filter.hh
+  /// \brief BiQuad vector3 filter
+  class BiQuadVector3 : public BiQuad<math::Vector3d>
+  {
+    /// \brief Constructor.
+    public: BiQuadVector3()
+    {
+      this->Set(math::Vector3d(0, 0, 0));
+    }
+
+    /// \brief Constructor.
+    /// \param[in] _fc Cutoff frequency.
+    /// \param[in] _fs Sample rate.
+    public: BiQuadVector3(double _fc, double _fs)
+      : BiQuad<math::Vector3d>(_fc, _fs)
+    {
+      this->Set(math::Vector3d(0, 0, 0));
+    }
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_FILTER_HH_
diff --git a/include/gz/math/Frustum.hh b/include/gz/math/Frustum.hh
index 462e1385a..e5bf69741 100644
--- a/include/gz/math/Frustum.hh
+++ b/include/gz/math/Frustum.hh
@@ -24,149 +24,146 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
 
-    /// \brief Mathematical representation of a frustum and related functions.
-    /// This is also known as a view frustum.
-    class GZ_MATH_VISIBLE Frustum
+  /// \brief Mathematical representation of a frustum and related functions.
+  /// This is also known as a view frustum.
+  class GZ_MATH_VISIBLE Frustum
+  {
+    /// \brief Planes that define the boundaries of the frustum.
+    public: enum FrustumPlane
     {
-      /// \brief Planes that define the boundaries of the frustum.
-      public: enum FrustumPlane
-      {
-        /// \brief Near plane
-        FRUSTUM_PLANE_NEAR   = 0,
-
-        /// \brief Far plane
-        FRUSTUM_PLANE_FAR    = 1,
-
-        /// \brief Left plane
-        FRUSTUM_PLANE_LEFT   = 2,
-
-        /// \brief Right plane
-        FRUSTUM_PLANE_RIGHT  = 3,
-
-        /// \brief Top plane
-        FRUSTUM_PLANE_TOP    = 4,
-
-        /// \brief Bottom plane
-        FRUSTUM_PLANE_BOTTOM = 5
-      };
-
-      /// \brief Default constructor. With the following default values:
-      ///
-      /// * near: 0.0
-      /// * far: 1.0
-      /// * fov: 0.78539 radians (45 degrees)
-      /// * aspect ratio: 1.0
-      /// * pose: Pose3d::Zero
-      public: Frustum();
-
-      /// \brief Constructor
-      /// \param[in] _near Near plane distance. This is the distance from
-      /// the frustum's vertex to the closest plane
-      /// \param[in] _far Far plane distance. This is the distance from the
-      /// frustum's vertex to the farthest plane.
-      /// \param[in] _fov Field of view. The field of view is the
-      /// angle between the frustum's vertex and the edges of the near or far
-      /// plane. This value represents the horizontal angle.
-      /// \param[in] _aspectRatio The aspect ratio, which is the width divided
-      /// by height of the near or far planes.
-      /// \param[in] _pose Pose of the frustum, which is the vertex (top of
-      /// the pyramid).
-      public: Frustum(double _near,
-                      double _far,
-                      const math::Angle &_fov,
-                      double _aspectRatio,
-                      const math::Pose3d &_pose = math::Pose3d::Zero);
-
-      /// \brief Get the near distance. This is the distance from the
-      /// frustum's vertex to the closest plane.
-      /// \return Near distance.
-      /// \sa SetNear
-      public: double Near() const;
-
-      /// \brief Set the near distance. This is the distance from the
-      /// frustum's vertex to the closest plane.
-      /// \param[in] _near Near distance.
-      /// \sa Near
-      public: void SetNear(double _near);
-
-      /// \brief Get the far distance. This is the distance from the
-      /// frustum's vertex to the farthest plane.
-      /// \return Far distance.
-      /// \sa SetFar
-      public: double Far() const;
-
-      /// \brief Set the far distance. This is the distance from the
-      /// frustum's vertex to the farthest plane.
-      /// \param[in] _far Far distance.
-      /// \sa Far
-      public: void SetFar(double _far);
-
-      /// \brief Get the horizontal field of view. The field of view is the
-      /// angle between the frustum's vertex and the edges of the near or far
-      /// plane. This value represents the horizontal angle.
-      /// \return The field of view.
-      /// \sa SetFOV
-      public: math::Angle FOV() const;
-
-      /// \brief Set the horizontal field of view. The field of view is the
-      /// angle between the frustum's vertex and the edges of the near or far
-      /// plane. This value represents the horizontal angle.
-      /// \param[in] _fov The field of view.
-      /// \sa FOV
-      public: void SetFOV(const math::Angle &_fov);
-
-      /// \brief Get the aspect ratio, which is the width divided by height
-      /// of the near or far planes.
-      /// \return The frustum's aspect ratio.
-      /// \sa SetAspectRatio
-      public: double AspectRatio() const;
-
-      /// \brief Set the aspect ratio, which is the width divided by height
-      /// of the near or far planes.
-      /// \param[in] _aspectRatio The frustum's aspect ratio.
-      /// \sa AspectRatio
-      public: void SetAspectRatio(double _aspectRatio);
-
-      /// \brief Get a plane of the frustum.
-      /// \param[in] _plane The plane to return.
-      /// \return Plane of the frustum.
-      public: Planed Plane(const FrustumPlane _plane) const;
-
-      /// \brief Check if a box lies inside the pyramid frustum.
-      /// \param[in] _b Box to check.
-      /// \return True if the box is inside the pyramid frustum.
-      public: bool Contains(const AxisAlignedBox &_b) const;
-
-      /// \brief Check if a point lies inside the pyramid frustum.
-      /// \param[in] _p Point to check.
-      /// \return True if the point is inside the pyramid frustum.
-      public: bool Contains(const Vector3d &_p) const;
-
-      /// \brief Get the pose of the frustum
-      /// \return Pose of the frustum
-      /// \sa SetPose
-      public: Pose3d Pose() const;
-
-      /// \brief Set the pose of the frustum
-      /// \param[in] _pose Pose of the frustum, top vertex.
-      /// \sa Pose
-      public: void SetPose(const Pose3d &_pose);
-
-      /// \brief Compute the planes of the frustum. This is called whenever
-      /// a property of the frustum is changed.
-      private: void ComputePlanes();
-
-      /// \brief Private data pointer
-      GZ_UTILS_IMPL_PTR(dataPtr)
+      /// \brief Near plane
+      FRUSTUM_PLANE_NEAR   = 0,
+
+      /// \brief Far plane
+      FRUSTUM_PLANE_FAR    = 1,
+
+      /// \brief Left plane
+      FRUSTUM_PLANE_LEFT   = 2,
+
+      /// \brief Right plane
+      FRUSTUM_PLANE_RIGHT  = 3,
+
+      /// \brief Top plane
+      FRUSTUM_PLANE_TOP    = 4,
+
+      /// \brief Bottom plane
+      FRUSTUM_PLANE_BOTTOM = 5
     };
-    }
-  }
-}
-#endif
+
+    /// \brief Default constructor. With the following default values:
+    ///
+    /// * near: 0.0
+    /// * far: 1.0
+    /// * fov: 0.78539 radians (45 degrees)
+    /// * aspect ratio: 1.0
+    /// * pose: Pose3d::Zero
+    public: Frustum();
+
+    /// \brief Constructor
+    /// \param[in] _near Near plane distance. This is the distance from
+    /// the frustum's vertex to the closest plane
+    /// \param[in] _far Far plane distance. This is the distance from the
+    /// frustum's vertex to the farthest plane.
+    /// \param[in] _fov Field of view. The field of view is the
+    /// angle between the frustum's vertex and the edges of the near or far
+    /// plane. This value represents the horizontal angle.
+    /// \param[in] _aspectRatio The aspect ratio, which is the width divided
+    /// by height of the near or far planes.
+    /// \param[in] _pose Pose of the frustum, which is the vertex (top of
+    /// the pyramid).
+    public: Frustum(double _near,
+                    double _far,
+                    const math::Angle &_fov,
+                    double _aspectRatio,
+                    const math::Pose3d &_pose = math::Pose3d::Zero);
+
+    /// \brief Get the near distance. This is the distance from the
+    /// frustum's vertex to the closest plane.
+    /// \return Near distance.
+    /// \sa SetNear
+    public: double Near() const;
+
+    /// \brief Set the near distance. This is the distance from the
+    /// frustum's vertex to the closest plane.
+    /// \param[in] _near Near distance.
+    /// \sa Near
+    public: void SetNear(double _near);
+
+    /// \brief Get the far distance. This is the distance from the
+    /// frustum's vertex to the farthest plane.
+    /// \return Far distance.
+    /// \sa SetFar
+    public: double Far() const;
+
+    /// \brief Set the far distance. This is the distance from the
+    /// frustum's vertex to the farthest plane.
+    /// \param[in] _far Far distance.
+    /// \sa Far
+    public: void SetFar(double _far);
+
+    /// \brief Get the horizontal field of view. The field of view is the
+    /// angle between the frustum's vertex and the edges of the near or far
+    /// plane. This value represents the horizontal angle.
+    /// \return The field of view.
+    /// \sa SetFOV
+    public: math::Angle FOV() const;
+
+    /// \brief Set the horizontal field of view. The field of view is the
+    /// angle between the frustum's vertex and the edges of the near or far
+    /// plane. This value represents the horizontal angle.
+    /// \param[in] _fov The field of view.
+    /// \sa FOV
+    public: void SetFOV(const math::Angle &_fov);
+
+    /// \brief Get the aspect ratio, which is the width divided by height
+    /// of the near or far planes.
+    /// \return The frustum's aspect ratio.
+    /// \sa SetAspectRatio
+    public: double AspectRatio() const;
+
+    /// \brief Set the aspect ratio, which is the width divided by height
+    /// of the near or far planes.
+    /// \param[in] _aspectRatio The frustum's aspect ratio.
+    /// \sa AspectRatio
+    public: void SetAspectRatio(double _aspectRatio);
+
+    /// \brief Get a plane of the frustum.
+    /// \param[in] _plane The plane to return.
+    /// \return Plane of the frustum.
+    public: Planed Plane(const FrustumPlane _plane) const;
+
+    /// \brief Check if a box lies inside the pyramid frustum.
+    /// \param[in] _b Box to check.
+    /// \return True if the box is inside the pyramid frustum.
+    public: bool Contains(const AxisAlignedBox &_b) const;
+
+    /// \brief Check if a point lies inside the pyramid frustum.
+    /// \param[in] _p Point to check.
+    /// \return True if the point is inside the pyramid frustum.
+    public: bool Contains(const Vector3d &_p) const;
+
+    /// \brief Get the pose of the frustum
+    /// \return Pose of the frustum
+    /// \sa SetPose
+    public: Pose3d Pose() const;
+
+    /// \brief Set the pose of the frustum
+    /// \param[in] _pose Pose of the frustum, top vertex.
+    /// \sa Pose
+    public: void SetPose(const Pose3d &_pose);
+
+    /// \brief Compute the planes of the frustum. This is called whenever
+    /// a property of the frustum is changed.
+    private: void ComputePlanes();
+
+    /// \brief Private data pointer
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_FRUSTUM_HH_
diff --git a/include/gz/math/GaussMarkovProcess.hh b/include/gz/math/GaussMarkovProcess.hh
index 59a43f2c2..96cdd8d11 100644
--- a/include/gz/math/GaussMarkovProcess.hh
+++ b/include/gz/math/GaussMarkovProcess.hh
@@ -22,116 +22,113 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Use a steady clock
-    using clock = std::chrono::steady_clock;
+  // Use a steady clock
+  using clock = std::chrono::steady_clock;
 
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /** \class GaussMarkovProcess GaussMarkovProcess.hh\
-     * gz/math/GaussMarkovProcess.hh
-     **/
-    /// \brief Implementation of a stationary gauss-markov process, also
-    /// known as a Ornstein Ulenbeck process.
-    ///
-    /// See the Update(const clock::duration &) for details on the forumla
-    /// used to update the process.
-    ///
-    /// ## Example usage
-    ///
-    /// \snippet examples/gauss_markov_process_example.cc complete
-    class GZ_MATH_VISIBLE GaussMarkovProcess
-    {
-      // Default constructor. This sets all the parameters to zero.
-      public: GaussMarkovProcess();
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /** \class GaussMarkovProcess GaussMarkovProcess.hh\
+   * gz/math/GaussMarkovProcess.hh
+   **/
+  /// \brief Implementation of a stationary gauss-markov process, also
+  /// known as a Ornstein Ulenbeck process.
+  ///
+  /// See the Update(const clock::duration &) for details on the forumla
+  /// used to update the process.
+  ///
+  /// ## Example usage
+  ///
+  /// \snippet examples/gauss_markov_process_example.cc complete
+  class GZ_MATH_VISIBLE GaussMarkovProcess
+  {
+    // Default constructor. This sets all the parameters to zero.
+    public: GaussMarkovProcess();
 
-      /// \brief Create a process with the provided process parameters.
-      /// This will also call Set(), and in turn Reset().
-      /// \param[in] _start The start value of the process.
-      /// \param[in] _theta The theta (\f$\theta\f$) parameter. A value of
-      /// zero will be used if this parameter is negative.
-      /// \param[in] _mu The mu (\f$\mu\f$) parameter.
-      /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter. A value of
-      /// zero will be used if this parameter is negative.
-      /// \sa Update(const clock::duration &)
-      public: GaussMarkovProcess(double _start, double _theta, double _mu,
-                  double _sigma);
+    /// \brief Create a process with the provided process parameters.
+    /// This will also call Set(), and in turn Reset().
+    /// \param[in] _start The start value of the process.
+    /// \param[in] _theta The theta (\f$\theta\f$) parameter. A value of
+    /// zero will be used if this parameter is negative.
+    /// \param[in] _mu The mu (\f$\mu\f$) parameter.
+    /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter. A value of
+    /// zero will be used if this parameter is negative.
+    /// \sa Update(const clock::duration &)
+    public: GaussMarkovProcess(double _start, double _theta, double _mu,
+                double _sigma);
 
-      /// \brief Set the process parameters. This will also call Reset().
-      /// \param[in] _start The start value of the process.
-      /// \param[in] _theta The theta (\f$\theta\f$) parameter.
-      /// \param[in] _mu The mu (\f$\mu\f$) parameter.
-      /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter.
-      /// \sa Update(const clock::duration &)
-      public: void Set(double _start, double _theta, double _mu, double _sigma);
+    /// \brief Set the process parameters. This will also call Reset().
+    /// \param[in] _start The start value of the process.
+    /// \param[in] _theta The theta (\f$\theta\f$) parameter.
+    /// \param[in] _mu The mu (\f$\mu\f$) parameter.
+    /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter.
+    /// \sa Update(const clock::duration &)
+    public: void Set(double _start, double _theta, double _mu, double _sigma);
 
-      /// \brief Get the start value.
-      /// \return The start value.
-      /// \sa Set(double, double, double, double)
-      public: double Start() const;
+    /// \brief Get the start value.
+    /// \return The start value.
+    /// \sa Set(double, double, double, double)
+    public: double Start() const;
 
-      /// \brief Get the current process value.
-      /// \return The value of the process.
-      public: double Value() const;
+    /// \brief Get the current process value.
+    /// \return The value of the process.
+    public: double Value() const;
 
-      /// \brief Get the theta (\f$\theta\f$) value.
-      /// \return The theta value.
-      /// \sa Set(double, double, double, double)
-      public: double Theta() const;
+    /// \brief Get the theta (\f$\theta\f$) value.
+    /// \return The theta value.
+    /// \sa Set(double, double, double, double)
+    public: double Theta() const;
 
-      /// \brief Get the mu (\f$\mu\f$) value.
-      /// \return The mu value.
-      /// \sa Set(double, double, double, double)
-      public: double Mu() const;
+    /// \brief Get the mu (\f$\mu\f$) value.
+    /// \return The mu value.
+    /// \sa Set(double, double, double, double)
+    public: double Mu() const;
 
-      /// \brief Get the sigma (\f$\sigma\f$) value.
-      /// \return The sigma value.
-      /// \sa Set(double, double, double, double)
-      public: double Sigma() const;
+    /// \brief Get the sigma (\f$\sigma\f$) value.
+    /// \return The sigma value.
+    /// \sa Set(double, double, double, double)
+    public: double Sigma() const;
 
-      /// \brief Reset the process. This will set the current process value
-      /// to the start value.
-      public: void Reset();
+    /// \brief Reset the process. This will set the current process value
+    /// to the start value.
+    public: void Reset();
 
-      /// \brief Update the process and get the new value.
-      ///
-      /// The following equation is computed:
-      ///
-      /// \f$x_{t+1} += \theta * (\mu - x_t) * dt + \sigma * dW_t\f$
-      ///
-      /// where
-      ///
-      ///   * \f$\theta, \mu, \sigma\f$ are parameters specified by the
-      ///     user. In order, the parameters are theta, mu, and sigma. Theta
-      ///     and sigma must be greater than or equal to zero. You can think
-      ///     of mu as representing the mean or equilibrium value, sigma as the
-      ///     degree of volatility, and theta as the rate by which changes
-      ///     dissipate and revert towards the mean.
-      ///   * \f$dt\f$ is the time step in seconds.
-      ///   * \f$dW_t\f$ is a random number drawm from a normal distribution
-      ///     with mean of zero and variance of 1.
-      ///   * \f$x_t\f$ is the current value of the Gauss-Markov process
-      ///   * \f$x_{t+1}\f$ is the new value of the Gauss-Markvov process
-      ///
-      /// See also: https://en.wikipedia.org/wiki/Ornstein%E2%80%93Uhlenbeck_process
-      ///
-      /// This implementation include a drift parameter, mu. In financial
-      /// mathematics, this is known as a Vasicek model.
-      ///
-      /// \param[in] _dt Length of the timestep after which a new sample
-      /// should be taken.
-      /// \return The new value of this process.
-      public: double Update(const clock::duration &_dt);
+    /// \brief Update the process and get the new value.
+    ///
+    /// The following equation is computed:
+    ///
+    /// \f$x_{t+1} += \theta * (\mu - x_t) * dt + \sigma * dW_t\f$
+    ///
+    /// where
+    ///
+    ///   * \f$\theta, \mu, \sigma\f$ are parameters specified by the
+    ///     user. In order, the parameters are theta, mu, and sigma. Theta
+    ///     and sigma must be greater than or equal to zero. You can think
+    ///     of mu as representing the mean or equilibrium value, sigma as the
+    ///     degree of volatility, and theta as the rate by which changes
+    ///     dissipate and revert towards the mean.
+    ///   * \f$dt\f$ is the time step in seconds.
+    ///   * \f$dW_t\f$ is a random number drawm from a normal distribution
+    ///     with mean of zero and variance of 1.
+    ///   * \f$x_t\f$ is the current value of the Gauss-Markov process
+    ///   * \f$x_{t+1}\f$ is the new value of the Gauss-Markvov process
+    ///
+    /// See also: https://en.wikipedia.org/wiki/Ornstein%E2%80%93Uhlenbeck_process
+    ///
+    /// This implementation include a drift parameter, mu. In financial
+    /// mathematics, this is known as a Vasicek model.
+    ///
+    /// \param[in] _dt Length of the timestep after which a new sample
+    /// should be taken.
+    /// \return The new value of this process.
+    public: double Update(const clock::duration &_dt);
 
-      public: double Update(double _dt);
+    public: double Update(double _dt);
 
-      /// \brief Private data pointer.
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
-#endif
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_GAUSSMARKOVPROCESS_HH_
diff --git a/include/gz/math/Helpers.hh b/include/gz/math/Helpers.hh
index 4d0f09d45..19100fc15 100644
--- a/include/gz/math/Helpers.hh
+++ b/include/gz/math/Helpers.hh
@@ -83,620 +83,615 @@ constexpr T GZ_MASSMATRIX3_DEFAULT_TOLERANCE = T(10);
 /// \param[in] _v Vector3d that contains the box's dimensions.
 #define GZ_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z())
 
-namespace gz
+namespace gz::math
 {
-  /// \brief Math classes and function useful in robot applications.
-  namespace math
-  {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \brief size_t type with a value of 0
-    static const size_t GZ_ZERO_SIZE_T  = 0u;
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \brief size_t type with a value of 0
+  static const size_t GZ_ZERO_SIZE_T  = 0u;
 
-    /// \brief size_t type with a value of 1
-    static const size_t GZ_ONE_SIZE_T   = 1u;
+  /// \brief size_t type with a value of 1
+  static const size_t GZ_ONE_SIZE_T   = 1u;
 
-    /// \brief size_t type with a value of 2
-    static const size_t GZ_TWO_SIZE_T   = 2u;
+  /// \brief size_t type with a value of 2
+  static const size_t GZ_TWO_SIZE_T   = 2u;
 
-    /// \brief size_t type with a value of 3
-    static const size_t GZ_THREE_SIZE_T = 3u;
+  /// \brief size_t type with a value of 3
+  static const size_t GZ_THREE_SIZE_T = 3u;
 
-    /// \brief size_t type with a value of 4
-    static const size_t GZ_FOUR_SIZE_T  = 4u;
+  /// \brief size_t type with a value of 4
+  static const size_t GZ_FOUR_SIZE_T  = 4u;
 
-    /// \brief size_t type with a value of 5
-    static const size_t GZ_FIVE_SIZE_T  = 5u;
+  /// \brief size_t type with a value of 5
+  static const size_t GZ_FIVE_SIZE_T  = 5u;
 
-    /// \brief size_t type with a value of 6
-    static const size_t GZ_SIX_SIZE_T   = 6u;
+  /// \brief size_t type with a value of 6
+  static const size_t GZ_SIX_SIZE_T   = 6u;
 
-    /// \brief size_t type with a value of 7
-    static const size_t GZ_SEVEN_SIZE_T = 7u;
+  /// \brief size_t type with a value of 7
+  static const size_t GZ_SEVEN_SIZE_T = 7u;
 
-    /// \brief size_t type with a value of 8
-    static const size_t GZ_EIGHT_SIZE_T = 8u;
+  /// \brief size_t type with a value of 8
+  static const size_t GZ_EIGHT_SIZE_T = 8u;
 
-    /// \brief size_t type with a value of 9
-    static const size_t GZ_NINE_SIZE_T  = 9u;
+  /// \brief size_t type with a value of 9
+  static const size_t GZ_NINE_SIZE_T  = 9u;
 
-    /// \brief Double maximum value. This value will be similar to 1.79769e+308
-    static const double MAX_D = std::numeric_limits<double>::max();
+  /// \brief Double maximum value. This value will be similar to 1.79769e+308
+  static const double MAX_D = std::numeric_limits<double>::max();
 
-    /// \brief Double min value. This value will be similar to 2.22507e-308
-    static const double MIN_D = std::numeric_limits<double>::min();
+  /// \brief Double min value. This value will be similar to 2.22507e-308
+  static const double MIN_D = std::numeric_limits<double>::min();
 
-    /// \brief Double low value, equivalent to -MAX_D
-    static const double LOW_D = std::numeric_limits<double>::lowest();
+  /// \brief Double low value, equivalent to -MAX_D
+  static const double LOW_D = std::numeric_limits<double>::lowest();
 
-    /// \brief Double positive infinite value
-    static const double INF_D = std::numeric_limits<double>::infinity();
+  /// \brief Double positive infinite value
+  static const double INF_D = std::numeric_limits<double>::infinity();
 
-    /// \brief Returns the representation of a quiet not a number (NAN)
-    static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
+  /// \brief Returns the representation of a quiet not a number (NAN)
+  static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
 
-    /// \brief Float maximum value. This value will be similar to 3.40282e+38
-    static const float MAX_F = std::numeric_limits<float>::max();
+  /// \brief Float maximum value. This value will be similar to 3.40282e+38
+  static const float MAX_F = std::numeric_limits<float>::max();
 
-    /// \brief Float minimum value. This value will be similar to 1.17549e-38
-    static const float MIN_F = std::numeric_limits<float>::min();
+  /// \brief Float minimum value. This value will be similar to 1.17549e-38
+  static const float MIN_F = std::numeric_limits<float>::min();
 
-    /// \brief Float low value, equivalent to -MAX_F
-    static const float LOW_F = std::numeric_limits<float>::lowest();
+  /// \brief Float low value, equivalent to -MAX_F
+  static const float LOW_F = std::numeric_limits<float>::lowest();
 
-    /// \brief float positive infinite value
-    static const float INF_F = std::numeric_limits<float>::infinity();
+  /// \brief float positive infinite value
+  static const float INF_F = std::numeric_limits<float>::infinity();
 
-    /// \brief Returns the representation of a quiet not a number (NAN)
-    static const float NAN_F = std::numeric_limits<float>::quiet_NaN();
+  /// \brief Returns the representation of a quiet not a number (NAN)
+  static const float NAN_F = std::numeric_limits<float>::quiet_NaN();
 
-    /// \brief 16bit unsigned integer maximum value
-    static const uint16_t MAX_UI16 = std::numeric_limits<uint16_t>::max();
+  /// \brief 16bit unsigned integer maximum value
+  static const uint16_t MAX_UI16 = std::numeric_limits<uint16_t>::max();
 
-    /// \brief 16bit unsigned integer minimum value
-    static const uint16_t MIN_UI16 = std::numeric_limits<uint16_t>::min();
+  /// \brief 16bit unsigned integer minimum value
+  static const uint16_t MIN_UI16 = std::numeric_limits<uint16_t>::min();
 
-    /// \brief 16bit unsigned integer lowest value. This is equivalent to
-    /// GZ_UINT16_MIN, and is defined here for completeness.
-    static const uint16_t LOW_UI16 = std::numeric_limits<uint16_t>::lowest();
+  /// \brief 16bit unsigned integer lowest value. This is equivalent to
+  /// GZ_UINT16_MIN, and is defined here for completeness.
+  static const uint16_t LOW_UI16 = std::numeric_limits<uint16_t>::lowest();
 
-    /// \brief 16-bit unsigned integer positive infinite value
-    static const uint16_t INF_UI16 = std::numeric_limits<uint16_t>::infinity();
+  /// \brief 16-bit unsigned integer positive infinite value
+  static const uint16_t INF_UI16 = std::numeric_limits<uint16_t>::infinity();
 
-    /// \brief 16bit unsigned integer maximum value
-    static const int16_t MAX_I16 = std::numeric_limits<int16_t>::max();
+  /// \brief 16bit unsigned integer maximum value
+  static const int16_t MAX_I16 = std::numeric_limits<int16_t>::max();
 
-    /// \brief 16bit unsigned integer minimum value
-    static const int16_t MIN_I16 = std::numeric_limits<int16_t>::min();
+  /// \brief 16bit unsigned integer minimum value
+  static const int16_t MIN_I16 = std::numeric_limits<int16_t>::min();
 
-    /// \brief 16bit unsigned integer lowest value. This is equivalent to
-    /// GZ_INT16_MIN, and is defined here for completeness.
-    static const int16_t LOW_I16 = std::numeric_limits<int16_t>::lowest();
+  /// \brief 16bit unsigned integer lowest value. This is equivalent to
+  /// GZ_INT16_MIN, and is defined here for completeness.
+  static const int16_t LOW_I16 = std::numeric_limits<int16_t>::lowest();
 
-    /// \brief 16-bit unsigned integer positive infinite value
-    static const int16_t INF_I16 = std::numeric_limits<int16_t>::infinity();
+  /// \brief 16-bit unsigned integer positive infinite value
+  static const int16_t INF_I16 = std::numeric_limits<int16_t>::infinity();
 
-    /// \brief 32bit unsigned integer maximum value
-    static const uint32_t MAX_UI32 = std::numeric_limits<uint32_t>::max();
+  /// \brief 32bit unsigned integer maximum value
+  static const uint32_t MAX_UI32 = std::numeric_limits<uint32_t>::max();
 
-    /// \brief 32bit unsigned integer minimum value
-    static const uint32_t MIN_UI32 = std::numeric_limits<uint32_t>::min();
+  /// \brief 32bit unsigned integer minimum value
+  static const uint32_t MIN_UI32 = std::numeric_limits<uint32_t>::min();
 
-    /// \brief 32bit unsigned integer lowest value. This is equivalent to
-    /// GZ_UINT32_MIN, and is defined here for completeness.
-    static const uint32_t LOW_UI32 = std::numeric_limits<uint32_t>::lowest();
+  /// \brief 32bit unsigned integer lowest value. This is equivalent to
+  /// GZ_UINT32_MIN, and is defined here for completeness.
+  static const uint32_t LOW_UI32 = std::numeric_limits<uint32_t>::lowest();
 
-    /// \brief 32-bit unsigned integer positive infinite value
-    static const uint32_t INF_UI32 = std::numeric_limits<uint32_t>::infinity();
+  /// \brief 32-bit unsigned integer positive infinite value
+  static const uint32_t INF_UI32 = std::numeric_limits<uint32_t>::infinity();
 
-    /// \brief 32bit unsigned integer maximum value
-    static const int32_t MAX_I32 = std::numeric_limits<int32_t>::max();
+  /// \brief 32bit unsigned integer maximum value
+  static const int32_t MAX_I32 = std::numeric_limits<int32_t>::max();
 
-    /// \brief 32bit unsigned integer minimum value
-    static const int32_t MIN_I32 = std::numeric_limits<int32_t>::min();
+  /// \brief 32bit unsigned integer minimum value
+  static const int32_t MIN_I32 = std::numeric_limits<int32_t>::min();
 
-    /// \brief 32bit unsigned integer lowest value. This is equivalent to
-    /// GZ_INT32_MIN, and is defined here for completeness.
-    static const int32_t LOW_I32 = std::numeric_limits<int32_t>::lowest();
+  /// \brief 32bit unsigned integer lowest value. This is equivalent to
+  /// GZ_INT32_MIN, and is defined here for completeness.
+  static const int32_t LOW_I32 = std::numeric_limits<int32_t>::lowest();
 
-    /// \brief 32-bit unsigned integer positive infinite value
-    static const int32_t INF_I32 = std::numeric_limits<int32_t>::infinity();
+  /// \brief 32-bit unsigned integer positive infinite value
+  static const int32_t INF_I32 = std::numeric_limits<int32_t>::infinity();
 
-    /// \brief 64bit unsigned integer maximum value
-    static const uint64_t MAX_UI64 = std::numeric_limits<uint64_t>::max();
+  /// \brief 64bit unsigned integer maximum value
+  static const uint64_t MAX_UI64 = std::numeric_limits<uint64_t>::max();
 
-    /// \brief 64bit unsigned integer minimum value
-    static const uint64_t MIN_UI64 = std::numeric_limits<uint64_t>::min();
+  /// \brief 64bit unsigned integer minimum value
+  static const uint64_t MIN_UI64 = std::numeric_limits<uint64_t>::min();
 
-    /// \brief 64bit unsigned integer lowest value. This is equivalent to
-    /// GZ_UINT64_MIN, and is defined here for completeness.
-    static const uint64_t LOW_UI64 = std::numeric_limits<uint64_t>::lowest();
+  /// \brief 64bit unsigned integer lowest value. This is equivalent to
+  /// GZ_UINT64_MIN, and is defined here for completeness.
+  static const uint64_t LOW_UI64 = std::numeric_limits<uint64_t>::lowest();
 
-    /// \brief 64-bit unsigned integer positive infinite value
-    static const uint64_t INF_UI64 = std::numeric_limits<uint64_t>::infinity();
+  /// \brief 64-bit unsigned integer positive infinite value
+  static const uint64_t INF_UI64 = std::numeric_limits<uint64_t>::infinity();
 
-    /// \brief 64bit unsigned integer maximum value
-    static const int64_t MAX_I64 = std::numeric_limits<int64_t>::max();
+  /// \brief 64bit unsigned integer maximum value
+  static const int64_t MAX_I64 = std::numeric_limits<int64_t>::max();
 
-    /// \brief 64bit unsigned integer minimum value
-    static const int64_t MIN_I64 = std::numeric_limits<int64_t>::min();
+  /// \brief 64bit unsigned integer minimum value
+  static const int64_t MIN_I64 = std::numeric_limits<int64_t>::min();
 
-    /// \brief 64bit unsigned integer lowest value. This is equivalent to
-    /// GZ_INT64_MIN, and is defined here for completeness.
-    static const int64_t LOW_I64 = std::numeric_limits<int64_t>::lowest();
+  /// \brief 64bit unsigned integer lowest value. This is equivalent to
+  /// GZ_INT64_MIN, and is defined here for completeness.
+  static const int64_t LOW_I64 = std::numeric_limits<int64_t>::lowest();
 
-    /// \brief 64-bit unsigned integer positive infinite value
-    static const int64_t INF_I64 = std::numeric_limits<int64_t>::infinity();
+  /// \brief 64-bit unsigned integer positive infinite value
+  static const int64_t INF_I64 = std::numeric_limits<int64_t>::infinity();
 
-    /// \brief Returns the representation of a quiet not a number (NAN)
-    static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
+  /// \brief Returns the representation of a quiet not a number (NAN)
+  static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
 
-    /// \brief Simple clamping function that constrains a value to
-    /// a range defined by a min and max value. This function is equivalent to
-    /// std::max(std::min(value, max), min).
-    /// \param[in] _v Value to clamp
-    /// \param[in] _min Minimum allowed value.
-    /// \param[in] _max Maximum allowed value.
-    /// \return The value _v clamped to the range defined by _min and _max.
-    template<typename T>
-    inline T clamp(T _v, T _min, T _max)
-    {
-      return std::max(std::min(_v, _max), _min);
-    }
+  /// \brief Simple clamping function that constrains a value to
+  /// a range defined by a min and max value. This function is equivalent to
+  /// std::max(std::min(value, max), min).
+  /// \param[in] _v Value to clamp
+  /// \param[in] _min Minimum allowed value.
+  /// \param[in] _max Maximum allowed value.
+  /// \return The value _v clamped to the range defined by _min and _max.
+  template<typename T>
+  inline T clamp(T _v, T _min, T _max)
+  {
+    return std::max(std::min(_v, _max), _min);
+  }
 
-    /// \brief Check if a float is NaN
-    /// \param[in] _v The value to check.
-    /// \return True if _v is not a number, false otherwise.
-    inline bool isnan(float _v)
-    {
-      return (std::isnan)(_v);
-    }
+  /// \brief Check if a float is NaN
+  /// \param[in] _v The value to check.
+  /// \return True if _v is not a number, false otherwise.
+  inline bool isnan(float _v)
+  {
+    return (std::isnan)(_v);
+  }
 
-    /// \brief Check if a double is NaN.
-    /// \param[in] _v The value to check
-    /// \return True if _v is not a number, false otherwise.
-    inline bool isnan(double _v)
-    {
-      return (std::isnan)(_v);
-    }
+  /// \brief Check if a double is NaN.
+  /// \param[in] _v The value to check
+  /// \return True if _v is not a number, false otherwise.
+  inline bool isnan(double _v)
+  {
+    return (std::isnan)(_v);
+  }
 
-    /// \brief Fix a float NaN value.
-    /// \param[in] _v Value to correct.
-    /// \return 0 if _v is NaN or infinite, _v otherwise.
-    inline float fixnan(float _v)
-    {
-      return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
-    }
+  /// \brief Fix a float NaN value.
+  /// \param[in] _v Value to correct.
+  /// \return 0 if _v is NaN or infinite, _v otherwise.
+  inline float fixnan(float _v)
+  {
+    return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
+  }
 
-    /// \brief Fix a double NaN value.
-    /// \param[in] _v Value to correct.
-    /// \return 0 if _v is NaN or is infinite, _v otherwise.
-    inline double fixnan(double _v)
-    {
-      return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
-    }
+  /// \brief Fix a double NaN value.
+  /// \param[in] _v Value to correct.
+  /// \return 0 if _v is NaN or is infinite, _v otherwise.
+  inline double fixnan(double _v)
+  {
+    return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
+  }
 
-    /// \brief Check if an int is even.
-    /// \param[in] _v Value to check.
-    /// \return True if _v is even.
-    inline bool isEven(const int _v)
-    {
-      return !(_v % 2);
-    }
+  /// \brief Check if an int is even.
+  /// \param[in] _v Value to check.
+  /// \return True if _v is even.
+  inline bool isEven(const int _v)
+  {
+    return !(_v % 2);
+  }
 
-    /// \brief Check if an unsigned int is even.
-    /// \param[in] _v Value to check.
-    /// \return True if _v is even.
-    inline bool isEven(const unsigned int _v)
-    {
-      return !(_v % 2);
-    }
+  /// \brief Check if an unsigned int is even.
+  /// \param[in] _v Value to check.
+  /// \return True if _v is even.
+  inline bool isEven(const unsigned int _v)
+  {
+    return !(_v % 2);
+  }
 
-    /// \brief Check if an int is odd.
-    /// \param[in] _v Value to check.
-    /// \return True if _v is odd.
-    inline bool isOdd(const int _v)
-    {
-      return (_v % 2) != 0;
-    }
+  /// \brief Check if an int is odd.
+  /// \param[in] _v Value to check.
+  /// \return True if _v is odd.
+  inline bool isOdd(const int _v)
+  {
+    return (_v % 2) != 0;
+  }
 
-    /// \brief Check if an unsigned int is odd.
-    /// \param[in] _v Value to check.
-    /// \return True if _v is odd.
-    inline bool isOdd(const unsigned int _v)
-    {
-      return (_v % 2) != 0;
-    }
+  /// \brief Check if an unsigned int is odd.
+  /// \param[in] _v Value to check.
+  /// \return True if _v is odd.
+  inline bool isOdd(const unsigned int _v)
+  {
+    return (_v % 2) != 0;
+  }
 
-    /// \brief The signum function.
-    ///
-    /// Returns 0 for zero values, -1 for negative values,
-    /// +1 for positive values.
-    /// \param[in] _value The value.
-    /// \return The signum of the value.
-    template<typename T>
-    inline int sgn(T _value)
-    {
-      return (T(0) < _value) - (_value < T(0));
-    }
+  /// \brief The signum function.
+  ///
+  /// Returns 0 for zero values, -1 for negative values,
+  /// +1 for positive values.
+  /// \param[in] _value The value.
+  /// \return The signum of the value.
+  template<typename T>
+  inline int sgn(T _value)
+  {
+    return (T(0) < _value) - (_value < T(0));
+  }
 
-    /// \brief The signum function.
-    ///
-    /// Returns 0 for zero values, -1 for negative values,
-    /// +1 for positive values.
-    /// \param[in] _value The value.
-    /// \return The signum of the value.
-    template<typename T>
-    inline int signum(T _value)
-    {
-      return sgn(_value);
-    }
+  /// \brief The signum function.
+  ///
+  /// Returns 0 for zero values, -1 for negative values,
+  /// +1 for positive values.
+  /// \param[in] _value The value.
+  /// \return The signum of the value.
+  template<typename T>
+  inline int signum(T _value)
+  {
+    return sgn(_value);
+  }
 
-    /// \brief Get mean value in a vector of values
-    /// \param[in] _values The vector of values.
-    /// \return The mean value in the provided vector.
-    template<typename T>
-    inline T mean(const std::vector<T> &_values)
-    {
-      T sum = 0;
-      for (unsigned int i = 0; i < _values.size(); ++i)
-        sum += _values[i];
-      return sum / static_cast<T>(_values.size());
-    }
+  /// \brief Get mean value in a vector of values
+  /// \param[in] _values The vector of values.
+  /// \return The mean value in the provided vector.
+  template<typename T>
+  inline T mean(const std::vector<T> &_values)
+  {
+    T sum = 0;
+    for (unsigned int i = 0; i < _values.size(); ++i)
+      sum += _values[i];
+    return sum / static_cast<T>(_values.size());
+  }
 
-    /// \brief Get the variance of a vector of values.
-    /// \param[in] _values The vector of values.
-    /// \return The squared deviation of the vector of values.
-    template<typename T>
-    inline T variance(const std::vector<T> &_values)
-    {
-      T avg = mean<T>(_values);
+  /// \brief Get the variance of a vector of values.
+  /// \param[in] _values The vector of values.
+  /// \return The squared deviation of the vector of values.
+  template<typename T>
+  inline T variance(const std::vector<T> &_values)
+  {
+    T avg = mean<T>(_values);
 
-      T sum = 0;
-      for (unsigned int i = 0; i < _values.size(); ++i)
-        sum += (_values[i] - avg) * (_values[i] - avg);
-      return sum / static_cast<T>(_values.size());
-    }
+    T sum = 0;
+    for (unsigned int i = 0; i < _values.size(); ++i)
+      sum += (_values[i] - avg) * (_values[i] - avg);
+    return sum / static_cast<T>(_values.size());
+  }
 
-    /// \brief Get the maximum value of vector of values.
-    /// \param[in] _values The vector of values.
-    /// \return Maximum value in the vector.
-    template<typename T>
-    inline T max(const std::vector<T> &_values)
-    {
-      return *std::max_element(std::begin(_values), std::end(_values));
-    }
+  /// \brief Get the maximum value of vector of values.
+  /// \param[in] _values The vector of values.
+  /// \return Maximum value in the vector.
+  template<typename T>
+  inline T max(const std::vector<T> &_values)
+  {
+    return *std::max_element(std::begin(_values), std::end(_values));
+  }
 
-    /// \brief Get the minimum value of vector of values.
-    /// \param[in] _values The vector of values.
-    /// \return Minimum value in the vector.
-    template<typename T>
-    inline T min(const std::vector<T> &_values)
-    {
-      return *std::min_element(std::begin(_values), std::end(_values));
-    }
+  /// \brief Get the minimum value of vector of values.
+  /// \param[in] _values The vector of values.
+  /// \return Minimum value in the vector.
+  template<typename T>
+  inline T min(const std::vector<T> &_values)
+  {
+    return *std::min_element(std::begin(_values), std::end(_values));
+  }
 
-    /// \brief Check if two values are equal, within a tolerance.
-    /// \param[in] _a The first value.
-    /// \param[in] _b The second value.
-    /// \param[in] _epsilon The tolerance
-    /// \return True if the two values fall within the given tolerance.
-    template<typename T>
-    inline bool equal(const T &_a, const T &_b,
-                      const T &_epsilon = T(1e-6))
-    {
-      GZ_FP_VOLATILE T diff = std::abs(_a - _b);
-      return diff <= _epsilon;
-    }
+  /// \brief Check if two values are equal, within a tolerance.
+  /// \param[in] _a The first value.
+  /// \param[in] _b The second value.
+  /// \param[in] _epsilon The tolerance
+  /// \return True if the two values fall within the given tolerance.
+  template<typename T>
+  inline bool equal(const T &_a, const T &_b,
+                    const T &_epsilon = T(1e-6))
+  {
+    GZ_FP_VOLATILE T diff = std::abs(_a - _b);
+    return diff <= _epsilon;
+  }
 
-    /// \brief Less than or near test, within a tolerance.
-    /// \param[in] _a The first value.
-    /// \param[in] _b The second value.
-    /// \param[in] _epsilon The tolerance.
-    /// \return True if _a < _b + _tol.
-    template<typename T>
-    inline bool lessOrNearEqual(const T &_a, const T &_b,
-                            const T &_epsilon = 1e-6)
-    {
-      return _a < _b + _epsilon;
-    }
+  /// \brief Less than or near test, within a tolerance.
+  /// \param[in] _a The first value.
+  /// \param[in] _b The second value.
+  /// \param[in] _epsilon The tolerance.
+  /// \return True if _a < _b + _tol.
+  template<typename T>
+  inline bool lessOrNearEqual(const T &_a, const T &_b,
+                          const T &_epsilon = 1e-6)
+  {
+    return _a < _b + _epsilon;
+  }
 
-    /// \brief Greater than or near test, within a tolerance.
-    /// \param[in] _a The first value.
-    /// \param[in] _b The second value.
-    /// \param[in] _epsilon The tolerance.
-    /// \return True if _a > _b - _epsilon.
-    template<typename T>
-    inline bool greaterOrNearEqual(const T &_a, const T &_b,
-                               const T &_epsilon = 1e-6)
-    {
-      return _a > _b - _epsilon;
-    }
+  /// \brief Greater than or near test, within a tolerance.
+  /// \param[in] _a The first value.
+  /// \param[in] _b The second value.
+  /// \param[in] _epsilon The tolerance.
+  /// \return True if _a > _b - _epsilon.
+  template<typename T>
+  inline bool greaterOrNearEqual(const T &_a, const T &_b,
+                             const T &_epsilon = 1e-6)
+  {
+    return _a > _b - _epsilon;
+  }
 
-    /// \brief Get the value at a specified precision.
-    /// \param[in] _a The number.
-    /// \param[in] _precision The precision.
-    /// \return The value for the specified precision.
-    template<typename T>
-    inline T precision(const T &_a, const unsigned int &_precision)
-    {
-      auto p = std::pow(10, _precision);
-      return static_cast<T>(std::round(_a * p) / p);
-    }
+  /// \brief Get the value at a specified precision.
+  /// \param[in] _a The number.
+  /// \param[in] _precision The precision.
+  /// \return The value for the specified precision.
+  template<typename T>
+  inline T precision(const T &_a, const unsigned int &_precision)
+  {
+    auto p = std::pow(10, _precision);
+    return static_cast<T>(std::round(_a * p) / p);
+  }
 
-    /// \brief Sort two numbers, such that _a <= _b.
-    /// \param[in, out] _a The first number. This variable will contain the
-    /// lower of the two values after this function completes.
-    /// \param[in, out] _b The second number. This variable will contain the
-    /// higher of the two values after this function completes.
-    template<typename T>
-    inline void sort2(T &_a, T &_b)
-    {
-      if (_b < _a)
-        std::swap(_a, _b);
-    }
+  /// \brief Sort two numbers, such that _a <= _b.
+  /// \param[in, out] _a The first number. This variable will contain the
+  /// lower of the two values after this function completes.
+  /// \param[in, out] _b The second number. This variable will contain the
+  /// higher of the two values after this function completes.
+  template<typename T>
+  inline void sort2(T &_a, T &_b)
+  {
+    if (_b < _a)
+      std::swap(_a, _b);
+  }
 
-    /// \brief Sort three numbers, such that _a <= _b <= _c.
-    /// \param[in,out] _a The first number. This variable will contain the
-    /// lowest of the three values after this function completes.
-    /// \param[in,out] _b The second number. This variable will contain the
-    /// middle of the three values after this function completes.
-    /// \param[in,out] _c The third number. This variable will contain the
-    /// highest of the three values after this function completes.
-    template<typename T>
-    inline void sort3(T &_a, T &_b, T &_c)
-    {
-      // _a <= _b
-      sort2(_a, _b);
-      // _a <= _c, _b <= _c
-      sort2(_b, _c);
-      // _a <= _b <= _c
-      sort2(_a, _b);
-    }
+  /// \brief Sort three numbers, such that _a <= _b <= _c.
+  /// \param[in,out] _a The first number. This variable will contain the
+  /// lowest of the three values after this function completes.
+  /// \param[in,out] _b The second number. This variable will contain the
+  /// middle of the three values after this function completes.
+  /// \param[in,out] _c The third number. This variable will contain the
+  /// highest of the three values after this function completes.
+  template<typename T>
+  inline void sort3(T &_a, T &_b, T &_c)
+  {
+    // _a <= _b
+    sort2(_a, _b);
+    // _a <= _c, _b <= _c
+    sort2(_b, _c);
+    // _a <= _b <= _c
+    sort2(_a, _b);
+  }
 
-    /// \brief Append a number to a stream. Makes sure "-0" is returned as "0".
-    /// \param[out] _out Output stream.
-    /// \param[in] _number Number to append.
-    template<typename T>
-    inline void appendToStream(std::ostream &_out, T _number)
+  /// \brief Append a number to a stream. Makes sure "-0" is returned as "0".
+  /// \param[out] _out Output stream.
+  /// \param[in] _number Number to append.
+  template<typename T>
+  inline void appendToStream(std::ostream &_out, T _number)
+  {
+    if (std::fpclassify(_number) == FP_ZERO)
     {
-      if (std::fpclassify(_number) == FP_ZERO)
-      {
-        _out << 0;
-      }
-      else
-      {
-        _out << _number;
-      }
+      _out << 0;
     }
-
-    /// \brief Append a number to a stream, specialized for int.
-    /// \param[out] _out Output stream.
-    /// \param[in] _number Number to append.
-    template<>
-    inline void appendToStream(std::ostream &_out, int _number)
+    else
     {
       _out << _number;
     }
+  }
 
-    /// \brief Is the parameter a power of 2?
-    /// \param[in] _x The number to check.
-    /// \return True if _x is a power of 2, false otherwise.
-    inline bool isPowerOfTwo(unsigned int _x)
-    {
-      return ((_x != 0) && ((_x & (~_x + 1)) == _x));
-    }
+  /// \brief Append a number to a stream, specialized for int.
+  /// \param[out] _out Output stream.
+  /// \param[in] _number Number to append.
+  template<>
+  inline void appendToStream(std::ostream &_out, int _number)
+  {
+    _out << _number;
+  }
 
-    /// \brief Get the smallest power of two that is greater than or equal to
-    /// a given value.
-    /// \param[in] _x The value which marks the lower bound of the result.
-    /// \return The same value if _x is already a power of two. Otherwise,
-    /// it returns the smallest power of two that is greater than _x
-    inline unsigned int roundUpPowerOfTwo(unsigned int _x)
-    {
-      if (_x == 0)
-        return 1;
+  /// \brief Is the parameter a power of 2?
+  /// \param[in] _x The number to check.
+  /// \return True if _x is a power of 2, false otherwise.
+  inline bool isPowerOfTwo(unsigned int _x)
+  {
+    return ((_x != 0) && ((_x & (~_x + 1)) == _x));
+  }
 
-      if (isPowerOfTwo(_x))
-        return _x;
+  /// \brief Get the smallest power of two that is greater than or equal to
+  /// a given value.
+  /// \param[in] _x The value which marks the lower bound of the result.
+  /// \return The same value if _x is already a power of two. Otherwise,
+  /// it returns the smallest power of two that is greater than _x
+  inline unsigned int roundUpPowerOfTwo(unsigned int _x)
+  {
+    if (_x == 0)
+      return 1;
 
-      while (_x & (_x - 1))
-        _x = _x & (_x - 1);
+    if (isPowerOfTwo(_x))
+      return _x;
 
-      _x = _x << 1;
+    while (_x & (_x - 1))
+      _x = _x & (_x - 1);
 
-      return _x;
-    }
+    _x = _x << 1;
 
-    /// \brief Round a number up to the nearest multiple. For example, if
-    /// the input number is 12 and the multiple is 10, the result is 20.
-    /// If the input number is negative, then the nearest multiple will be
-    /// greater than or equal to the input number. For example, if the input
-    /// number is -9 and the multiple is 2 then the output is -8.
-    /// \param[in] _num Input number to round up.
-    /// \param[in] _multiple The multiple. If the multiple is <= zero, then
-    /// the input number is returned.
-    /// \return The nearest multiple of _multiple that is greater than
-    /// or equal to _num.
-    inline int roundUpMultiple(int _num, int _multiple)
-    {
-      if (_multiple == 0)
-        return _num;
+    return _x;
+  }
 
-      int remainder = std::abs(_num) % _multiple;
-      if (remainder == 0)
-        return _num;
+  /// \brief Round a number up to the nearest multiple. For example, if
+  /// the input number is 12 and the multiple is 10, the result is 20.
+  /// If the input number is negative, then the nearest multiple will be
+  /// greater than or equal to the input number. For example, if the input
+  /// number is -9 and the multiple is 2 then the output is -8.
+  /// \param[in] _num Input number to round up.
+  /// \param[in] _multiple The multiple. If the multiple is <= zero, then
+  /// the input number is returned.
+  /// \return The nearest multiple of _multiple that is greater than
+  /// or equal to _num.
+  inline int roundUpMultiple(int _num, int _multiple)
+  {
+    if (_multiple == 0)
+      return _num;
 
-      if (_num < 0)
-        return -(std::abs(_num) - remainder);
-      else
-        return _num + _multiple - remainder;
-    }
+    int remainder = std::abs(_num) % _multiple;
+    if (remainder == 0)
+      return _num;
 
-    /// \brief Parse string into an integer.
-    /// \param[in] _input The input string.
-    /// \return An integer, or NAN_I if unable to parse the input.
-    int GZ_MATH_VISIBLE parseInt(const std::string &_input);
-
-    /// \brief parse string into float.
-    /// \param [in] _input The string.
-    /// \return A floating point number (can be NaN) or NAN_D if the
-    /// _input could not be parsed.
-    double GZ_MATH_VISIBLE parseFloat(const std::string &_input);
-
-    /// \brief Convert a std::chrono::steady_clock::time_point to a seconds and
-    /// nanoseconds pair.
-    /// \param[in] _time The time point to convert.
-    /// \return A pair where the first element is the number of seconds and
-    /// the second is the number of nanoseconds.
-    std::pair<int64_t, int64_t> GZ_MATH_VISIBLE timePointToSecNsec(
-        const std::chrono::steady_clock::time_point &_time);
-
-    /// \brief Convert seconds and nanoseconds to
-    /// std::chrono::steady_clock::time_point.
-    /// \param[in] _sec The seconds to convert.
-    /// \param[in] _nanosec The nanoseconds to convert.
-    /// \return A std::chrono::steady_clock::time_point based on the number of
-    /// seconds and the number of nanoseconds.
-    std::chrono::steady_clock::time_point GZ_MATH_VISIBLE
-      secNsecToTimePoint(
-        const uint64_t &_sec, const uint64_t &_nanosec);
-
-    /// \brief Convert seconds and nanoseconds to
-    /// std::chrono::steady_clock::duration.
-    /// \param[in] _sec The seconds to convert.
-    /// \param[in] _nanosec The nanoseconds to convert.
-    /// \return A std::chrono::steady_clock::duration based on the number of
-    /// seconds and the number of nanoseconds.
-    std::chrono::steady_clock::duration GZ_MATH_VISIBLE secNsecToDuration(
-        const uint64_t &_sec, const uint64_t &_nanosec);
-
-    /// \brief Convert a std::chrono::steady_clock::duration to a seconds and
-    /// nanoseconds pair.
-    /// \param[in] _dur The duration to convert.
-    /// \return A pair where the first element is the number of seconds and
-    /// the second is the number of nanoseconds.
-    std::pair<int64_t, int64_t> GZ_MATH_VISIBLE durationToSecNsec(
-        const std::chrono::steady_clock::duration &_dur);
-
-    // TODO(anyone): Replace this with std::chrono::days.
-    /// This will exist in C++-20
-    typedef std::chrono::duration<uint64_t, std::ratio<86400>> days;
-
-    /// \brief break down durations
-    /// NOTE: the template arguments must be properly ordered according
-    /// to magnitude and there can be no duplicates.
-    /// This function uses the braces initializer to split all the templated
-    /// duration. The initializer will be called recursievely due the `...`
-    /// \param[in] d Duration to break down
-    /// \return A tuple based on the durations specified
-    template<class...Durations, class DurationIn>
-    std::tuple<Durations...> breakDownDurations(DurationIn d) {
-      std::tuple<Durations...> retval;
-      using discard = int[];
-      (void)discard{0, (void((
-        (std::get<Durations>(retval) =
-          std::chrono::duration_cast<Durations>(d)),
-        (d -= std::chrono::duration_cast<DurationIn>(
-          std::get<Durations>(retval))))), 0)...};
-      return retval;
-    }
+    if (_num < 0)
+      return -(std::abs(_num) - remainder);
+    else
+      return _num + _multiple - remainder;
+  }
 
-    /// \brief Convert a std::chrono::steady_clock::time_point to a string
-    /// \param[in] _point The std::chrono::steady_clock::time_point to convert.
-    /// \return A string formatted with the time_point
-    std::string GZ_MATH_VISIBLE timePointToString(
-        const std::chrono::steady_clock::time_point &_point);
-
-    /// \brief Convert a std::chrono::steady_clock::duration to a string
-    /// \param[in] _duration The std::chrono::steady_clock::duration to convert.
-    /// \return A string formatted with the duration
-    std::string GZ_MATH_VISIBLE durationToString(
-        const std::chrono::steady_clock::duration &_duration);
-
-    /// \brief Split a std::chrono::steady_clock::duration to a string
-    /// \param[in] _timeString The string to convert in general format
-    /// \param[out] numberDays number of days in the string
-    /// \param[out] numberHours number of hours in the string
-    /// \param[out] numberMinutes number of minutes in the string
-    /// \param[out] numberSeconds number of seconds in the string
-    /// \param[out] numberMilliseconds number of milliseconds in the string
-    /// \return True if the regex was able to split the string otherwise False
-    bool GZ_MATH_VISIBLE splitTimeBasedOnTimeRegex(
-        const std::string &_timeString,
-        uint64_t & numberDays, uint64_t & numberHours,
-        uint64_t & numberMinutes, uint64_t & numberSeconds,
-        uint64_t & numberMilliseconds);
-
-    /// \brief Check if the given string represents a time.
-    /// An example time string is "0 00:00:00.000", which has the format
-    /// "DAYS HOURS:MINUTES:SECONDS.MILLISECONDS"
-    /// \return True if the regex was able to split the string otherwise False
-    inline bool isTimeString(const std::string &_timeString)
-    {
-      // These will be thrown away, just for making the function call
-      uint64_t d, h, m, s, ms;
-      return splitTimeBasedOnTimeRegex(_timeString, d, h, m, s, ms);
-    }
+  /// \brief Parse string into an integer.
+  /// \param[in] _input The input string.
+  /// \return An integer, or NAN_I if unable to parse the input.
+  int GZ_MATH_VISIBLE parseInt(const std::string &_input);
+
+  /// \brief parse string into float.
+  /// \param [in] _input The string.
+  /// \return A floating point number (can be NaN) or NAN_D if the
+  /// _input could not be parsed.
+  double GZ_MATH_VISIBLE parseFloat(const std::string &_input);
+
+  /// \brief Convert a std::chrono::steady_clock::time_point to a seconds and
+  /// nanoseconds pair.
+  /// \param[in] _time The time point to convert.
+  /// \return A pair where the first element is the number of seconds and
+  /// the second is the number of nanoseconds.
+  std::pair<int64_t, int64_t> GZ_MATH_VISIBLE timePointToSecNsec(
+      const std::chrono::steady_clock::time_point &_time);
+
+  /// \brief Convert seconds and nanoseconds to
+  /// std::chrono::steady_clock::time_point.
+  /// \param[in] _sec The seconds to convert.
+  /// \param[in] _nanosec The nanoseconds to convert.
+  /// \return A std::chrono::steady_clock::time_point based on the number of
+  /// seconds and the number of nanoseconds.
+  std::chrono::steady_clock::time_point GZ_MATH_VISIBLE
+    secNsecToTimePoint(
+      const uint64_t &_sec, const uint64_t &_nanosec);
+
+  /// \brief Convert seconds and nanoseconds to
+  /// std::chrono::steady_clock::duration.
+  /// \param[in] _sec The seconds to convert.
+  /// \param[in] _nanosec The nanoseconds to convert.
+  /// \return A std::chrono::steady_clock::duration based on the number of
+  /// seconds and the number of nanoseconds.
+  std::chrono::steady_clock::duration GZ_MATH_VISIBLE secNsecToDuration(
+      const uint64_t &_sec, const uint64_t &_nanosec);
+
+  /// \brief Convert a std::chrono::steady_clock::duration to a seconds and
+  /// nanoseconds pair.
+  /// \param[in] _dur The duration to convert.
+  /// \return A pair where the first element is the number of seconds and
+  /// the second is the number of nanoseconds.
+  std::pair<int64_t, int64_t> GZ_MATH_VISIBLE durationToSecNsec(
+      const std::chrono::steady_clock::duration &_dur);
+
+  // TODO(anyone): Replace this with std::chrono::days.
+  /// This will exist in C++-20
+  typedef std::chrono::duration<uint64_t, std::ratio<86400>> days;
+
+  /// \brief break down durations
+  /// NOTE: the template arguments must be properly ordered according
+  /// to magnitude and there can be no duplicates.
+  /// This function uses the braces initializer to split all the templated
+  /// duration. The initializer will be called recursievely due the `...`
+  /// \param[in] d Duration to break down
+  /// \return A tuple based on the durations specified
+  template<class...Durations, class DurationIn>
+  std::tuple<Durations...> breakDownDurations(DurationIn d) {
+    std::tuple<Durations...> retval;
+    using discard = int[];
+    (void)discard{0, (void((
+      (std::get<Durations>(retval) =
+        std::chrono::duration_cast<Durations>(d)),
+      (d -= std::chrono::duration_cast<DurationIn>(
+        std::get<Durations>(retval))))), 0)...};
+    return retval;
+  }
 
-    /// \brief Convert a string to a std::chrono::steady_clock::duration
-    /// \param[in] _timeString The string to convert in general format
-    /// "dd hh:mm:ss.nnn" where n is millisecond value
-    /// \return A std::chrono::steady_clock::duration containing the
-    /// string's time value. If it isn't possible to convert, the duration will
-    /// be zero.
-    std::chrono::steady_clock::duration GZ_MATH_VISIBLE stringToDuration(
-        const std::string &_timeString);
-
-    /// \brief Convert a string to a std::chrono::steady_clock::time_point
-    /// \param[in] _timeString The string to convert in general format
-    /// "dd hh:mm:ss.nnn" where n is millisecond value
-    /// \return A std::chrono::steady_clock::time_point containing the
-    /// string's time value. If it isn't possible to convert, the time will
-    /// be negative 1 second.
-    std::chrono::steady_clock::time_point
-    GZ_MATH_VISIBLE stringToTimePoint(const std::string &_timeString);
-
-    // Degrade precision on Windows, which cannot handle 'long double'
-    // values properly. See the implementation of Unpair.
-    // 32 bit ARM processors also define 'long double' to be the same
-    // size as 'double', and must also be degraded
+  /// \brief Convert a std::chrono::steady_clock::time_point to a string
+  /// \param[in] _point The std::chrono::steady_clock::time_point to convert.
+  /// \return A string formatted with the time_point
+  std::string GZ_MATH_VISIBLE timePointToString(
+      const std::chrono::steady_clock::time_point &_point);
+
+  /// \brief Convert a std::chrono::steady_clock::duration to a string
+  /// \param[in] _duration The std::chrono::steady_clock::duration to convert.
+  /// \return A string formatted with the duration
+  std::string GZ_MATH_VISIBLE durationToString(
+      const std::chrono::steady_clock::duration &_duration);
+
+  /// \brief Split a std::chrono::steady_clock::duration to a string
+  /// \param[in] _timeString The string to convert in general format
+  /// \param[out] numberDays number of days in the string
+  /// \param[out] numberHours number of hours in the string
+  /// \param[out] numberMinutes number of minutes in the string
+  /// \param[out] numberSeconds number of seconds in the string
+  /// \param[out] numberMilliseconds number of milliseconds in the string
+  /// \return True if the regex was able to split the string otherwise False
+  bool GZ_MATH_VISIBLE splitTimeBasedOnTimeRegex(
+      const std::string &_timeString,
+      uint64_t & numberDays, uint64_t & numberHours,
+      uint64_t & numberMinutes, uint64_t & numberSeconds,
+      uint64_t & numberMilliseconds);
+
+  /// \brief Check if the given string represents a time.
+  /// An example time string is "0 00:00:00.000", which has the format
+  /// "DAYS HOURS:MINUTES:SECONDS.MILLISECONDS"
+  /// \return True if the regex was able to split the string otherwise False
+  inline bool isTimeString(const std::string &_timeString)
+  {
+    // These will be thrown away, just for making the function call
+    uint64_t d, h, m, s, ms;
+    return splitTimeBasedOnTimeRegex(_timeString, d, h, m, s, ms);
+  }
+
+  /// \brief Convert a string to a std::chrono::steady_clock::duration
+  /// \param[in] _timeString The string to convert in general format
+  /// "dd hh:mm:ss.nnn" where n is millisecond value
+  /// \return A std::chrono::steady_clock::duration containing the
+  /// string's time value. If it isn't possible to convert, the duration will
+  /// be zero.
+  std::chrono::steady_clock::duration GZ_MATH_VISIBLE stringToDuration(
+      const std::string &_timeString);
+
+  /// \brief Convert a string to a std::chrono::steady_clock::time_point
+  /// \param[in] _timeString The string to convert in general format
+  /// "dd hh:mm:ss.nnn" where n is millisecond value
+  /// \return A std::chrono::steady_clock::time_point containing the
+  /// string's time value. If it isn't possible to convert, the time will
+  /// be negative 1 second.
+  std::chrono::steady_clock::time_point
+  GZ_MATH_VISIBLE stringToTimePoint(const std::string &_timeString);
+
+  // Degrade precision on Windows, which cannot handle 'long double'
+  // values properly. See the implementation of Unpair.
+  // 32 bit ARM processors also define 'long double' to be the same
+  // size as 'double', and must also be degraded
 #if defined _MSC_VER || defined __arm__
-    using PairInput = uint16_t;
-    using PairOutput = uint32_t;
+  using PairInput = uint16_t;
+  using PairOutput = uint32_t;
 #else
-    using PairInput = uint32_t;
-    using PairOutput = uint64_t;
+  using PairInput = uint32_t;
+  using PairOutput = uint64_t;
 #endif
 
-    /// \brief A pairing function that maps two values to a unique third
-    /// value. This is an implementation of Szudzik's function.
-    /// \param[in] _a First value, must be a non-negative integer. On
-    /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t.
-    /// \param[in] _b Second value, must be a non-negative integer. On
-    /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t.
-    /// \return A unique non-negative integer value. On Windows the return
-    /// value is uint32_t. On Linux/OSX the return value is uint64_t
-    /// \sa Unpair
-    PairOutput GZ_MATH_VISIBLE Pair(
-        const PairInput _a, const PairInput _b);
-
-    /// \brief The reverse of the Pair function. Accepts a key, produced
-    /// from the Pair function, and returns a tuple consisting of the two
-    /// non-negative integer values used to create the _key.
-    /// \param[in] _key A non-negative integer generated from the Pair
-    /// function. On Windows this value is uint32_t. On Linux/OSX, this
-    /// value is uint64_t.
-    /// \return A tuple that consists of the two non-negative integers that
-    /// will generate _key when used with the Pair function. On Windows the
-    /// tuple contains two uint16_t values. On Linux/OSX the tuple contains
-    /// two uint32_t values.
-    /// \sa Pair
-    std::tuple<PairInput, PairInput> GZ_MATH_VISIBLE Unpair(
-        const PairOutput _key);
-    }
-  }
-}
-
-#endif
+  /// \brief A pairing function that maps two values to a unique third
+  /// value. This is an implementation of Szudzik's function.
+  /// \param[in] _a First value, must be a non-negative integer. On
+  /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t.
+  /// \param[in] _b Second value, must be a non-negative integer. On
+  /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t.
+  /// \return A unique non-negative integer value. On Windows the return
+  /// value is uint32_t. On Linux/OSX the return value is uint64_t
+  /// \sa Unpair
+  PairOutput GZ_MATH_VISIBLE Pair(
+      const PairInput _a, const PairInput _b);
+
+  /// \brief The reverse of the Pair function. Accepts a key, produced
+  /// from the Pair function, and returns a tuple consisting of the two
+  /// non-negative integer values used to create the _key.
+  /// \param[in] _key A non-negative integer generated from the Pair
+  /// function. On Windows this value is uint32_t. On Linux/OSX, this
+  /// value is uint64_t.
+  /// \return A tuple that consists of the two non-negative integers that
+  /// will generate _key when used with the Pair function. On Windows the
+  /// tuple contains two uint16_t values. On Linux/OSX the tuple contains
+  /// two uint32_t values.
+  /// \sa Pair
+  std::tuple<PairInput, PairInput> GZ_MATH_VISIBLE Unpair(
+      const PairOutput _key);
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_FUNCTIONS_HH_
diff --git a/include/gz/math/Inertial.hh b/include/gz/math/Inertial.hh
index 5aad35f3c..025ca4b31 100644
--- a/include/gz/math/Inertial.hh
+++ b/include/gz/math/Inertial.hh
@@ -25,414 +25,411 @@
 #include "gz/math/Matrix6.hh"
 #include "gz/math/Pose3.hh"
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Inertial Inertial.hh gz/math/Inertial.hh
+  /// \brief The Inertial object provides a representation for the mass and
+  /// inertia matrix of a body B. The components of the inertia matrix are
+  /// expressed in what we call the "inertial" frame Bi of the body, i.e.
+  /// the frame in which these inertia components are measured. The inertial
+  /// frame Bi must be located at the center of mass of the body, but not
+  /// necessarily aligned with the body’s frame. In addition, this class
+  /// allows users to specify a frame F for these inertial properties by
+  /// specifying the pose X_FBi of the inertial frame Bi in the
+  /// inertial object frame F.
+  ///
+  /// Throughout this documentation, the terms "body", "object" and "link"
+  /// are used interchangeably.
+  ///
+  /// For information about the X_FBi notation, see
+  /// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
+  template<typename T>
+  class Inertial
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Inertial Inertial.hh gz/math/Inertial.hh
-    /// \brief The Inertial object provides a representation for the mass and
-    /// inertia matrix of a body B. The components of the inertia matrix are
-    /// expressed in what we call the "inertial" frame Bi of the body, i.e.
-    /// the frame in which these inertia components are measured. The inertial
-    /// frame Bi must be located at the center of mass of the body, but not
-    /// necessarily aligned with the body’s frame. In addition, this class
-    /// allows users to specify a frame F for these inertial properties by
-    /// specifying the pose X_FBi of the inertial frame Bi in the
-    /// inertial object frame F.
+    /// \brief Default Constructor
+    public: Inertial()
+    {}
+
+    /// \brief Constructs an inertial object from the mass matrix for a body
+    /// B, about its center of mass Bcm, and expressed in a frame that we’ll
+    /// call the "inertial" frame Bi, i.e. the frame in which the components
+    /// of the mass matrix are specified (see this class’s documentation for
+    /// details). The pose object specifies the pose X_FBi of the inertial
+    /// frame Bi in the frame F of this inertial object
+    /// (see class’s documentation).
+    /// \param[in] _massMatrix Mass and inertia matrix.
+    /// \param[in] _pose Pose of center of mass reference frame.
+    public: Inertial(const MassMatrix3<T> &_massMatrix,
+                     const Pose3<T> &_pose)
+    : massMatrix(_massMatrix), pose(_pose)
+    {}
+
+    /// \brief Constructs an inertial object from the mass matrix for a body
+    /// B, about its center of mass Bcm, and expressed in a frame that we'll
+    /// call the "inertial" frame Bi, i.e. the frame in which the components
+    /// of the mass matrix are specified (see this class's documentation for
+    /// details). The pose object specifies the pose X_FBi of the inertial
+    /// frame Bi in the frame F of this inertial object
+    /// (see class's documentation). The added mass matrix is expressed
+    /// in the link origin frame F.
+    /// \param[in] _massMatrix Mass and inertia matrix.
+    /// \param[in] _pose Pose of center of mass reference frame.
+    /// \param[in] _addedMass Coefficients for fluid added mass.
+    public: Inertial(const MassMatrix3<T> &_massMatrix,
+                     const Pose3<T> &_pose,
+                     const Matrix6<T> &_addedMass)
+    : massMatrix(_massMatrix), pose(_pose), addedMass(_addedMass)
+    {}
+
+    /// \brief Copy constructor.
+    /// \param[in] _inertial Inertial element to copy
+    public: Inertial(const Inertial<T> &_inertial) = default;
+
+    /// \brief Destructor.
+    public: ~Inertial() = default;
+
+    /// \brief Set the mass and inertia matrix.
     ///
-    /// Throughout this documentation, the terms "body", "object" and "link"
-    /// are used interchangeably.
+    /// \param[in] _m New MassMatrix3 object.
+    /// \param[in] _tolerance Tolerance is passed to
+    /// MassMatrix3::IsValid and is the amount of error
+    /// to accept when checking whether the MassMatrix3 _m is valid.
+    /// Refer to MassMatrix3::Epsilon for detailed description of
+    /// _tolerance.
     ///
-    /// For information about the X_FBi notation, see
-    /// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
-    template<typename T>
-    class Inertial
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetMassMatrix(const MassMatrix3<T> &_m,
+                const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
     {
-      /// \brief Default Constructor
-      public: Inertial()
-      {}
-
-      /// \brief Constructs an inertial object from the mass matrix for a body
-      /// B, about its center of mass Bcm, and expressed in a frame that we’ll
-      /// call the "inertial" frame Bi, i.e. the frame in which the components
-      /// of the mass matrix are specified (see this class’s documentation for
-      /// details). The pose object specifies the pose X_FBi of the inertial
-      /// frame Bi in the frame F of this inertial object
-      /// (see class’s documentation).
-      /// \param[in] _massMatrix Mass and inertia matrix.
-      /// \param[in] _pose Pose of center of mass reference frame.
-      public: Inertial(const MassMatrix3<T> &_massMatrix,
-                       const Pose3<T> &_pose)
-      : massMatrix(_massMatrix), pose(_pose)
-      {}
-
-      /// \brief Constructs an inertial object from the mass matrix for a body
-      /// B, about its center of mass Bcm, and expressed in a frame that we'll
-      /// call the "inertial" frame Bi, i.e. the frame in which the components
-      /// of the mass matrix are specified (see this class's documentation for
-      /// details). The pose object specifies the pose X_FBi of the inertial
-      /// frame Bi in the frame F of this inertial object
-      /// (see class's documentation). The added mass matrix is expressed
-      /// in the link origin frame F.
-      /// \param[in] _massMatrix Mass and inertia matrix.
-      /// \param[in] _pose Pose of center of mass reference frame.
-      /// \param[in] _addedMass Coefficients for fluid added mass.
-      public: Inertial(const MassMatrix3<T> &_massMatrix,
-                       const Pose3<T> &_pose,
-                       const Matrix6<T> &_addedMass)
-      : massMatrix(_massMatrix), pose(_pose), addedMass(_addedMass)
-      {}
-
-      /// \brief Copy constructor.
-      /// \param[in] _inertial Inertial element to copy
-      public: Inertial(const Inertial<T> &_inertial) = default;
-
-      /// \brief Destructor.
-      public: ~Inertial() = default;
-
-      /// \brief Set the mass and inertia matrix.
-      ///
-      /// \param[in] _m New MassMatrix3 object.
-      /// \param[in] _tolerance Tolerance is passed to
-      /// MassMatrix3::IsValid and is the amount of error
-      /// to accept when checking whether the MassMatrix3 _m is valid.
-      /// Refer to MassMatrix3::Epsilon for detailed description of
-      /// _tolerance.
-      ///
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetMassMatrix(const MassMatrix3<T> &_m,
-                  const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
-      {
-        this->massMatrix = _m;
-        return this->massMatrix.IsValid(_tolerance);
-      }
+      this->massMatrix = _m;
+      return this->massMatrix.IsValid(_tolerance);
+    }
 
-      /// \brief Get the mass and inertia matrix.
-      /// \return The mass matrix about the body’s center of mass and
-      /// expressed in the inertial frame Bi as defined by this class’s
-      /// documentation
-      public: const MassMatrix3<T> &MassMatrix() const
-      {
-        return this->massMatrix;
-      }
+    /// \brief Get the mass and inertia matrix.
+    /// \return The mass matrix about the body’s center of mass and
+    /// expressed in the inertial frame Bi as defined by this class’s
+    /// documentation
+    public: const MassMatrix3<T> &MassMatrix() const
+    {
+      return this->massMatrix;
+    }
 
-      /// \brief Set the pose of the center of mass reference frame.
-      /// \param[in] _pose New pose.
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetPose(const Pose3<T> &_pose)
-      {
-        this->pose = _pose;
-        return this->massMatrix.IsValid();
-      }
+    /// \brief Set the pose of the center of mass reference frame.
+    /// \param[in] _pose New pose.
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetPose(const Pose3<T> &_pose)
+    {
+      this->pose = _pose;
+      return this->massMatrix.IsValid();
+    }
 
-      /// \brief Get the pose of the center of mass reference frame.
-      /// \return The pose of the inertial frame Bi in the frame F of this
-      /// Inertial object as defined by this class’s documentation.
-      public: const Pose3<T> &Pose() const
-      {
-        return this->pose;
-      }
+    /// \brief Get the pose of the center of mass reference frame.
+    /// \return The pose of the inertial frame Bi in the frame F of this
+    /// Inertial object as defined by this class’s documentation.
+    public: const Pose3<T> &Pose() const
+    {
+      return this->pose;
+    }
 
-      /// \brief Set the matrix representing the inertia of the fluid that is
-      /// dislocated when the body moves. Added mass should be zero if the
-      /// density of the surrounding fluid is negligible with respect to the
-      /// body's density.
-      ///
-      /// \param[in] _m New Matrix6 object, which must be a symmetric matrix.
-      /// \return True if the Matrix6 is symmetric.
-      /// \sa SpatialMatrix
-      public: bool SetFluidAddedMass(const Matrix6<T> &_m)
-      {
-        this->addedMass = _m;
-        return this->addedMass.value() == this->addedMass.value().Transposed();
-      }
+    /// \brief Set the matrix representing the inertia of the fluid that is
+    /// dislocated when the body moves. Added mass should be zero if the
+    /// density of the surrounding fluid is negligible with respect to the
+    /// body's density.
+    ///
+    /// \param[in] _m New Matrix6 object, which must be a symmetric matrix.
+    /// \return True if the Matrix6 is symmetric.
+    /// \sa SpatialMatrix
+    public: bool SetFluidAddedMass(const Matrix6<T> &_m)
+    {
+      this->addedMass = _m;
+      return this->addedMass.value() == this->addedMass.value().Transposed();
+    }
 
-      /// \brief Get the fluid added mass matrix.
-      /// \return The added mass matrix. It will be nullopt if the added mass
-      /// was never set.
-      public: const std::optional< Matrix6<T> > &FluidAddedMass() const
-      {
-        return this->addedMass;
-      }
+    /// \brief Get the fluid added mass matrix.
+    /// \return The added mass matrix. It will be nullopt if the added mass
+    /// was never set.
+    public: const std::optional< Matrix6<T> > &FluidAddedMass() const
+    {
+      return this->addedMass;
+    }
 
-      /// \brief Get the moment of inertia matrix computer about the body's
-      /// center of mass and expressed in this Inertial object’s frame F.
-      /// \return The inertia matrix computed about the body’s center of
-      /// mass and expressed in this Inertial object’s frame F, as defined
-      /// in this class’s documentation.
-      public: Matrix3<T> Moi() const
-      {
-        auto R = Matrix3<T>(this->pose.Rot());
-        return R * this->massMatrix.Moi() * R.Transposed();
-      }
+    /// \brief Get the moment of inertia matrix computer about the body's
+    /// center of mass and expressed in this Inertial object’s frame F.
+    /// \return The inertia matrix computed about the body’s center of
+    /// mass and expressed in this Inertial object’s frame F, as defined
+    /// in this class’s documentation.
+    public: Matrix3<T> Moi() const
+    {
+      auto R = Matrix3<T>(this->pose.Rot());
+      return R * this->massMatrix.Moi() * R.Transposed();
+    }
 
-      /// \brief Spatial mass matrix for body B. It does not include fluid
-      /// added mass. The matrix is expressed in the object's frame F, not to
-      /// be confused with the center of mass frame Bi.
-      ///
-      /// The matrix is arranged as follows:
-      ///
-      /// | m          0          0          0           m * Pz    -m * Py |
-      /// | 0          m          0          -m * Pz    0           m * Px |
-      /// | 0          0          m           m * Py    -m * Px    0       |
-      /// | 0          -m * Pz     m * Py    Ixx        Ixy        Ixz     |
-      /// |  m * Pz    0          -m * Px    Ixy        Iyy        Iyz     |
-      /// | -m * Py     m* Px     0          Ixz        Iyz        Izz     |
-      ///
-      /// \return The body's 6x6 inertial matrix.
-      /// \sa SpatialMatrix
-      public: Matrix6<T> BodyMatrix() const
-      {
-        Matrix6<T> result;
+    /// \brief Spatial mass matrix for body B. It does not include fluid
+    /// added mass. The matrix is expressed in the object's frame F, not to
+    /// be confused with the center of mass frame Bi.
+    ///
+    /// The matrix is arranged as follows:
+    ///
+    /// | m          0          0          0           m * Pz    -m * Py |
+    /// | 0          m          0          -m * Pz    0           m * Px |
+    /// | 0          0          m           m * Py    -m * Px    0       |
+    /// | 0          -m * Pz     m * Py    Ixx        Ixy        Ixz     |
+    /// |  m * Pz    0          -m * Px    Ixy        Iyy        Iyz     |
+    /// | -m * Py     m* Px     0          Ixz        Iyz        Izz     |
+    ///
+    /// \return The body's 6x6 inertial matrix.
+    /// \sa SpatialMatrix
+    public: Matrix6<T> BodyMatrix() const
+    {
+      Matrix6<T> result;
 
-        result.SetSubmatrix(Matrix6<T>::TOP_LEFT,
-            Matrix3<T>::Identity * this->massMatrix.Mass());
+      result.SetSubmatrix(Matrix6<T>::TOP_LEFT,
+          Matrix3<T>::Identity * this->massMatrix.Mass());
 
-        result.SetSubmatrix(Matrix6<T>::BOTTOM_RIGHT, this->Moi());
+      result.SetSubmatrix(Matrix6<T>::BOTTOM_RIGHT, this->Moi());
 
-        auto x = this->pose.Pos().X();
-        auto y = this->pose.Pos().Y();
-        auto z = this->pose.Pos().Z();
-        auto skew = Matrix3<T>(
-            0, -z, y,
-            z, 0, -x,
-            -y, x, 0) * this->massMatrix.Mass();
-        result.SetSubmatrix(Matrix6<T>::BOTTOM_LEFT, skew);
-        result.SetSubmatrix(Matrix6<T>::TOP_RIGHT, skew.Transposed());
+      auto x = this->pose.Pos().X();
+      auto y = this->pose.Pos().Y();
+      auto z = this->pose.Pos().Z();
+      auto skew = Matrix3<T>(
+          0, -z, y,
+          z, 0, -x,
+          -y, x, 0) * this->massMatrix.Mass();
+      result.SetSubmatrix(Matrix6<T>::BOTTOM_LEFT, skew);
+      result.SetSubmatrix(Matrix6<T>::TOP_RIGHT, skew.Transposed());
 
-        return result;
-      }
+      return result;
+    }
 
-      /// \brief Spatial mass matrix, which includes the body's inertia, as well
-      /// as the inertia of the fluid that is dislocated when the body moves.
-      /// The matrix is expressed in the object's frame F, not to be confused
-      /// with the center of mass frame Bi.
-      /// \return The spatial mass matrix.
-      /// \sa BodyMatrix
-      /// \sa FluidAddedMass
-      public: Matrix6<T> SpatialMatrix() const
-      {
-        return this->addedMass.has_value() ?
-            this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
-      }
+    /// \brief Spatial mass matrix, which includes the body's inertia, as well
+    /// as the inertia of the fluid that is dislocated when the body moves.
+    /// The matrix is expressed in the object's frame F, not to be confused
+    /// with the center of mass frame Bi.
+    /// \return The spatial mass matrix.
+    /// \sa BodyMatrix
+    /// \sa FluidAddedMass
+    public: Matrix6<T> SpatialMatrix() const
+    {
+      return this->addedMass.has_value() ?
+          this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
+    }
 
-      /// \brief Set the inertial pose rotation without affecting the
-      /// MOI in the base coordinate frame.
-      /// \param[in] _q New rotation for inertial pose.
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetInertialRotation(const Quaternion<T> &_q)
-      {
-        auto moi = this->Moi();
-        this->pose.Rot() = _q;
-        auto R = Matrix3<T>(_q);
-        return this->massMatrix.SetMoi(R.Transposed() * moi * R);
-      }
+    /// \brief Set the inertial pose rotation without affecting the
+    /// MOI in the base coordinate frame.
+    /// \param[in] _q New rotation for inertial pose.
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetInertialRotation(const Quaternion<T> &_q)
+    {
+      auto moi = this->Moi();
+      this->pose.Rot() = _q;
+      auto R = Matrix3<T>(_q);
+      return this->massMatrix.SetMoi(R.Transposed() * moi * R);
+    }
+
+    /// \brief Set the MassMatrix rotation (eigenvectors of inertia matrix)
+    /// without affecting the MOI in the base coordinate frame.
+    /// Note that symmetries in inertia matrix may prevent the output of
+    /// MassMatrix3::PrincipalAxesOffset to match this function's input _q,
+    /// but it is guaranteed that the MOI in the base frame will not change.
+    /// A negative value of _tol (such as -1e-6) can be passed to ensure
+    /// that diagonal values are always sorted.
+    /// \param[in] _q New rotation.
+    /// \param[in] _tol Relative tolerance given by absolute value
+    /// of _tol. This is passed to the MassMatrix3
+    /// PrincipalMoments and PrincipalAxesOffset functions.
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetMassMatrixRotation(const Quaternion<T> &_q,
+                                       const T _tol = 1e-6)
+    {
+      this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
+                          _q.Inverse();
+      const auto moments = this->MassMatrix().PrincipalMoments(_tol);
+      const auto diag = Matrix3<T>(
+          moments[0], 0, 0,
+          0, moments[1], 0,
+          0, 0, moments[2]);
+      const auto R = Matrix3<T>(_q);
+      return this->massMatrix.SetMoi(R * diag * R.Transposed());
+    }
+
+    /// \brief Equal operator.
+    /// \param[in] _inertial Inertial to copy.
+    /// \return Reference to this object.
+    public: Inertial &operator=(const Inertial<T> &_inertial) = default;
+
+    /// \brief Equality comparison operator.
+    /// \param[in] _inertial Inertial to copy.
+    /// \return true if each component is equal within a default tolerance,
+    /// false otherwise
+    public: bool operator==(const Inertial<T> &_inertial) const
+    {
+      return (this->pose == _inertial.Pose()) &&
+             (this->massMatrix == _inertial.MassMatrix()) &&
+             (this->addedMass == _inertial.FluidAddedMass());
+    }
+
+    /// \brief Inequality test operator
+    /// \param[in] _inertial Inertial<T> to test
+    /// \return True if not equal (using the default tolerance of 1e-6)
+    public: bool operator!=(const Inertial<T> &_inertial) const
+    {
+      return !(*this == _inertial);
+    }
+
+    /// \brief Adds inertial properties to current object.
+    /// The mass, center of mass location, and inertia matrix are updated
+    /// as long as the total mass is positive.
+    /// \param[in] _inertial Inertial to add.
+    /// \return Reference to this object.
+    public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
+    {
+      T m1 = this->massMatrix.Mass();
+      T m2 = _inertial.MassMatrix().Mass();
+
+      // Total mass
+      T mass = m1 + m2;
 
-      /// \brief Set the MassMatrix rotation (eigenvectors of inertia matrix)
-      /// without affecting the MOI in the base coordinate frame.
-      /// Note that symmetries in inertia matrix may prevent the output of
-      /// MassMatrix3::PrincipalAxesOffset to match this function's input _q,
-      /// but it is guaranteed that the MOI in the base frame will not change.
-      /// A negative value of _tol (such as -1e-6) can be passed to ensure
-      /// that diagonal values are always sorted.
-      /// \param[in] _q New rotation.
-      /// \param[in] _tol Relative tolerance given by absolute value
-      /// of _tol. This is passed to the MassMatrix3
-      /// PrincipalMoments and PrincipalAxesOffset functions.
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetMassMatrixRotation(const Quaternion<T> &_q,
-                                         const T _tol = 1e-6)
+      // Only continue if total mass is positive
+      if (mass <= 0)
       {
-        this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
-                            _q.Inverse();
-        const auto moments = this->MassMatrix().PrincipalMoments(_tol);
-        const auto diag = Matrix3<T>(
-            moments[0], 0, 0,
-            0, moments[1], 0,
-            0, 0, moments[2]);
-        const auto R = Matrix3<T>(_q);
-        return this->massMatrix.SetMoi(R * diag * R.Transposed());
+        return *this;
       }
 
-      /// \brief Equal operator.
-      /// \param[in] _inertial Inertial to copy.
-      /// \return Reference to this object.
-      public: Inertial &operator=(const Inertial<T> &_inertial) = default;
+      auto com1 = this->Pose().Pos();
+      auto com2 = _inertial.Pose().Pos();
+      // New center of mass location in base frame
+      auto com = (m1*com1 + m2*com2) / mass;
 
-      /// \brief Equality comparison operator.
-      /// \param[in] _inertial Inertial to copy.
-      /// \return true if each component is equal within a default tolerance,
-      /// false otherwise
-      public: bool operator==(const Inertial<T> &_inertial) const
+      // Components of new moment of inertia matrix
+      Vector3<T> ixxyyzz;
+      Vector3<T> ixyxzyz;
+      // First add matrices in base frame
       {
-        return (this->pose == _inertial.Pose()) &&
-               (this->massMatrix == _inertial.MassMatrix()) &&
-               (this->addedMass == _inertial.FluidAddedMass());
+        auto moi = this->Moi() + _inertial.Moi();
+        ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
+        ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
       }
-
-      /// \brief Inequality test operator
-      /// \param[in] _inertial Inertial<T> to test
-      /// \return True if not equal (using the default tolerance of 1e-6)
-      public: bool operator!=(const Inertial<T> &_inertial) const
+      // Then account for parallel axis theorem
       {
-        return !(*this == _inertial);
+        auto dc = com1 - com;
+        ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
+        ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
+        ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
+        ixyxzyz.X() -= m1 * dc[0] * dc[1];
+        ixyxzyz.Y() -= m1 * dc[0] * dc[2];
+        ixyxzyz.Z() -= m1 * dc[1] * dc[2];
       }
-
-      /// \brief Adds inertial properties to current object.
-      /// The mass, center of mass location, and inertia matrix are updated
-      /// as long as the total mass is positive.
-      /// \param[in] _inertial Inertial to add.
-      /// \return Reference to this object.
-      public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
       {
-        T m1 = this->massMatrix.Mass();
-        T m2 = _inertial.MassMatrix().Mass();
-
-        // Total mass
-        T mass = m1 + m2;
-
-        // Only continue if total mass is positive
-        if (mass <= 0)
-        {
-          return *this;
-        }
-
-        auto com1 = this->Pose().Pos();
-        auto com2 = _inertial.Pose().Pos();
-        // New center of mass location in base frame
-        auto com = (m1*com1 + m2*com2) / mass;
-
-        // Components of new moment of inertia matrix
-        Vector3<T> ixxyyzz;
-        Vector3<T> ixyxzyz;
-        // First add matrices in base frame
-        {
-          auto moi = this->Moi() + _inertial.Moi();
-          ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
-          ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
-        }
-        // Then account for parallel axis theorem
-        {
-          auto dc = com1 - com;
-          ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
-          ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
-          ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
-          ixyxzyz.X() -= m1 * dc[0] * dc[1];
-          ixyxzyz.Y() -= m1 * dc[0] * dc[2];
-          ixyxzyz.Z() -= m1 * dc[1] * dc[2];
-        }
-        {
-          auto dc = com2 - com;
-          ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
-          ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
-          ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
-          ixyxzyz.X() -= m2 * dc[0] * dc[1];
-          ixyxzyz.Y() -= m2 * dc[0] * dc[2];
-          ixyxzyz.Z() -= m2 * dc[1] * dc[2];
-        }
-        this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
-        this->pose = Pose3<T>(com, Quaternion<T>::Identity);
-
-        return *this;
+        auto dc = com2 - com;
+        ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
+        ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
+        ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
+        ixyxzyz.X() -= m2 * dc[0] * dc[1];
+        ixyxzyz.Y() -= m2 * dc[0] * dc[2];
+        ixyxzyz.Z() -= m2 * dc[1] * dc[2];
       }
+      this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
+      this->pose = Pose3<T>(com, Quaternion<T>::Identity);
 
-      /// \brief Subtracts inertial properties from current object.
-      /// The mass, center of mass location, and inertia matrix are updated
-      /// as long as the remaining mass is positive.
-      /// \param[in] _inertial Inertial to subtract.
-      /// \return Reference to this object.
-     public: Inertial<T> &operator-=(const Inertial<T> &_inertial)
-      {
-        T m = this->massMatrix.Mass();
-        T m2 = _inertial.MassMatrix().Mass();
-
-        // Remaining mass
-        T m1 = m - m2;
-
-        // Only continue if remaining mass is positive
-        if (m1 <= 0)
-        {
-          return *this;
-        }
-
-        auto com = this->Pose().Pos();
-        auto com2 = _inertial.Pose().Pos();
-        // Remaining center of mass location in base frame
-        auto com1 = (m*com - m2*com2)/m1;
-
-        // Components of new moment of inertia matrix
-        Vector3<T> ixxyyzz;
-        Vector3<T> ixyxzyz;
-
-        // First subtract matrices in base frame
-        {
-          auto moi = this->Moi() - _inertial.Moi();
-          ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
-          ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
-        }
-        // Then account for parallel axis theorem
-        {
-          auto dc = com1 - com;
-          ixxyyzz.X() -= m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
-          ixxyyzz.Y() -= m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
-          ixxyyzz.Z() -= m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
-          ixyxzyz.X() += m1 * dc[0] * dc[1];
-          ixyxzyz.Y() += m1 * dc[0] * dc[2];
-          ixyxzyz.Z() += m1 * dc[1] * dc[2];
-        }
-        {
-          auto dc = com2 - com;
-          ixxyyzz.X() -= m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
-          ixxyyzz.Y() -= m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
-          ixxyyzz.Z() -= m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
-          ixyxzyz.X() += m2 * dc[0] * dc[1];
-          ixyxzyz.Y() += m2 * dc[0] * dc[2];
-          ixyxzyz.Z() += m2 * dc[1] * dc[2];
-        }
-        this->massMatrix = MassMatrix3<T>(m1, ixxyyzz, ixyxzyz);
-        this->pose = Pose3<T>(com1, Quaternion<T>::Identity);
+      return *this;
+    }
+
+    /// \brief Subtracts inertial properties from current object.
+    /// The mass, center of mass location, and inertia matrix are updated
+    /// as long as the remaining mass is positive.
+    /// \param[in] _inertial Inertial to subtract.
+    /// \return Reference to this object.
+   public: Inertial<T> &operator-=(const Inertial<T> &_inertial)
+    {
+      T m = this->massMatrix.Mass();
+      T m2 = _inertial.MassMatrix().Mass();
+
+      // Remaining mass
+      T m1 = m - m2;
 
+      // Only continue if remaining mass is positive
+      if (m1 <= 0)
+      {
         return *this;
       }
 
-      /// \brief Adds inertial properties to current object.
-      /// The mass, center of mass location, and inertia matrix are updated
-      /// as long as the total mass is positive.
-      /// \param[in] _inertial Inertial to add.
-      /// \return Sum of inertials as new object.
-      public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
+      auto com = this->Pose().Pos();
+      auto com2 = _inertial.Pose().Pos();
+      // Remaining center of mass location in base frame
+      auto com1 = (m*com - m2*com2)/m1;
+
+      // Components of new moment of inertia matrix
+      Vector3<T> ixxyyzz;
+      Vector3<T> ixyxzyz;
+
+      // First subtract matrices in base frame
       {
-        return Inertial<T>(*this) += _inertial;
+        auto moi = this->Moi() - _inertial.Moi();
+        ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
+        ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
       }
-
-      /// \brief Subtracts inertial properties from current object.
-      /// The mass, center of mass location, and inertia matrix are updated
-      /// as long as the remaining mass is positive.
-      /// \param[in] _inertial Inertial to subtract.
-      /// \return Reference to this object.
-      public: const Inertial<T> operator-(const Inertial<T> &_inertial) const
+      // Then account for parallel axis theorem
       {
-        return Inertial<T>(*this) -= _inertial;
+        auto dc = com1 - com;
+        ixxyyzz.X() -= m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
+        ixxyyzz.Y() -= m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
+        ixxyyzz.Z() -= m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
+        ixyxzyz.X() += m1 * dc[0] * dc[1];
+        ixyxzyz.Y() += m1 * dc[0] * dc[2];
+        ixyxzyz.Z() += m1 * dc[1] * dc[2];
       }
+      {
+        auto dc = com2 - com;
+        ixxyyzz.X() -= m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
+        ixxyyzz.Y() -= m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
+        ixxyyzz.Z() -= m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
+        ixyxzyz.X() += m2 * dc[0] * dc[1];
+        ixyxzyz.Y() += m2 * dc[0] * dc[2];
+        ixyxzyz.Z() += m2 * dc[1] * dc[2];
+      }
+      this->massMatrix = MassMatrix3<T>(m1, ixxyyzz, ixyxzyz);
+      this->pose = Pose3<T>(com1, Quaternion<T>::Identity);
 
-      /// \brief Mass and inertia matrix of the object expressed in the
-      /// center of mass reference frame.
-      private: MassMatrix3<T> massMatrix;
-
-      /// \brief Pose offset of center of mass reference frame relative
-      /// to a base frame.
-      private: Pose3<T> pose;
+      return *this;
+    }
 
-      /// \brief Fluid added mass.
-      private: std::optional<Matrix6<T>> addedMass;
-    };
+    /// \brief Adds inertial properties to current object.
+    /// The mass, center of mass location, and inertia matrix are updated
+    /// as long as the total mass is positive.
+    /// \param[in] _inertial Inertial to add.
+    /// \return Sum of inertials as new object.
+    public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
+    {
+      return Inertial<T>(*this) += _inertial;
+    }
 
-    typedef Inertial<double> Inertiald;
-    typedef Inertial<float> Inertialf;
+    /// \brief Subtracts inertial properties from current object.
+    /// The mass, center of mass location, and inertia matrix are updated
+    /// as long as the remaining mass is positive.
+    /// \param[in] _inertial Inertial to subtract.
+    /// \return Reference to this object.
+    public: const Inertial<T> operator-(const Inertial<T> &_inertial) const
+    {
+      return Inertial<T>(*this) -= _inertial;
     }
-  }
-}
-#endif
+
+    /// \brief Mass and inertia matrix of the object expressed in the
+    /// center of mass reference frame.
+    private: MassMatrix3<T> massMatrix;
+
+    /// \brief Pose offset of center of mass reference frame relative
+    /// to a base frame.
+    private: Pose3<T> pose;
+
+    /// \brief Fluid added mass.
+    private: std::optional<Matrix6<T>> addedMass;
+  };
+
+  typedef Inertial<double> Inertiald;
+  typedef Inertial<float> Inertialf;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_INERTIAL_HH_
diff --git a/include/gz/math/Interval.hh b/include/gz/math/Interval.hh
index a3f8ae9fc..c0bf4da0a 100644
--- a/include/gz/math/Interval.hh
+++ b/include/gz/math/Interval.hh
@@ -25,275 +25,271 @@
 
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Interval Interval.hh gz/math/Interval.hh
+  /// \brief The Interval class represents a range of real numbers.
+  /// Intervals may be open (a, b), left-closed [a, b), right-closed
+  /// (a, b], or fully closed [a, b].
+  ///
+  /// ## Example
+  ///
+  /// \snippet examples/interval_example.cc complete
+  template <typename T>
+  class Interval
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Interval Interval.hh gz/math/Interval.hh
-    /// \brief The Interval class represents a range of real numbers.
-    /// Intervals may be open (a, b), left-closed [a, b), right-closed
-    /// (a, b], or fully closed [a, b].
-    ///
-    /// ## Example
-    ///
-    /// \snippet examples/interval_example.cc complete
-    template <typename T>
-    class Interval
+    /// \brief An unbounded interval (-∞, ∞)
+    public: static const Interval<T> &Unbounded;
+
+    /// \brief Constructor
+    public: Interval() = default;
+
+    /// \brief Constructor
+    /// \param[in] _leftValue leftmost interval value
+    /// \param[in] _leftClosed whether the interval is
+    ///   left-closed or not
+    /// \param[in] _rightValue rightmost interval value
+    /// \param[in] _rightClosed whether the interval
+    ///   is right-closed or not
+    public: constexpr Interval(
+        T _leftValue, bool _leftClosed,
+        T _rightValue, bool _rightClosed)
+    : leftValue(std::move(_leftValue)),
+      rightValue(std::move(_rightValue)),
+      leftClosed(_leftClosed),
+      rightClosed(_rightClosed)
+    {
+    }
+
+    /// \brief Make an open interval (`_leftValue`, `_rightValue`)
+    /// \param[in] _leftValue leftmost interval value
+    /// \param[in] _rightValue rightmost interval value
+    /// \return the open interval
+    public: static constexpr Interval<T>
+    Open(T _leftValue, T _rightValue)
+    {
+      return Interval<T>(
+        std::move(_leftValue), false,
+        std::move(_rightValue), false);
+    }
+
+    /// \brief Make a left-closed interval [`_leftValue`, `_rightValue`)
+    /// \param[in] _leftValue leftmost interval value
+    /// \param[in] _rightValue rightmost interval value
+    /// \return the left-closed interval
+    public: static constexpr Interval<T>
+    LeftClosed(T _leftValue, T _rightValue)
+    {
+      return Interval<T>(
+        std::move(_leftValue), true,
+        std::move(_rightValue), false);
+    }
+
+    /// \brief Make a right-closed interval (`_leftValue`, `_rightValue`]
+    /// \param[in] _leftValue leftmost interval value
+    /// \param[in] _rightValue rightmost interval value
+    /// \return the left-closed interval
+    public: static constexpr Interval<T>
+    RightClosed(T _leftValue, T _rightValue)
     {
-      /// \brief An unbounded interval (-∞, ∞)
-      public: static const Interval<T> &Unbounded;
+      return Interval<T>(
+        std::move(_leftValue), false,
+        std::move(_rightValue), true);
+    }
+
+    /// \brief Make a closed interval [`_leftValue`, `_rightValue`]
+    /// \param[in] _leftValue leftmost interval value
+    /// \param[in] _rightValue rightmost interval value
+    /// \return the closed interval
+    public: static constexpr Interval<T>
+    Closed(T _leftValue, T _rightValue)
+    {
+      return Interval<T>{
+        std::move(_leftValue), true,
+        std::move(_rightValue), true};
+    }
 
-      /// \brief Constructor
-      public: Interval() = default;
+    /// \brief Get the leftmost interval value
+    /// \return the leftmost interval value
+    public: const T &LeftValue() const { return this->leftValue; }
 
-      /// \brief Constructor
-      /// \param[in] _leftValue leftmost interval value
-      /// \param[in] _leftClosed whether the interval is
-      ///   left-closed or not
-      /// \param[in] _rightValue rightmost interval value
-      /// \param[in] _rightClosed whether the interval
-      ///   is right-closed or not
-      public: constexpr Interval(
-          T _leftValue, bool _leftClosed,
-          T _rightValue, bool _rightClosed)
-      : leftValue(std::move(_leftValue)),
-        rightValue(std::move(_rightValue)),
-        leftClosed(_leftClosed),
-        rightClosed(_rightClosed)
+    /// \brief Check if the interval is left-closed
+    /// \return true if the interval is left-closed, false otherwise
+    public: bool IsLeftClosed() const { return this->leftClosed; }
+
+    /// \brief Get the rightmost interval value
+    /// \return the rightmost interval value
+    public: const T &RightValue() const { return this->rightValue; }
+
+    /// \brief Check if the interval is right-closed
+    /// \return true if the interval is right-closed, false otherwise
+    public: bool IsRightClosed() const { return this->rightClosed; }
+
+    /// \brief Check if the interval is empty
+    /// Some examples of empty intervals include
+    /// (a, a), [a, a), and [a + 1, a].
+    /// \return true if it is empty, false otherwise
+    public: bool Empty() const
+    {
+      if (this->leftClosed && this->rightClosed)
       {
+        return this->rightValue < this->leftValue;
       }
+      return this->rightValue <= this->leftValue;
+    }
 
-      /// \brief Make an open interval (`_leftValue`, `_rightValue`)
-      /// \param[in] _leftValue leftmost interval value
-      /// \param[in] _rightValue rightmost interval value
-      /// \return the open interval
-      public: static constexpr Interval<T>
-      Open(T _leftValue, T _rightValue)
+    /// \brief Check if the interval contains `_value`
+    /// \param[in] _value value to check for membership
+    /// \return true if it is contained, false otherwise
+    public: bool Contains(const T &_value) const
+    {
+      if (this->leftClosed && this->rightClosed)
       {
-        return Interval<T>(
-          std::move(_leftValue), false,
-          std::move(_rightValue), false);
+        return this->leftValue <= _value && _value <= this->rightValue;
       }
-
-      /// \brief Make a left-closed interval [`_leftValue`, `_rightValue`)
-      /// \param[in] _leftValue leftmost interval value
-      /// \param[in] _rightValue rightmost interval value
-      /// \return the left-closed interval
-      public: static constexpr Interval<T>
-      LeftClosed(T _leftValue, T _rightValue)
+      if (this->leftClosed)
       {
-        return Interval<T>(
-          std::move(_leftValue), true,
-          std::move(_rightValue), false);
+        return this->leftValue <= _value && _value < this->rightValue;
       }
-
-      /// \brief Make a right-closed interval (`_leftValue`, `_rightValue`]
-      /// \param[in] _leftValue leftmost interval value
-      /// \param[in] _rightValue rightmost interval value
-      /// \return the left-closed interval
-      public: static constexpr Interval<T>
-      RightClosed(T _leftValue, T _rightValue)
+      if (this->rightClosed)
       {
-        return Interval<T>(
-          std::move(_leftValue), false,
-          std::move(_rightValue), true);
+        return this->leftValue < _value && _value <= this->rightValue;
       }
+      return this->leftValue < _value && _value < this->rightValue;
+    }
 
-      /// \brief Make a closed interval [`_leftValue`, `_rightValue`]
-      /// \param[in] _leftValue leftmost interval value
-      /// \param[in] _rightValue rightmost interval value
-      /// \return the closed interval
-      public: static constexpr Interval<T>
-      Closed(T _leftValue, T _rightValue)
+    /// \brief Check if the interval contains `_other` interval
+    /// \param[in] _other interval to check for membership
+    /// \return true if it is contained, false otherwise
+    public: bool Contains(const Interval<T> &_other) const
+    {
+      if (this->Empty() || _other.Empty())
       {
-        return Interval<T>{
-          std::move(_leftValue), true,
-          std::move(_rightValue), true};
+        return false;
       }
-
-      /// \brief Get the leftmost interval value
-      /// \return the leftmost interval value
-      public: const T &LeftValue() const { return this->leftValue; }
-
-      /// \brief Check if the interval is left-closed
-      /// \return true if the interval is left-closed, false otherwise
-      public: bool IsLeftClosed() const { return this->leftClosed; }
-
-      /// \brief Get the rightmost interval value
-      /// \return the rightmost interval value
-      public: const T &RightValue() const { return this->rightValue; }
-
-      /// \brief Check if the interval is right-closed
-      /// \return true if the interval is right-closed, false otherwise
-      public: bool IsRightClosed() const { return this->rightClosed; }
-
-      /// \brief Check if the interval is empty
-      /// Some examples of empty intervals include
-      /// (a, a), [a, a), and [a + 1, a].
-      /// \return true if it is empty, false otherwise
-      public: bool Empty() const
+      if (!this->leftClosed && _other.leftClosed)
       {
-        if (this->leftClosed && this->rightClosed)
+        if (_other.leftValue <= this->leftValue)
         {
-          return this->rightValue < this->leftValue;
+          return false;
         }
-        return this->rightValue <= this->leftValue;
       }
-
-      /// \brief Check if the interval contains `_value`
-      /// \param[in] _value value to check for membership
-      /// \return true if it is contained, false otherwise
-      public: bool Contains(const T &_value) const
+      else
       {
-        if (this->leftClosed && this->rightClosed)
+        if (_other.leftValue < this->leftValue)
         {
-          return this->leftValue <= _value && _value <= this->rightValue;
-        }
-        if (this->leftClosed)
-        {
-          return this->leftValue <= _value && _value < this->rightValue;
-        }
-        if (this->rightClosed)
-        {
-          return this->leftValue < _value && _value <= this->rightValue;
+          return false;
         }
-        return this->leftValue < _value && _value < this->rightValue;
       }
-
-      /// \brief Check if the interval contains `_other` interval
-      /// \param[in] _other interval to check for membership
-      /// \return true if it is contained, false otherwise
-      public: bool Contains(const Interval<T> &_other) const
+      if (!this->rightClosed && _other.rightClosed)
       {
-        if (this->Empty() || _other.Empty())
+        if (this->rightValue <= _other.rightValue)
         {
           return false;
         }
-        if (!this->leftClosed && _other.leftClosed)
-        {
-          if (_other.leftValue <= this->leftValue)
-          {
-            return false;
-          }
-        }
-        else
-        {
-          if (_other.leftValue < this->leftValue)
-          {
-            return false;
-          }
-        }
-        if (!this->rightClosed && _other.rightClosed)
-        {
-          if (this->rightValue <= _other.rightValue)
-          {
-            return false;
-          }
-        }
-        else
+      }
+      else
+      {
+        if (this->rightValue < _other.rightValue)
         {
-          if (this->rightValue < _other.rightValue)
-          {
-            return false;
-          }
+          return false;
         }
-        return true;
       }
+      return true;
+    }
 
-      /// \brief Check if the interval intersects `_other` interval
-      /// \param[in] _other interval to check for intersection
-      /// \return true if both intervals intersect, false otherwise
-      public: bool Intersects(const Interval<T> &_other) const
+    /// \brief Check if the interval intersects `_other` interval
+    /// \param[in] _other interval to check for intersection
+    /// \return true if both intervals intersect, false otherwise
+    public: bool Intersects(const Interval<T> &_other) const
+    {
+      if (this->Empty() || _other.Empty())
       {
-        if (this->Empty() || _other.Empty())
+        return false;
+      }
+      if (this->rightClosed && _other.leftClosed)
+      {
+        if (this->rightValue < _other.leftValue)
         {
           return false;
         }
-        if (this->rightClosed && _other.leftClosed)
-        {
-          if (this->rightValue < _other.leftValue)
-          {
-            return false;
-          }
-        }
-        else
-        {
-          if (this->rightValue <= _other.leftValue)
-          {
-            return false;
-          }
-        }
-        if (_other.rightClosed && this->leftClosed)
+      }
+      else
+      {
+        if (this->rightValue <= _other.leftValue)
         {
-          if (_other.rightValue < this->leftValue)
-          {
-            return false;
-          }
+          return false;
         }
-        else
+      }
+      if (_other.rightClosed && this->leftClosed)
+      {
+        if (_other.rightValue < this->leftValue)
         {
-          if (_other.rightValue <= this->leftValue)
-          {
-            return false;
-          }
+          return false;
         }
-        return true;
       }
-
-      /// \brief Equality test operator
-      /// \param _other interval to check for equality
-      /// \return true if intervals are equal, false otherwise
-      public: bool operator==(const Interval<T> &_other) const
+      else
       {
-        return this->Contains(_other) && _other.Contains(*this);
+        if (_other.rightValue <= this->leftValue)
+        {
+          return false;
+        }
       }
+      return true;
+    }
 
-      /// \brief Inequality test operator
-      /// \param _other interval to check for inequality
-      /// \return true if intervals are unequal, false otherwise
-      public: bool operator!=(const Interval<T> &_other) const
-      {
-        return !this->Contains(_other) || !_other.Contains(*this);
-      }
+    /// \brief Equality test operator
+    /// \param _other interval to check for equality
+    /// \return true if intervals are equal, false otherwise
+    public: bool operator==(const Interval<T> &_other) const
+    {
+      return this->Contains(_other) && _other.Contains(*this);
+    }
 
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _interval Interval to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-        std::ostream &_out, const gz::math::Interval<T> &_interval)
-      {
-        return _out << (_interval.leftClosed ? "[" : "(")
-                    << _interval.leftValue << ", " << _interval.rightValue
-                    << (_interval.rightClosed ? "]" : ")");
-      }
+    /// \brief Inequality test operator
+    /// \param _other interval to check for inequality
+    /// \return true if intervals are unequal, false otherwise
+    public: bool operator!=(const Interval<T> &_other) const
+    {
+      return !this->Contains(_other) || !_other.Contains(*this);
+    }
 
-      /// \brief The leftmost interval value
-      private: T leftValue{0};
-      /// \brief The righmost interval value
-      private: T rightValue{0};
-      /// \brief Whether the interval is left-closed or not
-      private: bool leftClosed{false};
-      /// \brief Whether the interval is right-closed or not
-      private: bool rightClosed{false};
-    };
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _interval Interval to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+      std::ostream &_out, const gz::math::Interval<T> &_interval)
+    {
+      return _out << (_interval.leftClosed ? "[" : "(")
+                  << _interval.leftValue << ", " << _interval.rightValue
+                  << (_interval.rightClosed ? "]" : ")");
+    }
 
-    namespace detail {
-       template<typename T>
-       constexpr Interval<T> gUnboundedInterval =
-           Interval<T>::Open(-std::numeric_limits<T>::infinity(),
-                             std::numeric_limits<T>::infinity());
-    }  // namespace detail
-    template<typename T>
-    const Interval<T> &Interval<T>::Unbounded = detail::gUnboundedInterval<T>;
+    /// \brief The leftmost interval value
+    private: T leftValue{0};
+    /// \brief The righmost interval value
+    private: T rightValue{0};
+    /// \brief Whether the interval is left-closed or not
+    private: bool leftClosed{false};
+    /// \brief Whether the interval is right-closed or not
+    private: bool rightClosed{false};
+  };
 
-    using Intervalf = Interval<float>;
-    using Intervald = Interval<double>;
-    }
-  }
-}
+  namespace detail {
+     template<typename T>
+     constexpr Interval<T> gUnboundedInterval =
+         Interval<T>::Open(-std::numeric_limits<T>::infinity(),
+                           std::numeric_limits<T>::infinity());
+  }  // namespace detail
+  template<typename T>
+  const Interval<T> &Interval<T>::Unbounded = detail::gUnboundedInterval<T>;
 
-#endif
+  using Intervalf = Interval<float>;
+  using Intervald = Interval<double>;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_INTERVAL_HH_
diff --git a/include/gz/math/Kmeans.hh b/include/gz/math/Kmeans.hh
index 13c198d56..45856b34c 100644
--- a/include/gz/math/Kmeans.hh
+++ b/include/gz/math/Kmeans.hh
@@ -24,62 +24,58 @@
 
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-
-    /// \class Kmeans Kmeans.hh math/gzmath.hh
-    /// \brief K-Means clustering algorithm. Given a set of observations,
-    /// k-means partitions the observations into k sets so as to minimize the
-    /// within-cluster sum of squares.
-    /// Description based on http://en.wikipedia.org/wiki/K-means_clustering.
-    class GZ_MATH_VISIBLE Kmeans
-    {
-      /// \brief constructor
-      /// \param[in] _obs Set of observations to cluster.
-      public: explicit Kmeans(const std::vector<Vector3d> &_obs);
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
 
-      /// \brief Get the observations to cluster.
-      /// \return The vector of observations.
-      public: std::vector<Vector3d> Observations() const;
+  /// \class Kmeans Kmeans.hh math/gzmath.hh
+  /// \brief K-Means clustering algorithm. Given a set of observations,
+  /// k-means partitions the observations into k sets so as to minimize the
+  /// within-cluster sum of squares.
+  /// Description based on http://en.wikipedia.org/wiki/K-means_clustering.
+  class GZ_MATH_VISIBLE Kmeans
+  {
+    /// \brief constructor
+    /// \param[in] _obs Set of observations to cluster.
+    public: explicit Kmeans(const std::vector<Vector3d> &_obs);
 
-      /// \brief Set the observations to cluster.
-      /// \param[in] _obs The new vector of observations.
-      /// \return True if the vector is not empty or false otherwise.
-      public: bool Observations(const std::vector<Vector3d> &_obs);
+    /// \brief Get the observations to cluster.
+    /// \return The vector of observations.
+    public: std::vector<Vector3d> Observations() const;
 
-      /// \brief Add observations to the cluster.
-      /// \param[in] _obs Vector of observations.
-      /// \return True if the _obs vector is not empty or false otherwise.
-      public: bool AppendObservations(const std::vector<Vector3d> &_obs);
+    /// \brief Set the observations to cluster.
+    /// \param[in] _obs The new vector of observations.
+    /// \return True if the vector is not empty or false otherwise.
+    public: bool Observations(const std::vector<Vector3d> &_obs);
 
-      /// \brief Executes the k-means algorithm.
-      /// \param[in] _k Number of partitions to cluster.
-      /// \param[out] _centroids Vector of centroids. Each element contains the
-      /// centroid of one cluster.
-      /// \param[out] _labels Vector of labels. The size of this vector is
-      /// equals to the number of observations. Each element represents the
-      /// cluster to which observation belongs.
-      /// \return True when the operation succeed or false otherwise. The
-      /// operation will fail if the number of observations is not positive,
-      /// if the number of clusters is non positive, or if the number of
-      /// clusters if greater than the number of observations.
-      public: bool Cluster(int _k,
-                           std::vector<Vector3d> &_centroids,
-                           std::vector<unsigned int> &_labels);
+    /// \brief Add observations to the cluster.
+    /// \param[in] _obs Vector of observations.
+    /// \return True if the _obs vector is not empty or false otherwise.
+    public: bool AppendObservations(const std::vector<Vector3d> &_obs);
 
-      /// \brief Given an observation, it returns the closest centroid to it.
-      /// \param[in] _p Point to check.
-      /// \return The index of the closest centroid to the point _p.
-      private: unsigned int ClosestCentroid(const Vector3d &_p) const;
+    /// \brief Executes the k-means algorithm.
+    /// \param[in] _k Number of partitions to cluster.
+    /// \param[out] _centroids Vector of centroids. Each element contains the
+    /// centroid of one cluster.
+    /// \param[out] _labels Vector of labels. The size of this vector is
+    /// equals to the number of observations. Each element represents the
+    /// cluster to which observation belongs.
+    /// \return True when the operation succeed or false otherwise. The
+    /// operation will fail if the number of observations is not positive,
+    /// if the number of clusters is non positive, or if the number of
+    /// clusters if greater than the number of observations.
+    public: bool Cluster(int _k,
+                         std::vector<Vector3d> &_centroids,
+                         std::vector<unsigned int> &_labels);
 
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
+    /// \brief Given an observation, it returns the closest centroid to it.
+    /// \param[in] _p Point to check.
+    /// \return The index of the closest centroid to the point _p.
+    private: unsigned int ClosestCentroid(const Vector3d &_p) const;
 
-#endif
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_KMEANS_HH_
diff --git a/include/gz/math/Line2.hh b/include/gz/math/Line2.hh
index 2d46ff7df..a09c13e7d 100644
--- a/include/gz/math/Line2.hh
+++ b/include/gz/math/Line2.hh
@@ -21,300 +21,297 @@
 #include <gz/math/Vector2.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Line2 Line2.hh gz/math/Line2.hh
+  /// \brief A two dimensional line segment. The line is defined by a
+  /// start and end point.
+  template<typename T>
+  class Line2
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Line2 Line2.hh gz/math/Line2.hh
-    /// \brief A two dimensional line segment. The line is defined by a
-    /// start and end point.
-    template<typename T>
-    class Line2
+    /// \brief Constructor.
+    /// \param[in] _ptA Start point of the line segment
+    /// \param[in] _ptB End point of the line segment
+    public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
     {
-      /// \brief Constructor.
-      /// \param[in] _ptA Start point of the line segment
-      /// \param[in] _ptB End point of the line segment
-      public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
-      {
-        this->Set(_ptA, _ptB);
-      }
-
-      /// \brief Constructor.
-      /// \param[in] _x1 X coordinate of the start point.
-      /// \param[in] _y1 Y coordinate of the start point.
-      /// \param[in] _x2 X coordinate of the end point.
-      /// \param[in] _y2 Y coordinate of the end point.
-      public: Line2(double _x1, double _y1, double _x2, double _y2)
-      {
-        this->Set(_x1, _y1, _x2, _y2);
-      }
-
-      /// \brief Set the start and end point of the line segment
-      /// \param[in] _ptA Start point of the line segment
-      /// \param[in] _ptB End point of the line segment
-      public: void Set(const math::Vector2<T> &_ptA,
-                       const math::Vector2<T> &_ptB)
-      {
-        this->pts[0] = _ptA;
-        this->pts[1] = _ptB;
-      }
-
-      /// \brief Set the start and end point of the line segment
-      /// \param[in] _x1 X coordinate of the start point.
-      /// \param[in] _y1 Y coordinate of the start point.
-      /// \param[in] _x2 X coordinate of the end point.
-      /// \param[in] _y2 Y coordinate of the end point.
-      public: void Set(double _x1, double _y1, double _x2, double _y2)
-      {
-        this->pts[0].Set(_x1, _y1);
-        this->pts[1].Set(_x2, _y2);
-      }
+      this->Set(_ptA, _ptB);
+    }
 
-      /// \brief Return the cross product of this line and the given line.
-      /// Give 'a' as this line and 'b' as given line, the equation is:
-      /// (a.start.x - a.end.x) * (b.start.y - b.end.y) -
-      /// (a.start.y - a.end.y) * (b.start.x - b.end.x)
-      /// \param[in] _line Line for the cross product computation.
-      /// \return Return the cross product of this line and the given line.
-      public: double CrossProduct(const Line2<T> &_line) const
-      {
-        return (this->pts[0].X() - this->pts[1].X()) *
-               (_line[0].Y() -_line[1].Y()) -
-               (this->pts[0].Y() - this->pts[1].Y()) *
-               (_line[0].X() - _line[1].X());
-      }
+    /// \brief Constructor.
+    /// \param[in] _x1 X coordinate of the start point.
+    /// \param[in] _y1 Y coordinate of the start point.
+    /// \param[in] _x2 X coordinate of the end point.
+    /// \param[in] _y2 Y coordinate of the end point.
+    public: Line2(double _x1, double _y1, double _x2, double _y2)
+    {
+      this->Set(_x1, _y1, _x2, _y2);
+    }
 
-      /// \brief Return the cross product of this line and the given point.
-      /// Given 'a' and 'b' as the start and end points, the equation is:
-      //  (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
-      /// \param[in] _pt Point for the cross product computation.
-      /// \return Return the cross product of this line and the given point.
-      public: double CrossProduct(const Vector2<T> &_pt) const
-      {
-        return (_pt.Y() - this->pts[0].Y()) *
-               (this->pts[1].X() - this->pts[0].X()) -
-               (_pt.X() - this->pts[0].X()) *
-               (this->pts[1].Y() - this->pts[0].Y());
-      }
+    /// \brief Set the start and end point of the line segment
+    /// \param[in] _ptA Start point of the line segment
+    /// \param[in] _ptB End point of the line segment
+    public: void Set(const math::Vector2<T> &_ptA,
+                     const math::Vector2<T> &_ptB)
+    {
+      this->pts[0] = _ptA;
+      this->pts[1] = _ptB;
+    }
 
-      /// \brief Check if the given point is collinear with this line.
-      /// \param[in] _pt The point to check.
-      /// \param[in] _epsilon The error bounds within which the collinear
-      /// check will return true.
-      /// \return Return true if the point is collinear with this line, false
-      /// otherwise.
-      public: bool Collinear(const math::Vector2<T> &_pt,
-                             double _epsilon = 1e-6) const
-      {
-        return math::equal(this->CrossProduct(_pt),
-            0., _epsilon);
-      }
+    /// \brief Set the start and end point of the line segment
+    /// \param[in] _x1 X coordinate of the start point.
+    /// \param[in] _y1 Y coordinate of the start point.
+    /// \param[in] _x2 X coordinate of the end point.
+    /// \param[in] _y2 Y coordinate of the end point.
+    public: void Set(double _x1, double _y1, double _x2, double _y2)
+    {
+      this->pts[0].Set(_x1, _y1);
+      this->pts[1].Set(_x2, _y2);
+    }
 
-      /// \brief Check if the given line is parallel with this line.
-      /// \param[in] _line The line to check.
-      /// \param[in] _epsilon The error bounds within which the parallel
-      /// check will return true.
-      /// \return Return true if the line is parallel with this line, false
-      /// otherwise. Return true if either line is a point (line with zero
-      /// length).
-      public: bool Parallel(const math::Line2<T> &_line,
-                            double _epsilon = 1e-6) const
-      {
-        return math::equal(this->CrossProduct(_line),
-            0., _epsilon);
-      }
+    /// \brief Return the cross product of this line and the given line.
+    /// Give 'a' as this line and 'b' as given line, the equation is:
+    /// (a.start.x - a.end.x) * (b.start.y - b.end.y) -
+    /// (a.start.y - a.end.y) * (b.start.x - b.end.x)
+    /// \param[in] _line Line for the cross product computation.
+    /// \return Return the cross product of this line and the given line.
+    public: double CrossProduct(const Line2<T> &_line) const
+    {
+      return (this->pts[0].X() - this->pts[1].X()) *
+             (_line[0].Y() -_line[1].Y()) -
+             (this->pts[0].Y() - this->pts[1].Y()) *
+             (_line[0].X() - _line[1].X());
+    }
 
-      /// \brief Check if the given line is collinear with this line. This
-      /// is the AND of Parallel and Intersect.
-      /// \param[in] _line The line to check.
-      /// \param[in] _epsilon The error bounds within which the collinear
-      /// check will return true.
-      /// \return Return true if the line is collinear with this line, false
-      /// otherwise.
-      public: bool Collinear(const math::Line2<T> &_line,
-                             double _epsilon = 1e-6) const
-      {
-        return this->Parallel(_line, _epsilon) &&
-               this->Intersect(_line, _epsilon);
-      }
+    /// \brief Return the cross product of this line and the given point.
+    /// Given 'a' and 'b' as the start and end points, the equation is:
+    //  (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
+    /// \param[in] _pt Point for the cross product computation.
+    /// \return Return the cross product of this line and the given point.
+    public: double CrossProduct(const Vector2<T> &_pt) const
+    {
+      return (_pt.Y() - this->pts[0].Y()) *
+             (this->pts[1].X() - this->pts[0].X()) -
+             (_pt.X() - this->pts[0].X()) *
+             (this->pts[1].Y() - this->pts[0].Y());
+    }
 
-      /// \brief Return whether the given point is on this line segment.
-      /// \param[in] _pt Point to check.
-      /// \param[in] _epsilon The error bounds within which the OnSegment
-      /// check will return true.
-      /// \return True if the point is on the segement.
-      public: bool OnSegment(const math::Vector2<T> &_pt,
-                             double _epsilon = 1e-6) const
-      {
-        return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
-      }
+    /// \brief Check if the given point is collinear with this line.
+    /// \param[in] _pt The point to check.
+    /// \param[in] _epsilon The error bounds within which the collinear
+    /// check will return true.
+    /// \return Return true if the point is collinear with this line, false
+    /// otherwise.
+    public: bool Collinear(const math::Vector2<T> &_pt,
+                           double _epsilon = 1e-6) const
+    {
+      return math::equal(this->CrossProduct(_pt),
+          0., _epsilon);
+    }
 
-      /// \brief Check if the given point is between the start and end
-      /// points of the line segment. This does not imply that the point is
-      /// on the segment.
-      /// \param[in] _pt Point to check.
-      /// \param[in] _epsilon The error bounds within which the within
-      /// check will return true.
-      /// \return True if the point is on the segement.
-      public: bool Within(const math::Vector2<T> &_pt,
+    /// \brief Check if the given line is parallel with this line.
+    /// \param[in] _line The line to check.
+    /// \param[in] _epsilon The error bounds within which the parallel
+    /// check will return true.
+    /// \return Return true if the line is parallel with this line, false
+    /// otherwise. Return true if either line is a point (line with zero
+    /// length).
+    public: bool Parallel(const math::Line2<T> &_line,
                           double _epsilon = 1e-6) const
-      {
-        return _pt.X() <= std::max(this->pts[0].X(),
-                                   this->pts[1].X()) + _epsilon &&
-               _pt.X() >= std::min(this->pts[0].X(),
-                                   this->pts[1].X()) - _epsilon &&
-               _pt.Y() <= std::max(this->pts[0].Y(),
-                                   this->pts[1].Y()) + _epsilon &&
-               _pt.Y() >= std::min(this->pts[0].Y(),
-                                   this->pts[1].Y()) - _epsilon;
-      }
-
-      /// \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
-      /// check will return true.
-      /// \return True if an intersection was found.
-      public: bool Intersect(const Line2<T> &_line,
-                             double _epsilon = 1e-6) const
-      {
-        math::Vector2<T> ignore;
-        return this->Intersect(_line, ignore, _epsilon);
-      }
+    {
+      return math::equal(this->CrossProduct(_line),
+          0., _epsilon);
+    }
 
-      /// \brief Check if this line intersects the given line segment. The
-      /// point of intersection is returned in the _result parameter.
-      /// \param[in] _line The line to check for intersection.
-      /// \param[out] _pt The point of intersection. This value is only
-      /// valid if the return value is true.
-      /// \param[in] _epsilon The error bounds within which the intersection
-      /// check will return true.
-      /// \return True if an intersection was found.
-      public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
-                             double _epsilon = 1e-6) const
-      {
-        double d = this->CrossProduct(_line);
+    /// \brief Check if the given line is collinear with this line. This
+    /// is the AND of Parallel and Intersect.
+    /// \param[in] _line The line to check.
+    /// \param[in] _epsilon The error bounds within which the collinear
+    /// check will return true.
+    /// \return Return true if the line is collinear with this line, false
+    /// otherwise.
+    public: bool Collinear(const math::Line2<T> &_line,
+                           double _epsilon = 1e-6) const
+    {
+      return this->Parallel(_line, _epsilon) &&
+             this->Intersect(_line, _epsilon);
+    }
 
-        // d is zero if the two line are collinear. Must check special
-        // cases.
-        if (math::equal(d, 0.0, _epsilon))
-        {
-          // Check if _line's starting point is on the line.
-          if (this->Within(_line[0], _epsilon))
-          {
-            _pt = _line[0];
-            return true;
-          }
-          // Check if _line's ending point is on the line.
-          else if (this->Within(_line[1], _epsilon))
-          {
-            _pt = _line[1];
-            return true;
-          }
-          // Other wise return false.
-          else
-            return false;
-        }
+    /// \brief Return whether the given point is on this line segment.
+    /// \param[in] _pt Point to check.
+    /// \param[in] _epsilon The error bounds within which the OnSegment
+    /// check will return true.
+    /// \return True if the point is on the segement.
+    public: bool OnSegment(const math::Vector2<T> &_pt,
+                           double _epsilon = 1e-6) const
+    {
+      return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
+    }
 
-        _pt.X((_line[0].X() - _line[1].X()) *
-              (this->pts[0].X() * this->pts[1].Y() -
-               this->pts[0].Y() * this->pts[1].X()) -
-              (this->pts[0].X() - this->pts[1].X()) *
-              (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
+    /// \brief Check if the given point is between the start and end
+    /// points of the line segment. This does not imply that the point is
+    /// on the segment.
+    /// \param[in] _pt Point to check.
+    /// \param[in] _epsilon The error bounds within which the within
+    /// check will return true.
+    /// \return True if the point is on the segement.
+    public: bool Within(const math::Vector2<T> &_pt,
+                        double _epsilon = 1e-6) const
+    {
+      return _pt.X() <= std::max(this->pts[0].X(),
+                                 this->pts[1].X()) + _epsilon &&
+             _pt.X() >= std::min(this->pts[0].X(),
+                                 this->pts[1].X()) - _epsilon &&
+             _pt.Y() <= std::max(this->pts[0].Y(),
+                                 this->pts[1].Y()) + _epsilon &&
+             _pt.Y() >= std::min(this->pts[0].Y(),
+                                 this->pts[1].Y()) - _epsilon;
+    }
 
-        _pt.Y((_line[0].Y() - _line[1].Y()) *
-              (this->pts[0].X() * this->pts[1].Y() -
-               this->pts[0].Y() * this->pts[1].X()) -
-              (this->pts[0].Y() - this->pts[1].Y()) *
-              (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
+    /// \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
+    /// check will return true.
+    /// \return True if an intersection was found.
+    public: bool Intersect(const Line2<T> &_line,
+                           double _epsilon = 1e-6) const
+    {
+      math::Vector2<T> ignore;
+      return this->Intersect(_line, ignore, _epsilon);
+    }
 
-        _pt /= d;
+    /// \brief Check if this line intersects the given line segment. The
+    /// point of intersection is returned in the _result parameter.
+    /// \param[in] _line The line to check for intersection.
+    /// \param[out] _pt The point of intersection. This value is only
+    /// valid if the return value is true.
+    /// \param[in] _epsilon The error bounds within which the intersection
+    /// check will return true.
+    /// \return True if an intersection was found.
+    public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
+                           double _epsilon = 1e-6) const
+    {
+      double d = this->CrossProduct(_line);
 
-        if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
-            _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
-            _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
-            _pt.X() > std::max(_line[0].X(), _line[1].X()))
+      // d is zero if the two line are collinear. Must check special
+      // cases.
+      if (math::equal(d, 0.0, _epsilon))
+      {
+        // Check if _line's starting point is on the line.
+        if (this->Within(_line[0], _epsilon))
         {
-          return false;
+          _pt = _line[0];
+          return true;
         }
-
-        if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
-            _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
-            _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
-            _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
+        // Check if _line's ending point is on the line.
+        else if (this->Within(_line[1], _epsilon))
         {
-          return false;
+          _pt = _line[1];
+          return true;
         }
-
-        return true;
+        // Other wise return false.
+        else
+          return false;
       }
 
-      /// \brief Get the length of the line
-      /// \return The length of the line.
-      public: T Length() const
-      {
-        return sqrt((this->pts[0].X() - this->pts[1].X()) *
-                    (this->pts[0].X() - this->pts[1].X()) +
-                    (this->pts[0].Y() - this->pts[1].Y()) *
-                    (this->pts[0].Y() - this->pts[1].Y()));
-      }
+      _pt.X((_line[0].X() - _line[1].X()) *
+            (this->pts[0].X() * this->pts[1].Y() -
+             this->pts[0].Y() * this->pts[1].X()) -
+            (this->pts[0].X() - this->pts[1].X()) *
+            (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
 
-      /// \brief Get the slope of the line
-      /// \return The slope of the line, NAN_D if the line is vertical.
-      public: double Slope() const
-      {
-        if (math::equal(this->pts[1].X(), this->pts[0].X()))
-          return NAN_D;
+      _pt.Y((_line[0].Y() - _line[1].Y()) *
+            (this->pts[0].X() * this->pts[1].Y() -
+             this->pts[0].Y() * this->pts[1].X()) -
+            (this->pts[0].Y() - this->pts[1].Y()) *
+            (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
 
-        return (this->pts[1].Y() - this->pts[0].Y()) /
-               static_cast<double>(this->pts[1].X() - this->pts[0].X());
-      }
+      _pt /= d;
 
-      /// \brief Equality operator.
-      /// \param[in] _line Line to compare for equality.
-      /// \return True if the given line is equal to this line
-      public: bool operator==(const Line2<T> &_line) const
+      if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
+          _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
+          _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
+          _pt.X() > std::max(_line[0].X(), _line[1].X()))
       {
-        return this->pts[0] == _line[0] && this->pts[1] == _line[1];
+        return false;
       }
 
-      /// \brief Inequality operator.
-      /// \param[in] _line Line to compare for inequality.
-      /// \return True if the given line is not to this line
-      public: bool operator!=(const Line2<T> &_line) const
+      if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
+          _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
+          _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
+          _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
       {
-        return !(*this == _line);
+        return false;
       }
 
-      /// \brief Get the start or end point.
-      /// \param[in] _index 0 = start point, 1 = end point. The _index
-      /// is clamped to the range [0, 1]
-      public: math::Vector2<T> operator[](size_t _index) const
-      {
-        return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
-      }
+      return true;
+    }
 
-      /// \brief Stream extraction operator
-      /// \param[in] _out output stream
-      /// \param[in] _line Line2 to output
-      /// \return The stream
-      public: friend std::ostream &operator<<(
-                  std::ostream &_out, const Line2<T> &_line)
-      {
-        _out << _line[0] << " " << _line[1];
-        return _out;
-      }
+    /// \brief Get the length of the line
+    /// \return The length of the line.
+    public: T Length() const
+    {
+      return sqrt((this->pts[0].X() - this->pts[1].X()) *
+                  (this->pts[0].X() - this->pts[1].X()) +
+                  (this->pts[0].Y() - this->pts[1].Y()) *
+                  (this->pts[0].Y() - this->pts[1].Y()));
+    }
 
-      private: math::Vector2<T> pts[2];
-    };
+    /// \brief Get the slope of the line
+    /// \return The slope of the line, NAN_D if the line is vertical.
+    public: double Slope() const
+    {
+      if (math::equal(this->pts[1].X(), this->pts[0].X()))
+        return NAN_D;
 
+      return (this->pts[1].Y() - this->pts[0].Y()) /
+             static_cast<double>(this->pts[1].X() - this->pts[0].X());
+    }
 
-    typedef Line2<int> Line2i;
-    typedef Line2<double> Line2d;
-    typedef Line2<float> Line2f;
+    /// \brief Equality operator.
+    /// \param[in] _line Line to compare for equality.
+    /// \return True if the given line is equal to this line
+    public: bool operator==(const Line2<T> &_line) const
+    {
+      return this->pts[0] == _line[0] && this->pts[1] == _line[1];
     }
-  }
-}
-#endif
+
+    /// \brief Inequality operator.
+    /// \param[in] _line Line to compare for inequality.
+    /// \return True if the given line is not to this line
+    public: bool operator!=(const Line2<T> &_line) const
+    {
+      return !(*this == _line);
+    }
+
+    /// \brief Get the start or end point.
+    /// \param[in] _index 0 = start point, 1 = end point. The _index
+    /// is clamped to the range [0, 1]
+    public: math::Vector2<T> operator[](size_t _index) const
+    {
+      return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
+    }
+
+    /// \brief Stream extraction operator
+    /// \param[in] _out output stream
+    /// \param[in] _line Line2 to output
+    /// \return The stream
+    public: friend std::ostream &operator<<(
+                std::ostream &_out, const Line2<T> &_line)
+    {
+      _out << _line[0] << " " << _line[1];
+      return _out;
+    }
+
+    private: math::Vector2<T> pts[2];
+  };
+
+
+  typedef Line2<int> Line2i;
+  typedef Line2<double> Line2d;
+  typedef Line2<float> Line2f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_LINE2_HH_
diff --git a/include/gz/math/Line3.hh b/include/gz/math/Line3.hh
index 7a0fc0b66..0a3bcf0ef 100644
--- a/include/gz/math/Line3.hh
+++ b/include/gz/math/Line3.hh
@@ -21,394 +21,391 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Line3 Line3.hh gz/math/Line3.hh
+  /// \brief A three dimensional line segment. The line is defined by a
+  /// start and end point.
+  template<typename T>
+  class Line3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Line3 Line3.hh gz/math/Line3.hh
-    /// \brief A three dimensional line segment. The line is defined by a
-    /// start and end point.
-    template<typename T>
-    class Line3
+    /// \brief Line Constructor
+    public: Line3() = default;
+
+    /// \brief Copy constructor
+    /// \param[in] _line a line object
+    public: Line3(const Line3<T> &_line) = default;
+
+    /// \brief Constructor.
+    /// \param[in] _ptA Start point of the line segment
+    /// \param[in] _ptB End point of the line segment
+    public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
     {
-      /// \brief Line Constructor
-      public: Line3() = default;
+      this->Set(_ptA, _ptB);
+    }
 
-      /// \brief Copy constructor
-      /// \param[in] _line a line object
-      public: Line3(const Line3<T> &_line) = default;
+    /// \brief 2D Constructor where Z coordinates are 0
+    /// \param[in] _x1 X coordinate of the start point.
+    /// \param[in] _y1 Y coordinate of the start point.
+    /// \param[in] _x2 X coordinate of the end point.
+    /// \param[in] _y2 Y coordinate of the end point.
+    public: Line3(const double _x1, const double _y1,
+      const double _x2, const double _y2)
+    {
+      this->Set(_x1, _y1, _x2, _y2);
+    }
 
-      /// \brief Constructor.
-      /// \param[in] _ptA Start point of the line segment
-      /// \param[in] _ptB End point of the line segment
-      public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
-      {
-        this->Set(_ptA, _ptB);
-      }
+    /// \brief Constructor.
+    /// \param[in] _x1 X coordinate of the start point.
+    /// \param[in] _y1 Y coordinate of the start point.
+    /// \param[in] _z1 Z coordinate of the start point.
+    /// \param[in] _x2 X coordinate of the end point.
+    /// \param[in] _y2 Y coordinate of the end point.
+    /// \param[in] _z2 Z coordinate of the end point.
+    public: Line3(const double _x1, const double _y1,
+      const double _z1, const double _x2,
+      const double _y2, const double _z2)
+    {
+      this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
+    }
 
-      /// \brief 2D Constructor where Z coordinates are 0
-      /// \param[in] _x1 X coordinate of the start point.
-      /// \param[in] _y1 Y coordinate of the start point.
-      /// \param[in] _x2 X coordinate of the end point.
-      /// \param[in] _y2 Y coordinate of the end point.
-      public: Line3(const double _x1, const double _y1,
-        const double _x2, const double _y2)
-      {
-        this->Set(_x1, _y1, _x2, _y2);
-      }
+    /// \brief Set the start and end point of the line segment
+    /// \param[in] _ptA Start point of the line segment
+    /// \param[in] _ptB End point of the line segment
+    public: void Set(const math::Vector3<T> &_ptA,
+                     const math::Vector3<T> &_ptB)
+    {
+      this->pts[0] = _ptA;
+      this->pts[1] = _ptB;
+    }
 
-      /// \brief Constructor.
-      /// \param[in] _x1 X coordinate of the start point.
-      /// \param[in] _y1 Y coordinate of the start point.
-      /// \param[in] _z1 Z coordinate of the start point.
-      /// \param[in] _x2 X coordinate of the end point.
-      /// \param[in] _y2 Y coordinate of the end point.
-      /// \param[in] _z2 Z coordinate of the end point.
-      public: Line3(const double _x1, const double _y1,
-        const double _z1, const double _x2,
-        const double _y2, const double _z2)
-      {
-        this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
-      }
+    /// \brief Set the start point of the line segment
+    /// \param[in] _ptA Start point of the line segment
+    public: void SetA(const math::Vector3<T> &_ptA)
+    {
+      this->pts[0] = _ptA;
+    }
 
-      /// \brief Set the start and end point of the line segment
-      /// \param[in] _ptA Start point of the line segment
-      /// \param[in] _ptB End point of the line segment
-      public: void Set(const math::Vector3<T> &_ptA,
-                       const math::Vector3<T> &_ptB)
-      {
-        this->pts[0] = _ptA;
-        this->pts[1] = _ptB;
-      }
+    /// \brief Set the end point of the line segment
+    /// \param[in] _ptB End point of the line segment
+    public: void SetB(const math::Vector3<T> &_ptB)
+    {
+      this->pts[1] = _ptB;
+    }
 
-      /// \brief Set the start point of the line segment
-      /// \param[in] _ptA Start point of the line segment
-      public: void SetA(const math::Vector3<T> &_ptA)
-      {
-        this->pts[0] = _ptA;
-      }
+    /// \brief Set the start and end point of the line segment, assuming that
+    /// both points have the same height.
+    /// \param[in] _x1 X coordinate of the start point.
+    /// \param[in] _y1 Y coordinate of the start point.
+    /// \param[in] _x2 X coordinate of the end point.
+    /// \param[in] _y2 Y coordinate of the end point.
+    /// \param[in] _z Z coordinate of both points,
+    /// by default _z is set to 0.
+    public: void Set(const double _x1, const double _y1,
+      const double _x2, const double _y2,
+      const double _z = 0)
+    {
+      this->pts[0].Set(_x1, _y1, _z);
+      this->pts[1].Set(_x2, _y2, _z);
+    }
 
-      /// \brief Set the end point of the line segment
-      /// \param[in] _ptB End point of the line segment
-      public: void SetB(const math::Vector3<T> &_ptB)
-      {
-        this->pts[1] = _ptB;
-      }
+    /// \brief Set the start and end point of the line segment
+    /// \param[in] _x1 X coordinate of the start point.
+    /// \param[in] _y1 Y coordinate of the start point.
+    /// \param[in] _z1 Z coordinate of the start point.
+    /// \param[in] _x2 X coordinate of the end point.
+    /// \param[in] _y2 Y coordinate of the end point.
+    /// \param[in] _z2 Z coordinate of the end point.
+    public: void Set(const double _x1, const double _y1,
+      const double _z1, const double _x2,
+      const double _y2, const double _z2)
+    {
+      this->pts[0].Set(_x1, _y1, _z1);
+      this->pts[1].Set(_x2, _y2, _z2);
+    }
 
-      /// \brief Set the start and end point of the line segment, assuming that
-      /// both points have the same height.
-      /// \param[in] _x1 X coordinate of the start point.
-      /// \param[in] _y1 Y coordinate of the start point.
-      /// \param[in] _x2 X coordinate of the end point.
-      /// \param[in] _y2 Y coordinate of the end point.
-      /// \param[in] _z Z coordinate of both points,
-      /// by default _z is set to 0.
-      public: void Set(const double _x1, const double _y1,
-        const double _x2, const double _y2,
-        const double _z = 0)
-      {
-        this->pts[0].Set(_x1, _y1, _z);
-        this->pts[1].Set(_x2, _y2, _z);
-      }
+    /// \brief Get the direction of the line
+    /// \return The direction vector
+    public: math::Vector3<T> Direction() const
+    {
+      return (this->pts[1] - this->pts[0]).Normalize();
+    }
 
-      /// \brief Set the start and end point of the line segment
-      /// \param[in] _x1 X coordinate of the start point.
-      /// \param[in] _y1 Y coordinate of the start point.
-      /// \param[in] _z1 Z coordinate of the start point.
-      /// \param[in] _x2 X coordinate of the end point.
-      /// \param[in] _y2 Y coordinate of the end point.
-      /// \param[in] _z2 Z coordinate of the end point.
-      public: void Set(const double _x1, const double _y1,
-        const double _z1, const double _x2,
-        const double _y2, const double _z2)
-      {
-        this->pts[0].Set(_x1, _y1, _z1);
-        this->pts[1].Set(_x2, _y2, _z2);
-      }
+    /// \brief Get the length of the line
+    /// \return The length of the line.
+    public: T Length() const
+    {
+      return this->pts[0].Distance(this->pts[1]);
+    }
 
-      /// \brief Get the direction of the line
-      /// \return The direction vector
-      public: math::Vector3<T> Direction() const
+    /// \brief Get the shortest line between this line and the
+    /// provided line.
+    ///
+    /// In the case when the two lines are parallel, we choose the first
+    /// point of this line and the closest point in the provided line.
+    /// \param[in] _line Line to compare against this.
+    /// \param[out] _result The shortest line between _line and this.
+    /// \param[in] _epsilon Error tolerance.
+    /// \return True if a solution was found. False if a solution is not
+    /// possible.
+    public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
+                          const double _epsilon = 1e-6) const
+    {
+      Vector3<T> p13 = this->pts[0] - _line[0];
+      Vector3<T> p43 = _line[1] - _line[0];
+
+      if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
+          std::abs(p43.Z()) < _epsilon)
       {
-        return (this->pts[1] - this->pts[0]).Normalize();
+        return false;
       }
 
-      /// \brief Get the length of the line
-      /// \return The length of the line.
-      public: T Length() const
+      Vector3<T> p21 = this->pts[1] - this->pts[0];
+
+      if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
+          std::abs(p21.Z()) < _epsilon)
       {
-        return this->pts[0].Distance(this->pts[1]);
+        return false;
       }
 
-      /// \brief Get the shortest line between this line and the
-      /// provided line.
-      ///
-      /// In the case when the two lines are parallel, we choose the first
-      /// point of this line and the closest point in the provided line.
-      /// \param[in] _line Line to compare against this.
-      /// \param[out] _result The shortest line between _line and this.
-      /// \param[in] _epsilon Error tolerance.
-      /// \return True if a solution was found. False if a solution is not
-      /// possible.
-      public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
-                            const double _epsilon = 1e-6) const
+      double d1343 = p13.Dot(p43);
+      double d4321 = p43.Dot(p21);
+      double d1321 = p13.Dot(p21);
+      double d4343 = p43.Dot(p43);
+      double d2121 = p21.Dot(p21);
+
+      double denom = d2121 * d4343 - d4321 * d4321;
+
+      // In this case, we choose the first point in this line,
+      // and the closest point in the provided line.
+      if (std::abs(denom) < _epsilon)
       {
-        Vector3<T> p13 = this->pts[0] - _line[0];
-        Vector3<T> p43 = _line[1] - _line[0];
+        double d1 = this->pts[0].Distance(_line[0]);
+        double d2 = this->pts[0].Distance(_line[1]);
+
+        double d3 = this->pts[1].Distance(_line[0]);
+        double d4 = this->pts[1].Distance(_line[1]);
 
-        if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
-            std::abs(p43.Z()) < _epsilon)
+        if (d1 <= d2 && d1 <= d3 && d1 <= d4)
         {
-          return false;
+          _result.SetA(this->pts[0]);
+          _result.SetB(_line[0]);
         }
-
-        Vector3<T> p21 = this->pts[1] - this->pts[0];
-
-        if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
-            std::abs(p21.Z()) < _epsilon)
+        else if (d2 <= d3 && d2 <= d4)
         {
-          return false;
+          _result.SetA(this->pts[0]);
+          _result.SetB(_line[1]);
         }
-
-        double d1343 = p13.Dot(p43);
-        double d4321 = p43.Dot(p21);
-        double d1321 = p13.Dot(p21);
-        double d4343 = p43.Dot(p43);
-        double d2121 = p21.Dot(p21);
-
-        double denom = d2121 * d4343 - d4321 * d4321;
-
-        // In this case, we choose the first point in this line,
-        // and the closest point in the provided line.
-        if (std::abs(denom) < _epsilon)
+        else if (d3 <= d4)
         {
-          double d1 = this->pts[0].Distance(_line[0]);
-          double d2 = this->pts[0].Distance(_line[1]);
-
-          double d3 = this->pts[1].Distance(_line[0]);
-          double d4 = this->pts[1].Distance(_line[1]);
-
-          if (d1 <= d2 && d1 <= d3 && d1 <= d4)
-          {
-            _result.SetA(this->pts[0]);
-            _result.SetB(_line[0]);
-          }
-          else if (d2 <= d3 && d2 <= d4)
-          {
-            _result.SetA(this->pts[0]);
-            _result.SetB(_line[1]);
-          }
-          else if (d3 <= d4)
-          {
-            _result.SetA(this->pts[1]);
-            _result.SetB(_line[0]);
-          }
-          else
-          {
-            _result.SetA(this->pts[1]);
-            _result.SetB(_line[1]);
-          }
-
-          return true;
+          _result.SetA(this->pts[1]);
+          _result.SetB(_line[0]);
+        }
+        else
+        {
+          _result.SetA(this->pts[1]);
+          _result.SetB(_line[1]);
         }
-
-        double numer = d1343 * d4321 - d1321 * d4343;
-
-        double mua = clamp(numer / denom, 0.0, 1.0);
-        double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
-
-        _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
 
         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];
+      double numer = d1343 * d4321 - d1321 * d4343;
 
-        // Point is projected beyond pt0 or the line has length 0
-        if (ptTo0.Dot(line) <= 0.0)
-        {
-          return ptTo0.Length();
-        }
+      double mua = clamp(numer / denom, 0.0, 1.0);
+      double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
 
-        // Point is projected beyond pt1
-        if (ptTo1.Dot(line) >= 0.0)
-        {
-          return ptTo1.Length();
-        }
+      _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
 
-        // 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;
-      }
+      return true;
+    }
 
-      /// \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
-      /// check will return true.
-      /// \return True if an intersection was found.
-      public: bool Intersect(const Line3<T> &_line,
-                             double _epsilon = 1e-6) const
-      {
-        math::Vector3<T> ignore;
-        return this->Intersect(_line, ignore, _epsilon);
-      }
+    /// \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];
 
-      /// \brief Test if this line and the given line are coplanar.
-      /// \param[in] _line Line to check against.
-      /// \param[in] _epsilon The error bounds within which the
-      /// check will return true.
-      /// \return True if the two lines are coplanar.
-      public: bool Coplanar(const Line3<T> &_line,
-                            const double _epsilon = 1e-6) const
+      // Point is projected beyond pt0 or the line has length 0
+      if (ptTo0.Dot(line) <= 0.0)
       {
-        return std::abs((_line[0] - this->pts[0]).Dot(
-              (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
-          <= _epsilon;
+        return ptTo0.Length();
       }
 
-      /// \brief Test if this line and the given line are parallel.
-      /// \param[in] _line Line to check against.
-      /// \param[in] _epsilon The error bounds within which the
-      /// check will return true.
-      /// \return True if the two lines are parallel.
-      public: bool Parallel(const Line3<T> &_line,
-                            const double _epsilon = 1e-6) const
+      // Point is projected beyond pt1
+      if (ptTo1.Dot(line) >= 0.0)
       {
-        return (this->pts[1] - this->pts[0]).Cross(
-            _line[1] - _line[0]).Length() <= _epsilon;
+        return ptTo1.Length();
       }
 
-      /// \brief Check if this line intersects the given line segment. The
-      /// point of intersection is returned in the _pt parameter.
-      /// \param[in] _line The line to check for intersection.
-      /// \param[out] _pt The point of intersection. This value is only
-      /// valid if the return value is true.
-      /// \param[in] _epsilon The error bounds within which the intersection
-      /// check will return true.
-      /// \return True if an intersection was found.
-      public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
-                             double _epsilon = 1e-6) const
+      // 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
+    /// check will return true.
+    /// \return True if an intersection was found.
+    public: bool Intersect(const Line3<T> &_line,
+                           double _epsilon = 1e-6) const
+    {
+      math::Vector3<T> ignore;
+      return this->Intersect(_line, ignore, _epsilon);
+    }
+
+    /// \brief Test if this line and the given line are coplanar.
+    /// \param[in] _line Line to check against.
+    /// \param[in] _epsilon The error bounds within which the
+    /// check will return true.
+    /// \return True if the two lines are coplanar.
+    public: bool Coplanar(const Line3<T> &_line,
+                          const double _epsilon = 1e-6) const
+    {
+      return std::abs((_line[0] - this->pts[0]).Dot(
+            (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
+        <= _epsilon;
+    }
+
+    /// \brief Test if this line and the given line are parallel.
+    /// \param[in] _line Line to check against.
+    /// \param[in] _epsilon The error bounds within which the
+    /// check will return true.
+    /// \return True if the two lines are parallel.
+    public: bool Parallel(const Line3<T> &_line,
+                          const double _epsilon = 1e-6) const
+    {
+      return (this->pts[1] - this->pts[0]).Cross(
+          _line[1] - _line[0]).Length() <= _epsilon;
+    }
+
+    /// \brief Check if this line intersects the given line segment. The
+    /// point of intersection is returned in the _pt parameter.
+    /// \param[in] _line The line to check for intersection.
+    /// \param[out] _pt The point of intersection. This value is only
+    /// valid if the return value is true.
+    /// \param[in] _epsilon The error bounds within which the intersection
+    /// check will return true.
+    /// \return True if an intersection was found.
+    public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
+                           double _epsilon = 1e-6) const
+    {
+      // Handle special case when lines are parallel
+      if (this->Parallel(_line, _epsilon))
       {
-        // Handle special case when lines are parallel
-        if (this->Parallel(_line, _epsilon))
+        // Check if _line's starting point is on the line.
+        if (this->Within(_line[0], _epsilon))
         {
-          // Check if _line's starting point is on the line.
-          if (this->Within(_line[0], _epsilon))
-          {
-            _pt = _line[0];
-            return true;
-          }
-          // Check if _line's ending point is on the line.
-          else if (this->Within(_line[1], _epsilon))
-          {
-            _pt = _line[1];
-            return true;
-          }
-          // Otherwise return false.
-          else
-            return false;
+          _pt = _line[0];
+          return true;
         }
-
-        // Get the line that is the shortest distance between this and _line
-        math::Line3<T> distLine;
-        this->Distance(_line, distLine, _epsilon);
-
-        // If the length of the line is less than epsilon, then they
-        // intersect.
-        if (distLine.Length() < _epsilon)
+        // Check if _line's ending point is on the line.
+        else if (this->Within(_line[1], _epsilon))
         {
-          _pt = distLine[0];
+          _pt = _line[1];
           return true;
         }
-
-        return false;
+        // Otherwise return false.
+        else
+          return false;
       }
 
-      /// \brief Check if the given point is between the start and end
-      /// points of the line segment.
-      /// \param[in] _pt Point to check.
-      /// \param[in] _epsilon The error bounds within which the within
-      /// check will return true.
-      /// \return True if the point is on the segement.
-      public: bool Within(const math::Vector3<T> &_pt,
-                          double _epsilon = 1e-6) const
-      {
-        return _pt.X() <= std::max(this->pts[0].X(),
-                                   this->pts[1].X()) + _epsilon &&
-               _pt.X() >= std::min(this->pts[0].X(),
-                                   this->pts[1].X()) - _epsilon &&
-               _pt.Y() <= std::max(this->pts[0].Y(),
-                                   this->pts[1].Y()) + _epsilon &&
-               _pt.Y() >= std::min(this->pts[0].Y(),
-                                   this->pts[1].Y()) - _epsilon &&
-               _pt.Z() <= std::max(this->pts[0].Z(),
-                                   this->pts[1].Z()) + _epsilon &&
-               _pt.Z() >= std::min(this->pts[0].Z(),
-                                   this->pts[1].Z()) - _epsilon;
-      }
+      // Get the line that is the shortest distance between this and _line
+      math::Line3<T> distLine;
+      this->Distance(_line, distLine, _epsilon);
 
-      /// \brief Equality operator.
-      /// \param[in] _line Line to compare for equality.
-      /// \return True if the given line is equal to this line
-      public: bool operator==(const Line3<T> &_line) const
+      // If the length of the line is less than epsilon, then they
+      // intersect.
+      if (distLine.Length() < _epsilon)
       {
-        return this->pts[0] == _line[0] && this->pts[1] == _line[1];
+        _pt = distLine[0];
+        return true;
       }
 
-      /// \brief Inequality operator.
-      /// \param[in] _line Line to compare for inequality.
-      /// \return True if the given line is not to this line
-      public: bool operator!=(const Line3<T> &_line) const
-      {
-        return !(*this == _line);
-      }
+      return false;
+    }
 
-      /// \brief Get the start or end point.
-      /// \param[in] _index 0 = start point, 1 = end point. The _index
-      /// parameter is clamped to the range [0, 1].
-      public: math::Vector3<T> operator[](const size_t _index) const
-      {
-        return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
-      }
+    /// \brief Check if the given point is between the start and end
+    /// points of the line segment.
+    /// \param[in] _pt Point to check.
+    /// \param[in] _epsilon The error bounds within which the within
+    /// check will return true.
+    /// \return True if the point is on the segement.
+    public: bool Within(const math::Vector3<T> &_pt,
+                        double _epsilon = 1e-6) const
+    {
+      return _pt.X() <= std::max(this->pts[0].X(),
+                                 this->pts[1].X()) + _epsilon &&
+             _pt.X() >= std::min(this->pts[0].X(),
+                                 this->pts[1].X()) - _epsilon &&
+             _pt.Y() <= std::max(this->pts[0].Y(),
+                                 this->pts[1].Y()) + _epsilon &&
+             _pt.Y() >= std::min(this->pts[0].Y(),
+                                 this->pts[1].Y()) - _epsilon &&
+             _pt.Z() <= std::max(this->pts[0].Z(),
+                                 this->pts[1].Z()) + _epsilon &&
+             _pt.Z() >= std::min(this->pts[0].Z(),
+                                 this->pts[1].Z()) - _epsilon;
+    }
 
-      /// \brief Stream extraction operator
-      /// \param[in] _out output stream
-      /// \param[in] _line Line3 to output
-      /// \return The stream
-      public: friend std::ostream &operator<<(
-                  std::ostream &_out, const Line3<T> &_line)
-      {
-        _out << _line[0] << " " << _line[1];
-        return _out;
-      }
+    /// \brief Equality operator.
+    /// \param[in] _line Line to compare for equality.
+    /// \return True if the given line is equal to this line
+    public: bool operator==(const Line3<T> &_line) const
+    {
+      return this->pts[0] == _line[0] && this->pts[1] == _line[1];
+    }
 
-      /// \brief Assignment operator
-      /// \param[in] _line a new value
-      /// \return this
-      public: Line3 &operator=(const Line3<T> &_line) = default;
+    /// \brief Inequality operator.
+    /// \param[in] _line Line to compare for inequality.
+    /// \return True if the given line is not to this line
+    public: bool operator!=(const Line3<T> &_line) const
+    {
+      return !(*this == _line);
+    }
 
-      /// \brief Vector for storing the start and end points of the line
-      private: math::Vector3<T> pts[2];
-    };
+    /// \brief Get the start or end point.
+    /// \param[in] _index 0 = start point, 1 = end point. The _index
+    /// parameter is clamped to the range [0, 1].
+    public: math::Vector3<T> operator[](const size_t _index) const
+    {
+      return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
+    }
 
-    typedef Line3<int> Line3i;
-    typedef Line3<double> Line3d;
-    typedef Line3<float> Line3f;
+    /// \brief Stream extraction operator
+    /// \param[in] _out output stream
+    /// \param[in] _line Line3 to output
+    /// \return The stream
+    public: friend std::ostream &operator<<(
+                std::ostream &_out, const Line3<T> &_line)
+    {
+      _out << _line[0] << " " << _line[1];
+      return _out;
     }
-  }
-}
-#endif
+
+    /// \brief Assignment operator
+    /// \param[in] _line a new value
+    /// \return this
+    public: Line3 &operator=(const Line3<T> &_line) = default;
+
+    /// \brief Vector for storing the start and end points of the line
+    private: math::Vector3<T> pts[2];
+  };
+
+  typedef Line3<int> Line3i;
+  typedef Line3<double> Line3d;
+  typedef Line3<float> Line3f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE_
+}  // namespace gz::math
+#endif  // GZ_MATH_LINE3_HH_
diff --git a/include/gz/math/MassMatrix3.hh b/include/gz/math/MassMatrix3.hh
index 40b7fb3fb..f28446a69 100644
--- a/include/gz/math/MassMatrix3.hh
+++ b/include/gz/math/MassMatrix3.hh
@@ -31,1154 +31,1151 @@
 #include "gz/math/Vector3.hh"
 #include "gz/math/Matrix3.hh"
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class MassMatrix3 MassMatrix3.hh gz/math/MassMatrix3.hh
+  /// \brief A class for inertial information about a rigid body
+  /// consisting of the scalar mass and a 3x3 symmetric moment
+  /// of inertia matrix stored as two Vector3's.
+  template<typename T>
+  class MassMatrix3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class MassMatrix3 MassMatrix3.hh gz/math/MassMatrix3.hh
-    /// \brief A class for inertial information about a rigid body
-    /// consisting of the scalar mass and a 3x3 symmetric moment
-    /// of inertia matrix stored as two Vector3's.
-    template<typename T>
-    class MassMatrix3
+    /// \brief Default Constructor, which inializes the mass and moments
+    /// to zero.
+    public: MassMatrix3() : mass(0)
+    {}
+
+    /// \brief Constructor.
+    /// \param[in] _mass Mass value in kg if using metric.
+    /// \param[in] _ixxyyzz Diagonal moments of inertia.
+    /// \param[in] _ixyxzyz Off-diagonal moments of inertia
+    public: MassMatrix3(const T &_mass,
+                        const Vector3<T> &_ixxyyzz,
+                        const Vector3<T> &_ixyxzyz)
+    : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
+    {}
+
+    /// \brief Copy constructor.
+    /// \param[in] _m MassMatrix3 element to copy
+    public: MassMatrix3(const MassMatrix3<T> &_m) = default;
+
+    /// \brief Destructor.
+    public: ~MassMatrix3() = default;
+
+    /// \brief Set the mass.
+    /// \param[in] _m New mass value.
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetMass(const T &_m)
     {
-      /// \brief Default Constructor, which inializes the mass and moments
-      /// to zero.
-      public: MassMatrix3() : mass(0)
-      {}
-
-      /// \brief Constructor.
-      /// \param[in] _mass Mass value in kg if using metric.
-      /// \param[in] _ixxyyzz Diagonal moments of inertia.
-      /// \param[in] _ixyxzyz Off-diagonal moments of inertia
-      public: MassMatrix3(const T &_mass,
-                          const Vector3<T> &_ixxyyzz,
-                          const Vector3<T> &_ixyxzyz)
-      : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
-      {}
-
-      /// \brief Copy constructor.
-      /// \param[in] _m MassMatrix3 element to copy
-      public: MassMatrix3(const MassMatrix3<T> &_m) = default;
-
-      /// \brief Destructor.
-      public: ~MassMatrix3() = default;
-
-      /// \brief Set the mass.
-      /// \param[in] _m New mass value.
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetMass(const T &_m)
-      {
-        this->mass = _m;
-        return this->IsValid();
-      }
+      this->mass = _m;
+      return this->IsValid();
+    }
 
-      /// \brief Get the mass
-      /// \return The mass value
-      public: T Mass() const
-      {
-        return this->mass;
-      }
+    /// \brief Get the mass
+    /// \return The mass value
+    public: T Mass() const
+    {
+      return this->mass;
+    }
 
-      /// \brief Set the moment of inertia matrix.
-      /// \param[in] _ixx X second moment of inertia (MOI) about x axis.
-      /// \param[in] _iyy Y second moment of inertia about y axis.
-      /// \param[in] _izz Z second moment of inertia about z axis.
-      /// \param[in] _ixy XY inertia.
-      /// \param[in] _ixz XZ inertia.
-      /// \param[in] _iyz YZ inertia.
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetInertiaMatrix(
-                  const T &_ixx, const T &_iyy, const T &_izz,
-                  const T &_ixy, const T &_ixz, const T &_iyz)
-      {
-        this->Ixxyyzz.Set(_ixx, _iyy, _izz);
-        this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
-        return this->IsValid();
-      }
+    /// \brief Set the moment of inertia matrix.
+    /// \param[in] _ixx X second moment of inertia (MOI) about x axis.
+    /// \param[in] _iyy Y second moment of inertia about y axis.
+    /// \param[in] _izz Z second moment of inertia about z axis.
+    /// \param[in] _ixy XY inertia.
+    /// \param[in] _ixz XZ inertia.
+    /// \param[in] _iyz YZ inertia.
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetInertiaMatrix(
+                const T &_ixx, const T &_iyy, const T &_izz,
+                const T &_ixy, const T &_ixz, const T &_iyz)
+    {
+      this->Ixxyyzz.Set(_ixx, _iyy, _izz);
+      this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
+      return this->IsValid();
+    }
 
-      /// \brief Get the diagonal moments of inertia (Ixx, Iyy, Izz).
-      /// \return The diagonal moments.
-      public: Vector3<T> DiagonalMoments() const
-      {
-        return this->Ixxyyzz;
-      }
+    /// \brief Get the diagonal moments of inertia (Ixx, Iyy, Izz).
+    /// \return The diagonal moments.
+    public: Vector3<T> DiagonalMoments() const
+    {
+      return this->Ixxyyzz;
+    }
 
-      /// \brief Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
-      /// \return The off-diagonal moments of inertia.
-      public: Vector3<T> OffDiagonalMoments() const
-      {
-        return this->Ixyxzyz;
-      }
+    /// \brief Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
+    /// \return The off-diagonal moments of inertia.
+    public: Vector3<T> OffDiagonalMoments() const
+    {
+      return this->Ixyxzyz;
+    }
 
-      /// \brief Set the diagonal moments of inertia (Ixx, Iyy, Izz).
-      /// \param[in] _ixxyyzz diagonal moments of inertia
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetDiagonalMoments(const Vector3<T> &_ixxyyzz)
-      {
-        this->Ixxyyzz = _ixxyyzz;
-        return this->IsValid();
-      }
+    /// \brief Set the diagonal moments of inertia (Ixx, Iyy, Izz).
+    /// \param[in] _ixxyyzz diagonal moments of inertia
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetDiagonalMoments(const Vector3<T> &_ixxyyzz)
+    {
+      this->Ixxyyzz = _ixxyyzz;
+      return this->IsValid();
+    }
 
-      /// \brief Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
-      /// \param[in] _ixyxzyz off-diagonal moments of inertia
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetOffDiagonalMoments(const Vector3<T> &_ixyxzyz)
-      {
-        this->Ixyxzyz = _ixyxzyz;
-        return this->IsValid();
-      }
+    /// \brief Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
+    /// \param[in] _ixyxzyz off-diagonal moments of inertia
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetOffDiagonalMoments(const Vector3<T> &_ixyxzyz)
+    {
+      this->Ixyxzyz = _ixyxzyz;
+      return this->IsValid();
+    }
 
-      /// \brief Get IXX
-      /// \return IXX value
-      public: T Ixx() const
-      {
-        return this->Ixxyyzz[0];
-      }
+    /// \brief Get IXX
+    /// \return IXX value
+    public: T Ixx() const
+    {
+      return this->Ixxyyzz[0];
+    }
 
-      /// \brief Get IYY
-      /// \return IYY value
-      public: T Iyy() const
-      {
-        return this->Ixxyyzz[1];
-      }
+    /// \brief Get IYY
+    /// \return IYY value
+    public: T Iyy() const
+    {
+      return this->Ixxyyzz[1];
+    }
 
-      /// \brief Get IZZ
-      /// \return IZZ value
-      public: T Izz() const
-      {
-        return this->Ixxyyzz[2];
-      }
+    /// \brief Get IZZ
+    /// \return IZZ value
+    public: T Izz() const
+    {
+      return this->Ixxyyzz[2];
+    }
 
-      /// \brief Get IXY
-      /// \return IXY value
-      public: T Ixy() const
-      {
-        return this->Ixyxzyz[0];
-      }
+    /// \brief Get IXY
+    /// \return IXY value
+    public: T Ixy() const
+    {
+      return this->Ixyxzyz[0];
+    }
 
-      /// \brief Get IXZ
-      /// \return IXZ value
-      public: T Ixz() const
-      {
-        return this->Ixyxzyz[1];
-      }
+    /// \brief Get IXZ
+    /// \return IXZ value
+    public: T Ixz() const
+    {
+      return this->Ixyxzyz[1];
+    }
 
-      /// \brief Get IYZ
-      /// \return IYZ value
-      public: T Iyz() const
-      {
-        return this->Ixyxzyz[2];
-      }
+    /// \brief Get IYZ
+    /// \return IYZ value
+    public: T Iyz() const
+    {
+      return this->Ixyxzyz[2];
+    }
 
-      /// \brief Set IXX
-      /// \param[in] _v IXX value
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetIxx(const T &_v)
-      {
-        this->Ixxyyzz.X(_v);
-        return this->IsValid();
-      }
+    /// \brief Set IXX
+    /// \param[in] _v IXX value
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetIxx(const T &_v)
+    {
+      this->Ixxyyzz.X(_v);
+      return this->IsValid();
+    }
 
-      /// \brief Set IYY
-      /// \param[in] _v IYY value
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetIyy(const T &_v)
-      {
-        this->Ixxyyzz.Y(_v);
-        return this->IsValid();
-      }
+    /// \brief Set IYY
+    /// \param[in] _v IYY value
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetIyy(const T &_v)
+    {
+      this->Ixxyyzz.Y(_v);
+      return this->IsValid();
+    }
 
-      /// \brief Set IZZ
-      /// \param[in] _v IZZ value
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetIzz(const T &_v)
-      {
-        this->Ixxyyzz.Z(_v);
-        return this->IsValid();
-      }
+    /// \brief Set IZZ
+    /// \param[in] _v IZZ value
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetIzz(const T &_v)
+    {
+      this->Ixxyyzz.Z(_v);
+      return this->IsValid();
+    }
 
-      /// \brief Set IXY
-      /// \param[in] _v IXY value
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetIxy(const T &_v)
-      {
-        this->Ixyxzyz.X(_v);
-        return this->IsValid();
-      }
+    /// \brief Set IXY
+    /// \param[in] _v IXY value
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetIxy(const T &_v)
+    {
+      this->Ixyxzyz.X(_v);
+      return this->IsValid();
+    }
 
-      /// \brief Set IXZ
-      /// \param[in] _v IXZ value
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetIxz(const T &_v)
-      {
-        this->Ixyxzyz.Y(_v);
-        return this->IsValid();
-      }
+    /// \brief Set IXZ
+    /// \param[in] _v IXZ value
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetIxz(const T &_v)
+    {
+      this->Ixyxzyz.Y(_v);
+      return this->IsValid();
+    }
 
-      /// \brief Set IYZ
-      /// \param[in] _v IYZ value
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetIyz(const T &_v)
-      {
-        this->Ixyxzyz.Z(_v);
-        return this->IsValid();
-      }
+    /// \brief Set IYZ
+    /// \param[in] _v IYZ value
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetIyz(const T &_v)
+    {
+      this->Ixyxzyz.Z(_v);
+      return this->IsValid();
+    }
 
-      /// \brief returns Moments of Inertia as a Matrix3
-      /// \return Moments of Inertia as a Matrix3
-      public: Matrix3<T> Moi() const
-      {
-        return Matrix3<T>(
-          this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
-          this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
-          this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
-      }
+    /// \brief returns Moments of Inertia as a Matrix3
+    /// \return Moments of Inertia as a Matrix3
+    public: Matrix3<T> Moi() const
+    {
+      return Matrix3<T>(
+        this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
+        this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
+        this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
+    }
 
-      /// \brief Sets Moments of Inertia (MOI) from a Matrix3.
-      /// Symmetric component of input matrix is used by averaging
-      /// off-axis terms.
-      /// \param[in] _moi Moments of Inertia as a Matrix3
-      /// \return True if the MassMatrix3 is valid.
-      public: bool SetMoi(const Matrix3<T> &_moi)
-      {
-        this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
-        this->Ixyxzyz.Set(
-          0.5*(_moi(0, 1) + _moi(1, 0)),
-          0.5*(_moi(0, 2) + _moi(2, 0)),
-          0.5*(_moi(1, 2) + _moi(2, 1)));
-        return this->IsValid();
-      }
+    /// \brief Sets Moments of Inertia (MOI) from a Matrix3.
+    /// Symmetric component of input matrix is used by averaging
+    /// off-axis terms.
+    /// \param[in] _moi Moments of Inertia as a Matrix3
+    /// \return True if the MassMatrix3 is valid.
+    public: bool SetMoi(const Matrix3<T> &_moi)
+    {
+      this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
+      this->Ixyxzyz.Set(
+        0.5*(_moi(0, 1) + _moi(1, 0)),
+        0.5*(_moi(0, 2) + _moi(2, 0)),
+        0.5*(_moi(1, 2) + _moi(2, 1)));
+      return this->IsValid();
+    }
 
-      /// \brief Equal operator.
-      /// \param[in] _massMatrix MassMatrix3 to copy.
-      /// \return Reference to this object.
-      public: MassMatrix3 &operator=(const MassMatrix3<T> &_massMatrix)
-        = default;
-
-      /// \brief Equality comparison operator.
-      /// \param[in] _m MassMatrix3 to copy.
-      /// \return true if each component is equal within a default tolerance,
-      /// false otherwise
-      public: bool operator==(const MassMatrix3<T> &_m) const
-      {
-        return equal<T>(this->mass, _m.Mass()) &&
-               (this->Ixxyyzz == _m.DiagonalMoments()) &&
-               (this->Ixyxzyz == _m.OffDiagonalMoments());
-      }
+    /// \brief Equal operator.
+    /// \param[in] _massMatrix MassMatrix3 to copy.
+    /// \return Reference to this object.
+    public: MassMatrix3 &operator=(const MassMatrix3<T> &_massMatrix)
+      = default;
+
+    /// \brief Equality comparison operator.
+    /// \param[in] _m MassMatrix3 to copy.
+    /// \return true if each component is equal within a default tolerance,
+    /// false otherwise
+    public: bool operator==(const MassMatrix3<T> &_m) const
+    {
+      return equal<T>(this->mass, _m.Mass()) &&
+             (this->Ixxyyzz == _m.DiagonalMoments()) &&
+             (this->Ixyxzyz == _m.OffDiagonalMoments());
+    }
 
-      /// \brief Inequality test operator
-      /// \param[in] _m MassMatrix3<T> to test
-      /// \return True if not equal (using the default tolerance of 1e-6)
-      public: bool operator!=(const MassMatrix3<T> &_m) const
-      {
-        return !(*this == _m);
-      }
+    /// \brief Inequality test operator
+    /// \param[in] _m MassMatrix3<T> to test
+    /// \return True if not equal (using the default tolerance of 1e-6)
+    public: bool operator!=(const MassMatrix3<T> &_m) const
+    {
+      return !(*this == _m);
+    }
 
-      /// \brief Verify that inertia values are positive semidefinite
-      ///
-      /// \param[in] _tolerance The amount of relative error to accept when
-      /// checking whether this MassMatrix3 has a valid mass and moment
-      /// of inertia. Refer to Epsilon() for a description of _tolerance.
-      ///
-      /// \return True if mass is nonnegative and moment of inertia matrix
-      /// is positive semidefinite. The following is how the return value is
-      /// calculated
-      ///
-      /// \code
-      /// const T epsilon = this->Epsilon(_tolerance);
-      /// return (this->mass + epsilon >= 0) &&
-      ///         (this->IXX() + epsilon  >= 0) &&
-      ///         (this->IXX() * this->IYY() - std::pow(this->IXY(), 2) +
-      ///          epsilon >= 0) &&
-      ///         (this->Moi().Determinant() + epsilon >= 0);
-      /// \endcode
-      ///
-      public: bool IsNearPositive(const T _tolerance =
-                  GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
-      {
-        const T epsilon = this->Epsilon(_tolerance);
-
-        // Check if mass and determinants of all upper left submatrices
-        // of moment of inertia matrix are positive
-        return (this->mass >= 0) &&
-               (this->Ixx() + epsilon >= 0) &&
-               (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
-                epsilon >= 0) &&
-               (this->Moi().Determinant() + epsilon >= 0);
-      }
+    /// \brief Verify that inertia values are positive semidefinite
+    ///
+    /// \param[in] _tolerance The amount of relative error to accept when
+    /// checking whether this MassMatrix3 has a valid mass and moment
+    /// of inertia. Refer to Epsilon() for a description of _tolerance.
+    ///
+    /// \return True if mass is nonnegative and moment of inertia matrix
+    /// is positive semidefinite. The following is how the return value is
+    /// calculated
+    ///
+    /// \code
+    /// const T epsilon = this->Epsilon(_tolerance);
+    /// return (this->mass + epsilon >= 0) &&
+    ///         (this->IXX() + epsilon  >= 0) &&
+    ///         (this->IXX() * this->IYY() - std::pow(this->IXY(), 2) +
+    ///          epsilon >= 0) &&
+    ///         (this->Moi().Determinant() + epsilon >= 0);
+    /// \endcode
+    ///
+    public: bool IsNearPositive(const T _tolerance =
+                GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
+    {
+      const T epsilon = this->Epsilon(_tolerance);
+
+      // Check if mass and determinants of all upper left submatrices
+      // of moment of inertia matrix are positive
+      return (this->mass >= 0) &&
+             (this->Ixx() + epsilon >= 0) &&
+             (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
+              epsilon >= 0) &&
+             (this->Moi().Determinant() + epsilon >= 0);
+    }
 
-      /// \brief Verify that inertia values are positive definite
-      ///
-      /// \param[in] _tolerance The amount of error to accept when
-      /// checking whether this MassMatrix3 has a valid mass and moment
-      /// of inertia. Refer to Epsilon() for a description of _tolerance.
-      ///
-      /// \return True if mass is positive and moment of inertia matrix
-      /// is positive definite. The following is how the return value is
-      /// calculated
-      ///
-      /// \code
-      /// const T epsilon = this->Epsilon(_tolerance);
-      /// return (this->mass + epsilon > 0) &&
-      ///         (this->IXX() + epsilon  > 0) &&
-      ///         (this->IXX() * this->IYY() - std::pow(this->IXY(), 2) +
-      ///          epsilon > 0) &&
-      ///         (this->Moi().Determinant() + epsilon > 0);
-      /// \endcode
-      ///
-      public: bool IsPositive(const T _tolerance =
-                  GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
-      {
-        const T epsilon = this->Epsilon(_tolerance);
-
-        // Check if mass and determinants of all upper left submatrices
-        // of moment of inertia matrix are positive
-        return (this->mass > 0) &&
-               (this->Ixx() + epsilon > 0) &&
-               (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
-                epsilon > 0) &&
-               (this->Moi().Determinant() + epsilon > 0);
-      }
+    /// \brief Verify that inertia values are positive definite
+    ///
+    /// \param[in] _tolerance The amount of error to accept when
+    /// checking whether this MassMatrix3 has a valid mass and moment
+    /// of inertia. Refer to Epsilon() for a description of _tolerance.
+    ///
+    /// \return True if mass is positive and moment of inertia matrix
+    /// is positive definite. The following is how the return value is
+    /// calculated
+    ///
+    /// \code
+    /// const T epsilon = this->Epsilon(_tolerance);
+    /// return (this->mass + epsilon > 0) &&
+    ///         (this->IXX() + epsilon  > 0) &&
+    ///         (this->IXX() * this->IYY() - std::pow(this->IXY(), 2) +
+    ///          epsilon > 0) &&
+    ///         (this->Moi().Determinant() + epsilon > 0);
+    /// \endcode
+    ///
+    public: bool IsPositive(const T _tolerance =
+                GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
+    {
+      const T epsilon = this->Epsilon(_tolerance);
+
+      // Check if mass and determinants of all upper left submatrices
+      // of moment of inertia matrix are positive
+      return (this->mass > 0) &&
+             (this->Ixx() + epsilon > 0) &&
+             (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
+              epsilon > 0) &&
+             (this->Moi().Determinant() + epsilon > 0);
+    }
 
-      ///
-      /// \brief \copybrief Epsilon(const Vector3<T>&,const T)
-      ///
-      /// \param[in] _tolerance A factor that is used to adjust the return
-      /// value. A value of zero will cause the return value to be zero.
-      /// A good value is 10, which is also the
-      /// MASSMATRIX3_DEFAULT_TOLERANCE.
-      public: T Epsilon(const T _tolerance =
-                  GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
-      {
-        return Epsilon(this->DiagonalMoments(), _tolerance);
-      }
+    ///
+    /// \brief \copybrief Epsilon(const Vector3<T>&,const T)
+    ///
+    /// \param[in] _tolerance A factor that is used to adjust the return
+    /// value. A value of zero will cause the return value to be zero.
+    /// A good value is 10, which is also the
+    /// MASSMATRIX3_DEFAULT_TOLERANCE.
+    public: T Epsilon(const T _tolerance =
+                GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
+    {
+      return Epsilon(this->DiagonalMoments(), _tolerance);
+    }
 
-      /// \brief Get an epsilon value that represents the amount of
-      /// acceptable error in a MassMatrix3. The epsilon value
-      /// is related to machine precision multiplied by the largest possible
-      /// moment of inertia.
-      ///
-      /// This function is used by IsValid(), IsNearPositive(), IsPositive(),
-      /// and ValidMoments().
-      ///
-      /// \param[in] _moments Principal moments of inertia.
-      /// \param[in] _tolerance A factor that is used to adjust the return
-      /// value. A value of zero will cause the return value to be zero.
-      /// A good value is 10, which is also the
-      /// MASSMATRIX3_DEFAULT_TOLERANCE.
-      ///
-      /// \return The epsilon value computed using:
-      ///
-      /// \code
-      /// T maxPossibleMoI = 0.5 * std::abs(_moments.Sum());
-      /// return _tolerance *
-      ///   std::numeric_limits<T>::epsilon() * maxPossibleMoI;
-      /// \endcode
-      public: static T Epsilon(const Vector3<T> &_moments,
-                  const T _tolerance =
-                  GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
-      {
-        // The following was borrowed heavily from:
-        // https://github.com/RobotLocomotion/drake/blob/v0.27.0/multibody/tree/rotational_inertia.h
+    /// \brief Get an epsilon value that represents the amount of
+    /// acceptable error in a MassMatrix3. The epsilon value
+    /// is related to machine precision multiplied by the largest possible
+    /// moment of inertia.
+    ///
+    /// This function is used by IsValid(), IsNearPositive(), IsPositive(),
+    /// and ValidMoments().
+    ///
+    /// \param[in] _moments Principal moments of inertia.
+    /// \param[in] _tolerance A factor that is used to adjust the return
+    /// value. A value of zero will cause the return value to be zero.
+    /// A good value is 10, which is also the
+    /// MASSMATRIX3_DEFAULT_TOLERANCE.
+    ///
+    /// \return The epsilon value computed using:
+    ///
+    /// \code
+    /// T maxPossibleMoI = 0.5 * std::abs(_moments.Sum());
+    /// return _tolerance *
+    ///   std::numeric_limits<T>::epsilon() * maxPossibleMoI;
+    /// \endcode
+    public: static T Epsilon(const Vector3<T> &_moments,
+                const T _tolerance =
+                GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
+    {
+      // The following was borrowed heavily from:
+      // https://github.com/RobotLocomotion/drake/blob/v0.27.0/multibody/tree/rotational_inertia.h
+
+      // Compute the maximum possible moment of inertia, which will be
+      // used to compute whether the moments are valid.
+      //
+      // The maximum moment of inertia is bounded by:
+      // trace / 3 <= maxPossibleMoi <= trace / 2.
+      //
+      // The trace of a matrix is the sum of the coefficients on the
+      // main diagonal. For a mass matrix, this is equal to
+      // ixx + iyy + izz, or _moments.Sum() for this function's
+      // implementation.
+      //
+      // It is okay if maxPossibleMoi == zero.
+      T maxPossibleMoI = 0.5 * std::abs(_moments.Sum());
+
+      // In order to check validity of the moments we need to use an
+      // epsilon value that is related to machine precision
+      // multiplied by the largest possible moment of inertia.
+      return _tolerance *
+        std::numeric_limits<T>::epsilon() * maxPossibleMoI;
+    }
 
-        // Compute the maximum possible moment of inertia, which will be
-        // used to compute whether the moments are valid.
-        //
-        // The maximum moment of inertia is bounded by:
-        // trace / 3 <= maxPossibleMoi <= trace / 2.
-        //
-        // The trace of a matrix is the sum of the coefficients on the
-        // main diagonal. For a mass matrix, this is equal to
-        // ixx + iyy + izz, or _moments.Sum() for this function's
-        // implementation.
-        //
-        // It is okay if maxPossibleMoi == zero.
-        T maxPossibleMoI = 0.5 * std::abs(_moments.Sum());
-
-        // In order to check validity of the moments we need to use an
-        // epsilon value that is related to machine precision
-        // multiplied by the largest possible moment of inertia.
-        return _tolerance *
-          std::numeric_limits<T>::epsilon() * maxPossibleMoI;
-      }
+    /// \brief Verify that inertia values are positive semi-definite
+    /// and satisfy the triangle inequality.
+    ///
+    /// \param[in] _tolerance The amount of error to accept when
+    /// checking whether the MassMatrix3 has a valid mass and moment
+    /// of inertia. This value is passed on to IsNearPositive() and
+    /// ValidMoments(), which in turn pass the tolerance value to
+    /// Epsilon(). Refer to Epsilon() for a description of _tolerance.
+    ///
+    /// \return True if IsNearPositive(_tolerance) and
+    /// ValidMoments(this->PrincipalMoments(), _tolerance) both return true.
+    public: bool IsValid(const T _tolerance =
+                GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
+    {
+      return this->IsNearPositive(_tolerance) &&
+             ValidMoments(this->PrincipalMoments(), _tolerance);
+    }
 
-      /// \brief Verify that inertia values are positive semi-definite
-      /// and satisfy the triangle inequality.
-      ///
-      /// \param[in] _tolerance The amount of error to accept when
-      /// checking whether the MassMatrix3 has a valid mass and moment
-      /// of inertia. This value is passed on to IsNearPositive() and
-      /// ValidMoments(), which in turn pass the tolerance value to
-      /// Epsilon(). Refer to Epsilon() for a description of _tolerance.
-      ///
-      /// \return True if IsNearPositive(_tolerance) and
-      /// ValidMoments(this->PrincipalMoments(), _tolerance) both return true.
-      public: bool IsValid(const T _tolerance =
-                  GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
+    /// \brief Verify that principal moments are positive
+    /// and satisfy the triangle inequality.
+    /// \param[in] _moments Principal moments of inertia.
+    /// \param[in] _tolerance The amount of error to accept when
+    /// checking whether the moments are positive and satisfy the triangle
+    /// inequality. Refer to Epsilon() for a description of _tolerance.
+    /// \return True if moments of inertia are positive
+    /// and satisfy the triangle inequality. The following is how the
+    /// return value is calculated.
+    ///
+    /// \code
+    /// T epsilon = this->Epsilon(_tolerance);
+    ///
+    /// return _moments[0] + epsilon >= 0 &&
+    ///   _moments[1] + epsilon >= 0 &&
+    ///   _moments[2] + epsilon >= 0 &&
+    ///   _moments[0] + _moments[1] + epsilon >= _moments[2] &&
+    ///   _moments[1] + _moments[2] + epsilon >= _moments[0] &&
+    ///   _moments[2] + _moments[0] + epsilon >= _moments[1];
+    /// \endcode
+    public: static bool ValidMoments(const Vector3<T> &_moments,
+                const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
+            {
+              T epsilon = Epsilon(_moments, _tolerance);
+
+              return _moments[0] + epsilon >= 0 &&
+                     _moments[1] + epsilon >= 0 &&
+                     _moments[2] + epsilon >= 0 &&
+                     _moments[0] + _moments[1] + epsilon >= _moments[2] &&
+                     _moments[1] + _moments[2] + epsilon >= _moments[0] &&
+                     _moments[2] + _moments[0] + epsilon >= _moments[1];
+            }
+
+    /// \brief Compute principal moments of inertia,
+    /// which are the eigenvalues of the moment of inertia matrix.
+    /// \param[in] _tol Relative tolerance given by absolute value
+    /// of _tol.
+    /// Negative values of _tol are interpreted as a flag that
+    /// causes principal moments to always be sorted from smallest
+    /// to largest.
+    /// \return Principal moments of inertia.
+    /// If the matrix is already diagonal and _tol is positive,
+    /// they are returned in the existing order.
+    /// Otherwise, the moments are sorted from smallest to largest.
+    public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
+    {
+      // Compute tolerance relative to maximum value of inertia diagonal
+      T tol = _tol * this->Ixxyyzz.Max();
+      if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
       {
-        return this->IsNearPositive(_tolerance) &&
-               ValidMoments(this->PrincipalMoments(), _tolerance);
+        // Matrix is already diagonalized, return diagonal moments
+        return this->Ixxyyzz;
       }
 
-      /// \brief Verify that principal moments are positive
-      /// and satisfy the triangle inequality.
-      /// \param[in] _moments Principal moments of inertia.
-      /// \param[in] _tolerance The amount of error to accept when
-      /// checking whether the moments are positive and satisfy the triangle
-      /// inequality. Refer to Epsilon() for a description of _tolerance.
-      /// \return True if moments of inertia are positive
-      /// and satisfy the triangle inequality. The following is how the
-      /// return value is calculated.
-      ///
-      /// \code
-      /// T epsilon = this->Epsilon(_tolerance);
-      ///
-      /// return _moments[0] + epsilon >= 0 &&
-      ///   _moments[1] + epsilon >= 0 &&
-      ///   _moments[2] + epsilon >= 0 &&
-      ///   _moments[0] + _moments[1] + epsilon >= _moments[2] &&
-      ///   _moments[1] + _moments[2] + epsilon >= _moments[0] &&
-      ///   _moments[2] + _moments[0] + epsilon >= _moments[1];
-      /// \endcode
-      public: static bool ValidMoments(const Vector3<T> &_moments,
-                  const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
-              {
-                T epsilon = Epsilon(_moments, _tolerance);
-
-                return _moments[0] + epsilon >= 0 &&
-                       _moments[1] + epsilon >= 0 &&
-                       _moments[2] + epsilon >= 0 &&
-                       _moments[0] + _moments[1] + epsilon >= _moments[2] &&
-                       _moments[1] + _moments[2] + epsilon >= _moments[0] &&
-                       _moments[2] + _moments[0] + epsilon >= _moments[1];
-              }
-
-      /// \brief Compute principal moments of inertia,
-      /// which are the eigenvalues of the moment of inertia matrix.
-      /// \param[in] _tol Relative tolerance given by absolute value
-      /// of _tol.
-      /// Negative values of _tol are interpreted as a flag that
-      /// causes principal moments to always be sorted from smallest
-      /// to largest.
-      /// \return Principal moments of inertia.
-      /// If the matrix is already diagonal and _tol is positive,
-      /// they are returned in the existing order.
-      /// Otherwise, the moments are sorted from smallest to largest.
-      public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
-      {
-        // Compute tolerance relative to maximum value of inertia diagonal
-        T tol = _tol * this->Ixxyyzz.Max();
-        if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
-        {
-          // Matrix is already diagonalized, return diagonal moments
-          return this->Ixxyyzz;
-        }
+      // Algorithm based on http://arxiv.org/abs/1306.6291v4
+      // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
+      // Matrix, by Maarten Kronenburg
+      Vector3<T> Id(this->Ixxyyzz);
+      Vector3<T> Ip(this->Ixyxzyz);
+      // b = Ixx + Iyy + Izz
+      T b = Id.Sum();
+      // c = Ixx*Iyy - Ixy^2  +  Ixx*Izz - Ixz^2  +  Iyy*Izz - Iyz^2
+      T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
+          + Id[0]*Id[2] - std::pow(Ip[1], 2)
+          + Id[1]*Id[2] - std::pow(Ip[2], 2);
+      // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
+      T d = Id[0]*std::pow(Ip[2], 2)
+          + Id[1]*std::pow(Ip[1], 2)
+          + Id[2]*std::pow(Ip[0], 2)
+          - Id[0]*Id[1]*Id[2]
+          - 2*Ip[0]*Ip[1]*Ip[2];
+      // p = b^2 - 3c
+      T p = std::pow(b, 2) - 3*c;
+
+      // At this point, it is important to check that p is not close
+      //  to zero, since its inverse is used to compute delta.
+      // In equation 4.7, p is expressed as a sum of squares
+      //  that is only zero if the matrix is diagonal
+      //  with identical principal moments.
+      // This check has no test coverage, since this function returns
+      //  immediately if a diagonal matrix is detected.
+      if (p < std::pow(tol, 2))
+        return b / 3.0 * Vector3<T>::One;
+
+      // q = 2b^3 - 9bc - 27d
+      T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
+
+      // delta = acos(q / (2 * p^(1.5)))
+      // additionally clamp the argument to [-1,1]
+      T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
+
+      // sort the moments from smallest to largest
+      T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
+      T moment1 = (b + 2*sqrt(p) * cos((delta + 2*GZ_PI)/3.0)) / 3.0;
+      T moment2 = (b + 2*sqrt(p) * cos((delta - 2*GZ_PI)/3.0)) / 3.0;
+      sort3(moment0, moment1, moment2);
+      return Vector3<T>(moment0, moment1, moment2);
+    }
 
-        // Algorithm based on http://arxiv.org/abs/1306.6291v4
-        // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
-        // Matrix, by Maarten Kronenburg
-        Vector3<T> Id(this->Ixxyyzz);
-        Vector3<T> Ip(this->Ixyxzyz);
-        // b = Ixx + Iyy + Izz
-        T b = Id.Sum();
-        // c = Ixx*Iyy - Ixy^2  +  Ixx*Izz - Ixz^2  +  Iyy*Izz - Iyz^2
-        T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
-            + Id[0]*Id[2] - std::pow(Ip[1], 2)
-            + Id[1]*Id[2] - std::pow(Ip[2], 2);
-        // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
-        T d = Id[0]*std::pow(Ip[2], 2)
-            + Id[1]*std::pow(Ip[1], 2)
-            + Id[2]*std::pow(Ip[0], 2)
-            - Id[0]*Id[1]*Id[2]
-            - 2*Ip[0]*Ip[1]*Ip[2];
-        // p = b^2 - 3c
-        T p = std::pow(b, 2) - 3*c;
-
-        // At this point, it is important to check that p is not close
-        //  to zero, since its inverse is used to compute delta.
-        // In equation 4.7, p is expressed as a sum of squares
-        //  that is only zero if the matrix is diagonal
-        //  with identical principal moments.
-        // This check has no test coverage, since this function returns
-        //  immediately if a diagonal matrix is detected.
-        if (p < std::pow(tol, 2))
-          return b / 3.0 * Vector3<T>::One;
-
-        // q = 2b^3 - 9bc - 27d
-        T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
-
-        // delta = acos(q / (2 * p^(1.5)))
-        // additionally clamp the argument to [-1,1]
-        T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
-
-        // sort the moments from smallest to largest
-        T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
-        T moment1 = (b + 2*sqrt(p) * cos((delta + 2*GZ_PI)/3.0)) / 3.0;
-        T moment2 = (b + 2*sqrt(p) * cos((delta - 2*GZ_PI)/3.0)) / 3.0;
-        sort3(moment0, moment1, moment2);
-        return Vector3<T>(moment0, moment1, moment2);
+    /// \brief Compute rotational offset of principal axes.
+    /// \param[in] _tol Relative tolerance given by absolute value
+    /// of _tol.
+    /// Negative values of _tol are interpreted as a flag that
+    /// causes principal moments to always be sorted from smallest
+    /// to largest.
+    /// \return Quaternion representing rotational offset of principal axes.
+    /// With a rotation matrix constructed from this quaternion R(q)
+    /// and a diagonal matrix L with principal moments on the diagonal,
+    /// the original moment of inertia matrix MOI can be reconstructed
+    /// with MOI = R(q).Transpose() * L * R(q)
+    public: Quaternion<T> PrincipalAxesOffset(const T _tol = 1e-6) const
+    {
+      Vector3<T> moments = this->PrincipalMoments(_tol);
+      // Compute tolerance relative to maximum value of inertia diagonal
+      T tol = _tol * this->Ixxyyzz.Max();
+      if (moments.Equal(this->Ixxyyzz, tol) ||
+          (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
+           math::equal<T>(moments[0], moments[2], std::abs(tol))))
+      {
+        // matrix is already aligned with principal axes
+        // or all three moments are approximately equal
+        // return identity rotation
+        return Quaternion<T>::Identity;
       }
 
-      /// \brief Compute rotational offset of principal axes.
-      /// \param[in] _tol Relative tolerance given by absolute value
-      /// of _tol.
-      /// Negative values of _tol are interpreted as a flag that
-      /// causes principal moments to always be sorted from smallest
-      /// to largest.
-      /// \return Quaternion representing rotational offset of principal axes.
-      /// With a rotation matrix constructed from this quaternion R(q)
-      /// and a diagonal matrix L with principal moments on the diagonal,
-      /// the original moment of inertia matrix MOI can be reconstructed
-      /// with MOI = R(q).Transpose() * L * R(q)
-      public: Quaternion<T> PrincipalAxesOffset(const T _tol = 1e-6) const
+      // Algorithm based on http://arxiv.org/abs/1306.6291v4
+      // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
+      // Matrix, by Maarten Kronenburg
+      // A real, symmetric matrix can be diagonalized by an orthogonal matrix
+      // (due to the finite-dimensional spectral theorem
+      // https://en.wikipedia.org/wiki/Spectral_theorem
+      // #Hermitian_maps_and_Hermitian_matrices ),
+      // and another name for orthogonal matrix is rotation matrix.
+      // Section 5 of the paper shows how to compute Euler angles
+      // phi1, phi2, and phi3 that map to a rotation matrix.
+      // In some cases, there are multiple possible values for a given angle,
+      // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc.
+      // Similar variable names are used to the paper so that the paper
+      // can be used as an additional reference.
+
+      // f1, f2 defined in equations 5.5, 5.6
+      Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
+      Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
+                 -2*this->Ixyxzyz[2]);
+
+      // Check if two moments are equal, since different equations are used
+      // The moments vector is already sorted, so just check adjacent values.
+      Vector2<T> momentsDiff(moments[0] - moments[1],
+                             moments[1] - moments[2]);
+
+      // index of unequal moment
+      int unequalMoment = -1;
+      if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
+        unequalMoment = 2;
+      else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
+        unequalMoment = 0;
+
+      if (unequalMoment >= 0)
       {
-        Vector3<T> moments = this->PrincipalMoments(_tol);
-        // Compute tolerance relative to maximum value of inertia diagonal
-        T tol = _tol * this->Ixxyyzz.Max();
-        if (moments.Equal(this->Ixxyyzz, tol) ||
-            (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
-             math::equal<T>(moments[0], moments[2], std::abs(tol))))
-        {
-          // matrix is already aligned with principal axes
-          // or all three moments are approximately equal
-          // return identity rotation
-          return Quaternion<T>::Identity;
-        }
-
-        // Algorithm based on http://arxiv.org/abs/1306.6291v4
-        // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
-        // Matrix, by Maarten Kronenburg
-        // A real, symmetric matrix can be diagonalized by an orthogonal matrix
-        // (due to the finite-dimensional spectral theorem
-        // https://en.wikipedia.org/wiki/Spectral_theorem
-        // #Hermitian_maps_and_Hermitian_matrices ),
-        // and another name for orthogonal matrix is rotation matrix.
-        // Section 5 of the paper shows how to compute Euler angles
-        // phi1, phi2, and phi3 that map to a rotation matrix.
-        // In some cases, there are multiple possible values for a given angle,
-        // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc.
-        // Similar variable names are used to the paper so that the paper
-        // can be used as an additional reference.
-
-        // f1, f2 defined in equations 5.5, 5.6
-        Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
-        Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
-                   -2*this->Ixyxzyz[2]);
-
-        // Check if two moments are equal, since different equations are used
-        // The moments vector is already sorted, so just check adjacent values.
-        Vector2<T> momentsDiff(moments[0] - moments[1],
-                               moments[1] - moments[2]);
-
-        // index of unequal moment
-        int unequalMoment = -1;
-        if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
-          unequalMoment = 2;
-        else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
-          unequalMoment = 0;
-
-        if (unequalMoment >= 0)
-        {
-          // moments[1] is the repeated value
-          // it is not equal to moments[unequalMoment]
-          // momentsDiff3 = lambda - lambda3
-          T momentsDiff3 = moments[1] - moments[unequalMoment];
-          // eq 5.21:
-          // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3)
-          // s >= 0 since A_11 is in range [lambda, lambda3]
-          T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
-          // set phi3 to zero for repeated moments (eq 5.23)
-          T phi3 = 0;
-          // phi2 = +- acos(sqrt(s))
-          // start with just the positive value
-          // also clamp the acos argument to prevent NaN's
-          T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
-
-          // The paper defines variables phi11 and phi12
-          // which are candidate values of angle phi1.
-          // phi12 is straightforward to compute as a function of f2 and g2.
-          // eq 5.25:
-          Vector2<T> g2(momentsDiff3 * s, 0);
-          // combining eq 5.12 and 5.14, and subtracting psi2
-          // instead of multiplying by its rotation matrix:
-          math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
-          phi12.Normalize();
-
-          // The paragraph prior to equation 5.16 describes how to choose
-          // the candidate value of phi1 based on the length
-          // of the f1 and f2 vectors.
-          // * When |f1| != 0 and |f2| != 0, then one should choose the
-          //   value of phi2 so that phi11 = phi12
-          // * When |f1| == 0 and f2 != 0, then phi1 = phi12
-          //   phi11 can be ignored, and either sign of phi2 can be used
-          // * The case of |f2| == 0 can be ignored at this point in the code
-          //   since having a repeated moment when |f2| == 0 implies that
-          //   the matrix is diagonal. But this function returns a unit
-          //   quaternion for diagonal matrices, so we can assume |f2| != 0
-          //   See MassMatrix3.ipynb for a more complete discussion.
-          //
-          // Since |f2| != 0, we only need to consider |f1|
-          // * |f1| == 0: phi1 = phi12
-          // * |f1| != 0: choose phi2 so that phi11 == phi12
-          // In either case, phi1 = phi12,
-          // and the sign of phi2 must be chosen to make phi11 == phi12
-          T phi1 = phi12.Radian();
-
-          bool f1small = f1.SquaredLength() < std::pow(tol, 2);
-          if (!f1small)
-          {
-            // a: phi2 > 0
-            // eq. 5.24
-            Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
-            // combining eq 5.11 and 5.13, and subtracting psi1
-            // instead of multiplying by its rotation matrix:
-            math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
-            phi11a.Normalize();
-
-            // b: phi2 < 0
-            // eq. 5.24
-            Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
-            // combining eq 5.11 and 5.13, and subtracting psi1
-            // instead of multiplying by its rotation matrix:
-            math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
-            phi11b.Normalize();
-
-            // choose sign of phi2
-            // based on whether phi11a or phi11b is closer to phi12
-            // use sin and cos to account for angle wrapping
-            T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2)
-                   + std::pow(cos(phi1) - cos(phi11a.Radian()), 2);
-            T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2)
-                   + std::pow(cos(phi1) - cos(phi11b.Radian()), 2);
-            if (errb < erra)
-            {
-              phi2 *= -1;
-            }
-          }
-
-          // I determined these arguments using trial and error
-          Quaternion<T> result = Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
-
-          // Previous equations assume repeated moments are at the beginning
-          // of the moments vector (moments[0] == moments[1]).
-          // We have the vectors sorted by size, so it's possible that the
-          // repeated moments are at the end (moments[1] == moments[2]).
-          // In this case (unequalMoment == 0), we apply an extra
-          // rotation that exchanges moment[0] and moment[2]
-          // Rotation matrix = [ 0  0  1]
-          //                   [ 0  1  0]
-          //                   [-1  0  0]
-          // That is equivalent to a 90 degree pitch
-          if (unequalMoment == 0)
-            result *= Quaternion<T>(0, GZ_PI_2, 0);
-
-          return result;
-        }
-
-        // No repeated principal moments
-        // eq 5.1:
-        T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
-              +(this->Ixxyyzz[0] - moments[2])
-              *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
-            / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
-        // value of w depends on v
-        T w;
-        if (v < std::abs(tol))
-        {
-          // first sentence after eq 5.4:
-          // "In the case that v = 0, then w = 1."
-          w = 1;
-        }
-        else
-        {
-          // eq 5.2:
-          w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
-              / ((moments[0] - moments[1]) * v);
-        }
-        // initialize values of angle phi1, phi2, phi3
-        T phi1 = 0;
-        // eq 5.3: start with positive value
-        T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
-        // eq 5.4: start with positive value
-        T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
-
-        // compute g1, g2 for phi2,phi3 >= 0
-        // equations 5.7, 5.8
-        Vector2<T> g1(
-          0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
-          0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
-        Vector2<T> g2(
-          (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
-          (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
+        // moments[1] is the repeated value
+        // it is not equal to moments[unequalMoment]
+        // momentsDiff3 = lambda - lambda3
+        T momentsDiff3 = moments[1] - moments[unequalMoment];
+        // eq 5.21:
+        // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3)
+        // s >= 0 since A_11 is in range [lambda, lambda3]
+        T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
+        // set phi3 to zero for repeated moments (eq 5.23)
+        T phi3 = 0;
+        // phi2 = +- acos(sqrt(s))
+        // start with just the positive value
+        // also clamp the acos argument to prevent NaN's
+        T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
+
+        // The paper defines variables phi11 and phi12
+        // which are candidate values of angle phi1.
+        // phi12 is straightforward to compute as a function of f2 and g2.
+        // eq 5.25:
+        Vector2<T> g2(momentsDiff3 * s, 0);
+        // combining eq 5.12 and 5.14, and subtracting psi2
+        // instead of multiplying by its rotation matrix:
+        math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
+        phi12.Normalize();
 
         // The paragraph prior to equation 5.16 describes how to choose
         // the candidate value of phi1 based on the length
         // of the f1 and f2 vectors.
-        // * The case of |f1| == |f2| == 0 implies a repeated moment,
-        //   which should not be possible at this point in the code
         // * When |f1| != 0 and |f2| != 0, then one should choose the
         //   value of phi2 so that phi11 = phi12
         // * When |f1| == 0 and f2 != 0, then phi1 = phi12
-        //   phi11 can be ignored, and either sign of phi2, phi3 can be used
-        // * When |f2| == 0 and f1 != 0, then phi1 = phi11
-        //   phi12 can be ignored, and either sign of phi2, phi3 can be used
+        //   phi11 can be ignored, and either sign of phi2 can be used
+        // * The case of |f2| == 0 can be ignored at this point in the code
+        //   since having a repeated moment when |f2| == 0 implies that
+        //   the matrix is diagonal. But this function returns a unit
+        //   quaternion for diagonal matrices, so we can assume |f2| != 0
+        //   See MassMatrix3.ipynb for a more complete discussion.
+        //
+        // Since |f2| != 0, we only need to consider |f1|
+        // * |f1| == 0: phi1 = phi12
+        // * |f1| != 0: choose phi2 so that phi11 == phi12
+        // In either case, phi1 = phi12,
+        // and the sign of phi2 must be chosen to make phi11 == phi12
+        T phi1 = phi12.Radian();
+
         bool f1small = f1.SquaredLength() < std::pow(tol, 2);
-        bool f2small = f2.SquaredLength() < std::pow(tol, 2);
-        if (f1small && f2small)
-        {
-          // this should never happen
-          // f1small && f2small implies a repeated moment
-          // return invalid quaternion
-          /// \todo Use a mock class to test this line
-          return Quaternion<T>::Zero;
-        }
-        else if (f1small)
-        {
-          // use phi12 (equations 5.12, 5.14)
-          math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
-          phi12.Normalize();
-          phi1 = phi12.Radian();
-        }
-        else if (f2small)
-        {
-          // use phi11 (equations 5.11, 5.13)
-          math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
-          phi11.Normalize();
-          phi1 = phi11.Radian();
-        }
-        else
+        if (!f1small)
         {
-          // check for when phi11 == phi12
-          // eqs 5.11, 5.13:
-          math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
-          phi11.Normalize();
-          // eqs 5.12, 5.14:
-          math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
-          phi12.Normalize();
-          T err  = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2)
-                 + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2);
-          phi1 = phi11.Radian();
-          math::Vector2<T> signsPhi23(1, 1);
-          // case a: phi2 <= 0
-          {
-            Vector2<T> g1a = Vector2<T>(1, -1) * g1;
-            Vector2<T> g2a = Vector2<T>(1, -1) * g2;
-            math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
-            math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
-            phi11a.Normalize();
-            phi12a.Normalize();
-            T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2)
-                   + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2);
-            if (erra < err)
-            {
-              err = erra;
-              phi1 = phi11a.Radian();
-              signsPhi23.Set(-1, 1);
-            }
-          }
-          // case b: phi3 <= 0
-          {
-            Vector2<T> g1b = Vector2<T>(-1, 1) * g1;
-            Vector2<T> g2b = Vector2<T>(1, -1) * g2;
-            math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
-            math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
-            phi11b.Normalize();
-            phi12b.Normalize();
-            T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2)
-                   + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2);
-            if (errb < err)
-            {
-              err = errb;
-              phi1 = phi11b.Radian();
-              signsPhi23.Set(1, -1);
-            }
-          }
-          // case c: phi2,phi3 <= 0
+          // a: phi2 > 0
+          // eq. 5.24
+          Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
+          // combining eq 5.11 and 5.13, and subtracting psi1
+          // instead of multiplying by its rotation matrix:
+          math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
+          phi11a.Normalize();
+
+          // b: phi2 < 0
+          // eq. 5.24
+          Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
+          // combining eq 5.11 and 5.13, and subtracting psi1
+          // instead of multiplying by its rotation matrix:
+          math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
+          phi11b.Normalize();
+
+          // choose sign of phi2
+          // based on whether phi11a or phi11b is closer to phi12
+          // use sin and cos to account for angle wrapping
+          T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2)
+                 + std::pow(cos(phi1) - cos(phi11a.Radian()), 2);
+          T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2)
+                 + std::pow(cos(phi1) - cos(phi11b.Radian()), 2);
+          if (errb < erra)
           {
-            Vector2<T> g1c = Vector2<T>(-1, -1) * g1;
-            Vector2<T> g2c = g2;
-            math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
-            math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
-            phi11c.Normalize();
-            phi12c.Normalize();
-            T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2)
-                   + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2);
-            if (errc < err)
-            {
-              phi1 = phi11c.Radian();
-              signsPhi23.Set(-1, -1);
-            }
+            phi2 *= -1;
           }
-
-          // apply sign changes
-          phi2 *= signsPhi23[0];
-          phi3 *= signsPhi23[1];
         }
 
         // I determined these arguments using trial and error
-        return Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
+        Quaternion<T> result = Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
+
+        // Previous equations assume repeated moments are at the beginning
+        // of the moments vector (moments[0] == moments[1]).
+        // We have the vectors sorted by size, so it's possible that the
+        // repeated moments are at the end (moments[1] == moments[2]).
+        // In this case (unequalMoment == 0), we apply an extra
+        // rotation that exchanges moment[0] and moment[2]
+        // Rotation matrix = [ 0  0  1]
+        //                   [ 0  1  0]
+        //                   [-1  0  0]
+        // That is equivalent to a 90 degree pitch
+        if (unequalMoment == 0)
+          result *= Quaternion<T>(0, GZ_PI_2, 0);
+
+        return result;
       }
 
-      /// \brief Get dimensions and rotation offset of uniform box
-      /// with equivalent mass and moment of inertia.
-      /// To compute this, the Matrix3 is diagonalized.
-      /// The eigenvalues on the diagonal and the rotation offset
-      /// of the principal axes are returned.
-      /// \param[in] _size Dimensions of box aligned with principal axes.
-      /// \param[in] _rot Rotational offset of principal axes.
-      /// \param[in] _tol Relative tolerance.
-      /// \return True if box properties were computed successfully.
-      public: bool EquivalentBox(Vector3<T> &_size,
-                                 Quaternion<T> &_rot,
-                                 const T _tol = 1e-6) const
+      // No repeated principal moments
+      // eq 5.1:
+      T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
+            +(this->Ixxyyzz[0] - moments[2])
+            *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
+          / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
+      // value of w depends on v
+      T w;
+      if (v < std::abs(tol))
+      {
+        // first sentence after eq 5.4:
+        // "In the case that v = 0, then w = 1."
+        w = 1;
+      }
+      else
+      {
+        // eq 5.2:
+        w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
+            / ((moments[0] - moments[1]) * v);
+      }
+      // initialize values of angle phi1, phi2, phi3
+      T phi1 = 0;
+      // eq 5.3: start with positive value
+      T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
+      // eq 5.4: start with positive value
+      T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
+
+      // compute g1, g2 for phi2,phi3 >= 0
+      // equations 5.7, 5.8
+      Vector2<T> g1(
+        0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
+        0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
+      Vector2<T> g2(
+        (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
+        (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
+
+      // The paragraph prior to equation 5.16 describes how to choose
+      // the candidate value of phi1 based on the length
+      // of the f1 and f2 vectors.
+      // * The case of |f1| == |f2| == 0 implies a repeated moment,
+      //   which should not be possible at this point in the code
+      // * When |f1| != 0 and |f2| != 0, then one should choose the
+      //   value of phi2 so that phi11 = phi12
+      // * When |f1| == 0 and f2 != 0, then phi1 = phi12
+      //   phi11 can be ignored, and either sign of phi2, phi3 can be used
+      // * When |f2| == 0 and f1 != 0, then phi1 = phi11
+      //   phi12 can be ignored, and either sign of phi2, phi3 can be used
+      bool f1small = f1.SquaredLength() < std::pow(tol, 2);
+      bool f2small = f2.SquaredLength() < std::pow(tol, 2);
+      if (f1small && f2small)
+      {
+        // this should never happen
+        // f1small && f2small implies a repeated moment
+        // return invalid quaternion
+        /// \todo Use a mock class to test this line
+        return Quaternion<T>::Zero;
+      }
+      else if (f1small)
+      {
+        // use phi12 (equations 5.12, 5.14)
+        math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
+        phi12.Normalize();
+        phi1 = phi12.Radian();
+      }
+      else if (f2small)
       {
-        if (!this->IsPositive(0))
+        // use phi11 (equations 5.11, 5.13)
+        math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
+        phi11.Normalize();
+        phi1 = phi11.Radian();
+      }
+      else
+      {
+        // check for when phi11 == phi12
+        // eqs 5.11, 5.13:
+        math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
+        phi11.Normalize();
+        // eqs 5.12, 5.14:
+        math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
+        phi12.Normalize();
+        T err  = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2)
+               + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2);
+        phi1 = phi11.Radian();
+        math::Vector2<T> signsPhi23(1, 1);
+        // case a: phi2 <= 0
         {
-          // inertia is not positive, cannot compute equivalent box
-          return false;
+          Vector2<T> g1a = Vector2<T>(1, -1) * g1;
+          Vector2<T> g2a = Vector2<T>(1, -1) * g2;
+          math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
+          math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
+          phi11a.Normalize();
+          phi12a.Normalize();
+          T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2)
+                 + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2);
+          if (erra < err)
+          {
+            err = erra;
+            phi1 = phi11a.Radian();
+            signsPhi23.Set(-1, 1);
+          }
         }
-
-        Vector3<T> moments = this->PrincipalMoments(_tol);
-        if (!ValidMoments(moments))
+        // case b: phi3 <= 0
         {
-          // principal moments don't satisfy the triangle identity
-          return false;
+          Vector2<T> g1b = Vector2<T>(-1, 1) * g1;
+          Vector2<T> g2b = Vector2<T>(1, -1) * g2;
+          math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
+          math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
+          phi11b.Normalize();
+          phi12b.Normalize();
+          T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2)
+                 + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2);
+          if (errb < err)
+          {
+            err = errb;
+            phi1 = phi11b.Radian();
+            signsPhi23.Set(1, -1);
+          }
         }
-
-        // The reason for checking that the principal moments satisfy
-        // the triangle inequality
-        // I1 + I2 - I3 >= 0
-        // is to ensure that the arguments to sqrt in these equations
-        // are positive and the box size is real.
-        _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass));
-        _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass));
-        _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass));
-
-        _rot = this->PrincipalAxesOffset(_tol);
-
-        if (_rot == Quaternion<T>::Zero)
+        // case c: phi2,phi3 <= 0
         {
-          // _rot is an invalid quaternion
-          /// \todo Use a mock class to test this line
-          return false;
+          Vector2<T> g1c = Vector2<T>(-1, -1) * g1;
+          Vector2<T> g2c = g2;
+          math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
+          math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
+          phi11c.Normalize();
+          phi12c.Normalize();
+          T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2)
+                 + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2);
+          if (errc < err)
+          {
+            phi1 = phi11c.Radian();
+            signsPhi23.Set(-1, -1);
+          }
         }
 
-        return true;
+        // apply sign changes
+        phi2 *= signsPhi23[0];
+        phi3 *= signsPhi23[1];
       }
 
-      /// \brief Set inertial properties based on a Material and equivalent box.
-      /// \param[in] _mat Material that specifies a density. Uniform density
-      /// is used.
-      /// \param[in] _size Size of equivalent box.
-      /// \param[in] _rot Rotational offset of equivalent box.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromBox(const Material &_mat,
-                              const Vector3<T> &_size,
-                            const Quaternion<T> &_rot = Quaternion<T>::Identity)
-     {
-        T volume = _size.X() * _size.Y() * _size.Z();
-        return this->SetFromBox(_mat.Density() * volume, _size, _rot);
-     }
-
-      /// \brief Set inertial properties based on mass and equivalent box.
-      /// \param[in] _mass Mass to set.
-      /// \param[in] _size Size of equivalent box.
-      /// \param[in] _rot Rotational offset of equivalent box.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromBox(const T _mass,
-                              const Vector3<T> &_size,
-                            const Quaternion<T> &_rot = Quaternion<T>::Identity)
+      // I determined these arguments using trial and error
+      return Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
+    }
+
+    /// \brief Get dimensions and rotation offset of uniform box
+    /// with equivalent mass and moment of inertia.
+    /// To compute this, the Matrix3 is diagonalized.
+    /// The eigenvalues on the diagonal and the rotation offset
+    /// of the principal axes are returned.
+    /// \param[in] _size Dimensions of box aligned with principal axes.
+    /// \param[in] _rot Rotational offset of principal axes.
+    /// \param[in] _tol Relative tolerance.
+    /// \return True if box properties were computed successfully.
+    public: bool EquivalentBox(Vector3<T> &_size,
+                               Quaternion<T> &_rot,
+                               const T _tol = 1e-6) const
+    {
+      if (!this->IsPositive(0))
       {
-        // Check that _mass and _size are strictly positive
-        // and that quaternion is valid
-        if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
-        {
-          return false;
-        }
-        this->SetMass(_mass);
-        return this->SetFromBox(_size, _rot);
+        // inertia is not positive, cannot compute equivalent box
+        return false;
       }
 
-      /// \brief Set inertial properties based on equivalent box
-      /// using the current mass value.
-      /// \param[in] _size Size of equivalent box.
-      /// \param[in] _rot Rotational offset of equivalent box.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromBox(const Vector3<T> &_size,
-                            const Quaternion<T> &_rot = Quaternion<T>::Identity)
+      Vector3<T> moments = this->PrincipalMoments(_tol);
+      if (!ValidMoments(moments))
       {
-        // Check that _mass and _size are strictly positive
-        // and that quaternion is valid
-        if (this->Mass() <= 0 || _size.Min() <= 0 ||
-            _rot == Quaternion<T>::Zero)
-        {
-          return false;
-        }
-
-        // Diagonal matrix L with principal moments
-        Matrix3<T> L;
-        T x2 = std::pow(_size.X(), 2);
-        T y2 = std::pow(_size.Y(), 2);
-        T z2 = std::pow(_size.Z(), 2);
-        L(0, 0) = this->mass / 12.0 * (y2 + z2);
-        L(1, 1) = this->mass / 12.0 * (z2 + x2);
-        L(2, 2) = this->mass / 12.0 * (x2 + y2);
-        Matrix3<T> R(_rot);
-        return this->SetMoi(R * L * R.Transposed());
+        // principal moments don't satisfy the triangle identity
+        return false;
       }
 
-      /// \brief Set inertial properties based on a Material and equivalent
-      /// cone aligned with Z axis.
-      /// \param[in] _mat Material that specifies a density. Uniform density
-      /// is used.
-      /// \param[in] _length Length of cone along Z axis.
-      /// \param[in] _radius Radius of cone.
-      /// \param[in] _rot Rotational offset of equivalent cone.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromConeZ(const Material &_mat,
-                                const T _length,
-                                const T _radius,
-                                const Quaternion<T> &_rot =
-                                Quaternion<T>::Identity)
+      // The reason for checking that the principal moments satisfy
+      // the triangle inequality
+      // I1 + I2 - I3 >= 0
+      // is to ensure that the arguments to sqrt in these equations
+      // are positive and the box size is real.
+      _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass));
+      _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass));
+      _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass));
+
+      _rot = this->PrincipalAxesOffset(_tol);
+
+      if (_rot == Quaternion<T>::Zero)
       {
-        // Check that density, _radius and _length are strictly positive
-        // and that quaternion is valid
-        if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
-            _rot == Quaternion<T>::Zero)
-        {
-          return false;
-        }
-        T volume = GZ_PI * _radius * _radius * _length / 3.0;
-        return this->SetFromConeZ(_mat.Density() * volume,
-                                      _length, _radius, _rot);
+        // _rot is an invalid quaternion
+        /// \todo Use a mock class to test this line
+        return false;
       }
 
-      /// \brief Set inertial properties based on mass and equivalent cone
-      /// aligned with Z axis.
-      /// \param[in] _mass Mass to set.
-      /// \param[in] _length Length of cone along Z axis.
-      /// \param[in] _radius Radius of cone.
-      /// \param[in] _rot Rotational offset of equivalent cone.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromConeZ(const T _mass,
-                                const T _length,
-                                const T _radius,
-                                const Quaternion<T> &_rot =
-                                Quaternion<T>::Identity)
+      return true;
+    }
+
+    /// \brief Set inertial properties based on a Material and equivalent box.
+    /// \param[in] _mat Material that specifies a density. Uniform density
+    /// is used.
+    /// \param[in] _size Size of equivalent box.
+    /// \param[in] _rot Rotational offset of equivalent box.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromBox(const Material &_mat,
+                            const Vector3<T> &_size,
+                          const Quaternion<T> &_rot = Quaternion<T>::Identity)
+   {
+      T volume = _size.X() * _size.Y() * _size.Z();
+      return this->SetFromBox(_mat.Density() * volume, _size, _rot);
+   }
+
+    /// \brief Set inertial properties based on mass and equivalent box.
+    /// \param[in] _mass Mass to set.
+    /// \param[in] _size Size of equivalent box.
+    /// \param[in] _rot Rotational offset of equivalent box.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromBox(const T _mass,
+                            const Vector3<T> &_size,
+                          const Quaternion<T> &_rot = Quaternion<T>::Identity)
+    {
+      // Check that _mass and _size are strictly positive
+      // and that quaternion is valid
+      if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
       {
-        // Check that _mass, _radius and _length are strictly positive
-        // and that quaternion is valid
-        if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
-            _rot == Quaternion<T>::Zero)
-        {
-          return false;
-        }
-        this->SetMass(_mass);
-        return this->SetFromConeZ(_length, _radius, _rot);
+        return false;
       }
+      this->SetMass(_mass);
+      return this->SetFromBox(_size, _rot);
+    }
 
-      /// \brief Set inertial properties based on equivalent cone
-      /// aligned with Z axis using the current mass value.
-      /// \param[in] _length Length of cone along Z axis.
-      /// \param[in] _radius Radius of cone.
-      /// \param[in] _rot Rotational offset of equivalent cone.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromConeZ(const T _length,
-                                const T _radius,
-                                const Quaternion<T> &_rot)
+    /// \brief Set inertial properties based on equivalent box
+    /// using the current mass value.
+    /// \param[in] _size Size of equivalent box.
+    /// \param[in] _rot Rotational offset of equivalent box.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromBox(const Vector3<T> &_size,
+                          const Quaternion<T> &_rot = Quaternion<T>::Identity)
+    {
+      // Check that _mass and _size are strictly positive
+      // and that quaternion is valid
+      if (this->Mass() <= 0 || _size.Min() <= 0 ||
+          _rot == Quaternion<T>::Zero)
       {
-        // Check that _mass and _size are strictly positive
-        // and that quaternion is valid
-        if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
-            _rot == Quaternion<T>::Zero)
-        {
-          return false;
-        }
-
-        // Diagonal matrix L with principal moments
-        T radius2 = std::pow(_radius, 2);
-        Matrix3<T> L;
-        L(0, 0) = 3.0 * this->mass * (4.0 * radius2 +
-                  std::pow(_length, 2)) / 80.0;
-        L(1, 1) = L(0, 0);
-        L(2, 2) = 3.0 * this->mass * radius2 / 10.0;
-        Matrix3<T> R(_rot);
-        return this->SetMoi(R * L * R.Transposed());
+        return false;
       }
 
-      /// \brief Set inertial properties based on a Material and equivalent
-      /// cylinder aligned with Z axis.
-      /// \param[in] _mat Material that specifies a density. Uniform density
-      /// is used.
-      /// \param[in] _length Length of cylinder along Z axis.
-      /// \param[in] _radius Radius of cylinder.
-      /// \param[in] _rot Rotational offset of equivalent cylinder.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromCylinderZ(const Material &_mat,
-                                    const T _length,
-                                    const T _radius,
-                            const Quaternion<T> &_rot = Quaternion<T>::Identity)
+      // Diagonal matrix L with principal moments
+      Matrix3<T> L;
+      T x2 = std::pow(_size.X(), 2);
+      T y2 = std::pow(_size.Y(), 2);
+      T z2 = std::pow(_size.Z(), 2);
+      L(0, 0) = this->mass / 12.0 * (y2 + z2);
+      L(1, 1) = this->mass / 12.0 * (z2 + x2);
+      L(2, 2) = this->mass / 12.0 * (x2 + y2);
+      Matrix3<T> R(_rot);
+      return this->SetMoi(R * L * R.Transposed());
+    }
+
+    /// \brief Set inertial properties based on a Material and equivalent
+    /// cone aligned with Z axis.
+    /// \param[in] _mat Material that specifies a density. Uniform density
+    /// is used.
+    /// \param[in] _length Length of cone along Z axis.
+    /// \param[in] _radius Radius of cone.
+    /// \param[in] _rot Rotational offset of equivalent cone.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromConeZ(const Material &_mat,
+                              const T _length,
+                              const T _radius,
+                              const Quaternion<T> &_rot =
+                              Quaternion<T>::Identity)
+    {
+      // Check that density, _radius and _length are strictly positive
+      // and that quaternion is valid
+      if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
+          _rot == Quaternion<T>::Zero)
       {
-        // Check that density, _radius and _length are strictly positive
-        // and that quaternion is valid
-        if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
-            _rot == Quaternion<T>::Zero)
-        {
-          return false;
-        }
-        T volume = GZ_PI * _radius * _radius * _length;
-        return this->SetFromCylinderZ(_mat.Density() * volume,
-                                      _length, _radius, _rot);
+        return false;
       }
+      T volume = GZ_PI * _radius * _radius * _length / 3.0;
+      return this->SetFromConeZ(_mat.Density() * volume,
+                                    _length, _radius, _rot);
+    }
 
-      /// \brief Set inertial properties based on mass and equivalent cylinder
-      /// aligned with Z axis.
-      /// \param[in] _mass Mass to set.
-      /// \param[in] _length Length of cylinder along Z axis.
-      /// \param[in] _radius Radius of cylinder.
-      /// \param[in] _rot Rotational offset of equivalent cylinder.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromCylinderZ(const T _mass,
-                                    const T _length,
-                                    const T _radius,
-                            const Quaternion<T> &_rot = Quaternion<T>::Identity)
+    /// \brief Set inertial properties based on mass and equivalent cone
+    /// aligned with Z axis.
+    /// \param[in] _mass Mass to set.
+    /// \param[in] _length Length of cone along Z axis.
+    /// \param[in] _radius Radius of cone.
+    /// \param[in] _rot Rotational offset of equivalent cone.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromConeZ(const T _mass,
+                              const T _length,
+                              const T _radius,
+                              const Quaternion<T> &_rot =
+                              Quaternion<T>::Identity)
+    {
+      // Check that _mass, _radius and _length are strictly positive
+      // and that quaternion is valid
+      if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
+          _rot == Quaternion<T>::Zero)
       {
-        // Check that _mass, _radius and _length are strictly positive
-        // and that quaternion is valid
-        if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
-            _rot == Quaternion<T>::Zero)
-        {
-          return false;
-        }
-        this->SetMass(_mass);
-        return this->SetFromCylinderZ(_length, _radius, _rot);
+        return false;
       }
+      this->SetMass(_mass);
+      return this->SetFromConeZ(_length, _radius, _rot);
+    }
 
-      /// \brief Set inertial properties based on equivalent cylinder
-      /// aligned with Z axis using the current mass value.
-      /// \param[in] _length Length of cylinder along Z axis.
-      /// \param[in] _radius Radius of cylinder.
-      /// \param[in] _rot Rotational offset of equivalent cylinder.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromCylinderZ(const T _length,
-                                    const T _radius,
-                                    const Quaternion<T> &_rot)
+    /// \brief Set inertial properties based on equivalent cone
+    /// aligned with Z axis using the current mass value.
+    /// \param[in] _length Length of cone along Z axis.
+    /// \param[in] _radius Radius of cone.
+    /// \param[in] _rot Rotational offset of equivalent cone.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromConeZ(const T _length,
+                              const T _radius,
+                              const Quaternion<T> &_rot)
+    {
+      // Check that _mass and _size are strictly positive
+      // and that quaternion is valid
+      if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
+          _rot == Quaternion<T>::Zero)
       {
-        // Check that _mass and _size are strictly positive
-        // and that quaternion is valid
-        if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
-            _rot == Quaternion<T>::Zero)
-        {
-          return false;
-        }
-
-        // Diagonal matrix L with principal moments
-        T radius2 = std::pow(_radius, 2);
-        Matrix3<T> L;
-        L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
-        L(1, 1) = L(0, 0);
-        L(2, 2) = this->mass / 2.0 * radius2;
-        Matrix3<T> R(_rot);
-        return this->SetMoi(R * L * R.Transposed());
+        return false;
       }
 
-      /// \brief Set inertial properties based on a material and
-      /// equivalent sphere.
-      /// \param[in] _mat Material that specifies a density. Uniform density
-      /// is used.
-      /// \param[in] _radius Radius of equivalent, uniform sphere.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromSphere(const Material &_mat, const T _radius)
-      {
-        // Check that the density and _radius are strictly positive
-        if (_mat.Density() <= 0 || _radius <= 0)
-        {
-          return false;
-        }
+      // Diagonal matrix L with principal moments
+      T radius2 = std::pow(_radius, 2);
+      Matrix3<T> L;
+      L(0, 0) = 3.0 * this->mass * (4.0 * radius2 +
+                std::pow(_length, 2)) / 80.0;
+      L(1, 1) = L(0, 0);
+      L(2, 2) = 3.0 * this->mass * radius2 / 10.0;
+      Matrix3<T> R(_rot);
+      return this->SetMoi(R * L * R.Transposed());
+    }
 
-        T volume = (4.0/3.0) * GZ_PI * std::pow(_radius, 3);
-        return this->SetFromSphere(_mat.Density() * volume, _radius);
+    /// \brief Set inertial properties based on a Material and equivalent
+    /// cylinder aligned with Z axis.
+    /// \param[in] _mat Material that specifies a density. Uniform density
+    /// is used.
+    /// \param[in] _length Length of cylinder along Z axis.
+    /// \param[in] _radius Radius of cylinder.
+    /// \param[in] _rot Rotational offset of equivalent cylinder.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromCylinderZ(const Material &_mat,
+                                  const T _length,
+                                  const T _radius,
+                          const Quaternion<T> &_rot = Quaternion<T>::Identity)
+    {
+      // Check that density, _radius and _length are strictly positive
+      // and that quaternion is valid
+      if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
+          _rot == Quaternion<T>::Zero)
+      {
+        return false;
       }
+      T volume = GZ_PI * _radius * _radius * _length;
+      return this->SetFromCylinderZ(_mat.Density() * volume,
+                                    _length, _radius, _rot);
+    }
 
-      /// \brief Set inertial properties based on mass and equivalent sphere.
-      /// \param[in] _mass Mass to set.
-      /// \param[in] _radius Radius of equivalent, uniform sphere.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromSphere(const T _mass, const T _radius)
+    /// \brief Set inertial properties based on mass and equivalent cylinder
+    /// aligned with Z axis.
+    /// \param[in] _mass Mass to set.
+    /// \param[in] _length Length of cylinder along Z axis.
+    /// \param[in] _radius Radius of cylinder.
+    /// \param[in] _rot Rotational offset of equivalent cylinder.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromCylinderZ(const T _mass,
+                                  const T _length,
+                                  const T _radius,
+                          const Quaternion<T> &_rot = Quaternion<T>::Identity)
+    {
+      // Check that _mass, _radius and _length are strictly positive
+      // and that quaternion is valid
+      if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
+          _rot == Quaternion<T>::Zero)
       {
-        // Check that _mass and _radius are strictly positive
-        if (_mass <= 0 || _radius <= 0)
-        {
-          return false;
-        }
-        this->SetMass(_mass);
-        return this->SetFromSphere(_radius);
+        return false;
       }
+      this->SetMass(_mass);
+      return this->SetFromCylinderZ(_length, _radius, _rot);
+    }
 
-      /// \brief Set inertial properties based on equivalent sphere
-      /// using the current mass value.
-      /// \param[in] _radius Radius of equivalent, uniform sphere.
-      /// \return True if inertial properties were set successfully.
-      public: bool SetFromSphere(const T _radius)
+    /// \brief Set inertial properties based on equivalent cylinder
+    /// aligned with Z axis using the current mass value.
+    /// \param[in] _length Length of cylinder along Z axis.
+    /// \param[in] _radius Radius of cylinder.
+    /// \param[in] _rot Rotational offset of equivalent cylinder.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromCylinderZ(const T _length,
+                                  const T _radius,
+                                  const Quaternion<T> &_rot)
+    {
+      // Check that _mass and _size are strictly positive
+      // and that quaternion is valid
+      if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
+          _rot == Quaternion<T>::Zero)
       {
-        // Check that _mass and _radius are strictly positive
-        if (this->Mass() <= 0 || _radius <= 0)
-        {
-          return false;
-        }
-
-        // Diagonal matrix L with principal moments
-        T radius2 = std::pow(_radius, 2);
-        Matrix3<T> L;
-        L(0, 0) = 0.4 * this->mass * radius2;
-        L(1, 1) = 0.4 * this->mass * radius2;
-        L(2, 2) = 0.4 * this->mass * radius2;
-        return this->SetMoi(L);
+        return false;
       }
 
-      /// \brief Square root of positive numbers, otherwise zero.
-      /// \param[in] _x Number to be square rooted.
-      /// \return sqrt(_x) if _x > 0, otherwise 0
-      private: static inline T ClampedSqrt(const T &_x)
+      // Diagonal matrix L with principal moments
+      T radius2 = std::pow(_radius, 2);
+      Matrix3<T> L;
+      L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
+      L(1, 1) = L(0, 0);
+      L(2, 2) = this->mass / 2.0 * radius2;
+      Matrix3<T> R(_rot);
+      return this->SetMoi(R * L * R.Transposed());
+    }
+
+    /// \brief Set inertial properties based on a material and
+    /// equivalent sphere.
+    /// \param[in] _mat Material that specifies a density. Uniform density
+    /// is used.
+    /// \param[in] _radius Radius of equivalent, uniform sphere.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromSphere(const Material &_mat, const T _radius)
+    {
+      // Check that the density and _radius are strictly positive
+      if (_mat.Density() <= 0 || _radius <= 0)
       {
-        if (_x <= 0)
-          return 0;
-        return sqrt(_x);
+        return false;
       }
 
-      /// \brief Angle formed by direction of a Vector2.
-      /// \param[in] _v Vector whose direction is to be computed.
-      /// \param[in] _eps Minimum length of vector required for computing angle.
-      /// \return Angle formed between vector and X axis,
-      /// or zero if vector has length less than 1e-6.
-      private: static T Angle2(const Vector2<T> &_v, const T _eps = 1e-6)
+      T volume = (4.0/3.0) * GZ_PI * std::pow(_radius, 3);
+      return this->SetFromSphere(_mat.Density() * volume, _radius);
+    }
+
+    /// \brief Set inertial properties based on mass and equivalent sphere.
+    /// \param[in] _mass Mass to set.
+    /// \param[in] _radius Radius of equivalent, uniform sphere.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromSphere(const T _mass, const T _radius)
+    {
+      // Check that _mass and _radius are strictly positive
+      if (_mass <= 0 || _radius <= 0)
       {
-        if (_v.SquaredLength() < std::pow(_eps, 2))
-          return 0;
-        return atan2(_v[1], _v[0]);
+        return false;
       }
+      this->SetMass(_mass);
+      return this->SetFromSphere(_radius);
+    }
 
-      /// \brief Mass of the object. Default is 0.0.
-      private: T mass;
+    /// \brief Set inertial properties based on equivalent sphere
+    /// using the current mass value.
+    /// \param[in] _radius Radius of equivalent, uniform sphere.
+    /// \return True if inertial properties were set successfully.
+    public: bool SetFromSphere(const T _radius)
+    {
+      // Check that _mass and _radius are strictly positive
+      if (this->Mass() <= 0 || _radius <= 0)
+      {
+        return false;
+      }
 
-      /// \brief Principal moments of inertia. Default is (0.0 0.0 0.0)
-      /// These Moments of Inertia are specified in the local frame.
-      /// Where Ixxyyzz.x is Ixx, Ixxyyzz.y is Iyy and Ixxyyzz.z is Izz.
-      private: Vector3<T> Ixxyyzz;
+      // Diagonal matrix L with principal moments
+      T radius2 = std::pow(_radius, 2);
+      Matrix3<T> L;
+      L(0, 0) = 0.4 * this->mass * radius2;
+      L(1, 1) = 0.4 * this->mass * radius2;
+      L(2, 2) = 0.4 * this->mass * radius2;
+      return this->SetMoi(L);
+    }
 
-      /// \brief Product moments of inertia. Default is (0.0 0.0 0.0)
-      /// These MOI off-diagonals are specified in the local frame.
-      /// Where Ixyxzyz.x is Ixy, Ixyxzyz.y is Ixz and Ixyxzyz.z is Iyz.
-      private: Vector3<T> Ixyxzyz;
-    };
+    /// \brief Square root of positive numbers, otherwise zero.
+    /// \param[in] _x Number to be square rooted.
+    /// \return sqrt(_x) if _x > 0, otherwise 0
+    private: static inline T ClampedSqrt(const T &_x)
+    {
+      if (_x <= 0)
+        return 0;
+      return sqrt(_x);
+    }
 
-    typedef MassMatrix3<double> MassMatrix3d;
-    typedef MassMatrix3<float> MassMatrix3f;
+    /// \brief Angle formed by direction of a Vector2.
+    /// \param[in] _v Vector whose direction is to be computed.
+    /// \param[in] _eps Minimum length of vector required for computing angle.
+    /// \return Angle formed between vector and X axis,
+    /// or zero if vector has length less than 1e-6.
+    private: static T Angle2(const Vector2<T> &_v, const T _eps = 1e-6)
+    {
+      if (_v.SquaredLength() < std::pow(_eps, 2))
+        return 0;
+      return atan2(_v[1], _v[0]);
     }
-  }
-}
-#endif
+
+    /// \brief Mass of the object. Default is 0.0.
+    private: T mass;
+
+    /// \brief Principal moments of inertia. Default is (0.0 0.0 0.0)
+    /// These Moments of Inertia are specified in the local frame.
+    /// Where Ixxyyzz.x is Ixx, Ixxyyzz.y is Iyy and Ixxyyzz.z is Izz.
+    private: Vector3<T> Ixxyyzz;
+
+    /// \brief Product moments of inertia. Default is (0.0 0.0 0.0)
+    /// These MOI off-diagonals are specified in the local frame.
+    /// Where Ixyxzyz.x is Ixy, Ixyxzyz.y is Ixz and Ixyxzyz.z is Iyz.
+    private: Vector3<T> Ixyxzyz;
+  };
+
+  typedef MassMatrix3<double> MassMatrix3d;
+  typedef MassMatrix3<float> MassMatrix3f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_MASSMATRIX3_HH_
diff --git a/include/gz/math/Material.hh b/include/gz/math/Material.hh
index 99be5d5e8..34b2eaacd 100644
--- a/include/gz/math/Material.hh
+++ b/include/gz/math/Material.hh
@@ -25,118 +25,115 @@
 #include <gz/math/MaterialType.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+
+  /// \brief Contains information about a single material.
+  ///
+  /// Steel, wood, and iron are examples of materials. This class
+  /// allows you to create custom materials, or use built-in materials.
+  /// The list of built-in materials can be found in the ::MaterialType
+  /// enum.
+  ///
+  /// This class will replace the
+  /// [MaterialDensity class](https://github.com/gazebosim/gz-common/blob/main/include/gz/common/MaterialDensity.hh)
+  /// found in the Gazebo Common library, which was at version 1 at the
+  /// time of this writing.
+  ///
+  /// **How to create a wood material:**
+  ///
+  /// ~~~
+  /// Material mat(MaterialType::WOOD);
+  /// std::cout << "The density of " << mat.Name() << " is "
+  ///   << mat.Density() << std::endl;
+  /// ~~~
+  ///
+  /// **How to create a custom material:**
+  ///
+  /// ~~~
+  /// Material mat;
+  /// mat.SetDensity(12.23);
+  /// mat.SetName("my_material");
+  /// std::cout << "The density of " << mat.Name() is "
+  ///   << mat.Density() << std::endl;
+  /// ~~~
+  class GZ_MATH_VISIBLE Material
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-
-    /// \brief Contains information about a single material.
-    ///
-    /// Steel, wood, and iron are examples of materials. This class
-    /// allows you to create custom materials, or use built-in materials.
-    /// The list of built-in materials can be found in the ::MaterialType
-    /// enum.
-    ///
-    /// This class will replace the
-    /// [MaterialDensity class](https://github.com/gazebosim/gz-common/blob/main/include/gz/common/MaterialDensity.hh)
-    /// found in the Gazebo Common library, which was at version 1 at the
-    /// time of this writing.
-    ///
-    /// **How to create a wood material:**
-    ///
-    /// ~~~
-    /// Material mat(MaterialType::WOOD);
-    /// std::cout << "The density of " << mat.Name() << " is "
-    ///   << mat.Density() << std::endl;
-    /// ~~~
-    ///
-    /// **How to create a custom material:**
-    ///
-    /// ~~~
-    /// Material mat;
-    /// mat.SetDensity(12.23);
-    /// mat.SetName("my_material");
-    /// std::cout << "The density of " << mat.Name() is "
-    ///   << mat.Density() << std::endl;
-    /// ~~~
-    class GZ_MATH_VISIBLE Material
-    {
-      /// \brief Constructor.
-      public: Material();
-
-      /// \brief Construct a material based on a type.
-      /// \param[in] _type Built-in type to create.
-      public: explicit Material(const MaterialType _type);
-
-      /// \brief Construct a material based on a type name.
-      /// \param[in] _typename Name of the built-in type to create. String
-      /// names are listed in the ::MaterialType documentation.
-      public: explicit Material(const std::string &_typename);
-
-      /// \brief Construct a material based on a density value.
-      /// \param[in] _density Material density.
-      public: explicit Material(const double _density);
-
-      /// \brief Get all the built-in materials.
-      /// \return A map of all the materials. The map's key is
-      /// material type and the map's value is the material object.
-      public: static const std::map<MaterialType, Material> &Predefined();
-
-      /// \brief Set this Material to the built-in Material with
-      /// the nearest density value within _epsilon. If a built-in material
-      /// could not be found, then this Material is not changed.
-      /// \param[in] _value Density value of entry to match.
-      /// \param[in] _epsilon Allowable range of difference between _value,
-      /// and a material's density.
-      public: void SetToNearestDensity(
-                  const double _value,
-                  const double _epsilon = std::numeric_limits<double>::max());
-
-      /// \brief Equality operator. This compares type and density values.
-      /// \param[in] _material Material to evaluate this object against.
-      /// \return True if this material is equal to the given _material.
-      public: bool operator==(const Material &_material) const;
-
-      /// \brief Inequality operator. This compares type and density values.
-      /// \param[in] _material Material to evaluate this object against.
-      /// \return True if this material is not equal to the given _material.
-      public: bool operator!=(const Material &_material) const;
-
-      /// \brief Get the material's type.
-      /// \return The material's type.
-      public: MaterialType Type() const;
-
-      /// \brief Set the material's type. This will only set the type value.
-      /// Other properties, such as density, will not be changed.
-      /// \param[in] _type The material's type.
-      public: void SetType(const MaterialType _type);
-
-      /// \brief Get the name of the material. This will match the enum type
-      /// names used in ::MaterialType, but in lowercase, if a built-in
-      /// material is used.
-      /// \return The material's name.
-      /// \sa void SetName(const std::string &_name)
-      public: std::string Name() const;
-
-      /// \brief Set the name of the material.
-      /// \param[in] _name The material's name.
-      /// \sa std::string Name() const
-      public: void SetName(const std::string &_name);
-
-      /// \brief Get the density value of the material in kg/m^3.
-      /// \return The density of this material in kg/m^3.
-      public: double Density() const;
-
-      /// \brief Set the density value of the material in kg/m^3.
-      /// \param[in] _density The density of this material in kg/m^3.
-      public: void SetDensity(const double _density);
-
-      /// \brief Private data pointer.
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
-#endif
+    /// \brief Constructor.
+    public: Material();
+
+    /// \brief Construct a material based on a type.
+    /// \param[in] _type Built-in type to create.
+    public: explicit Material(const MaterialType _type);
+
+    /// \brief Construct a material based on a type name.
+    /// \param[in] _typename Name of the built-in type to create. String
+    /// names are listed in the ::MaterialType documentation.
+    public: explicit Material(const std::string &_typename);
+
+    /// \brief Construct a material based on a density value.
+    /// \param[in] _density Material density.
+    public: explicit Material(const double _density);
+
+    /// \brief Get all the built-in materials.
+    /// \return A map of all the materials. The map's key is
+    /// material type and the map's value is the material object.
+    public: static const std::map<MaterialType, Material> &Predefined();
+
+    /// \brief Set this Material to the built-in Material with
+    /// the nearest density value within _epsilon. If a built-in material
+    /// could not be found, then this Material is not changed.
+    /// \param[in] _value Density value of entry to match.
+    /// \param[in] _epsilon Allowable range of difference between _value,
+    /// and a material's density.
+    public: void SetToNearestDensity(
+                const double _value,
+                const double _epsilon = std::numeric_limits<double>::max());
+
+    /// \brief Equality operator. This compares type and density values.
+    /// \param[in] _material Material to evaluate this object against.
+    /// \return True if this material is equal to the given _material.
+    public: bool operator==(const Material &_material) const;
+
+    /// \brief Inequality operator. This compares type and density values.
+    /// \param[in] _material Material to evaluate this object against.
+    /// \return True if this material is not equal to the given _material.
+    public: bool operator!=(const Material &_material) const;
+
+    /// \brief Get the material's type.
+    /// \return The material's type.
+    public: MaterialType Type() const;
+
+    /// \brief Set the material's type. This will only set the type value.
+    /// Other properties, such as density, will not be changed.
+    /// \param[in] _type The material's type.
+    public: void SetType(const MaterialType _type);
+
+    /// \brief Get the name of the material. This will match the enum type
+    /// names used in ::MaterialType, but in lowercase, if a built-in
+    /// material is used.
+    /// \return The material's name.
+    /// \sa void SetName(const std::string &_name)
+    public: std::string Name() const;
+
+    /// \brief Set the name of the material.
+    /// \param[in] _name The material's name.
+    /// \sa std::string Name() const
+    public: void SetName(const std::string &_name);
+
+    /// \brief Get the density value of the material in kg/m^3.
+    /// \return The density of this material in kg/m^3.
+    public: double Density() const;
+
+    /// \brief Set the density value of the material in kg/m^3.
+    /// \param[in] _density The density of this material in kg/m^3.
+    public: void SetDensity(const double _density);
+
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_MATERIAL_HH_
diff --git a/include/gz/math/MaterialType.hh b/include/gz/math/MaterialType.hh
index 31fadb585..56e36e2a9 100644
--- a/include/gz/math/MaterialType.hh
+++ b/include/gz/math/MaterialType.hh
@@ -20,80 +20,77 @@
 #include <gz/math/Export.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \enum MaterialType
+  /// \brief This enum lists the supported material types. A value can be
+  /// used to create a Material instance.
+  /// Source: https://en.wikipedia.org/wiki/Density
+  /// \sa Material
+  // Developer Note: When modifying this enum, make sure to also modify
+  // the kMaterials map in src/MaterialTypes.hh.
+  enum class MaterialType
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \enum MaterialType
-    /// \brief This enum lists the supported material types. A value can be
-    /// used to create a Material instance.
-    /// Source: https://en.wikipedia.org/wiki/Density
-    /// \sa Material
-    // Developer Note: When modifying this enum, make sure to also modify
-    // the kMaterials map in src/MaterialTypes.hh.
-    enum class MaterialType
-    {
-      /// \brief Styrofoam, density = 75.0 kg/m^3
-      /// String name = "styrofoam"
-      STYROFOAM = 0,
+    /// \brief Styrofoam, density = 75.0 kg/m^3
+    /// String name = "styrofoam"
+    STYROFOAM = 0,
 
-      /// \brief Pine, density = 373.0 kg/m^3
-      /// String name = "pine"
-      PINE,
+    /// \brief Pine, density = 373.0 kg/m^3
+    /// String name = "pine"
+    PINE,
 
-      /// \brief Wood, density = 700.0 kg/m^3
-      /// String name = "wood"
-      WOOD,
+    /// \brief Wood, density = 700.0 kg/m^3
+    /// String name = "wood"
+    WOOD,
 
-      /// \brief Oak, density = 710.0 kg/m^3
-      /// String name = "oak"
-      OAK,
+    /// \brief Oak, density = 710.0 kg/m^3
+    /// String name = "oak"
+    OAK,
 
-      /// \brief Plastic, density = 1175.0 kg/m^3
-      /// String name = "plastic"
-      PLASTIC,
+    /// \brief Plastic, density = 1175.0 kg/m^3
+    /// String name = "plastic"
+    PLASTIC,
 
-      /// \brief Concrete, density = 2000.0 kg/m^3
-      /// String name = "concrete"
-      CONCRETE,
+    /// \brief Concrete, density = 2000.0 kg/m^3
+    /// String name = "concrete"
+    CONCRETE,
 
-      /// \brief Aluminum, density = 2700.0 kg/m^3
-      /// String name = "aluminum"
-      ALUMINUM,
+    /// \brief Aluminum, density = 2700.0 kg/m^3
+    /// String name = "aluminum"
+    ALUMINUM,
 
-      /// \brief Steel alloy, density = 7600.0 kg/m^3
-      /// String name = "steel_alloy"
-      STEEL_ALLOY,
+    /// \brief Steel alloy, density = 7600.0 kg/m^3
+    /// String name = "steel_alloy"
+    STEEL_ALLOY,
 
-      /// \brief Stainless steel, density = 7800.0 kg/m^3
-      /// String name = "steel_stainless"
-      STEEL_STAINLESS,
+    /// \brief Stainless steel, density = 7800.0 kg/m^3
+    /// String name = "steel_stainless"
+    STEEL_STAINLESS,
 
-      /// \brief Iron, density = 7870.0 kg/m^3
-      /// String name = "iron"
-      IRON,
+    /// \brief Iron, density = 7870.0 kg/m^3
+    /// String name = "iron"
+    IRON,
 
-      /// \brief Brass, density = 8600.0 kg/m^3
-      /// String name = "brass"
-      BRASS,
+    /// \brief Brass, density = 8600.0 kg/m^3
+    /// String name = "brass"
+    BRASS,
 
-      /// \brief Copper, density = 8940.0 kg/m^3
-      /// String name = "copper"
-      COPPER,
+    /// \brief Copper, density = 8940.0 kg/m^3
+    /// String name = "copper"
+    COPPER,
 
-      /// \brief Tungsten, density = 19300.0 kg/m^3
-      /// String name = "tungsten"
-      TUNGSTEN,
+    /// \brief Tungsten, density = 19300.0 kg/m^3
+    /// String name = "tungsten"
+    TUNGSTEN,
 
-      /// \brief Represents an invalid or unknown material.
-      // This value should always be last in the enum; it is used in
-      // MaterialDensity_TEST.
-      UNKNOWN_MATERIAL
-    };
-    }
-  }
-}
-#endif
+    /// \brief Represents an invalid or unknown material.
+    // This value should always be last in the enum; it is used in
+    // MaterialDensity_TEST.
+    UNKNOWN_MATERIAL
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  /// GZ_MATH_MATERIALTYPE_HH_
diff --git a/include/gz/math/Matrix3.hh b/include/gz/math/Matrix3.hh
index 92bcb6d40..18e70387b 100644
--- a/include/gz/math/Matrix3.hh
+++ b/include/gz/math/Matrix3.hh
@@ -25,606 +25,603 @@
 #include <gz/math/Quaternion.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  template <typename T> class Quaternion;
+
+  /// \class Matrix3 Matrix3.hh gz/math/Matrix3.hh
+  /// \brief A 3x3 matrix class.
+  ///
+  /// The following two type definitions are provided:
+  ///
+  /// * \ref Matrix3i : Equivalent to Matrix3<int>
+  /// * \ref Matrix3f : Equivalent to Matrix3<float>
+  /// * \ref Matrix3d : Equivalent to Matrix3<double>
+  /// ## Examples
+  ///
+  /// * C++
+  ///
+  /// \snippet examples/matrix3_example.cc complete
+  ///
+  /// * Ruby
+  /// \code{.rb}
+  /// # Modify the RUBYLIB environment variable to include the Gazebo Math
+  /// # library install path. For example, if you install to /user:
+  /// #
+  /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
+  /// #
+  /// require 'gz/math'
+  ///
+  /// # Construct a default matrix3.
+  /// m = Gz::Math::Matrix3d.new
+  /// printf("The default constructed matrix m has the following "+
+  ///        "values.\n\t" +
+  ///        "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n",
+  ///        m.(0, 0), m.(0, 1), m.(0, 2),
+  ///        m.(1, 0), m.(1, 1), m.(1, 2),
+  ///        m.(2, 0), m.(2, 1), m.(2, 2))
+  ///
+  /// # Set the first column of the matrix.
+  /// m.SetCol(0, Gz::Math::Vector3d.new(3, 4, 5))
+  /// printf("Setting the first column of the matrix m to 3, 4, 5.\n\t" +
+  ///        "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n",
+  ///        m.(0, 0), m.(0, 1), m.(0, 2),
+  ///        m.(1, 0), m.(1, 1), m.(1, 2),
+  ///        m.(2, 0), m.(2, 1), m.(2, 2))
+  ///
+  /// # Transpose the matrix.
+  /// t = m.Transposed()
+  /// printf("The transposed matrix t has the values.\n\t"+
+  ///        "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n",
+  ///        t.(0, 0), t.(0, 1), t.(0, 2),
+  ///        t.(1, 0), t.(1, 1), t.(1, 2),
+  ///        t.(2, 0), t.(2, 1), t.(2, 2))
+  ///
+  /// # Multiply the two matrices.
+  /// m = m * t
+  /// printf("m * t = " +
+  ///        "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n",
+  ///        m.(0, 0), m.(0, 1), m.(0, 2),
+  ///        m.(1, 0), m.(1, 1), m.(1, 2),
+  ///        m.(2, 0), m.(2, 1), m.(2, 2))
+  /// \endcode
+  template<typename T>
+  class Matrix3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    template <typename T> class Quaternion;
+    /// \brief A Matrix3 initialized to identity.
+    /// This is equivalent to math::Matrix3<T>(1, 0, 0, 0, 1, 0, 0, 0, 1).
+    public: static const Matrix3<T> &Identity;
 
-    /// \class Matrix3 Matrix3.hh gz/math/Matrix3.hh
-    /// \brief A 3x3 matrix class.
-    ///
-    /// The following two type definitions are provided:
-    ///
-    /// * \ref Matrix3i : Equivalent to Matrix3<int>
-    /// * \ref Matrix3f : Equivalent to Matrix3<float>
-    /// * \ref Matrix3d : Equivalent to Matrix3<double>
-    /// ## Examples
-    ///
-    /// * C++
-    ///
-    /// \snippet examples/matrix3_example.cc complete
-    ///
-    /// * Ruby
-    /// \code{.rb}
-    /// # Modify the RUBYLIB environment variable to include the Gazebo Math
-    /// # library install path. For example, if you install to /user:
-    /// #
-    /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
-    /// #
-    /// require 'gz/math'
-    ///
-    /// # Construct a default matrix3.
-    /// m = Gz::Math::Matrix3d.new
-    /// printf("The default constructed matrix m has the following "+
-    ///        "values.\n\t" +
-    ///        "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n",
-    ///        m.(0, 0), m.(0, 1), m.(0, 2),
-    ///        m.(1, 0), m.(1, 1), m.(1, 2),
-    ///        m.(2, 0), m.(2, 1), m.(2, 2))
-    ///
-    /// # Set the first column of the matrix.
-    /// m.SetCol(0, Gz::Math::Vector3d.new(3, 4, 5))
-    /// printf("Setting the first column of the matrix m to 3, 4, 5.\n\t" +
-    ///        "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n",
-    ///        m.(0, 0), m.(0, 1), m.(0, 2),
-    ///        m.(1, 0), m.(1, 1), m.(1, 2),
-    ///        m.(2, 0), m.(2, 1), m.(2, 2))
-    ///
-    /// # Transpose the matrix.
-    /// t = m.Transposed()
-    /// printf("The transposed matrix t has the values.\n\t"+
-    ///        "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n",
-    ///        t.(0, 0), t.(0, 1), t.(0, 2),
-    ///        t.(1, 0), t.(1, 1), t.(1, 2),
-    ///        t.(2, 0), t.(2, 1), t.(2, 2))
-    ///
-    /// # Multiply the two matrices.
-    /// m = m * t
-    /// printf("m * t = " +
-    ///        "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n",
-    ///        m.(0, 0), m.(0, 1), m.(0, 2),
-    ///        m.(1, 0), m.(1, 1), m.(1, 2),
-    ///        m.(2, 0), m.(2, 1), m.(2, 2))
-    /// \endcode
-    template<typename T>
-    class Matrix3
+    /// \brief A Matrix3 initialized to zero.
+    /// This is equivalent to math::Matrix3<T>(0, 0, 0, 0, 0, 0, 0, 0, 0).
+    public: static const Matrix3<T> &Zero;
+
+    /// \brief Default constructor that initializes the matrix3 to zero.
+    public: Matrix3()
     {
-      /// \brief A Matrix3 initialized to identity.
-      /// This is equivalent to math::Matrix3<T>(1, 0, 0, 0, 1, 0, 0, 0, 1).
-      public: static const Matrix3<T> &Identity;
+      std::memset(this->data, 0, sizeof(this->data[0][0])*9);
+    }
 
-      /// \brief A Matrix3 initialized to zero.
-      /// This is equivalent to math::Matrix3<T>(0, 0, 0, 0, 0, 0, 0, 0, 0).
-      public: static const Matrix3<T> &Zero;
+    /// \brief Copy constructor.
+    /// \param _m Matrix to copy
+    public: Matrix3(const Matrix3<T> &_m) = default;
+
+    /// \brief Construct a matrix3 using nine values.
+    /// \param[in] _v00 Row 0, Col 0 value
+    /// \param[in] _v01 Row 0, Col 1 value
+    /// \param[in] _v02 Row 0, Col 2 value
+    /// \param[in] _v10 Row 1, Col 0 value
+    /// \param[in] _v11 Row 1, Col 1 value
+    /// \param[in] _v12 Row 1, Col 2 value
+    /// \param[in] _v20 Row 2, Col 0 value
+    /// \param[in] _v21 Row 2, Col 1 value
+    /// \param[in] _v22 Row 2, Col 2 value
+    public: constexpr Matrix3(T _v00, T _v01, T _v02,
+                              T _v10, T _v11, T _v12,
+                              T _v20, T _v21, T _v22)
+    : data{{_v00, _v01, _v02},
+           {_v10, _v11, _v12},
+           {_v20, _v21, _v22}}
+    {
+    }
 
-      /// \brief Default constructor that initializes the matrix3 to zero.
-      public: Matrix3()
-      {
-        std::memset(this->data, 0, sizeof(this->data[0][0])*9);
-      }
+    /// \brief Construct 3x3 rotation Matrix from a quaternion.
+    /// \param[in] _q Quaternion to set the Matrix3 from.
+    public: explicit Matrix3(const Quaternion<T> &_q)
+    {
+      Quaternion<T> qt = _q;
+      qt.Normalize();
+      this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
+                2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
+                2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
+                2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
+                1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
+                2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
+                2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
+                2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
+                1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
+    }
 
-      /// \brief Copy constructor.
-      /// \param _m Matrix to copy
-      public: Matrix3(const Matrix3<T> &_m) = default;
-
-      /// \brief Construct a matrix3 using nine values.
-      /// \param[in] _v00 Row 0, Col 0 value
-      /// \param[in] _v01 Row 0, Col 1 value
-      /// \param[in] _v02 Row 0, Col 2 value
-      /// \param[in] _v10 Row 1, Col 0 value
-      /// \param[in] _v11 Row 1, Col 1 value
-      /// \param[in] _v12 Row 1, Col 2 value
-      /// \param[in] _v20 Row 2, Col 0 value
-      /// \param[in] _v21 Row 2, Col 1 value
-      /// \param[in] _v22 Row 2, Col 2 value
-      public: constexpr Matrix3(T _v00, T _v01, T _v02,
-                                T _v10, T _v11, T _v12,
-                                T _v20, T _v21, T _v22)
-      : data{{_v00, _v01, _v02},
-             {_v10, _v11, _v12},
-             {_v20, _v21, _v22}}
-      {
-      }
+    /// \brief Desctructor
+    public: ~Matrix3() = default;
 
-      /// \brief Construct 3x3 rotation Matrix from a quaternion.
-      /// \param[in] _q Quaternion to set the Matrix3 from.
-      public: explicit Matrix3(const Quaternion<T> &_q)
-      {
-        Quaternion<T> qt = _q;
-        qt.Normalize();
-        this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
-                  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
-                  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
-                  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
-                  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
-                  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
-                  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
-                  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
-                  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
-      }
+    /// \brief Set a single value.
+    /// \param[in] _row row index. _row is clamped to the range [0,2]
+    /// \param[in] _col column index. _col is clamped to the range [0,2]
+    /// \param[in] _v New value.
+    public: void Set(size_t _row, size_t _col, T _v)
+    {
+      this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
+                [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)] = _v;
+    }
 
-      /// \brief Desctructor
-      public: ~Matrix3() = default;
+    /// \brief Set values.
+    /// \param[in] _v00 Row 0, Col 0 value
+    /// \param[in] _v01 Row 0, Col 1 value
+    /// \param[in] _v02 Row 0, Col 2 value
+    /// \param[in] _v10 Row 1, Col 0 value
+    /// \param[in] _v11 Row 1, Col 1 value
+    /// \param[in] _v12 Row 1, Col 2 value
+    /// \param[in] _v20 Row 2, Col 0 value
+    /// \param[in] _v21 Row 2, Col 1 value
+    /// \param[in] _v22 Row 2, Col 2 value
+    public: void Set(T _v00, T _v01, T _v02,
+                     T _v10, T _v11, T _v12,
+                     T _v20, T _v21, T _v22)
+    {
+      this->data[0][0] = _v00;
+      this->data[0][1] = _v01;
+      this->data[0][2] = _v02;
+      this->data[1][0] = _v10;
+      this->data[1][1] = _v11;
+      this->data[1][2] = _v12;
+      this->data[2][0] = _v20;
+      this->data[2][1] = _v21;
+      this->data[2][2] = _v22;
+    }
 
-      /// \brief Set a single value.
-      /// \param[in] _row row index. _row is clamped to the range [0,2]
-      /// \param[in] _col column index. _col is clamped to the range [0,2]
-      /// \param[in] _v New value.
-      public: void Set(size_t _row, size_t _col, T _v)
-      {
-        this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
-                  [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)] = _v;
-      }
+    /// \brief Set the matrix from three axis (1 per column).
+    /// \param[in] _xAxis The x axis, the first column of the matrix.
+    /// \param[in] _yAxis The y axis, the second column of the matrix.
+    /// \param[in] _zAxis The z axis, the third column of the matrix.
+    public: void SetAxes(const Vector3<T> &_xAxis,
+                         const Vector3<T> &_yAxis,
+                         const Vector3<T> &_zAxis)
+    {
+      this->SetCol(0, _xAxis);
+      this->SetCol(1, _yAxis);
+      this->SetCol(2, _zAxis);
+    }
+
+    /// \brief Set as a rotation matrix from an axis and angle.
+    /// \param[in] _axis the axis
+    /// \param[in] _angle ccw rotation around the axis in radians
+    public: void SetFromAxisAngle(const Vector3<T> &_axis, T _angle)
+    {
+      T c = cos(_angle);
+      T s = sin(_angle);
+      T C = 1-c;
+
+      this->data[0][0] = _axis.X()*_axis.X()*C + c;
+      this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
+      this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
+
+      this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
+      this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
+      this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
+
+      this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
+      this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
+      this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
+    }
 
-      /// \brief Set values.
-      /// \param[in] _v00 Row 0, Col 0 value
-      /// \param[in] _v01 Row 0, Col 1 value
-      /// \param[in] _v02 Row 0, Col 2 value
-      /// \param[in] _v10 Row 1, Col 0 value
-      /// \param[in] _v11 Row 1, Col 1 value
-      /// \param[in] _v12 Row 1, Col 2 value
-      /// \param[in] _v20 Row 2, Col 0 value
-      /// \param[in] _v21 Row 2, Col 1 value
-      /// \param[in] _v22 Row 2, Col 2 value
-      public: void Set(T _v00, T _v01, T _v02,
-                       T _v10, T _v11, T _v12,
-                       T _v20, T _v21, T _v22)
+    /// \brief Set as a rotation matrix to represent rotation from
+    /// vector _v1 to vector _v2, so that
+    /// _v2.Normalize() == this * _v1.Normalize() holds.
+    ///
+    /// \param[in] _v1 The first vector
+    /// \param[in] _v2 The second vector
+    public: void SetFrom2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
+    {
+      const T _v1LengthSquared = _v1.SquaredLength();
+      if (_v1LengthSquared <= 0.0)
       {
-        this->data[0][0] = _v00;
-        this->data[0][1] = _v01;
-        this->data[0][2] = _v02;
-        this->data[1][0] = _v10;
-        this->data[1][1] = _v11;
-        this->data[1][2] = _v12;
-        this->data[2][0] = _v20;
-        this->data[2][1] = _v21;
-        this->data[2][2] = _v22;
+        // zero vector - we can't handle this
+        this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
+        return;
       }
 
-      /// \brief Set the matrix from three axis (1 per column).
-      /// \param[in] _xAxis The x axis, the first column of the matrix.
-      /// \param[in] _yAxis The y axis, the second column of the matrix.
-      /// \param[in] _zAxis The z axis, the third column of the matrix.
-      public: void SetAxes(const Vector3<T> &_xAxis,
-                           const Vector3<T> &_yAxis,
-                           const Vector3<T> &_zAxis)
+      const T _v2LengthSquared = _v2.SquaredLength();
+      if (_v2LengthSquared <= 0.0)
       {
-        this->SetCol(0, _xAxis);
-        this->SetCol(1, _yAxis);
-        this->SetCol(2, _zAxis);
+        // zero vector - we can't handle this
+        this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
+        return;
       }
 
-      /// \brief Set as a rotation matrix from an axis and angle.
-      /// \param[in] _axis the axis
-      /// \param[in] _angle ccw rotation around the axis in radians
-      public: void SetFromAxisAngle(const Vector3<T> &_axis, T _angle)
+      const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
+      if (fabs(dot - 1.0) <= 1e-6)
       {
-        T c = cos(_angle);
-        T s = sin(_angle);
-        T C = 1-c;
-
-        this->data[0][0] = _axis.X()*_axis.X()*C + c;
-        this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
-        this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
-
-        this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
-        this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
-        this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
-
-        this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
-        this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
-        this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
+        // the vectors are parallel
+        this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
+        return;
       }
-
-      /// \brief Set as a rotation matrix to represent rotation from
-      /// vector _v1 to vector _v2, so that
-      /// _v2.Normalize() == this * _v1.Normalize() holds.
-      ///
-      /// \param[in] _v1 The first vector
-      /// \param[in] _v2 The second vector
-      public: void SetFrom2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
+      else if (fabs(dot + 1.0) <= 1e-6)
       {
-        const T _v1LengthSquared = _v1.SquaredLength();
-        if (_v1LengthSquared <= 0.0)
-        {
-          // zero vector - we can't handle this
-          this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
-          return;
-        }
-
-        const T _v2LengthSquared = _v2.SquaredLength();
-        if (_v2LengthSquared <= 0.0)
-        {
-          // zero vector - we can't handle this
-          this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
-          return;
-        }
-
-        const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
-        if (fabs(dot - 1.0) <= 1e-6)
-        {
-          // the vectors are parallel
-          this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
-          return;
-        }
-        else if (fabs(dot + 1.0) <= 1e-6)
-        {
-          // the vectors are opposite
-          this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
-          return;
-        }
+        // the vectors are opposite
+        this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
+        return;
+      }
 
-        const Vector3<T> cross = _v1.Cross(_v2).Normalize();
+      const Vector3<T> cross = _v1.Cross(_v2).Normalize();
 
-        this->SetFromAxisAngle(cross, acos(dot));
-      }
+      this->SetFromAxisAngle(cross, acos(dot));
+    }
 
-      /// \brief Set a column.
-      /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the
-      /// range [0, 2].
-      /// \param[in] _v The value to set in each row of the column.
-      public: void SetCol(unsigned int _c, const Vector3<T> &_v)
-      {
-        unsigned int c = clamp(_c, 0u, 2u);
+    /// \brief Set a column.
+    /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the
+    /// range [0, 2].
+    /// \param[in] _v The value to set in each row of the column.
+    public: void SetCol(unsigned int _c, const Vector3<T> &_v)
+    {
+      unsigned int c = clamp(_c, 0u, 2u);
 
-        this->data[0][c] = _v.X();
-        this->data[1][c] = _v.Y();
-        this->data[2][c] = _v.Z();
-      }
+      this->data[0][c] = _v.X();
+      this->data[1][c] = _v.Y();
+      this->data[2][c] = _v.Z();
+    }
 
-      /// \brief Equal operator. this = _mat
-      /// \param _mat Matrix to copy.
-      /// \return This matrix.
-      public: Matrix3<T> &operator=(const Matrix3<T> &_mat) = default;
+    /// \brief Equal operator. this = _mat
+    /// \param _mat Matrix to copy.
+    /// \return This matrix.
+    public: Matrix3<T> &operator=(const Matrix3<T> &_mat) = default;
 
-      /// \brief Subtraction operator.
-      /// \param[in] _m Matrix to subtract.
-      /// \return The element wise difference of two matrices.
-      public: Matrix3<T> operator-(const Matrix3<T> &_m) const
-      {
-        return Matrix3<T>(
-            this->data[0][0] - _m(0, 0),
-            this->data[0][1] - _m(0, 1),
-            this->data[0][2] - _m(0, 2),
-            this->data[1][0] - _m(1, 0),
-            this->data[1][1] - _m(1, 1),
-            this->data[1][2] - _m(1, 2),
-            this->data[2][0] - _m(2, 0),
-            this->data[2][1] - _m(2, 1),
-            this->data[2][2] - _m(2, 2));
-      }
+    /// \brief Subtraction operator.
+    /// \param[in] _m Matrix to subtract.
+    /// \return The element wise difference of two matrices.
+    public: Matrix3<T> operator-(const Matrix3<T> &_m) const
+    {
+      return Matrix3<T>(
+          this->data[0][0] - _m(0, 0),
+          this->data[0][1] - _m(0, 1),
+          this->data[0][2] - _m(0, 2),
+          this->data[1][0] - _m(1, 0),
+          this->data[1][1] - _m(1, 1),
+          this->data[1][2] - _m(1, 2),
+          this->data[2][0] - _m(2, 0),
+          this->data[2][1] - _m(2, 1),
+          this->data[2][2] - _m(2, 2));
+    }
 
-      /// \brief Addition operation.
-      /// \param[in] _m Matrix to add.
-      /// \return The element wise sum of two matrices
-      public: Matrix3<T> operator+(const Matrix3<T> &_m) const
-      {
-        return Matrix3<T>(
-            this->data[0][0]+_m(0, 0),
-            this->data[0][1]+_m(0, 1),
-            this->data[0][2]+_m(0, 2),
-            this->data[1][0]+_m(1, 0),
-            this->data[1][1]+_m(1, 1),
-            this->data[1][2]+_m(1, 2),
-            this->data[2][0]+_m(2, 0),
-            this->data[2][1]+_m(2, 1),
-            this->data[2][2]+_m(2, 2));
-      }
+    /// \brief Addition operation.
+    /// \param[in] _m Matrix to add.
+    /// \return The element wise sum of two matrices
+    public: Matrix3<T> operator+(const Matrix3<T> &_m) const
+    {
+      return Matrix3<T>(
+          this->data[0][0]+_m(0, 0),
+          this->data[0][1]+_m(0, 1),
+          this->data[0][2]+_m(0, 2),
+          this->data[1][0]+_m(1, 0),
+          this->data[1][1]+_m(1, 1),
+          this->data[1][2]+_m(1, 2),
+          this->data[2][0]+_m(2, 0),
+          this->data[2][1]+_m(2, 1),
+          this->data[2][2]+_m(2, 2));
+    }
 
-      /// \brief Scalar multiplication operator.
-      /// \param[in] _s Value to multiply.
-      /// \return The element wise scalar multiplication.
-      public: Matrix3<T> operator*(const T &_s) const
-      {
-        return Matrix3<T>(
-          _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
-          _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
-          _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
-      }
+    /// \brief Scalar multiplication operator.
+    /// \param[in] _s Value to multiply.
+    /// \return The element wise scalar multiplication.
+    public: Matrix3<T> operator*(const T &_s) const
+    {
+      return Matrix3<T>(
+        _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
+        _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
+        _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
+    }
 
-      /// \brief Matrix multiplication operator
-      /// \param[in] _m Matrix3<T> to multiply
-      /// \return Product of this * _m
-      public: Matrix3<T> operator*(const Matrix3<T> &_m) const
-      {
-        return Matrix3<T>(
-            // first row
-            this->data[0][0]*_m(0, 0)+
-            this->data[0][1]*_m(1, 0)+
-            this->data[0][2]*_m(2, 0),
-
-            this->data[0][0]*_m(0, 1)+
-            this->data[0][1]*_m(1, 1)+
-            this->data[0][2]*_m(2, 1),
-
-            this->data[0][0]*_m(0, 2)+
-            this->data[0][1]*_m(1, 2)+
-            this->data[0][2]*_m(2, 2),
-
-            // second row
-            this->data[1][0]*_m(0, 0)+
-            this->data[1][1]*_m(1, 0)+
-            this->data[1][2]*_m(2, 0),
-
-            this->data[1][0]*_m(0, 1)+
-            this->data[1][1]*_m(1, 1)+
-            this->data[1][2]*_m(2, 1),
-
-            this->data[1][0]*_m(0, 2)+
-            this->data[1][1]*_m(1, 2)+
-            this->data[1][2]*_m(2, 2),
-
-            // third row
-            this->data[2][0]*_m(0, 0)+
-            this->data[2][1]*_m(1, 0)+
-            this->data[2][2]*_m(2, 0),
-
-            this->data[2][0]*_m(0, 1)+
-            this->data[2][1]*_m(1, 1)+
-            this->data[2][2]*_m(2, 1),
-
-            this->data[2][0]*_m(0, 2)+
-            this->data[2][1]*_m(1, 2)+
-            this->data[2][2]*_m(2, 2));
-      }
+    /// \brief Matrix multiplication operator
+    /// \param[in] _m Matrix3<T> to multiply
+    /// \return Product of this * _m
+    public: Matrix3<T> operator*(const Matrix3<T> &_m) const
+    {
+      return Matrix3<T>(
+          // first row
+          this->data[0][0]*_m(0, 0)+
+          this->data[0][1]*_m(1, 0)+
+          this->data[0][2]*_m(2, 0),
+
+          this->data[0][0]*_m(0, 1)+
+          this->data[0][1]*_m(1, 1)+
+          this->data[0][2]*_m(2, 1),
+
+          this->data[0][0]*_m(0, 2)+
+          this->data[0][1]*_m(1, 2)+
+          this->data[0][2]*_m(2, 2),
+
+          // second row
+          this->data[1][0]*_m(0, 0)+
+          this->data[1][1]*_m(1, 0)+
+          this->data[1][2]*_m(2, 0),
+
+          this->data[1][0]*_m(0, 1)+
+          this->data[1][1]*_m(1, 1)+
+          this->data[1][2]*_m(2, 1),
+
+          this->data[1][0]*_m(0, 2)+
+          this->data[1][1]*_m(1, 2)+
+          this->data[1][2]*_m(2, 2),
+
+          // third row
+          this->data[2][0]*_m(0, 0)+
+          this->data[2][1]*_m(1, 0)+
+          this->data[2][2]*_m(2, 0),
+
+          this->data[2][0]*_m(0, 1)+
+          this->data[2][1]*_m(1, 1)+
+          this->data[2][2]*_m(2, 1),
+
+          this->data[2][0]*_m(0, 2)+
+          this->data[2][1]*_m(1, 2)+
+          this->data[2][2]*_m(2, 2));
+    }
 
-      /// \brief Multiplication operator with Vector3 on the right
-      /// treated like a column vector.
-      /// \param _vec Vector3
-      /// \return Resulting vector from multiplication
-      public: Vector3<T> operator*(const Vector3<T> &_vec) const
-      {
-        return Vector3<T>(
-            this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
-            this->data[0][2]*_vec.Z(),
-            this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
-            this->data[1][2]*_vec.Z(),
-            this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
-            this->data[2][2]*_vec.Z());
-      }
+    /// \brief Multiplication operator with Vector3 on the right
+    /// treated like a column vector.
+    /// \param _vec Vector3
+    /// \return Resulting vector from multiplication
+    public: Vector3<T> operator*(const Vector3<T> &_vec) const
+    {
+      return Vector3<T>(
+          this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
+          this->data[0][2]*_vec.Z(),
+          this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
+          this->data[1][2]*_vec.Z(),
+          this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
+          this->data[2][2]*_vec.Z());
+    }
 
-      /// \brief Matrix multiplication operator for scaling.
-      /// \param[in] _s Scaling factor.
-      /// \param[in] _m Input matrix.
-      /// \return A scaled matrix.
-      public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
-      {
-        return _m * _s;
-      }
+    /// \brief Matrix multiplication operator for scaling.
+    /// \param[in] _s Scaling factor.
+    /// \param[in] _m Input matrix.
+    /// \return A scaled matrix.
+    public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
+    {
+      return _m * _s;
+    }
 
-      /// \brief Matrix left multiplication operator for Vector3.
-      /// Treats the Vector3 like a row vector multiplying the matrix
-      /// from the left.
-      /// \param[in] _v Input vector.
-      /// \param[in] _m Input matrix.
-      /// \return The product vector.
-      public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
-                                                 const Matrix3<T> &_m)
-      {
-        return Vector3<T>(
-            _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
-            _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
-            _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
-      }
+    /// \brief Matrix left multiplication operator for Vector3.
+    /// Treats the Vector3 like a row vector multiplying the matrix
+    /// from the left.
+    /// \param[in] _v Input vector.
+    /// \param[in] _m Input matrix.
+    /// \return The product vector.
+    public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
+                                               const Matrix3<T> &_m)
+    {
+      return Vector3<T>(
+          _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
+          _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
+          _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
+    }
 
-      /// \brief Equality test with tolerance.
-      /// \param[in] _m the matrix to compare to
-      /// \param[in] _tol equality tolerance.
-      /// \return true if the elements of the matrices are equal within
-      /// the tolerence specified by _tol.
-      public: bool Equal(const Matrix3 &_m, const T &_tol) const
-      {
-        return equal<T>(this->data[0][0], _m(0, 0), _tol)
-            && equal<T>(this->data[0][1], _m(0, 1), _tol)
-            && equal<T>(this->data[0][2], _m(0, 2), _tol)
-            && equal<T>(this->data[1][0], _m(1, 0), _tol)
-            && equal<T>(this->data[1][1], _m(1, 1), _tol)
-            && equal<T>(this->data[1][2], _m(1, 2), _tol)
-            && equal<T>(this->data[2][0], _m(2, 0), _tol)
-            && equal<T>(this->data[2][1], _m(2, 1), _tol)
-            && equal<T>(this->data[2][2], _m(2, 2), _tol);
-      }
+    /// \brief Equality test with tolerance.
+    /// \param[in] _m the matrix to compare to
+    /// \param[in] _tol equality tolerance.
+    /// \return true if the elements of the matrices are equal within
+    /// the tolerence specified by _tol.
+    public: bool Equal(const Matrix3 &_m, const T &_tol) const
+    {
+      return equal<T>(this->data[0][0], _m(0, 0), _tol)
+          && equal<T>(this->data[0][1], _m(0, 1), _tol)
+          && equal<T>(this->data[0][2], _m(0, 2), _tol)
+          && equal<T>(this->data[1][0], _m(1, 0), _tol)
+          && equal<T>(this->data[1][1], _m(1, 1), _tol)
+          && equal<T>(this->data[1][2], _m(1, 2), _tol)
+          && equal<T>(this->data[2][0], _m(2, 0), _tol)
+          && equal<T>(this->data[2][1], _m(2, 1), _tol)
+          && equal<T>(this->data[2][2], _m(2, 2), _tol);
+    }
 
-      /// \brief Equality test operator.
-      /// \param[in] _m Matrix3<T> to test.
-      /// \return True if equal (using the default tolerance of 1e-6).
-      public: bool operator==(const Matrix3<T> &_m) const
-      {
-        return this->Equal(_m, static_cast<T>(1e-6));
-      }
+    /// \brief Equality test operator.
+    /// \param[in] _m Matrix3<T> to test.
+    /// \return True if equal (using the default tolerance of 1e-6).
+    public: bool operator==(const Matrix3<T> &_m) const
+    {
+      return this->Equal(_m, static_cast<T>(1e-6));
+    }
 
-      /// \brief Set as a 3x3 rotation matrix from a quaternion.
-      /// \param[in] _q Quaternion to set the matrix3 from.
-      /// \return Reference to the new matrix3 object.
-      public: Matrix3<T> &operator=(const Quaternion<T> &_q)
-      {
-        return *this = Matrix3<T>(_q);
-      }
+    /// \brief Set as a 3x3 rotation matrix from a quaternion.
+    /// \param[in] _q Quaternion to set the matrix3 from.
+    /// \return Reference to the new matrix3 object.
+    public: Matrix3<T> &operator=(const Quaternion<T> &_q)
+    {
+      return *this = Matrix3<T>(_q);
+    }
 
-      /// \brief Inequality test operator.
-      /// \param[in] _m Matrix3<T> to test.
-      /// \return True if not equal (using the default tolerance of 1e-6).
-      public: bool operator!=(const Matrix3<T> &_m) const
-      {
-        return !(*this == _m);
-      }
+    /// \brief Inequality test operator.
+    /// \param[in] _m Matrix3<T> to test.
+    /// \return True if not equal (using the default tolerance of 1e-6).
+    public: bool operator!=(const Matrix3<T> &_m) const
+    {
+      return !(*this == _m);
+    }
 
-      /// \brief Array subscript operator.
-      /// \param[in] _row row index. _row is clamped to the range [0,2]
-      /// \param[in] _col column index. _col is clamped to the range [0,2]
-      /// \return a pointer to the row
-      public: inline T operator()(size_t _row, size_t _col) const
-      {
-        return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
-                         [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
-      }
+    /// \brief Array subscript operator.
+    /// \param[in] _row row index. _row is clamped to the range [0,2]
+    /// \param[in] _col column index. _col is clamped to the range [0,2]
+    /// \return a pointer to the row
+    public: inline T operator()(size_t _row, size_t _col) const
+    {
+      return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
+                       [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
+    }
 
-      /// \brief Array subscript operator.
-      /// \param[in] _row row index. _row is clamped to the range [0,2]
-      /// \param[in] _col column index. _col is clamped to the range [0,2]
-      /// \return a pointer to the row
-      public: inline T &operator()(size_t _row, size_t _col)
-      {
-        return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
-                         [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
-      }
+    /// \brief Array subscript operator.
+    /// \param[in] _row row index. _row is clamped to the range [0,2]
+    /// \param[in] _col column index. _col is clamped to the range [0,2]
+    /// \return a pointer to the row
+    public: inline T &operator()(size_t _row, size_t _col)
+    {
+      return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
+                       [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
+    }
 
-      /// \brief Return the determinant of the matrix.
-      /// \return Determinant of this matrix.
-      public: T Determinant() const
-      {
-        T t0 = this->data[2][2]*this->data[1][1]
-             - this->data[2][1]*this->data[1][2];
+    /// \brief Return the determinant of the matrix.
+    /// \return Determinant of this matrix.
+    public: T Determinant() const
+    {
+      T t0 = this->data[2][2]*this->data[1][1]
+           - this->data[2][1]*this->data[1][2];
 
-        T t1 = -(this->data[2][2]*this->data[1][0]
-                -this->data[2][0]*this->data[1][2]);
+      T t1 = -(this->data[2][2]*this->data[1][0]
+              -this->data[2][0]*this->data[1][2]);
 
-        T t2 = this->data[2][1]*this->data[1][0]
-             - this->data[2][0]*this->data[1][1];
+      T t2 = this->data[2][1]*this->data[1][0]
+           - this->data[2][0]*this->data[1][1];
 
-        return t0 * this->data[0][0]
-             + t1 * this->data[0][1]
-             + t2 * this->data[0][2];
-      }
+      return t0 * this->data[0][0]
+           + t1 * this->data[0][1]
+           + t2 * this->data[0][2];
+    }
 
-      /// \brief Return the inverse matrix.
-      /// \return Inverse of this matrix.
-      public: Matrix3<T> Inverse() const
-      {
-        T t0 = this->data[2][2]*this->data[1][1] -
-                    this->data[2][1]*this->data[1][2];
-
-        T t1 = -(this->data[2][2]*this->data[1][0] -
-                      this->data[2][0]*this->data[1][2]);
-
-        T t2 = this->data[2][1]*this->data[1][0] -
-                    this->data[2][0]*this->data[1][1];
-
-        T invDet = 1.0 / (t0 * this->data[0][0] +
-                               t1 * this->data[0][1] +
-                               t2 * this->data[0][2]);
-
-        return invDet * Matrix3<T>(
-          t0,
-          - (this->data[2][2] * this->data[0][1] -
-             this->data[2][1] * this->data[0][2]),
-          + (this->data[1][2] * this->data[0][1] -
-             this->data[1][1] * this->data[0][2]),
-          t1,
-          + (this->data[2][2] * this->data[0][0] -
-             this->data[2][0] * this->data[0][2]),
-          - (this->data[1][2] * this->data[0][0] -
-             this->data[1][0] * this->data[0][2]),
-          t2,
-          - (this->data[2][1] * this->data[0][0] -
-             this->data[2][0] * this->data[0][1]),
-          + (this->data[1][1] * this->data[0][0] -
-             this->data[1][0] * this->data[0][1]));
-      }
+    /// \brief Return the inverse matrix.
+    /// \return Inverse of this matrix.
+    public: Matrix3<T> Inverse() const
+    {
+      T t0 = this->data[2][2]*this->data[1][1] -
+                  this->data[2][1]*this->data[1][2];
+
+      T t1 = -(this->data[2][2]*this->data[1][0] -
+                    this->data[2][0]*this->data[1][2]);
+
+      T t2 = this->data[2][1]*this->data[1][0] -
+                  this->data[2][0]*this->data[1][1];
+
+      T invDet = 1.0 / (t0 * this->data[0][0] +
+                             t1 * this->data[0][1] +
+                             t2 * this->data[0][2]);
+
+      return invDet * Matrix3<T>(
+        t0,
+        - (this->data[2][2] * this->data[0][1] -
+           this->data[2][1] * this->data[0][2]),
+        + (this->data[1][2] * this->data[0][1] -
+           this->data[1][1] * this->data[0][2]),
+        t1,
+        + (this->data[2][2] * this->data[0][0] -
+           this->data[2][0] * this->data[0][2]),
+        - (this->data[1][2] * this->data[0][0] -
+           this->data[1][0] * this->data[0][2]),
+        t2,
+        - (this->data[2][1] * this->data[0][0] -
+           this->data[2][0] * this->data[0][1]),
+        + (this->data[1][1] * this->data[0][0] -
+           this->data[1][0] * this->data[0][1]));
+    }
 
-      /// \brief Transpose this matrix.
-      public: void Transpose()
-      {
-        std::swap(this->data[0][1], this->data[1][0]);
-        std::swap(this->data[0][2], this->data[2][0]);
-        std::swap(this->data[1][2], this->data[2][1]);
-      }
+    /// \brief Transpose this matrix.
+    public: void Transpose()
+    {
+      std::swap(this->data[0][1], this->data[1][0]);
+      std::swap(this->data[0][2], this->data[2][0]);
+      std::swap(this->data[1][2], this->data[2][1]);
+    }
 
-      /// \brief Return the transpose of this matrix.
-      /// \return Transpose of this matrix.
-      public: Matrix3<T> Transposed() const
-      {
-        return Matrix3<T>(
-          this->data[0][0], this->data[1][0], this->data[2][0],
-          this->data[0][1], this->data[1][1], this->data[2][1],
-          this->data[0][2], this->data[1][2], this->data[2][2]);
-      }
+    /// \brief Return the transpose of this matrix.
+    /// \return Transpose of this matrix.
+    public: Matrix3<T> Transposed() const
+    {
+      return Matrix3<T>(
+        this->data[0][0], this->data[1][0], this->data[2][0],
+        this->data[0][1], this->data[1][1], this->data[2][1],
+        this->data[0][2], this->data[1][2], this->data[2][2]);
+    }
 
-      /// \brief Stream insertion operator. This operator outputs all
-      /// 9 scalar values in the matrix separated by a single space.
-      /// \param[in, out] _out Output stream.
-      /// \param[in] _m Matrix to output.
-      /// \return The stream.
-      public: friend std::ostream &operator<<(
-                  std::ostream &_out, const gz::math::Matrix3<T> &_m)
+    /// \brief Stream insertion operator. This operator outputs all
+    /// 9 scalar values in the matrix separated by a single space.
+    /// \param[in, out] _out Output stream.
+    /// \param[in] _m Matrix to output.
+    /// \return The stream.
+    public: friend std::ostream &operator<<(
+                std::ostream &_out, const gz::math::Matrix3<T> &_m)
+    {
+      for (auto i : {0, 1, 2})
       {
-        for (auto i : {0, 1, 2})
+        for (auto j : {0, 1, 2})
         {
-          for (auto j : {0, 1, 2})
-          {
-            if (!(i == 0 && j == 0))
-              _out << " ";
+          if (!(i == 0 && j == 0))
+            _out << " ";
 
-            appendToStream(_out, _m(i, j));
-          }
+          appendToStream(_out, _m(i, j));
         }
-
-        return _out;
       }
 
-      /// \brief Stream extraction operator. This operator requires 9 space
-      /// separated scalar values, such as "1 2 3 4 5 6 7 8 9".
-      /// \param [in, out] _in Input stream.
-      /// \param [out] _m Matrix3 to read values into.
-      /// \return The stream.
-      public: friend std::istream &operator>>(
-                  std::istream &_in, gz::math::Matrix3<T> &_m)
+      return _out;
+    }
+
+    /// \brief Stream extraction operator. This operator requires 9 space
+    /// separated scalar values, such as "1 2 3 4 5 6 7 8 9".
+    /// \param [in, out] _in Input stream.
+    /// \param [out] _m Matrix3 to read values into.
+    /// \return The stream.
+    public: friend std::istream &operator>>(
+                std::istream &_in, gz::math::Matrix3<T> &_m)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      T d[9];
+      _in >> d[0] >> d[1] >> d[2]
+          >> d[3] >> d[4] >> d[5]
+          >> d[6] >> d[7] >> d[8];
+
+      if (!_in.fail())
       {
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        T d[9];
-        _in >> d[0] >> d[1] >> d[2]
-            >> d[3] >> d[4] >> d[5]
-            >> d[6] >> d[7] >> d[8];
-
-        if (!_in.fail())
-        {
-          _m.Set(d[0], d[1], d[2],
-                 d[3], d[4], d[5],
-                 d[6], d[7], d[8]);
-        }
-        return _in;
+        _m.Set(d[0], d[1], d[2],
+               d[3], d[4], d[5],
+               d[6], d[7], d[8]);
       }
+      return _in;
+    }
 
-      /// \brief the 3x3 matrix
-      private: T data[3][3];
-    };
+    /// \brief the 3x3 matrix
+    private: T data[3][3];
+  };
 
-    namespace detail {
+  namespace detail {
 
-      template<typename T>
-      constexpr Matrix3<T> gMatrix3Identity(
-          1, 0, 0,
-          0, 1, 0,
-          0, 0, 1);
+    template<typename T>
+    constexpr Matrix3<T> gMatrix3Identity(
+        1, 0, 0,
+        0, 1, 0,
+        0, 0, 1);
 
-      template<typename T>
-      constexpr Matrix3<T> gMatrix3Zero(
-          0, 0, 0,
-          0, 0, 0,
-          0, 0, 0);
+    template<typename T>
+    constexpr Matrix3<T> gMatrix3Zero(
+        0, 0, 0,
+        0, 0, 0,
+        0, 0, 0);
 
-    }  // namespace detail
+  }  // namespace detail
 
-    template<typename T>
-    const Matrix3<T> &Matrix3<T>::Identity = detail::gMatrix3Identity<T>;
+  template<typename T>
+  const Matrix3<T> &Matrix3<T>::Identity = detail::gMatrix3Identity<T>;
 
-    template<typename T>
-    const Matrix3<T> &Matrix3<T>::Zero = detail::gMatrix3Zero<T>;
+  template<typename T>
+  const Matrix3<T> &Matrix3<T>::Zero = detail::gMatrix3Zero<T>;
 
-    /// typedef Matrix3<int> as Matrix3i.
-    typedef Matrix3<int> Matrix3i;
+  /// typedef Matrix3<int> as Matrix3i.
+  typedef Matrix3<int> Matrix3i;
 
-    /// typedef Matrix3<double> as Matrix3d.
-    typedef Matrix3<double> Matrix3d;
+  /// typedef Matrix3<double> as Matrix3d.
+  typedef Matrix3<double> Matrix3d;
 
-    /// typedef Matrix3<float> as Matrix3f.
-    typedef Matrix3<float> Matrix3f;
-    }
-  }
-}
-#endif
+  /// typedef Matrix3<float> as Matrix3f.
+  typedef Matrix3<float> Matrix3f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_MATRIX3_HH_
diff --git a/include/gz/math/Matrix4.hh b/include/gz/math/Matrix4.hh
index d63393cba..24834096d 100644
--- a/include/gz/math/Matrix4.hh
+++ b/include/gz/math/Matrix4.hh
@@ -25,856 +25,853 @@
 #include <gz/math/Pose3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Matrix4 Matrix4.hh gz/math/Matrix4.hh
+  /// \brief A 4x4 matrix class
+  template<typename T>
+  class Matrix4
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Matrix4 Matrix4.hh gz/math/Matrix4.hh
-    /// \brief A 4x4 matrix class
-    template<typename T>
-    class Matrix4
-    {
-      /// \brief Identity matrix
-      public: static const Matrix4<T> &Identity;
+    /// \brief Identity matrix
+    public: static const Matrix4<T> &Identity;
 
-      /// \brief Zero matrix
-      public: static const Matrix4<T> &Zero;
+    /// \brief Zero matrix
+    public: static const Matrix4<T> &Zero;
 
-      /// \brief Constructor
-      public: Matrix4()
-      {
-        memset(this->data, 0, sizeof(this->data[0][0])*16);
-      }
+    /// \brief Constructor
+    public: Matrix4()
+    {
+      memset(this->data, 0, sizeof(this->data[0][0])*16);
+    }
 
-      /// \brief Copy constructor
-      /// \param _m Matrix to copy
-      public: Matrix4(const Matrix4<T> &_m) = default;
-
-      /// \brief Constructor
-      /// \param[in] _v00 Row 0, Col 0 value
-      /// \param[in] _v01 Row 0, Col 1 value
-      /// \param[in] _v02 Row 0, Col 2 value
-      /// \param[in] _v03 Row 0, Col 3 value
-      /// \param[in] _v10 Row 1, Col 0 value
-      /// \param[in] _v11 Row 1, Col 1 value
-      /// \param[in] _v12 Row 1, Col 2 value
-      /// \param[in] _v13 Row 1, Col 3 value
-      /// \param[in] _v20 Row 2, Col 0 value
-      /// \param[in] _v21 Row 2, Col 1 value
-      /// \param[in] _v22 Row 2, Col 2 value
-      /// \param[in] _v23 Row 2, Col 3 value
-      /// \param[in] _v30 Row 3, Col 0 value
-      /// \param[in] _v31 Row 3, Col 1 value
-      /// \param[in] _v32 Row 3, Col 2 value
-      /// \param[in] _v33 Row 3, Col 3 value
-      public: constexpr Matrix4(T _v00, T _v01, T _v02, T _v03,
-                                T _v10, T _v11, T _v12, T _v13,
-                                T _v20, T _v21, T _v22, T _v23,
-                                T _v30, T _v31, T _v32, T _v33)
-      : data{{_v00, _v01, _v02, _v03},
-             {_v10, _v11, _v12, _v13},
-             {_v20, _v21, _v22, _v23},
-             {_v30, _v31, _v32, _v33}}
-      {
-      }
+    /// \brief Copy constructor
+    /// \param _m Matrix to copy
+    public: Matrix4(const Matrix4<T> &_m) = default;
+
+    /// \brief Constructor
+    /// \param[in] _v00 Row 0, Col 0 value
+    /// \param[in] _v01 Row 0, Col 1 value
+    /// \param[in] _v02 Row 0, Col 2 value
+    /// \param[in] _v03 Row 0, Col 3 value
+    /// \param[in] _v10 Row 1, Col 0 value
+    /// \param[in] _v11 Row 1, Col 1 value
+    /// \param[in] _v12 Row 1, Col 2 value
+    /// \param[in] _v13 Row 1, Col 3 value
+    /// \param[in] _v20 Row 2, Col 0 value
+    /// \param[in] _v21 Row 2, Col 1 value
+    /// \param[in] _v22 Row 2, Col 2 value
+    /// \param[in] _v23 Row 2, Col 3 value
+    /// \param[in] _v30 Row 3, Col 0 value
+    /// \param[in] _v31 Row 3, Col 1 value
+    /// \param[in] _v32 Row 3, Col 2 value
+    /// \param[in] _v33 Row 3, Col 3 value
+    public: constexpr Matrix4(T _v00, T _v01, T _v02, T _v03,
+                              T _v10, T _v11, T _v12, T _v13,
+                              T _v20, T _v21, T _v22, T _v23,
+                              T _v30, T _v31, T _v32, T _v33)
+    : data{{_v00, _v01, _v02, _v03},
+           {_v10, _v11, _v12, _v13},
+           {_v20, _v21, _v22, _v23},
+           {_v30, _v31, _v32, _v33}}
+    {
+    }
 
-      /// \brief Construct Matrix4 from a quaternion.
-      /// \param[in] _q Quaternion.
-      public: explicit Matrix4(const Quaternion<T> &_q)
-      {
-        Quaternion<T> qt = _q;
-        qt.Normalize();
-        this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
-                  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
-                  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
-                  0,
-
-                  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
-                  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
-                  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
-                  0,
-
-                  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
-                  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
-                  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
-                  0,
-
-                  0, 0, 0, 1);
-      }
+    /// \brief Construct Matrix4 from a quaternion.
+    /// \param[in] _q Quaternion.
+    public: explicit Matrix4(const Quaternion<T> &_q)
+    {
+      Quaternion<T> qt = _q;
+      qt.Normalize();
+      this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
+                2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
+                2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
+                0,
+
+                2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
+                1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
+                2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
+                0,
+
+                2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
+                2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
+                1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
+                0,
+
+                0, 0, 0, 1);
+    }
 
-      /// \brief Construct Matrix4 from a math::Pose3
-      /// \param[in] _pose Pose.
-      public: explicit Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
-      {
-        this->SetTranslation(_pose.Pos());
-      }
+    /// \brief Construct Matrix4 from a math::Pose3
+    /// \param[in] _pose Pose.
+    public: explicit Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
+    {
+      this->SetTranslation(_pose.Pos());
+    }
 
-      /// \brief Destructor
-      public: ~Matrix4() = default;
-
-      /// \brief Change the values
-      /// \param[in] _v00 Row 0, Col 0 value
-      /// \param[in] _v01 Row 0, Col 1 value
-      /// \param[in] _v02 Row 0, Col 2 value
-      /// \param[in] _v03 Row 0, Col 3 value
-      /// \param[in] _v10 Row 1, Col 0 value
-      /// \param[in] _v11 Row 1, Col 1 value
-      /// \param[in] _v12 Row 1, Col 2 value
-      /// \param[in] _v13 Row 1, Col 3 value
-      /// \param[in] _v20 Row 2, Col 0 value
-      /// \param[in] _v21 Row 2, Col 1 value
-      /// \param[in] _v22 Row 2, Col 2 value
-      /// \param[in] _v23 Row 2, Col 3 value
-      /// \param[in] _v30 Row 3, Col 0 value
-      /// \param[in] _v31 Row 3, Col 1 value
-      /// \param[in] _v32 Row 3, Col 2 value
-      /// \param[in] _v33 Row 3, Col 3 value
-      public: void Set(
-            T _v00, T _v01, T _v02, T _v03,
-            T _v10, T _v11, T _v12, T _v13,
-            T _v20, T _v21, T _v22, T _v23,
-            T _v30, T _v31, T _v32, T _v33)
-      {
-        this->data[0][0] = _v00;
-        this->data[0][1] = _v01;
-        this->data[0][2] = _v02;
-        this->data[0][3] = _v03;
-
-        this->data[1][0] = _v10;
-        this->data[1][1] = _v11;
-        this->data[1][2] = _v12;
-        this->data[1][3] = _v13;
-
-        this->data[2][0] = _v20;
-        this->data[2][1] = _v21;
-        this->data[2][2] = _v22;
-        this->data[2][3] = _v23;
-
-        this->data[3][0] = _v30;
-        this->data[3][1] = _v31;
-        this->data[3][2] = _v32;
-        this->data[3][3] = _v33;
-      }
+    /// \brief Destructor
+    public: ~Matrix4() = default;
+
+    /// \brief Change the values
+    /// \param[in] _v00 Row 0, Col 0 value
+    /// \param[in] _v01 Row 0, Col 1 value
+    /// \param[in] _v02 Row 0, Col 2 value
+    /// \param[in] _v03 Row 0, Col 3 value
+    /// \param[in] _v10 Row 1, Col 0 value
+    /// \param[in] _v11 Row 1, Col 1 value
+    /// \param[in] _v12 Row 1, Col 2 value
+    /// \param[in] _v13 Row 1, Col 3 value
+    /// \param[in] _v20 Row 2, Col 0 value
+    /// \param[in] _v21 Row 2, Col 1 value
+    /// \param[in] _v22 Row 2, Col 2 value
+    /// \param[in] _v23 Row 2, Col 3 value
+    /// \param[in] _v30 Row 3, Col 0 value
+    /// \param[in] _v31 Row 3, Col 1 value
+    /// \param[in] _v32 Row 3, Col 2 value
+    /// \param[in] _v33 Row 3, Col 3 value
+    public: void Set(
+          T _v00, T _v01, T _v02, T _v03,
+          T _v10, T _v11, T _v12, T _v13,
+          T _v20, T _v21, T _v22, T _v23,
+          T _v30, T _v31, T _v32, T _v33)
+    {
+      this->data[0][0] = _v00;
+      this->data[0][1] = _v01;
+      this->data[0][2] = _v02;
+      this->data[0][3] = _v03;
+
+      this->data[1][0] = _v10;
+      this->data[1][1] = _v11;
+      this->data[1][2] = _v12;
+      this->data[1][3] = _v13;
+
+      this->data[2][0] = _v20;
+      this->data[2][1] = _v21;
+      this->data[2][2] = _v22;
+      this->data[2][3] = _v23;
+
+      this->data[3][0] = _v30;
+      this->data[3][1] = _v31;
+      this->data[3][2] = _v32;
+      this->data[3][3] = _v33;
+    }
 
-      /// \brief Set the upper-left 3x3 matrix from an axis and angle
-      /// \param[in] _axis the axis
-      /// \param[in] _angle ccw rotation around the axis in radians
-      public: void Axis(const Vector3<T> &_axis, T _angle)
-      {
-        T c = cos(_angle);
-        T s = sin(_angle);
-        T C = 1-c;
+    /// \brief Set the upper-left 3x3 matrix from an axis and angle
+    /// \param[in] _axis the axis
+    /// \param[in] _angle ccw rotation around the axis in radians
+    public: void Axis(const Vector3<T> &_axis, T _angle)
+    {
+      T c = cos(_angle);
+      T s = sin(_angle);
+      T C = 1-c;
 
-        this->data[0][0] = _axis.X()*_axis.X()*C + c;
-        this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
-        this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
+      this->data[0][0] = _axis.X()*_axis.X()*C + c;
+      this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
+      this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
 
-        this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
-        this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
-        this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
+      this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
+      this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
+      this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
 
-        this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
-        this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
-        this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
-      }
+      this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
+      this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
+      this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
+    }
 
-      /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ]
-      /// \param[in] _t Values to set
-      public: void SetTranslation(const Vector3<T> &_t)
-      {
-        this->data[0][3] = _t.X();
-        this->data[1][3] = _t.Y();
-        this->data[2][3] = _t.Z();
-      }
+    /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ]
+    /// \param[in] _t Values to set
+    public: void SetTranslation(const Vector3<T> &_t)
+    {
+      this->data[0][3] = _t.X();
+      this->data[1][3] = _t.Y();
+      this->data[2][3] = _t.Z();
+    }
 
-      /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ]
-      /// \param[in] _x X translation value.
-      /// \param[in] _y Y translation value.
-      /// \param[in] _z Z translation value.
-      public: void SetTranslation(T _x, T _y, T _z)
-      {
-        this->data[0][3] = _x;
-        this->data[1][3] = _y;
-        this->data[2][3] = _z;
-      }
+    /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ]
+    /// \param[in] _x X translation value.
+    /// \param[in] _y Y translation value.
+    /// \param[in] _z Z translation value.
+    public: void SetTranslation(T _x, T _y, T _z)
+    {
+      this->data[0][3] = _x;
+      this->data[1][3] = _y;
+      this->data[2][3] = _z;
+    }
 
-      /// \brief Get the translational values as a Vector3
-      /// \return x,y,z translation values
-      public: Vector3<T> Translation() const
-      {
-        return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
-      }
+    /// \brief Get the translational values as a Vector3
+    /// \return x,y,z translation values
+    public: Vector3<T> Translation() const
+    {
+      return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
+    }
 
-      /// \brief Get the scale values as a Vector3<T>
-      /// \return x,y,z scale values
-      public: Vector3<T> Scale() const
-      {
-        return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
-      }
+    /// \brief Get the scale values as a Vector3<T>
+    /// \return x,y,z scale values
+    public: Vector3<T> Scale() const
+    {
+      return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
+    }
 
-      /// \brief Get the rotation as a quaternion
-      /// \return the rotation
-      public: Quaternion<T> Rotation() const
-      {
-        Quaternion<T> q;
-        /// algorithm from Ogre::Quaternion<T> source, which in turn is based on
-        /// Ken Shoemake's article "Quaternion Calculus and Fast Animation".
-        T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
-        T root;
-        if (trace > 0)
+    /// \brief Get the rotation as a quaternion
+    /// \return the rotation
+    public: Quaternion<T> Rotation() const
+    {
+      Quaternion<T> q;
+      /// algorithm from Ogre::Quaternion<T> source, which in turn is based on
+      /// Ken Shoemake's article "Quaternion Calculus and Fast Animation".
+      T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
+      T root;
+      if (trace > 0)
+      {
+        root = sqrt(trace + 1.0);
+        q.SetW(root / 2.0);
+        root = 1.0 / (2.0 * root);
+        q.SetX((this->data[2][1] - this->data[1][2]) * root);
+        q.SetY((this->data[0][2] - this->data[2][0]) * root);
+        q.SetZ((this->data[1][0] - this->data[0][1]) * root);
+      }
+      else
+      {
+        static unsigned int s_iNext[3] = {1, 2, 0};
+        unsigned int i = 0;
+        if (this->data[1][1] > this->data[0][0])
+          i = 1;
+        if (this->data[2][2] > this->data[i][i])
+          i = 2;
+        unsigned int j = s_iNext[i];
+        unsigned int k = s_iNext[j];
+
+        root = sqrt(this->data[i][i] - this->data[j][j] -
+                    this->data[k][k] + 1.0);
+
+        T a, b, c;
+        a = root / 2.0;
+        root = 1.0 / (2.0 * root);
+        b = (this->data[j][i] + this->data[i][j]) * root;
+        c = (this->data[k][i] + this->data[i][k]) * root;
+
+        switch (i)
         {
-          root = sqrt(trace + 1.0);
-          q.SetW(root / 2.0);
-          root = 1.0 / (2.0 * root);
-          q.SetX((this->data[2][1] - this->data[1][2]) * root);
-          q.SetY((this->data[0][2] - this->data[2][0]) * root);
-          q.SetZ((this->data[1][0] - this->data[0][1]) * root);
-        }
-        else
+          default:
+          case 0: q.SetX(a); break;
+          case 1: q.SetY(a); break;
+          case 2: q.SetZ(a); break;
+        };
+        switch (j)
         {
-          static unsigned int s_iNext[3] = {1, 2, 0};
-          unsigned int i = 0;
-          if (this->data[1][1] > this->data[0][0])
-            i = 1;
-          if (this->data[2][2] > this->data[i][i])
-            i = 2;
-          unsigned int j = s_iNext[i];
-          unsigned int k = s_iNext[j];
-
-          root = sqrt(this->data[i][i] - this->data[j][j] -
-                      this->data[k][k] + 1.0);
-
-          T a, b, c;
-          a = root / 2.0;
-          root = 1.0 / (2.0 * root);
-          b = (this->data[j][i] + this->data[i][j]) * root;
-          c = (this->data[k][i] + this->data[i][k]) * root;
-
-          switch (i)
-          {
-            default:
-            case 0: q.SetX(a); break;
-            case 1: q.SetY(a); break;
-            case 2: q.SetZ(a); break;
-          };
-          switch (j)
-          {
-            default:
-            case 0: q.SetX(b); break;
-            case 1: q.SetY(b); break;
-            case 2: q.SetZ(b); break;
-          };
-          switch (k)
-          {
-            default:
-            case 0: q.SetX(c); break;
-            case 1: q.SetY(c); break;
-            case 2: q.SetZ(c); break;
-          };
-
-          q.SetW((this->data[k][j] - this->data[j][k]) * root);
-        }
+          default:
+          case 0: q.SetX(b); break;
+          case 1: q.SetY(b); break;
+          case 2: q.SetZ(b); break;
+        };
+        switch (k)
+        {
+          default:
+          case 0: q.SetX(c); break;
+          case 1: q.SetY(c); break;
+          case 2: q.SetZ(c); break;
+        };
 
-        return q;
+        q.SetW((this->data[k][j] - this->data[j][k]) * root);
       }
 
-      /// \brief Get the rotation as a Euler angles
-      /// \param[in] _firstSolution True to get the first Euler solution,
-      /// false to get the second.
-      /// \return the rotation
-      public: Vector3<T> EulerRotation(bool _firstSolution) const
+      return q;
+    }
+
+    /// \brief Get the rotation as a Euler angles
+    /// \param[in] _firstSolution True to get the first Euler solution,
+    /// false to get the second.
+    /// \return the rotation
+    public: Vector3<T> EulerRotation(bool _firstSolution) const
+    {
+      Vector3<T> euler;
+      Vector3<T> euler2;
+
+      T m31 = this->data[2][0];
+      T m11 = this->data[0][0];
+      T m12 = this->data[0][1];
+      T m13 = this->data[0][2];
+      T m32 = this->data[2][1];
+      T m33 = this->data[2][2];
+      T m21 = this->data[1][0];
+
+      if (std::abs(m31) >= 1.0)
       {
-        Vector3<T> euler;
-        Vector3<T> euler2;
-
-        T m31 = this->data[2][0];
-        T m11 = this->data[0][0];
-        T m12 = this->data[0][1];
-        T m13 = this->data[0][2];
-        T m32 = this->data[2][1];
-        T m33 = this->data[2][2];
-        T m21 = this->data[1][0];
-
-        if (std::abs(m31) >= 1.0)
+        euler.Z(0.0);
+        euler2.Z(0.0);
+
+        if (m31 < 0.0)
         {
-          euler.Z(0.0);
-          euler2.Z(0.0);
-
-          if (m31 < 0.0)
-          {
-            euler.Y(GZ_PI / 2.0);
-            euler2.Y(GZ_PI / 2.0);
-            euler.X(atan2(m12, m13));
-            euler2.X(atan2(m12, m13));
-          }
-          else
-          {
-            euler.Y(-GZ_PI / 2.0);
-            euler2.Y(-GZ_PI / 2.0);
-            euler.X(atan2(-m12, -m13));
-            euler2.X(atan2(-m12, -m13));
-          }
+          euler.Y(GZ_PI / 2.0);
+          euler2.Y(GZ_PI / 2.0);
+          euler.X(atan2(m12, m13));
+          euler2.X(atan2(m12, m13));
         }
         else
         {
-          euler.Y(-asin(m31));
-          euler2.Y(GZ_PI - euler.Y());
-
-          euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
-          euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
-
-          euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
-          euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
+          euler.Y(-GZ_PI / 2.0);
+          euler2.Y(-GZ_PI / 2.0);
+          euler.X(atan2(-m12, -m13));
+          euler2.X(atan2(-m12, -m13));
         }
-
-        if (_firstSolution)
-          return euler;
-        else
-          return euler2;
       }
-
-      /// \brief Get the transformation as math::Pose
-      /// \return the pose
-      public: Pose3<T> Pose() const
+      else
       {
-        return Pose3<T>(this->Translation(), this->Rotation());
-      }
+        euler.Y(-asin(m31));
+        euler2.Y(GZ_PI - euler.Y());
 
-      /// \brief Set the scale
-      /// \param[in] _s scale
-      public: void Scale(const Vector3<T> &_s)
-      {
-        this->data[0][0] = _s.X();
-        this->data[1][1] = _s.Y();
-        this->data[2][2] = _s.Z();
-        this->data[3][3] = 1.0;
-      }
+        euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
+        euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
 
-      /// \brief Set the scale
-      /// \param[in] _x X scale value.
-      /// \param[in] _y Y scale value.
-      /// \param[in] _z Z scale value.
-      public: void Scale(T _x, T _y, T _z)
-      {
-        this->data[0][0] = _x;
-        this->data[1][1] = _y;
-        this->data[2][2] = _z;
-        this->data[3][3] = 1.0;
+        euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
+        euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
       }
 
-      /// \brief Return true if the matrix is affine
-      /// \return true if the matrix is affine, false otherwise
-      public: bool IsAffine() const
-      {
-        return equal(this->data[3][0], static_cast<T>(0)) &&
-          equal(this->data[3][1], static_cast<T>(0)) &&
-          equal(this->data[3][2], static_cast<T>(0)) &&
-          equal(this->data[3][3], static_cast<T>(1));
-      }
+      if (_firstSolution)
+        return euler;
+      else
+        return euler2;
+    }
 
-      /// \brief Perform an affine transformation
-      /// \param[in] _v Vector3 value for the transformation
-      /// \param[out] _result  The result of the transformation. _result is
-      /// not changed if this matrix is not affine.
-      /// \return True if this matrix is affine, false otherwise.
-      public: bool TransformAffine(const Vector3<T> &_v,
-                                   Vector3<T> &_result) const
-      {
-        if (!this->IsAffine())
-          return false;
-
-        _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
-                    this->data[0][2]*_v.Z() + this->data[0][3],
-                    this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
-                    this->data[1][2]*_v.Z() + this->data[1][3],
-                    this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
-                    this->data[2][2]*_v.Z() + this->data[2][3]);
-        return true;
-      }
+    /// \brief Get the transformation as math::Pose
+    /// \return the pose
+    public: Pose3<T> Pose() const
+    {
+      return Pose3<T>(this->Translation(), this->Rotation());
+    }
 
-      /// \brief Return the determinant of the matrix
-      /// \return Determinant of this matrix.
-      public: T Determinant() const
-      {
-        T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
-
-        v0 = this->data[2][0]*this->data[3][1]
-           - this->data[2][1]*this->data[3][0];
-        v1 = this->data[2][0]*this->data[3][2]
-           - this->data[2][2]*this->data[3][0];
-        v2 = this->data[2][0]*this->data[3][3]
-           - this->data[2][3]*this->data[3][0];
-        v3 = this->data[2][1]*this->data[3][2]
-           - this->data[2][2]*this->data[3][1];
-        v4 = this->data[2][1]*this->data[3][3]
-           - this->data[2][3]*this->data[3][1];
-        v5 = this->data[2][2]*this->data[3][3]
-           - this->data[2][3]*this->data[3][2];
-
-        t00 =  v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
-        t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
-        t20 =  v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
-        t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
-
-        return t00 * this->data[0][0]
-             + t10 * this->data[0][1]
-             + t20 * this->data[0][2]
-             + t30 * this->data[0][3];
-      }
+    /// \brief Set the scale
+    /// \param[in] _s scale
+    public: void Scale(const Vector3<T> &_s)
+    {
+      this->data[0][0] = _s.X();
+      this->data[1][1] = _s.Y();
+      this->data[2][2] = _s.Z();
+      this->data[3][3] = 1.0;
+    }
 
-      /// \brief Return the inverse matrix.
-      /// This is a non-destructive operation.
-      /// \return Inverse of this matrix.
-      public: Matrix4<T> Inverse() const
-      {
-        T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
-        Matrix4<T> r;
-
-        v0 = this->data[2][0]*this->data[3][1] -
-          this->data[2][1]*this->data[3][0];
-        v1 = this->data[2][0]*this->data[3][2] -
-          this->data[2][2]*this->data[3][0];
-        v2 = this->data[2][0]*this->data[3][3] -
-          this->data[2][3]*this->data[3][0];
-        v3 = this->data[2][1]*this->data[3][2] -
-          this->data[2][2]*this->data[3][1];
-        v4 = this->data[2][1]*this->data[3][3] -
-          this->data[2][3]*this->data[3][1];
-        v5 = this->data[2][2]*this->data[3][3] -
-          this->data[2][3]*this->data[3][2];
-
-        t00 = +(v5*this->data[1][1] -
-            v4*this->data[1][2] + v3*this->data[1][3]);
-        t10 = -(v5*this->data[1][0] -
-            v2*this->data[1][2] + v1*this->data[1][3]);
-        t20 = +(v4*this->data[1][0] -
-            v2*this->data[1][1] + v0*this->data[1][3]);
-        t30 = -(v3*this->data[1][0] -
-            v1*this->data[1][1] + v0*this->data[1][2]);
-
-        T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
-            t20 * this->data[0][2] + t30 * this->data[0][3]);
-
-        r(0, 0) = t00 * invDet;
-        r(1, 0) = t10 * invDet;
-        r(2, 0) = t20 * invDet;
-        r(3, 0) = t30 * invDet;
-
-        r(0, 1) = -(v5*this->data[0][1] -
-            v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
-        r(1, 1) = +(v5*this->data[0][0] -
-            v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
-        r(2, 1) = -(v4*this->data[0][0] -
-            v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
-        r(3, 1) = +(v3*this->data[0][0] -
-            v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
-
-        v0 = this->data[1][0]*this->data[3][1] -
-          this->data[1][1]*this->data[3][0];
-        v1 = this->data[1][0]*this->data[3][2] -
-          this->data[1][2]*this->data[3][0];
-        v2 = this->data[1][0]*this->data[3][3] -
-          this->data[1][3]*this->data[3][0];
-        v3 = this->data[1][1]*this->data[3][2] -
-          this->data[1][2]*this->data[3][1];
-        v4 = this->data[1][1]*this->data[3][3] -
-          this->data[1][3]*this->data[3][1];
-        v5 = this->data[1][2]*this->data[3][3] -
-          this->data[1][3]*this->data[3][2];
-
-        r(0, 2) = +(v5*this->data[0][1] -
-            v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
-        r(1, 2) = -(v5*this->data[0][0] -
-            v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
-        r(2, 2) = +(v4*this->data[0][0] -
-            v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
-        r(3, 2) = -(v3*this->data[0][0] -
-            v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
-
-        v0 = this->data[2][1]*this->data[1][0] -
-          this->data[2][0]*this->data[1][1];
-        v1 = this->data[2][2]*this->data[1][0] -
-          this->data[2][0]*this->data[1][2];
-        v2 = this->data[2][3]*this->data[1][0] -
-          this->data[2][0]*this->data[1][3];
-        v3 = this->data[2][2]*this->data[1][1] -
-          this->data[2][1]*this->data[1][2];
-        v4 = this->data[2][3]*this->data[1][1] -
-          this->data[2][1]*this->data[1][3];
-        v5 = this->data[2][3]*this->data[1][2] -
-          this->data[2][2]*this->data[1][3];
-
-        r(0, 3) = -(v5*this->data[0][1] -
-            v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
-        r(1, 3) = +(v5*this->data[0][0] -
-            v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
-        r(2, 3) = -(v4*this->data[0][0] -
-            v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
-        r(3, 3) = +(v3*this->data[0][0] -
-            v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
-
-        return r;
-      }
+    /// \brief Set the scale
+    /// \param[in] _x X scale value.
+    /// \param[in] _y Y scale value.
+    /// \param[in] _z Z scale value.
+    public: void Scale(T _x, T _y, T _z)
+    {
+      this->data[0][0] = _x;
+      this->data[1][1] = _y;
+      this->data[2][2] = _z;
+      this->data[3][3] = 1.0;
+    }
 
-      /// \brief Transpose this matrix.
-      public: void Transpose()
-      {
-        std::swap(this->data[0][1], this->data[1][0]);
-        std::swap(this->data[0][2], this->data[2][0]);
-        std::swap(this->data[0][3], this->data[3][0]);
-        std::swap(this->data[1][2], this->data[2][1]);
-        std::swap(this->data[1][3], this->data[3][1]);
-        std::swap(this->data[2][3], this->data[3][2]);
-      }
+    /// \brief Return true if the matrix is affine
+    /// \return true if the matrix is affine, false otherwise
+    public: bool IsAffine() const
+    {
+      return equal(this->data[3][0], static_cast<T>(0)) &&
+        equal(this->data[3][1], static_cast<T>(0)) &&
+        equal(this->data[3][2], static_cast<T>(0)) &&
+        equal(this->data[3][3], static_cast<T>(1));
+    }
 
-      /// \brief Return the transpose of this matrix
-      /// \return Transpose of this matrix.
-      public: Matrix4<T> Transposed() const
-      {
-        return Matrix4<T>(
-        this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
-        this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
-        this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
-        this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
-      }
+    /// \brief Perform an affine transformation
+    /// \param[in] _v Vector3 value for the transformation
+    /// \param[out] _result  The result of the transformation. _result is
+    /// not changed if this matrix is not affine.
+    /// \return True if this matrix is affine, false otherwise.
+    public: bool TransformAffine(const Vector3<T> &_v,
+                                 Vector3<T> &_result) const
+    {
+      if (!this->IsAffine())
+        return false;
+
+      _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
+                  this->data[0][2]*_v.Z() + this->data[0][3],
+                  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
+                  this->data[1][2]*_v.Z() + this->data[1][3],
+                  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
+                  this->data[2][2]*_v.Z() + this->data[2][3]);
+      return true;
+    }
 
-      /// \brief Equal operator. this = _mat
-      /// \param _mat Incoming matrix
-      /// \return itself
-      public: Matrix4<T> &operator=(const Matrix4<T> &_mat) = default;
+    /// \brief Return the determinant of the matrix
+    /// \return Determinant of this matrix.
+    public: T Determinant() const
+    {
+      T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
+
+      v0 = this->data[2][0]*this->data[3][1]
+         - this->data[2][1]*this->data[3][0];
+      v1 = this->data[2][0]*this->data[3][2]
+         - this->data[2][2]*this->data[3][0];
+      v2 = this->data[2][0]*this->data[3][3]
+         - this->data[2][3]*this->data[3][0];
+      v3 = this->data[2][1]*this->data[3][2]
+         - this->data[2][2]*this->data[3][1];
+      v4 = this->data[2][1]*this->data[3][3]
+         - this->data[2][3]*this->data[3][1];
+      v5 = this->data[2][2]*this->data[3][3]
+         - this->data[2][3]*this->data[3][2];
+
+      t00 =  v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
+      t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
+      t20 =  v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
+      t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
+
+      return t00 * this->data[0][0]
+           + t10 * this->data[0][1]
+           + t20 * this->data[0][2]
+           + t30 * this->data[0][3];
+    }
 
-      /// \brief Equal operator for 3x3 matrix
-      /// \param _mat Incoming matrix
-      /// \return itself
-      public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
-      {
-        this->data[0][0] = _mat(0, 0);
-        this->data[0][1] = _mat(0, 1);
-        this->data[0][2] = _mat(0, 2);
+    /// \brief Return the inverse matrix.
+    /// This is a non-destructive operation.
+    /// \return Inverse of this matrix.
+    public: Matrix4<T> Inverse() const
+    {
+      T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
+      Matrix4<T> r;
+
+      v0 = this->data[2][0]*this->data[3][1] -
+        this->data[2][1]*this->data[3][0];
+      v1 = this->data[2][0]*this->data[3][2] -
+        this->data[2][2]*this->data[3][0];
+      v2 = this->data[2][0]*this->data[3][3] -
+        this->data[2][3]*this->data[3][0];
+      v3 = this->data[2][1]*this->data[3][2] -
+        this->data[2][2]*this->data[3][1];
+      v4 = this->data[2][1]*this->data[3][3] -
+        this->data[2][3]*this->data[3][1];
+      v5 = this->data[2][2]*this->data[3][3] -
+        this->data[2][3]*this->data[3][2];
+
+      t00 = +(v5*this->data[1][1] -
+          v4*this->data[1][2] + v3*this->data[1][3]);
+      t10 = -(v5*this->data[1][0] -
+          v2*this->data[1][2] + v1*this->data[1][3]);
+      t20 = +(v4*this->data[1][0] -
+          v2*this->data[1][1] + v0*this->data[1][3]);
+      t30 = -(v3*this->data[1][0] -
+          v1*this->data[1][1] + v0*this->data[1][2]);
+
+      T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
+          t20 * this->data[0][2] + t30 * this->data[0][3]);
+
+      r(0, 0) = t00 * invDet;
+      r(1, 0) = t10 * invDet;
+      r(2, 0) = t20 * invDet;
+      r(3, 0) = t30 * invDet;
+
+      r(0, 1) = -(v5*this->data[0][1] -
+          v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
+      r(1, 1) = +(v5*this->data[0][0] -
+          v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
+      r(2, 1) = -(v4*this->data[0][0] -
+          v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
+      r(3, 1) = +(v3*this->data[0][0] -
+          v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
+
+      v0 = this->data[1][0]*this->data[3][1] -
+        this->data[1][1]*this->data[3][0];
+      v1 = this->data[1][0]*this->data[3][2] -
+        this->data[1][2]*this->data[3][0];
+      v2 = this->data[1][0]*this->data[3][3] -
+        this->data[1][3]*this->data[3][0];
+      v3 = this->data[1][1]*this->data[3][2] -
+        this->data[1][2]*this->data[3][1];
+      v4 = this->data[1][1]*this->data[3][3] -
+        this->data[1][3]*this->data[3][1];
+      v5 = this->data[1][2]*this->data[3][3] -
+        this->data[1][3]*this->data[3][2];
+
+      r(0, 2) = +(v5*this->data[0][1] -
+          v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
+      r(1, 2) = -(v5*this->data[0][0] -
+          v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
+      r(2, 2) = +(v4*this->data[0][0] -
+          v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
+      r(3, 2) = -(v3*this->data[0][0] -
+          v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
+
+      v0 = this->data[2][1]*this->data[1][0] -
+        this->data[2][0]*this->data[1][1];
+      v1 = this->data[2][2]*this->data[1][0] -
+        this->data[2][0]*this->data[1][2];
+      v2 = this->data[2][3]*this->data[1][0] -
+        this->data[2][0]*this->data[1][3];
+      v3 = this->data[2][2]*this->data[1][1] -
+        this->data[2][1]*this->data[1][2];
+      v4 = this->data[2][3]*this->data[1][1] -
+        this->data[2][1]*this->data[1][3];
+      v5 = this->data[2][3]*this->data[1][2] -
+        this->data[2][2]*this->data[1][3];
+
+      r(0, 3) = -(v5*this->data[0][1] -
+          v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
+      r(1, 3) = +(v5*this->data[0][0] -
+          v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
+      r(2, 3) = -(v4*this->data[0][0] -
+          v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
+      r(3, 3) = +(v3*this->data[0][0] -
+          v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
+
+      return r;
+    }
 
-        this->data[1][0] = _mat(1, 0);
-        this->data[1][1] = _mat(1, 1);
-        this->data[1][2] = _mat(1, 2);
+    /// \brief Transpose this matrix.
+    public: void Transpose()
+    {
+      std::swap(this->data[0][1], this->data[1][0]);
+      std::swap(this->data[0][2], this->data[2][0]);
+      std::swap(this->data[0][3], this->data[3][0]);
+      std::swap(this->data[1][2], this->data[2][1]);
+      std::swap(this->data[1][3], this->data[3][1]);
+      std::swap(this->data[2][3], this->data[3][2]);
+    }
 
-        this->data[2][0] = _mat(2, 0);
-        this->data[2][1] = _mat(2, 1);
-        this->data[2][2] = _mat(2, 2);
+    /// \brief Return the transpose of this matrix
+    /// \return Transpose of this matrix.
+    public: Matrix4<T> Transposed() const
+    {
+      return Matrix4<T>(
+      this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
+      this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
+      this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
+      this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
+    }
 
-        return *this;
-      }
+    /// \brief Equal operator. this = _mat
+    /// \param _mat Incoming matrix
+    /// \return itself
+    public: Matrix4<T> &operator=(const Matrix4<T> &_mat) = default;
 
-      /// \brief Multiplication assignment operator. This matrix will
-      /// become equal to this * _m2.
-      /// \param[in] _m2 Incoming matrix.
-      /// \return This matrix * _mat.
-      public: Matrix4<T> operator*=(const Matrix4<T> &_m2)
-      {
-        (*this) = (*this) * _m2;
-        return *this;
-      }
+    /// \brief Equal operator for 3x3 matrix
+    /// \param _mat Incoming matrix
+    /// \return itself
+    public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
+    {
+      this->data[0][0] = _mat(0, 0);
+      this->data[0][1] = _mat(0, 1);
+      this->data[0][2] = _mat(0, 2);
 
-      /// \brief Multiplication operator
-      /// \param[in] _m2 Incoming matrix
-      /// \return This matrix * _mat
-      public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
-      {
-        return Matrix4<T>(
-          this->data[0][0] * _m2(0, 0) +
-          this->data[0][1] * _m2(1, 0) +
-          this->data[0][2] * _m2(2, 0) +
-          this->data[0][3] * _m2(3, 0),
-
-          this->data[0][0] * _m2(0, 1) +
-          this->data[0][1] * _m2(1, 1) +
-          this->data[0][2] * _m2(2, 1) +
-          this->data[0][3] * _m2(3, 1),
-
-          this->data[0][0] * _m2(0, 2) +
-          this->data[0][1] * _m2(1, 2) +
-          this->data[0][2] * _m2(2, 2) +
-          this->data[0][3] * _m2(3, 2),
-
-          this->data[0][0] * _m2(0, 3) +
-          this->data[0][1] * _m2(1, 3) +
-          this->data[0][2] * _m2(2, 3) +
-          this->data[0][3] * _m2(3, 3),
-
-          this->data[1][0] * _m2(0, 0) +
-          this->data[1][1] * _m2(1, 0) +
-          this->data[1][2] * _m2(2, 0) +
-          this->data[1][3] * _m2(3, 0),
-
-          this->data[1][0] * _m2(0, 1) +
-          this->data[1][1] * _m2(1, 1) +
-          this->data[1][2] * _m2(2, 1) +
-          this->data[1][3] * _m2(3, 1),
-
-          this->data[1][0] * _m2(0, 2) +
-          this->data[1][1] * _m2(1, 2) +
-          this->data[1][2] * _m2(2, 2) +
-          this->data[1][3] * _m2(3, 2),
-
-          this->data[1][0] * _m2(0, 3) +
-          this->data[1][1] * _m2(1, 3) +
-          this->data[1][2] * _m2(2, 3) +
-          this->data[1][3] * _m2(3, 3),
-
-          this->data[2][0] * _m2(0, 0) +
-          this->data[2][1] * _m2(1, 0) +
-          this->data[2][2] * _m2(2, 0) +
-          this->data[2][3] * _m2(3, 0),
-
-          this->data[2][0] * _m2(0, 1) +
-          this->data[2][1] * _m2(1, 1) +
-          this->data[2][2] * _m2(2, 1) +
-          this->data[2][3] * _m2(3, 1),
-
-          this->data[2][0] * _m2(0, 2) +
-          this->data[2][1] * _m2(1, 2) +
-          this->data[2][2] * _m2(2, 2) +
-          this->data[2][3] * _m2(3, 2),
-
-          this->data[2][0] * _m2(0, 3) +
-          this->data[2][1] * _m2(1, 3) +
-          this->data[2][2] * _m2(2, 3) +
-          this->data[2][3] * _m2(3, 3),
-
-          this->data[3][0] * _m2(0, 0) +
-          this->data[3][1] * _m2(1, 0) +
-          this->data[3][2] * _m2(2, 0) +
-          this->data[3][3] * _m2(3, 0),
-
-          this->data[3][0] * _m2(0, 1) +
-          this->data[3][1] * _m2(1, 1) +
-          this->data[3][2] * _m2(2, 1) +
-          this->data[3][3] * _m2(3, 1),
-
-          this->data[3][0] * _m2(0, 2) +
-          this->data[3][1] * _m2(1, 2) +
-          this->data[3][2] * _m2(2, 2) +
-          this->data[3][3] * _m2(3, 2),
-
-          this->data[3][0] * _m2(0, 3) +
-          this->data[3][1] * _m2(1, 3) +
-          this->data[3][2] * _m2(2, 3) +
-          this->data[3][3] * _m2(3, 3));
-      }
+      this->data[1][0] = _mat(1, 0);
+      this->data[1][1] = _mat(1, 1);
+      this->data[1][2] = _mat(1, 2);
 
-      /// \brief Multiplication operator
-      /// \param _vec Vector3
-      /// \return Resulting vector from multiplication
-      public: Vector3<T> operator*(const Vector3<T> &_vec) const
-      {
-        return Vector3<T>(
-            this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
-            this->data[0][2]*_vec.Z() + this->data[0][3],
-            this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
-            this->data[1][2]*_vec.Z() + this->data[1][3],
-            this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
-            this->data[2][2]*_vec.Z() + this->data[2][3]);
-      }
+      this->data[2][0] = _mat(2, 0);
+      this->data[2][1] = _mat(2, 1);
+      this->data[2][2] = _mat(2, 2);
 
-      /// \brief Get the value at the specified row, column index
-      /// \param[in] _col The column index. Index values are clamped to a
-      /// range of [0, 3].
-      /// \param[in] _row the row index. Index values are clamped to a
-      /// range of [0, 3].
-      /// \return The value at the specified index
-      public: inline const T &operator()(const size_t _row,
-                  const size_t _col) const
-      {
-        return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)][
-                          clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
-      }
+      return *this;
+    }
 
-      /// \brief Get a mutable version the value at the specified row,
-      /// column index
-      /// \param[in] _col The column index. Index values are clamped to a
-      /// range of [0, 3].
-      /// \param[in] _row the row index. Index values are clamped to a
-      /// range of [0, 3].
-      /// \return The value at the specified index
-      public: inline T &operator()(const size_t _row, const size_t _col)
-      {
-        return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)]
-                         [clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
-      }
+    /// \brief Multiplication assignment operator. This matrix will
+    /// become equal to this * _m2.
+    /// \param[in] _m2 Incoming matrix.
+    /// \return This matrix * _mat.
+    public: Matrix4<T> operator*=(const Matrix4<T> &_m2)
+    {
+      (*this) = (*this) * _m2;
+      return *this;
+    }
 
-      /// \brief Equality test with tolerance.
-      /// \param[in] _m the matrix to compare to
-      /// \param[in] _tol equality tolerance.
-      /// \return true if the elements of the matrices are equal within
-      /// the tolerence specified by _tol.
-      public: bool Equal(const Matrix4 &_m, const T &_tol) const
-      {
-        return equal<T>(this->data[0][0], _m(0, 0), _tol)
-            && equal<T>(this->data[0][1], _m(0, 1), _tol)
-            && equal<T>(this->data[0][2], _m(0, 2), _tol)
-            && equal<T>(this->data[0][3], _m(0, 3), _tol)
-            && equal<T>(this->data[1][0], _m(1, 0), _tol)
-            && equal<T>(this->data[1][1], _m(1, 1), _tol)
-            && equal<T>(this->data[1][2], _m(1, 2), _tol)
-            && equal<T>(this->data[1][3], _m(1, 3), _tol)
-            && equal<T>(this->data[2][0], _m(2, 0), _tol)
-            && equal<T>(this->data[2][1], _m(2, 1), _tol)
-            && equal<T>(this->data[2][2], _m(2, 2), _tol)
-            && equal<T>(this->data[2][3], _m(2, 3), _tol)
-            && equal<T>(this->data[3][0], _m(3, 0), _tol)
-            && equal<T>(this->data[3][1], _m(3, 1), _tol)
-            && equal<T>(this->data[3][2], _m(3, 2), _tol)
-            && equal<T>(this->data[3][3], _m(3, 3), _tol);
-      }
+    /// \brief Multiplication operator
+    /// \param[in] _m2 Incoming matrix
+    /// \return This matrix * _mat
+    public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
+    {
+      return Matrix4<T>(
+        this->data[0][0] * _m2(0, 0) +
+        this->data[0][1] * _m2(1, 0) +
+        this->data[0][2] * _m2(2, 0) +
+        this->data[0][3] * _m2(3, 0),
+
+        this->data[0][0] * _m2(0, 1) +
+        this->data[0][1] * _m2(1, 1) +
+        this->data[0][2] * _m2(2, 1) +
+        this->data[0][3] * _m2(3, 1),
+
+        this->data[0][0] * _m2(0, 2) +
+        this->data[0][1] * _m2(1, 2) +
+        this->data[0][2] * _m2(2, 2) +
+        this->data[0][3] * _m2(3, 2),
+
+        this->data[0][0] * _m2(0, 3) +
+        this->data[0][1] * _m2(1, 3) +
+        this->data[0][2] * _m2(2, 3) +
+        this->data[0][3] * _m2(3, 3),
+
+        this->data[1][0] * _m2(0, 0) +
+        this->data[1][1] * _m2(1, 0) +
+        this->data[1][2] * _m2(2, 0) +
+        this->data[1][3] * _m2(3, 0),
+
+        this->data[1][0] * _m2(0, 1) +
+        this->data[1][1] * _m2(1, 1) +
+        this->data[1][2] * _m2(2, 1) +
+        this->data[1][3] * _m2(3, 1),
+
+        this->data[1][0] * _m2(0, 2) +
+        this->data[1][1] * _m2(1, 2) +
+        this->data[1][2] * _m2(2, 2) +
+        this->data[1][3] * _m2(3, 2),
+
+        this->data[1][0] * _m2(0, 3) +
+        this->data[1][1] * _m2(1, 3) +
+        this->data[1][2] * _m2(2, 3) +
+        this->data[1][3] * _m2(3, 3),
+
+        this->data[2][0] * _m2(0, 0) +
+        this->data[2][1] * _m2(1, 0) +
+        this->data[2][2] * _m2(2, 0) +
+        this->data[2][3] * _m2(3, 0),
+
+        this->data[2][0] * _m2(0, 1) +
+        this->data[2][1] * _m2(1, 1) +
+        this->data[2][2] * _m2(2, 1) +
+        this->data[2][3] * _m2(3, 1),
+
+        this->data[2][0] * _m2(0, 2) +
+        this->data[2][1] * _m2(1, 2) +
+        this->data[2][2] * _m2(2, 2) +
+        this->data[2][3] * _m2(3, 2),
+
+        this->data[2][0] * _m2(0, 3) +
+        this->data[2][1] * _m2(1, 3) +
+        this->data[2][2] * _m2(2, 3) +
+        this->data[2][3] * _m2(3, 3),
+
+        this->data[3][0] * _m2(0, 0) +
+        this->data[3][1] * _m2(1, 0) +
+        this->data[3][2] * _m2(2, 0) +
+        this->data[3][3] * _m2(3, 0),
+
+        this->data[3][0] * _m2(0, 1) +
+        this->data[3][1] * _m2(1, 1) +
+        this->data[3][2] * _m2(2, 1) +
+        this->data[3][3] * _m2(3, 1),
+
+        this->data[3][0] * _m2(0, 2) +
+        this->data[3][1] * _m2(1, 2) +
+        this->data[3][2] * _m2(2, 2) +
+        this->data[3][3] * _m2(3, 2),
+
+        this->data[3][0] * _m2(0, 3) +
+        this->data[3][1] * _m2(1, 3) +
+        this->data[3][2] * _m2(2, 3) +
+        this->data[3][3] * _m2(3, 3));
+    }
 
-      /// \brief Equality operator
-      /// \param[in] _m Matrix3 to test
-      /// \return true if the 2 matrices are equal (using the tolerance 1e-6),
-      ///  false otherwise
-      public: bool operator==(const Matrix4<T> &_m) const
-      {
-        return this->Equal(_m, static_cast<T>(1e-6));
-      }
+    /// \brief Multiplication operator
+    /// \param _vec Vector3
+    /// \return Resulting vector from multiplication
+    public: Vector3<T> operator*(const Vector3<T> &_vec) const
+    {
+      return Vector3<T>(
+          this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
+          this->data[0][2]*_vec.Z() + this->data[0][3],
+          this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
+          this->data[1][2]*_vec.Z() + this->data[1][3],
+          this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
+          this->data[2][2]*_vec.Z() + this->data[2][3]);
+    }
 
-      /// \brief Inequality test operator
-      /// \param[in] _m Matrix4<T> to test
-      /// \return True if not equal (using the default tolerance of 1e-6)
-      public: bool operator!=(const Matrix4<T> &_m) const
-      {
-        return !(*this == _m);
-      }
+    /// \brief Get the value at the specified row, column index
+    /// \param[in] _col The column index. Index values are clamped to a
+    /// range of [0, 3].
+    /// \param[in] _row the row index. Index values are clamped to a
+    /// range of [0, 3].
+    /// \return The value at the specified index
+    public: inline const T &operator()(const size_t _row,
+                const size_t _col) const
+    {
+      return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)][
+                        clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
+    }
 
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _m Matrix to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-                  std::ostream &_out, const gz::math::Matrix4<T> &_m)
-      {
-        for (auto i : {0, 1, 2, 3})
-        {
-          for (auto j : {0, 1, 2, 3})
-          {
-            if (!(i == 0 && j == 0))
-              _out << " ";
+    /// \brief Get a mutable version the value at the specified row,
+    /// column index
+    /// \param[in] _col The column index. Index values are clamped to a
+    /// range of [0, 3].
+    /// \param[in] _row the row index. Index values are clamped to a
+    /// range of [0, 3].
+    /// \return The value at the specified index
+    public: inline T &operator()(const size_t _row, const size_t _col)
+    {
+      return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)]
+                       [clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
+    }
 
-            appendToStream(_out, _m(i, j));
-          }
-        }
+    /// \brief Equality test with tolerance.
+    /// \param[in] _m the matrix to compare to
+    /// \param[in] _tol equality tolerance.
+    /// \return true if the elements of the matrices are equal within
+    /// the tolerence specified by _tol.
+    public: bool Equal(const Matrix4 &_m, const T &_tol) const
+    {
+      return equal<T>(this->data[0][0], _m(0, 0), _tol)
+          && equal<T>(this->data[0][1], _m(0, 1), _tol)
+          && equal<T>(this->data[0][2], _m(0, 2), _tol)
+          && equal<T>(this->data[0][3], _m(0, 3), _tol)
+          && equal<T>(this->data[1][0], _m(1, 0), _tol)
+          && equal<T>(this->data[1][1], _m(1, 1), _tol)
+          && equal<T>(this->data[1][2], _m(1, 2), _tol)
+          && equal<T>(this->data[1][3], _m(1, 3), _tol)
+          && equal<T>(this->data[2][0], _m(2, 0), _tol)
+          && equal<T>(this->data[2][1], _m(2, 1), _tol)
+          && equal<T>(this->data[2][2], _m(2, 2), _tol)
+          && equal<T>(this->data[2][3], _m(2, 3), _tol)
+          && equal<T>(this->data[3][0], _m(3, 0), _tol)
+          && equal<T>(this->data[3][1], _m(3, 1), _tol)
+          && equal<T>(this->data[3][2], _m(3, 2), _tol)
+          && equal<T>(this->data[3][3], _m(3, 3), _tol);
+    }
 
-        return _out;
-      }
+    /// \brief Equality operator
+    /// \param[in] _m Matrix3 to test
+    /// \return true if the 2 matrices are equal (using the tolerance 1e-6),
+    ///  false otherwise
+    public: bool operator==(const Matrix4<T> &_m) const
+    {
+      return this->Equal(_m, static_cast<T>(1e-6));
+    }
 
-      /// \brief Stream extraction operator
-      /// \param[in,out] _in input stream
-      /// \param[out] _m Matrix4<T> to read values into
-      /// \return the stream
-      public: friend std::istream &operator>>(
-                  std::istream &_in, gz::math::Matrix4<T> &_m)
-      {
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        T d[16];
-        _in >> d[0] >> d[1] >> d[2] >> d[3]
-            >> d[4] >> d[5] >> d[6] >> d[7]
-            >> d[8] >> d[9] >> d[10] >> d[11]
-            >> d[12] >> d[13] >> d[14] >> d[15];
-
-        if (!_in.fail())
-        {
-          _m.Set(d[0], d[1], d[2], d[3],
-                 d[4], d[5], d[6], d[7],
-                 d[8], d[9], d[10], d[11],
-                 d[12], d[13], d[14], d[15]);
-        }
-        return _in;
-      }
+    /// \brief Inequality test operator
+    /// \param[in] _m Matrix4<T> to test
+    /// \return True if not equal (using the default tolerance of 1e-6)
+    public: bool operator!=(const Matrix4<T> &_m) const
+    {
+      return !(*this == _m);
+    }
 
-      /// \brief Get transform which translates to _eye and rotates the X axis
-      /// so it faces the _target. The rotation is such that Z axis is in the
-      /// _up direction, if possible. The coordinate system is right-handed,
-      /// \param[in] _eye Coordinate frame translation.
-      /// \param[in] _target Point which the X axis should face. If _target is
-      /// equal to _eye, the X axis won't be rotated.
-      /// \param[in] _up Direction in which the Z axis should point. If _up is
-      /// zero or parallel to X, it will be set to +Z.
-      /// \return Transformation matrix.
-      public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
-          const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _m Matrix to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+                std::ostream &_out, const gz::math::Matrix4<T> &_m)
+    {
+      for (auto i : {0, 1, 2, 3})
       {
-        // Most important constraint: direction to point X axis at
-        auto front = _target - _eye;
-
-        // Case when _eye == _target
-        if (front == Vector3<T>::Zero)
-          front = Vector3<T>::UnitX;
-        front.Normalize();
-
-        // Desired direction to point Z axis at
-        auto up = _up;
-
-        // Case when _up == Zero
-        if (up == Vector3<T>::Zero)
-          up = Vector3<T>::UnitZ;
-        else
-          up.Normalize();
-
-        // Case when _up is parallel to X
-        if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
-          up = Vector3<T>::UnitZ;
-
-        // Find direction to point Y axis at
-        auto left = up.Cross(front);
-
-        // Case when front is parallel to up
-        if (left == Vector3<T>::Zero)
-          left = Vector3<T>::UnitY;
-        else
-          left.Normalize();
-
-        // Fix up direction so it's perpendicular to XY
-        up = (front.Cross(left)).Normalize();
+        for (auto j : {0, 1, 2, 3})
+        {
+          if (!(i == 0 && j == 0))
+            _out << " ";
 
-        return Matrix4<T>(
-            front.X(), left.X(), up.X(), _eye.X(),
-            front.Y(), left.Y(), up.Y(), _eye.Y(),
-            front.Z(), left.Z(), up.Z(), _eye.Z(),
-                  0,      0,         0,        1);
+          appendToStream(_out, _m(i, j));
+        }
       }
 
-      /// \brief The 4x4 matrix
-      private: T data[4][4];
-    };
+      return _out;
+    }
 
-    namespace detail {
+    /// \brief Stream extraction operator
+    /// \param[in,out] _in input stream
+    /// \param[out] _m Matrix4<T> to read values into
+    /// \return the stream
+    public: friend std::istream &operator>>(
+                std::istream &_in, gz::math::Matrix4<T> &_m)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      T d[16];
+      _in >> d[0] >> d[1] >> d[2] >> d[3]
+          >> d[4] >> d[5] >> d[6] >> d[7]
+          >> d[8] >> d[9] >> d[10] >> d[11]
+          >> d[12] >> d[13] >> d[14] >> d[15];
+
+      if (!_in.fail())
+      {
+        _m.Set(d[0], d[1], d[2], d[3],
+               d[4], d[5], d[6], d[7],
+               d[8], d[9], d[10], d[11],
+               d[12], d[13], d[14], d[15]);
+      }
+      return _in;
+    }
 
-      template<typename T>
-      constexpr Matrix4<T> gMatrix4Identity(
-          1, 0, 0, 0,
-          0, 1, 0, 0,
-          0, 0, 1, 0,
-          0, 0, 0, 1);
+    /// \brief Get transform which translates to _eye and rotates the X axis
+    /// so it faces the _target. The rotation is such that Z axis is in the
+    /// _up direction, if possible. The coordinate system is right-handed,
+    /// \param[in] _eye Coordinate frame translation.
+    /// \param[in] _target Point which the X axis should face. If _target is
+    /// equal to _eye, the X axis won't be rotated.
+    /// \param[in] _up Direction in which the Z axis should point. If _up is
+    /// zero or parallel to X, it will be set to +Z.
+    /// \return Transformation matrix.
+    public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
+        const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
+    {
+      // Most important constraint: direction to point X axis at
+      auto front = _target - _eye;
+
+      // Case when _eye == _target
+      if (front == Vector3<T>::Zero)
+        front = Vector3<T>::UnitX;
+      front.Normalize();
+
+      // Desired direction to point Z axis at
+      auto up = _up;
+
+      // Case when _up == Zero
+      if (up == Vector3<T>::Zero)
+        up = Vector3<T>::UnitZ;
+      else
+        up.Normalize();
+
+      // Case when _up is parallel to X
+      if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
+        up = Vector3<T>::UnitZ;
+
+      // Find direction to point Y axis at
+      auto left = up.Cross(front);
+
+      // Case when front is parallel to up
+      if (left == Vector3<T>::Zero)
+        left = Vector3<T>::UnitY;
+      else
+        left.Normalize();
+
+      // Fix up direction so it's perpendicular to XY
+      up = (front.Cross(left)).Normalize();
+
+      return Matrix4<T>(
+          front.X(), left.X(), up.X(), _eye.X(),
+          front.Y(), left.Y(), up.Y(), _eye.Y(),
+          front.Z(), left.Z(), up.Z(), _eye.Z(),
+                0,      0,         0,        1);
+    }
 
-      template<typename T>
-      constexpr Matrix4<T> gMatrix4Zero(
-          0, 0, 0, 0,
-          0, 0, 0, 0,
-          0, 0, 0, 0,
-          0, 0, 0, 0);
+    /// \brief The 4x4 matrix
+    private: T data[4][4];
+  };
 
-    }  // namespace detail
+  namespace detail {
 
     template<typename T>
-    const Matrix4<T> &Matrix4<T>::Identity = detail::gMatrix4Identity<T>;
+    constexpr Matrix4<T> gMatrix4Identity(
+        1, 0, 0, 0,
+        0, 1, 0, 0,
+        0, 0, 1, 0,
+        0, 0, 0, 1);
 
     template<typename T>
-    const Matrix4<T> &Matrix4<T>::Zero = detail::gMatrix4Zero<T>;
-
-    typedef Matrix4<int> Matrix4i;
-    typedef Matrix4<double> Matrix4d;
-    typedef Matrix4<float> Matrix4f;
-    }
-  }
-}
-#endif
+    constexpr Matrix4<T> gMatrix4Zero(
+        0, 0, 0, 0,
+        0, 0, 0, 0,
+        0, 0, 0, 0,
+        0, 0, 0, 0);
+
+  }  // namespace detail
+
+  template<typename T>
+  const Matrix4<T> &Matrix4<T>::Identity = detail::gMatrix4Identity<T>;
+
+  template<typename T>
+  const Matrix4<T> &Matrix4<T>::Zero = detail::gMatrix4Zero<T>;
+
+  typedef Matrix4<int> Matrix4i;
+  typedef Matrix4<double> Matrix4d;
+  typedef Matrix4<float> Matrix4f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_MATRIX4_HH_
diff --git a/include/gz/math/Matrix6.hh b/include/gz/math/Matrix6.hh
index 1a8ee39d3..b6aeae29e 100644
--- a/include/gz/math/Matrix6.hh
+++ b/include/gz/math/Matrix6.hh
@@ -22,572 +22,569 @@
 #include <gz/math/Helpers.hh>
 #include <gz/math/Matrix3.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Matrix6 Matrix6.hh gz/math/Matrix6.hh
+  /// \brief A 6x6 matrix class
+  template<typename T>
+  class Matrix6
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Matrix6 Matrix6.hh gz/math/Matrix6.hh
-    /// \brief A 6x6 matrix class
-    template<typename T>
-    class Matrix6
+    /// \brief Identifiers for each of the 4 3x3 corners of the matrix.
+    public: enum Matrix6Corner
     {
-      /// \brief Identifiers for each of the 4 3x3 corners of the matrix.
-      public: enum Matrix6Corner
-      {
-        /// \brief Top-left corner, consisting of the intersection between the
-        /// first 3 rows and first 3 columns.
-        TOP_LEFT = 0,
+      /// \brief Top-left corner, consisting of the intersection between the
+      /// first 3 rows and first 3 columns.
+      TOP_LEFT = 0,
 
-        /// \brief Top-right corner, consisting of the intersection between the
-        /// first 3 rows and last 3 columns.
-        TOP_RIGHT = 1,
+      /// \brief Top-right corner, consisting of the intersection between the
+      /// first 3 rows and last 3 columns.
+      TOP_RIGHT = 1,
 
-        /// \brief Bottom-left corner, consisting of the intersection between
-        /// the last 3 rows and first 3 columns.
-        BOTTOM_LEFT = 2,
+      /// \brief Bottom-left corner, consisting of the intersection between
+      /// the last 3 rows and first 3 columns.
+      BOTTOM_LEFT = 2,
 
-        /// \brief Bottom-right corner, consisting of the intersection between
-        /// the last 3 rows and last 3 columns.
-        BOTTOM_RIGHT = 3
-      };
+      /// \brief Bottom-right corner, consisting of the intersection between
+      /// the last 3 rows and last 3 columns.
+      BOTTOM_RIGHT = 3
+    };
 
-      /// \brief Size of matrix is fixed to 6x6
-      public: static constexpr std::size_t MatrixSize{6};
+    /// \brief Size of matrix is fixed to 6x6
+    public: static constexpr std::size_t MatrixSize{6};
 
-      /// \brief Identity matrix
-      public: static const Matrix6<T> &Identity;
+    /// \brief Identity matrix
+    public: static const Matrix6<T> &Identity;
 
-      /// \brief Zero matrix
-      public: static const Matrix6<T> &Zero;
+    /// \brief Zero matrix
+    public: static const Matrix6<T> &Zero;
 
-      /// \brief Constructor
-      public: Matrix6()
-      {
-        memset(this->data, 0, sizeof(this->data[0][0])*MatrixSize*MatrixSize);
-      }
+    /// \brief Constructor
+    public: Matrix6()
+    {
+      memset(this->data, 0, sizeof(this->data[0][0])*MatrixSize*MatrixSize);
+    }
 
-      /// \brief Copy constructor
-      /// \param _m Matrix to copy
-      public: Matrix6(const Matrix6<T> &_m) = default;
-
-      /// \brief Constructor
-      /// \param[in] _v00 Row 0, Col 0 value
-      /// \param[in] _v01 Row 0, Col 1 value
-      /// \param[in] _v02 Row 0, Col 2 value
-      /// \param[in] _v03 Row 0, Col 3 value
-      /// \param[in] _v04 Row 0, Col 4 value
-      /// \param[in] _v05 Row 0, Col 5 value
-      /// \param[in] _v10 Row 1, Col 0 value
-      /// \param[in] _v11 Row 1, Col 1 value
-      /// \param[in] _v12 Row 1, Col 2 value
-      /// \param[in] _v13 Row 1, Col 3 value
-      /// \param[in] _v14 Row 1, Col 4 value
-      /// \param[in] _v15 Row 1, Col 5 value
-      /// \param[in] _v20 Row 2, Col 0 value
-      /// \param[in] _v21 Row 2, Col 1 value
-      /// \param[in] _v22 Row 2, Col 2 value
-      /// \param[in] _v23 Row 2, Col 3 value
-      /// \param[in] _v24 Row 2, Col 4 value
-      /// \param[in] _v25 Row 2, Col 5 value
-      /// \param[in] _v30 Row 3, Col 0 value
-      /// \param[in] _v31 Row 3, Col 1 value
-      /// \param[in] _v32 Row 3, Col 2 value
-      /// \param[in] _v33 Row 3, Col 3 value
-      /// \param[in] _v34 Row 3, Col 4 value
-      /// \param[in] _v35 Row 3, Col 5 value
-      /// \param[in] _v40 Row 4, Col 0 value
-      /// \param[in] _v41 Row 4, Col 1 value
-      /// \param[in] _v42 Row 4, Col 2 value
-      /// \param[in] _v43 Row 4, Col 3 value
-      /// \param[in] _v44 Row 4, Col 4 value
-      /// \param[in] _v45 Row 4, Col 5 value
-      /// \param[in] _v50 Row 5, Col 0 value
-      /// \param[in] _v51 Row 5, Col 1 value
-      /// \param[in] _v52 Row 5, Col 2 value
-      /// \param[in] _v53 Row 5, Col 3 value
-      /// \param[in] _v54 Row 5, Col 4 value
-      /// \param[in] _v55 Row 5, Col 5 value
-      public: constexpr Matrix6(T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
-                                T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
-                                T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
-                                T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
-                                T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
-                                T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
-      : data{{_v00, _v01, _v02, _v03, _v04, _v05},
-             {_v10, _v11, _v12, _v13, _v14, _v15},
-             {_v20, _v21, _v22, _v23, _v24, _v25},
-             {_v30, _v31, _v32, _v33, _v34, _v35},
-             {_v40, _v41, _v42, _v43, _v44, _v45},
-             {_v50, _v51, _v52, _v53, _v54, _v55}}
-      {
-      }
+    /// \brief Copy constructor
+    /// \param _m Matrix to copy
+    public: Matrix6(const Matrix6<T> &_m) = default;
+
+    /// \brief Constructor
+    /// \param[in] _v00 Row 0, Col 0 value
+    /// \param[in] _v01 Row 0, Col 1 value
+    /// \param[in] _v02 Row 0, Col 2 value
+    /// \param[in] _v03 Row 0, Col 3 value
+    /// \param[in] _v04 Row 0, Col 4 value
+    /// \param[in] _v05 Row 0, Col 5 value
+    /// \param[in] _v10 Row 1, Col 0 value
+    /// \param[in] _v11 Row 1, Col 1 value
+    /// \param[in] _v12 Row 1, Col 2 value
+    /// \param[in] _v13 Row 1, Col 3 value
+    /// \param[in] _v14 Row 1, Col 4 value
+    /// \param[in] _v15 Row 1, Col 5 value
+    /// \param[in] _v20 Row 2, Col 0 value
+    /// \param[in] _v21 Row 2, Col 1 value
+    /// \param[in] _v22 Row 2, Col 2 value
+    /// \param[in] _v23 Row 2, Col 3 value
+    /// \param[in] _v24 Row 2, Col 4 value
+    /// \param[in] _v25 Row 2, Col 5 value
+    /// \param[in] _v30 Row 3, Col 0 value
+    /// \param[in] _v31 Row 3, Col 1 value
+    /// \param[in] _v32 Row 3, Col 2 value
+    /// \param[in] _v33 Row 3, Col 3 value
+    /// \param[in] _v34 Row 3, Col 4 value
+    /// \param[in] _v35 Row 3, Col 5 value
+    /// \param[in] _v40 Row 4, Col 0 value
+    /// \param[in] _v41 Row 4, Col 1 value
+    /// \param[in] _v42 Row 4, Col 2 value
+    /// \param[in] _v43 Row 4, Col 3 value
+    /// \param[in] _v44 Row 4, Col 4 value
+    /// \param[in] _v45 Row 4, Col 5 value
+    /// \param[in] _v50 Row 5, Col 0 value
+    /// \param[in] _v51 Row 5, Col 1 value
+    /// \param[in] _v52 Row 5, Col 2 value
+    /// \param[in] _v53 Row 5, Col 3 value
+    /// \param[in] _v54 Row 5, Col 4 value
+    /// \param[in] _v55 Row 5, Col 5 value
+    public: constexpr Matrix6(T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
+                              T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
+                              T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
+                              T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
+                              T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
+                              T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
+    : data{{_v00, _v01, _v02, _v03, _v04, _v05},
+           {_v10, _v11, _v12, _v13, _v14, _v15},
+           {_v20, _v21, _v22, _v23, _v24, _v25},
+           {_v30, _v31, _v32, _v33, _v34, _v35},
+           {_v40, _v41, _v42, _v43, _v44, _v45},
+           {_v50, _v51, _v52, _v53, _v54, _v55}}
+    {
+    }
 
-      /// \brief Set a value in a specific row and col
-      /// param[in] _row Row of the matrix
-      /// param[in] _col Col of the matrix
-      /// param[in] _v Value to assign
-      /// \return Tru if the value was setted, False otherwise
-      public: bool SetValue(size_t _row, size_t _col, T _v)
+    /// \brief Set a value in a specific row and col
+    /// param[in] _row Row of the matrix
+    /// param[in] _col Col of the matrix
+    /// param[in] _v Value to assign
+    /// \return Tru if the value was setted, False otherwise
+    public: bool SetValue(size_t _row, size_t _col, T _v)
+    {
+      if (_row < MatrixSize && _col < MatrixSize)
       {
-        if (_row < MatrixSize && _col < MatrixSize)
-        {
-          this->data[_row][_col] = _v;
-          return true;
-        }
-        return false;
+        this->data[_row][_col] = _v;
+        return true;
       }
+      return false;
+    }
 
-      /// \brief Change the values
-      /// \param[in] _v00 Row 0, Col 0 value
-      /// \param[in] _v01 Row 0, Col 1 value
-      /// \param[in] _v02 Row 0, Col 2 value
-      /// \param[in] _v03 Row 0, Col 3 value
-      /// \param[in] _v04 Row 0, Col 4 value
-      /// \param[in] _v05 Row 0, Col 5 value
-      /// \param[in] _v10 Row 1, Col 0 value
-      /// \param[in] _v11 Row 1, Col 1 value
-      /// \param[in] _v12 Row 1, Col 2 value
-      /// \param[in] _v13 Row 1, Col 3 value
-      /// \param[in] _v14 Row 1, Col 4 value
-      /// \param[in] _v15 Row 1, Col 5 value
-      /// \param[in] _v20 Row 2, Col 0 value
-      /// \param[in] _v21 Row 2, Col 1 value
-      /// \param[in] _v22 Row 2, Col 2 value
-      /// \param[in] _v23 Row 2, Col 3 value
-      /// \param[in] _v24 Row 2, Col 4 value
-      /// \param[in] _v25 Row 2, Col 5 value
-      /// \param[in] _v30 Row 3, Col 0 value
-      /// \param[in] _v31 Row 3, Col 1 value
-      /// \param[in] _v32 Row 3, Col 2 value
-      /// \param[in] _v33 Row 3, Col 3 value
-      /// \param[in] _v34 Row 3, Col 4 value
-      /// \param[in] _v35 Row 3, Col 5 value
-      /// \param[in] _v40 Row 4, Col 0 value
-      /// \param[in] _v41 Row 4, Col 1 value
-      /// \param[in] _v42 Row 4, Col 2 value
-      /// \param[in] _v43 Row 4, Col 3 value
-      /// \param[in] _v44 Row 4, Col 4 value
-      /// \param[in] _v45 Row 4, Col 5 value
-      /// \param[in] _v50 Row 5, Col 0 value
-      /// \param[in] _v51 Row 5, Col 1 value
-      /// \param[in] _v52 Row 5, Col 2 value
-      /// \param[in] _v53 Row 5, Col 3 value
-      /// \param[in] _v54 Row 5, Col 4 value
-      /// \param[in] _v55 Row 5, Col 5 value
-      public: void Set(
-          T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
-          T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
-          T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
-          T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
-          T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
-          T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
-      {
-        this->data[0][0] = _v00;
-        this->data[0][1] = _v01;
-        this->data[0][2] = _v02;
-        this->data[0][3] = _v03;
-        this->data[0][4] = _v04;
-        this->data[0][5] = _v05;
-
-        this->data[1][0] = _v10;
-        this->data[1][1] = _v11;
-        this->data[1][2] = _v12;
-        this->data[1][3] = _v13;
-        this->data[1][4] = _v14;
-        this->data[1][5] = _v15;
-
-        this->data[2][0] = _v20;
-        this->data[2][1] = _v21;
-        this->data[2][2] = _v22;
-        this->data[2][3] = _v23;
-        this->data[2][4] = _v24;
-        this->data[2][5] = _v25;
-
-        this->data[3][0] = _v30;
-        this->data[3][1] = _v31;
-        this->data[3][2] = _v32;
-        this->data[3][3] = _v33;
-        this->data[3][4] = _v34;
-        this->data[3][5] = _v35;
-
-        this->data[4][0] = _v40;
-        this->data[4][1] = _v41;
-        this->data[4][2] = _v42;
-        this->data[4][3] = _v43;
-        this->data[4][4] = _v44;
-        this->data[4][5] = _v45;
-
-        this->data[5][0] = _v50;
-        this->data[5][1] = _v51;
-        this->data[5][2] = _v52;
-        this->data[5][3] = _v53;
-        this->data[5][4] = _v54;
-        this->data[5][5] = _v55;
-      }
+    /// \brief Change the values
+    /// \param[in] _v00 Row 0, Col 0 value
+    /// \param[in] _v01 Row 0, Col 1 value
+    /// \param[in] _v02 Row 0, Col 2 value
+    /// \param[in] _v03 Row 0, Col 3 value
+    /// \param[in] _v04 Row 0, Col 4 value
+    /// \param[in] _v05 Row 0, Col 5 value
+    /// \param[in] _v10 Row 1, Col 0 value
+    /// \param[in] _v11 Row 1, Col 1 value
+    /// \param[in] _v12 Row 1, Col 2 value
+    /// \param[in] _v13 Row 1, Col 3 value
+    /// \param[in] _v14 Row 1, Col 4 value
+    /// \param[in] _v15 Row 1, Col 5 value
+    /// \param[in] _v20 Row 2, Col 0 value
+    /// \param[in] _v21 Row 2, Col 1 value
+    /// \param[in] _v22 Row 2, Col 2 value
+    /// \param[in] _v23 Row 2, Col 3 value
+    /// \param[in] _v24 Row 2, Col 4 value
+    /// \param[in] _v25 Row 2, Col 5 value
+    /// \param[in] _v30 Row 3, Col 0 value
+    /// \param[in] _v31 Row 3, Col 1 value
+    /// \param[in] _v32 Row 3, Col 2 value
+    /// \param[in] _v33 Row 3, Col 3 value
+    /// \param[in] _v34 Row 3, Col 4 value
+    /// \param[in] _v35 Row 3, Col 5 value
+    /// \param[in] _v40 Row 4, Col 0 value
+    /// \param[in] _v41 Row 4, Col 1 value
+    /// \param[in] _v42 Row 4, Col 2 value
+    /// \param[in] _v43 Row 4, Col 3 value
+    /// \param[in] _v44 Row 4, Col 4 value
+    /// \param[in] _v45 Row 4, Col 5 value
+    /// \param[in] _v50 Row 5, Col 0 value
+    /// \param[in] _v51 Row 5, Col 1 value
+    /// \param[in] _v52 Row 5, Col 2 value
+    /// \param[in] _v53 Row 5, Col 3 value
+    /// \param[in] _v54 Row 5, Col 4 value
+    /// \param[in] _v55 Row 5, Col 5 value
+    public: void Set(
+        T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
+        T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
+        T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
+        T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
+        T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
+        T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
+    {
+      this->data[0][0] = _v00;
+      this->data[0][1] = _v01;
+      this->data[0][2] = _v02;
+      this->data[0][3] = _v03;
+      this->data[0][4] = _v04;
+      this->data[0][5] = _v05;
+
+      this->data[1][0] = _v10;
+      this->data[1][1] = _v11;
+      this->data[1][2] = _v12;
+      this->data[1][3] = _v13;
+      this->data[1][4] = _v14;
+      this->data[1][5] = _v15;
+
+      this->data[2][0] = _v20;
+      this->data[2][1] = _v21;
+      this->data[2][2] = _v22;
+      this->data[2][3] = _v23;
+      this->data[2][4] = _v24;
+      this->data[2][5] = _v25;
+
+      this->data[3][0] = _v30;
+      this->data[3][1] = _v31;
+      this->data[3][2] = _v32;
+      this->data[3][3] = _v33;
+      this->data[3][4] = _v34;
+      this->data[3][5] = _v35;
+
+      this->data[4][0] = _v40;
+      this->data[4][1] = _v41;
+      this->data[4][2] = _v42;
+      this->data[4][3] = _v43;
+      this->data[4][4] = _v44;
+      this->data[4][5] = _v45;
+
+      this->data[5][0] = _v50;
+      this->data[5][1] = _v51;
+      this->data[5][2] = _v52;
+      this->data[5][3] = _v53;
+      this->data[5][4] = _v54;
+      this->data[5][5] = _v55;
+    }
 
-      /// \brief Transpose this matrix.
-      public: void Transpose()
-      {
-        std::swap(this->data[0][1], this->data[1][0]);
-        std::swap(this->data[0][2], this->data[2][0]);
-        std::swap(this->data[0][3], this->data[3][0]);
-        std::swap(this->data[0][4], this->data[4][0]);
-        std::swap(this->data[0][5], this->data[5][0]);
-        std::swap(this->data[1][2], this->data[2][1]);
-        std::swap(this->data[1][3], this->data[3][1]);
-        std::swap(this->data[1][4], this->data[4][1]);
-        std::swap(this->data[1][5], this->data[5][1]);
-        std::swap(this->data[2][3], this->data[3][2]);
-        std::swap(this->data[2][4], this->data[4][2]);
-        std::swap(this->data[2][5], this->data[5][2]);
-        std::swap(this->data[3][4], this->data[4][3]);
-        std::swap(this->data[3][5], this->data[5][3]);
-        std::swap(this->data[4][5], this->data[5][4]);
-      }
+    /// \brief Transpose this matrix.
+    public: void Transpose()
+    {
+      std::swap(this->data[0][1], this->data[1][0]);
+      std::swap(this->data[0][2], this->data[2][0]);
+      std::swap(this->data[0][3], this->data[3][0]);
+      std::swap(this->data[0][4], this->data[4][0]);
+      std::swap(this->data[0][5], this->data[5][0]);
+      std::swap(this->data[1][2], this->data[2][1]);
+      std::swap(this->data[1][3], this->data[3][1]);
+      std::swap(this->data[1][4], this->data[4][1]);
+      std::swap(this->data[1][5], this->data[5][1]);
+      std::swap(this->data[2][3], this->data[3][2]);
+      std::swap(this->data[2][4], this->data[4][2]);
+      std::swap(this->data[2][5], this->data[5][2]);
+      std::swap(this->data[3][4], this->data[4][3]);
+      std::swap(this->data[3][5], this->data[5][3]);
+      std::swap(this->data[4][5], this->data[5][4]);
+    }
 
-      /// \brief Return the transpose of this matrix
-      /// \return Transpose of this matrix.
-      public: Matrix6<T> Transposed() const
-      {
-        return Matrix6<T>(
-            this->data[0][0],
-            this->data[1][0],
-            this->data[2][0],
-            this->data[3][0],
-            this->data[4][0],
-            this->data[5][0],
-
-            this->data[0][1],
-            this->data[1][1],
-            this->data[2][1],
-            this->data[3][1],
-            this->data[4][1],
-            this->data[5][1],
-
-            this->data[0][2],
-            this->data[1][2],
-            this->data[2][2],
-            this->data[3][2],
-            this->data[4][2],
-            this->data[5][2],
-
-            this->data[0][3],
-            this->data[1][3],
-            this->data[2][3],
-            this->data[3][3],
-            this->data[4][3],
-            this->data[5][3],
-
-            this->data[0][4],
-            this->data[1][4],
-            this->data[2][4],
-            this->data[3][4],
-            this->data[4][4],
-            this->data[5][4],
-
-            this->data[0][5],
-            this->data[1][5],
-            this->data[2][5],
-            this->data[3][5],
-            this->data[4][5],
-            this->data[5][5]);
-      }
+    /// \brief Return the transpose of this matrix
+    /// \return Transpose of this matrix.
+    public: Matrix6<T> Transposed() const
+    {
+      return Matrix6<T>(
+          this->data[0][0],
+          this->data[1][0],
+          this->data[2][0],
+          this->data[3][0],
+          this->data[4][0],
+          this->data[5][0],
+
+          this->data[0][1],
+          this->data[1][1],
+          this->data[2][1],
+          this->data[3][1],
+          this->data[4][1],
+          this->data[5][1],
+
+          this->data[0][2],
+          this->data[1][2],
+          this->data[2][2],
+          this->data[3][2],
+          this->data[4][2],
+          this->data[5][2],
+
+          this->data[0][3],
+          this->data[1][3],
+          this->data[2][3],
+          this->data[3][3],
+          this->data[4][3],
+          this->data[5][3],
+
+          this->data[0][4],
+          this->data[1][4],
+          this->data[2][4],
+          this->data[3][4],
+          this->data[4][4],
+          this->data[5][4],
+
+          this->data[0][5],
+          this->data[1][5],
+          this->data[2][5],
+          this->data[3][5],
+          this->data[4][5],
+          this->data[5][5]);
+    }
 
-      /// \brief Assignment operator. this = _mat
-      /// \param _mat Incoming matrix
-      /// \return itself
-      public: Matrix6<T> &operator=(const Matrix6<T> &_mat) = default;
+    /// \brief Assignment operator. this = _mat
+    /// \param _mat Incoming matrix
+    /// \return itself
+    public: Matrix6<T> &operator=(const Matrix6<T> &_mat) = default;
 
-      /// \brief Multiplication assignment operator. This matrix will
-      /// become equal to this * _m2.
-      /// \param[in] _m2 Incoming matrix.
-      /// \return This matrix * _m2.
-      public: Matrix6<T> operator*=(const Matrix6<T> &_m2)
-      {
-        (*this) = (*this) * _m2;
-        return *this;
-      }
+    /// \brief Multiplication assignment operator. This matrix will
+    /// become equal to this * _m2.
+    /// \param[in] _m2 Incoming matrix.
+    /// \return This matrix * _m2.
+    public: Matrix6<T> operator*=(const Matrix6<T> &_m2)
+    {
+      (*this) = (*this) * _m2;
+      return *this;
+    }
 
-      /// \brief Multiplication operator
-      /// \param[in] _m2 Incoming matrix
-      /// \return This matrix * _m2
-      public: Matrix6<T> operator*(const Matrix6<T> &_m2) const
+    /// \brief Multiplication operator
+    /// \param[in] _m2 Incoming matrix
+    /// \return This matrix * _m2
+    public: Matrix6<T> operator*(const Matrix6<T> &_m2) const
+    {
+      auto el = [&](size_t _row, size_t _col) -> T
       {
-        auto el = [&](size_t _row, size_t _col) -> T
-        {
-          T result = static_cast<T>(0);
-          for (size_t i = 0; i < MatrixSize; ++i)
-            result += this->data[_row][i] * _m2(i, _col);
-          return result;
-        };
-        return Matrix6<T>(
-            el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
-            el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
-            el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
-            el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
-            el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
-            el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
-      }
+        T result = static_cast<T>(0);
+        for (size_t i = 0; i < MatrixSize; ++i)
+          result += this->data[_row][i] * _m2(i, _col);
+        return result;
+      };
+      return Matrix6<T>(
+          el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
+          el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
+          el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
+          el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
+          el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
+          el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
+    }
 
-      /// \brief Addition assignment operator. This matrix will
-      /// become equal to this + _m2.
-      /// \param[in] _m2 Incoming matrix.
-      /// \return This matrix + _m2.
-      public: Matrix6<T> operator+=(const Matrix6<T> &_m2)
-      {
-        (*this) = (*this) + _m2;
-        return *this;
-      }
+    /// \brief Addition assignment operator. This matrix will
+    /// become equal to this + _m2.
+    /// \param[in] _m2 Incoming matrix.
+    /// \return This matrix + _m2.
+    public: Matrix6<T> operator+=(const Matrix6<T> &_m2)
+    {
+      (*this) = (*this) + _m2;
+      return *this;
+    }
 
-      /// \brief Addition operator
-      /// \param[in] _m2 Incoming matrix
-      /// \return This matrix + _m2
-      public: Matrix6<T> operator+(const Matrix6<T> &_m2) const
+    /// \brief Addition operator
+    /// \param[in] _m2 Incoming matrix
+    /// \return This matrix + _m2
+    public: Matrix6<T> operator+(const Matrix6<T> &_m2) const
+    {
+      auto el = [&](size_t _row, size_t _col) -> T
       {
-        auto el = [&](size_t _row, size_t _col) -> T
-        {
-          return this->data[_row][_col] + _m2(_row, _col);
-        };
-        return Matrix6<T>(
-            el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
-            el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
-            el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
-            el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
-            el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
-            el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
-      }
+        return this->data[_row][_col] + _m2(_row, _col);
+      };
+      return Matrix6<T>(
+          el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
+          el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
+          el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
+          el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
+          el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
+          el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
+    }
 
-     /// \brief Get the value at the specified row, column index
-     /// \param[in] _col The column index. Index values are clamped to a
-     /// range of [0, 5].
-     /// \param[in] _row the row index. Index values are clamped to a
-     /// range of [0, 5].
-     /// \return The value at the specified index
-     public: inline const T &operator()(const size_t _row,
-                 const size_t _col) const
+   /// \brief Get the value at the specified row, column index
+   /// \param[in] _col The column index. Index values are clamped to a
+   /// range of [0, 5].
+   /// \param[in] _row the row index. Index values are clamped to a
+   /// range of [0, 5].
+   /// \return The value at the specified index
+   public: inline const T &operator()(const size_t _row,
+               const size_t _col) const
+   {
+     return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)][
+                       clamp(_col, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)];
+   }
+
+   /// \brief Get a mutable version of the value at the specified row,
+   /// column index
+   /// \param[in] _row the row index. Index values are clamped to a
+   /// range of [0, 5].
+   /// \param[in] _col The column index. Index values are clamped to a
+   /// range of [0, 5].
+   /// \return The value at the specified index
+   public: inline T &operator()(const size_t _row, const size_t _col)
+   {
+     return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)]
+                      [clamp(_col, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)];
+   }
+
+   /// \brief Get one of the four 3x3 submatrices that compose this matrix.
+   /// These submatrices are formed by dividing the 6x6 matrix in 4 parts that
+   /// do not overlap with each other.
+   /// \param[in] _corner Which corner to retrieve.
+   /// \return A new matrix containing the values of the submatrix.
+   public: Matrix3<T> Submatrix(Matrix6Corner _corner) const
+   {
+     size_t row = 0;
+     size_t col = 0;
+     if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
      {
-       return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)][
-                         clamp(_col, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)];
+       row = 3;
      }
-
-     /// \brief Get a mutable version of the value at the specified row,
-     /// column index
-     /// \param[in] _row the row index. Index values are clamped to a
-     /// range of [0, 5].
-     /// \param[in] _col The column index. Index values are clamped to a
-     /// range of [0, 5].
-     /// \return The value at the specified index
-     public: inline T &operator()(const size_t _row, const size_t _col)
+     if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
      {
-       return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)]
-                        [clamp(_col, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)];
+       col = 3;
      }
-
-     /// \brief Get one of the four 3x3 submatrices that compose this matrix.
-     /// These submatrices are formed by dividing the 6x6 matrix in 4 parts that
-     /// do not overlap with each other.
-     /// \param[in] _corner Which corner to retrieve.
-     /// \return A new matrix containing the values of the submatrix.
-     public: Matrix3<T> Submatrix(Matrix6Corner _corner) const
+     return {this->data[row + 0][col + 0],
+             this->data[row + 0][col + 1],
+             this->data[row + 0][col + 2],
+             this->data[row + 1][col + 0],
+             this->data[row + 1][col + 1],
+             this->data[row + 1][col + 2],
+             this->data[row + 2][col + 0],
+             this->data[row + 2][col + 1],
+             this->data[row + 2][col + 2]};
+   }
+
+   /// \brief Set one of the four 3x3 submatrices that compose this matrix.
+   /// These submatrices are formed by dividing the 6x6 matrix in 4 parts that
+   /// do not overlap with each other.
+   /// \param[in] _corner Which corner to set.
+   /// \param[in] _mat The matrix to set.
+   public: void SetSubmatrix(Matrix6Corner _corner, const Matrix3<T> &_mat)
+   {
+     size_t row = 0;
+     size_t col = 0;
+     if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
      {
-       size_t row = 0;
-       size_t col = 0;
-       if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
-       {
-         row = 3;
-       }
-       if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
-       {
-         col = 3;
-       }
-       return {this->data[row + 0][col + 0],
-               this->data[row + 0][col + 1],
-               this->data[row + 0][col + 2],
-               this->data[row + 1][col + 0],
-               this->data[row + 1][col + 1],
-               this->data[row + 1][col + 2],
-               this->data[row + 2][col + 0],
-               this->data[row + 2][col + 1],
-               this->data[row + 2][col + 2]};
+       row = 3;
      }
-
-     /// \brief Set one of the four 3x3 submatrices that compose this matrix.
-     /// These submatrices are formed by dividing the 6x6 matrix in 4 parts that
-     /// do not overlap with each other.
-     /// \param[in] _corner Which corner to set.
-     /// \param[in] _mat The matrix to set.
-     public: void SetSubmatrix(Matrix6Corner _corner, const Matrix3<T> &_mat)
+     if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
      {
-       size_t row = 0;
-       size_t col = 0;
-       if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
-       {
-         row = 3;
-       }
-       if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
-       {
-         col = 3;
-       }
-       for (size_t r = 0; r < 3; ++r)
+       col = 3;
+     }
+     for (size_t r = 0; r < 3; ++r)
+     {
+       for (size_t c = 0; c < 3; ++c)
        {
-         for (size_t c = 0; c < 3; ++c)
-         {
-           this->data[row + r][col + c] = _mat(r, c);
-         }
+         this->data[row + r][col + c] = _mat(r, c);
        }
      }
+   }
+
+    /// \brief Equality test with tolerance.
+    /// \param[in] _m the matrix to compare to
+    /// \param[in] _tol equality tolerance.
+    /// \return true if the elements of the matrices are equal within
+    /// the tolerence specified by _tol.
+    public: bool Equal(const Matrix6 &_m, const T &_tol) const
+    {
+      return equal<T>(this->data[0][0], _m(0, 0), _tol)
+          && equal<T>(this->data[0][1], _m(0, 1), _tol)
+          && equal<T>(this->data[0][2], _m(0, 2), _tol)
+          && equal<T>(this->data[0][3], _m(0, 3), _tol)
+          && equal<T>(this->data[0][4], _m(0, 4), _tol)
+          && equal<T>(this->data[0][5], _m(0, 5), _tol)
+          && equal<T>(this->data[1][0], _m(1, 0), _tol)
+          && equal<T>(this->data[1][1], _m(1, 1), _tol)
+          && equal<T>(this->data[1][2], _m(1, 2), _tol)
+          && equal<T>(this->data[1][3], _m(1, 3), _tol)
+          && equal<T>(this->data[1][4], _m(1, 4), _tol)
+          && equal<T>(this->data[1][5], _m(1, 5), _tol)
+          && equal<T>(this->data[2][0], _m(2, 0), _tol)
+          && equal<T>(this->data[2][1], _m(2, 1), _tol)
+          && equal<T>(this->data[2][2], _m(2, 2), _tol)
+          && equal<T>(this->data[2][3], _m(2, 3), _tol)
+          && equal<T>(this->data[2][4], _m(2, 4), _tol)
+          && equal<T>(this->data[2][5], _m(2, 5), _tol)
+          && equal<T>(this->data[3][0], _m(3, 0), _tol)
+          && equal<T>(this->data[3][1], _m(3, 1), _tol)
+          && equal<T>(this->data[3][2], _m(3, 2), _tol)
+          && equal<T>(this->data[3][3], _m(3, 3), _tol)
+          && equal<T>(this->data[3][4], _m(3, 4), _tol)
+          && equal<T>(this->data[3][5], _m(3, 5), _tol)
+          && equal<T>(this->data[4][0], _m(4, 0), _tol)
+          && equal<T>(this->data[4][1], _m(4, 1), _tol)
+          && equal<T>(this->data[4][2], _m(4, 2), _tol)
+          && equal<T>(this->data[4][3], _m(4, 3), _tol)
+          && equal<T>(this->data[4][4], _m(4, 4), _tol)
+          && equal<T>(this->data[4][5], _m(4, 5), _tol)
+          && equal<T>(this->data[5][0], _m(5, 0), _tol)
+          && equal<T>(this->data[5][1], _m(5, 1), _tol)
+          && equal<T>(this->data[5][2], _m(5, 2), _tol)
+          && equal<T>(this->data[5][3], _m(5, 3), _tol)
+          && equal<T>(this->data[5][4], _m(5, 4), _tol)
+          && equal<T>(this->data[5][5], _m(5, 5), _tol);
+    }
 
-      /// \brief Equality test with tolerance.
-      /// \param[in] _m the matrix to compare to
-      /// \param[in] _tol equality tolerance.
-      /// \return true if the elements of the matrices are equal within
-      /// the tolerence specified by _tol.
-      public: bool Equal(const Matrix6 &_m, const T &_tol) const
-      {
-        return equal<T>(this->data[0][0], _m(0, 0), _tol)
-            && equal<T>(this->data[0][1], _m(0, 1), _tol)
-            && equal<T>(this->data[0][2], _m(0, 2), _tol)
-            && equal<T>(this->data[0][3], _m(0, 3), _tol)
-            && equal<T>(this->data[0][4], _m(0, 4), _tol)
-            && equal<T>(this->data[0][5], _m(0, 5), _tol)
-            && equal<T>(this->data[1][0], _m(1, 0), _tol)
-            && equal<T>(this->data[1][1], _m(1, 1), _tol)
-            && equal<T>(this->data[1][2], _m(1, 2), _tol)
-            && equal<T>(this->data[1][3], _m(1, 3), _tol)
-            && equal<T>(this->data[1][4], _m(1, 4), _tol)
-            && equal<T>(this->data[1][5], _m(1, 5), _tol)
-            && equal<T>(this->data[2][0], _m(2, 0), _tol)
-            && equal<T>(this->data[2][1], _m(2, 1), _tol)
-            && equal<T>(this->data[2][2], _m(2, 2), _tol)
-            && equal<T>(this->data[2][3], _m(2, 3), _tol)
-            && equal<T>(this->data[2][4], _m(2, 4), _tol)
-            && equal<T>(this->data[2][5], _m(2, 5), _tol)
-            && equal<T>(this->data[3][0], _m(3, 0), _tol)
-            && equal<T>(this->data[3][1], _m(3, 1), _tol)
-            && equal<T>(this->data[3][2], _m(3, 2), _tol)
-            && equal<T>(this->data[3][3], _m(3, 3), _tol)
-            && equal<T>(this->data[3][4], _m(3, 4), _tol)
-            && equal<T>(this->data[3][5], _m(3, 5), _tol)
-            && equal<T>(this->data[4][0], _m(4, 0), _tol)
-            && equal<T>(this->data[4][1], _m(4, 1), _tol)
-            && equal<T>(this->data[4][2], _m(4, 2), _tol)
-            && equal<T>(this->data[4][3], _m(4, 3), _tol)
-            && equal<T>(this->data[4][4], _m(4, 4), _tol)
-            && equal<T>(this->data[4][5], _m(4, 5), _tol)
-            && equal<T>(this->data[5][0], _m(5, 0), _tol)
-            && equal<T>(this->data[5][1], _m(5, 1), _tol)
-            && equal<T>(this->data[5][2], _m(5, 2), _tol)
-            && equal<T>(this->data[5][3], _m(5, 3), _tol)
-            && equal<T>(this->data[5][4], _m(5, 4), _tol)
-            && equal<T>(this->data[5][5], _m(5, 5), _tol);
-      }
-
-      /// \brief Equality operator
-      /// \param[in] _m Matrix6 to test
-      /// \return true if the 2 matrices are equal (using the tolerance 1e-6),
-      ///  false otherwise
-      public: bool operator==(const Matrix6<T> &_m) const
-      {
-        return this->Equal(_m, static_cast<T>(1e-6));
-      }
+    /// \brief Equality operator
+    /// \param[in] _m Matrix6 to test
+    /// \return true if the 2 matrices are equal (using the tolerance 1e-6),
+    ///  false otherwise
+    public: bool operator==(const Matrix6<T> &_m) const
+    {
+      return this->Equal(_m, static_cast<T>(1e-6));
+    }
 
-      /// \brief Inequality test operator
-      /// \param[in] _m Matrix6<T> to test
-      /// \return True if not equal (using the default tolerance of 1e-6)
-      public: bool operator!=(const Matrix6<T> &_m) const
-      {
-        return !(*this == _m);
-      }
+    /// \brief Inequality test operator
+    /// \param[in] _m Matrix6<T> to test
+    /// \return True if not equal (using the default tolerance of 1e-6)
+    public: bool operator!=(const Matrix6<T> &_m) const
+    {
+      return !(*this == _m);
+    }
 
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _m Matrix to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-                  std::ostream &_out, const gz::math::Matrix6<T> &_m)
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _m Matrix to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+                std::ostream &_out, const gz::math::Matrix6<T> &_m)
+    {
+     for (auto i : {0, 1, 2, 3, 4, 5})
       {
-       for (auto i : {0, 1, 2, 3, 4, 5})
+        for (auto j : {0, 1, 2, 3, 4, 5})
         {
-          for (auto j : {0, 1, 2, 3, 4, 5})
-          {
-            if (!(i == 0 && j == 0))
-              _out << " ";
+          if (!(i == 0 && j == 0))
+            _out << " ";
 
-            appendToStream(_out, _m(i, j));
-          }
+          appendToStream(_out, _m(i, j));
         }
-
-        return _out;
       }
 
-      /// \brief Stream extraction operator
-      /// \param[in,out] _in input stream
-      /// \param[out] _m Matrix6<T> to read values into
-      /// \return the stream
-      public: friend std::istream &operator>>(
-                  std::istream &_in, gz::math::Matrix6<T> &_m)
+      return _out;
+    }
+
+    /// \brief Stream extraction operator
+    /// \param[in,out] _in input stream
+    /// \param[out] _m Matrix6<T> to read values into
+    /// \return the stream
+    public: friend std::istream &operator>>(
+                std::istream &_in, gz::math::Matrix6<T> &_m)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      T d[36];
+      _in >> d[0] >> d[1] >> d[2] >> d[3] >> d[4] >> d[5]
+          >> d[6] >> d[7] >> d[8] >> d[9] >> d[10] >> d[11]
+          >> d[12] >> d[13] >> d[14] >> d[15] >> d[16] >> d[17]
+          >> d[18] >> d[19] >> d[20] >> d[21] >> d[22] >> d[23]
+          >> d[24] >> d[25] >> d[26] >> d[27] >> d[28] >> d[29]
+          >> d[30] >> d[31] >> d[32] >> d[33] >> d[34] >> d[35];
+
+      if (!_in.fail())
       {
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        T d[36];
-        _in >> d[0] >> d[1] >> d[2] >> d[3] >> d[4] >> d[5]
-            >> d[6] >> d[7] >> d[8] >> d[9] >> d[10] >> d[11]
-            >> d[12] >> d[13] >> d[14] >> d[15] >> d[16] >> d[17]
-            >> d[18] >> d[19] >> d[20] >> d[21] >> d[22] >> d[23]
-            >> d[24] >> d[25] >> d[26] >> d[27] >> d[28] >> d[29]
-            >> d[30] >> d[31] >> d[32] >> d[33] >> d[34] >> d[35];
-
-        if (!_in.fail())
-        {
-          _m.Set(d[0], d[1], d[2], d[3], d[4], d[5],
-                 d[6], d[7], d[8], d[9], d[10], d[11],
-                 d[12], d[13], d[14], d[15], d[16], d[17],
-                 d[18], d[19], d[20], d[21], d[22], d[23],
-                 d[24], d[25], d[26], d[27], d[28], d[29],
-                 d[30], d[31], d[32], d[33], d[34], d[35]);
-        }
-        return _in;
+        _m.Set(d[0], d[1], d[2], d[3], d[4], d[5],
+               d[6], d[7], d[8], d[9], d[10], d[11],
+               d[12], d[13], d[14], d[15], d[16], d[17],
+               d[18], d[19], d[20], d[21], d[22], d[23],
+               d[24], d[25], d[26], d[27], d[28], d[29],
+               d[30], d[31], d[32], d[33], d[34], d[35]);
       }
+      return _in;
+    }
 
-      /// \brief The 6x6 matrix
-      private: T data[MatrixSize][MatrixSize];
-    };
-
-    namespace detail {
-
-      template<typename T>
-      constexpr Matrix6<T> gMatrix6Identity(
-          1, 0, 0, 0, 0, 0,
-          0, 1, 0, 0, 0, 0,
-          0, 0, 1, 0, 0, 0,
-          0, 0, 0, 1, 0, 0,
-          0, 0, 0, 0, 1, 0,
-          0, 0, 0, 0, 0, 1);
-
-      template<typename T>
-      constexpr Matrix6<T> gMatrix6Zero(
-          0, 0, 0, 0, 0, 0,
-          0, 0, 0, 0, 0, 0,
-          0, 0, 0, 0, 0, 0,
-          0, 0, 0, 0, 0, 0,
-          0, 0, 0, 0, 0, 0,
-          0, 0, 0, 0, 0, 0);
+    /// \brief The 6x6 matrix
+    private: T data[MatrixSize][MatrixSize];
+  };
 
-    }  // namespace detail
+  namespace detail {
 
     template<typename T>
-    const Matrix6<T> &Matrix6<T>::Identity = detail::gMatrix6Identity<T>;
+    constexpr Matrix6<T> gMatrix6Identity(
+        1, 0, 0, 0, 0, 0,
+        0, 1, 0, 0, 0, 0,
+        0, 0, 1, 0, 0, 0,
+        0, 0, 0, 1, 0, 0,
+        0, 0, 0, 0, 1, 0,
+        0, 0, 0, 0, 0, 1);
 
     template<typename T>
-    const Matrix6<T> &Matrix6<T>::Zero = detail::gMatrix6Zero<T>;
-
-    typedef Matrix6<int> Matrix6i;
-    typedef Matrix6<double> Matrix6d;
-    typedef Matrix6<float> Matrix6f;
-    }
-  }
-}
-#endif
+    constexpr Matrix6<T> gMatrix6Zero(
+        0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0);
+
+  }  // namespace detail
+
+  template<typename T>
+  const Matrix6<T> &Matrix6<T>::Identity = detail::gMatrix6Identity<T>;
+
+  template<typename T>
+  const Matrix6<T> &Matrix6<T>::Zero = detail::gMatrix6Zero<T>;
+
+  typedef Matrix6<int> Matrix6i;
+  typedef Matrix6<double> Matrix6d;
+  typedef Matrix6<float> Matrix6f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_MATRIX6_HH_
diff --git a/include/gz/math/MecanumDriveOdometry.hh b/include/gz/math/MecanumDriveOdometry.hh
index 21c71e15b..6d021b0ca 100644
--- a/include/gz/math/MecanumDriveOdometry.hh
+++ b/include/gz/math/MecanumDriveOdometry.hh
@@ -23,115 +23,112 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \class MecanumDriveOdometry MecanumDriveOdometry.hh
+  /// gz/math/MecanumDriveOdometry.hh
+  ///
+  /// \brief Computes odometry values based on a set of kinematic
+  /// properties and wheel speeds for a Mecanum-drive vehicle.
+  ///
+  /// Note: when computing velocity the math currently assumes that
+  /// all wheels have a radius of 1.0.
+  ///
+  /// A vehicle with a heading of zero degrees has a local
+  /// reference frame according to the diagram below.
+  ///
+  ///       Y
+  ///       ^
+  ///       |
+  ///       |
+  ///       O--->X(forward)
+  class GZ_MATH_VISIBLE MecanumDriveOdometry
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \class MecanumDriveOdometry MecanumDriveOdometry.hh
-    /// gz/math/MecanumDriveOdometry.hh
-    ///
-    /// \brief Computes odometry values based on a set of kinematic
-    /// properties and wheel speeds for a Mecanum-drive vehicle.
-    ///
-    /// Note: when computing velocity the math currently assumes that
-    /// all wheels have a radius of 1.0.
-    ///
-    /// A vehicle with a heading of zero degrees has a local
-    /// reference frame according to the diagram below.
-    ///
-    ///       Y
-    ///       ^
-    ///       |
-    ///       |
-    ///       O--->X(forward)
-    class GZ_MATH_VISIBLE MecanumDriveOdometry
-    {
-      // Use a steady clock
-      public: using clock = std::chrono::steady_clock;
-
-      /// \brief Constructor.
-      /// \param[in] _windowSize Rolling window size used to compute the
-      /// velocity mean
-      public: explicit MecanumDriveOdometry(size_t _windowSize = 10);
-
-      /// \brief Initialize the odometry
-      /// \param[in] _time Current time.
-      public: void Init(const clock::time_point &_time);
-
-      /// \brief Get whether Init has been called.
-      /// \return True if Init has been called, false otherwise.
-      public: bool Initialized() const;
-
-      /// \brief Updates the odometry class with latest wheels and
-      /// steerings position
-      /// \param[in] _frontLeftPos Left wheel position in radians.
-      /// \param[in] _frontRightPos Right wheel postion in radians.
-      /// \param[in] _backLeftPos Left wheel position in radians.
-      /// \param[in] _backRightPos Right wheel postion in radians.
-      /// \param[in] _time Current time point.
-      /// \return True if the odometry is actually updated.
-      public: bool Update(
-        const Angle &_frontLeftPos, const Angle &_frontRightPos,
-        const Angle &_backLeftPos, const Angle &_backRightPos,
-        const clock::time_point &_time);
-
-      /// \brief Get the heading.
-      /// \return The heading in radians.
-      public: const Angle &Heading() const;
-
-      /// \brief Get the X position.
-      ///  \return The X position in meters
-      public: double X() const;
-
-      /// \brief Get the Y position.
-      /// \return The Y position in meters.
-      public: double Y() const;
-
-      /// \brief Get the linear velocity.
-      /// \return The linear velocity in meter/second.
-      public: double LinearVelocity() const;
-
-      /// \brief Get the lateral velocity.
-      /// \return The lateral velocity in meter/second.
-      public: double LateralVelocity() const;
-
-      /// \brief Get the angular velocity.
-      /// \return The angular velocity in radian/second.
-      public: const Angle &AngularVelocity() const;
-
-      /// \brief Set the wheel parameters including the radius and separation.
-      /// \param[in] _wheelSeparation Distance between left and right wheels.
-      /// \param[in] _wheelBase Distance between front and back wheels.
-      /// \param[in] _leftWheelRadius Radius of the left wheel.
-      /// \param[in] _rightWheelRadius Radius of the right wheel.
-      public: void SetWheelParams(double _wheelSeparation, double _wheelBase,
-                      double _leftWheelRadius, double _rightWheelRadius);
-      /// \brief Set the velocity rolling window size.
-      /// \param[in] _size The Velocity rolling window size.
-      public: void SetVelocityRollingWindowSize(size_t _size);
-
-      /// \brief Get the wheel separation
-      /// \return Distance between left and right wheels in meters.
-      public: double WheelSeparation() const;
-
-      /// \brief Get the wheel base
-      /// \return Distance between front and back wheels in meters.
-      public: double WheelBase() const;
-
-      /// \brief Get the left wheel radius
-      /// \return Left wheel radius in meters.
-      public: double LeftWheelRadius() const;
-
-      /// \brief Get the rightwheel radius
-      /// \return Right wheel radius in meters.
-      public: double RightWheelRadius() const;
-
-      /// \brief Private data pointer.
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }  // namespace GZ_MATH_VERSION_NAMESPACE
-  }  // namespace math
-}  // namespace gz
+    // Use a steady clock
+    public: using clock = std::chrono::steady_clock;
+
+    /// \brief Constructor.
+    /// \param[in] _windowSize Rolling window size used to compute the
+    /// velocity mean
+    public: explicit MecanumDriveOdometry(size_t _windowSize = 10);
+
+    /// \brief Initialize the odometry
+    /// \param[in] _time Current time.
+    public: void Init(const clock::time_point &_time);
+
+    /// \brief Get whether Init has been called.
+    /// \return True if Init has been called, false otherwise.
+    public: bool Initialized() const;
+
+    /// \brief Updates the odometry class with latest wheels and
+    /// steerings position
+    /// \param[in] _frontLeftPos Left wheel position in radians.
+    /// \param[in] _frontRightPos Right wheel postion in radians.
+    /// \param[in] _backLeftPos Left wheel position in radians.
+    /// \param[in] _backRightPos Right wheel postion in radians.
+    /// \param[in] _time Current time point.
+    /// \return True if the odometry is actually updated.
+    public: bool Update(
+      const Angle &_frontLeftPos, const Angle &_frontRightPos,
+      const Angle &_backLeftPos, const Angle &_backRightPos,
+      const clock::time_point &_time);
+
+    /// \brief Get the heading.
+    /// \return The heading in radians.
+    public: const Angle &Heading() const;
+
+    /// \brief Get the X position.
+    ///  \return The X position in meters
+    public: double X() const;
+
+    /// \brief Get the Y position.
+    /// \return The Y position in meters.
+    public: double Y() const;
+
+    /// \brief Get the linear velocity.
+    /// \return The linear velocity in meter/second.
+    public: double LinearVelocity() const;
+
+    /// \brief Get the lateral velocity.
+    /// \return The lateral velocity in meter/second.
+    public: double LateralVelocity() const;
+
+    /// \brief Get the angular velocity.
+    /// \return The angular velocity in radian/second.
+    public: const Angle &AngularVelocity() const;
+
+    /// \brief Set the wheel parameters including the radius and separation.
+    /// \param[in] _wheelSeparation Distance between left and right wheels.
+    /// \param[in] _wheelBase Distance between front and back wheels.
+    /// \param[in] _leftWheelRadius Radius of the left wheel.
+    /// \param[in] _rightWheelRadius Radius of the right wheel.
+    public: void SetWheelParams(double _wheelSeparation, double _wheelBase,
+                    double _leftWheelRadius, double _rightWheelRadius);
+    /// \brief Set the velocity rolling window size.
+    /// \param[in] _size The Velocity rolling window size.
+    public: void SetVelocityRollingWindowSize(size_t _size);
+
+    /// \brief Get the wheel separation
+    /// \return Distance between left and right wheels in meters.
+    public: double WheelSeparation() const;
+
+    /// \brief Get the wheel base
+    /// \return Distance between front and back wheels in meters.
+    public: double WheelBase() const;
+
+    /// \brief Get the left wheel radius
+    /// \return Left wheel radius in meters.
+    public: double LeftWheelRadius() const;
+
+    /// \brief Get the rightwheel radius
+    /// \return Right wheel radius in meters.
+    public: double RightWheelRadius() const;
+
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
 #endif  // GZ_MATH_MECANUMDRIVEODOMETRY_HH_
diff --git a/include/gz/math/MovingWindowFilter.hh b/include/gz/math/MovingWindowFilter.hh
index 2a0b331d1..34bbf56ab 100644
--- a/include/gz/math/MovingWindowFilter.hh
+++ b/include/gz/math/MovingWindowFilter.hh
@@ -26,70 +26,68 @@
 
 #include <gz/utils/SuppressWarning.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \brief Base class for MovingWindowFilter. This replaces the
+  /// version of MovingWindowFilter in the Gazebo Common library.
+  ///
+  /// The default window size is 4.
+  template< typename T>
+  class MovingWindowFilter
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \brief Base class for MovingWindowFilter. This replaces the
-    /// version of MovingWindowFilter in the Gazebo Common library.
-    ///
-    /// The default window size is 4.
-    template< typename T>
-    class MovingWindowFilter
-    {
-      /// \brief Constructor
-      public: MovingWindowFilter(unsigned int _windowSize = 4);
+    /// \brief Constructor
+    public: MovingWindowFilter(unsigned int _windowSize = 4);
 
-      /// \brief Destructor
-      public: virtual ~MovingWindowFilter() = default;
+    /// \brief Destructor
+    public: virtual ~MovingWindowFilter() = default;
 
-      /// \brief Update value of filter
-      /// \param[in] _val new raw value
-      public: void Update(const T _val);
+    /// \brief Update value of filter
+    /// \param[in] _val new raw value
+    public: void Update(const T _val);
 
-      /// \brief Set window size
-      /// \param[in] _n new desired window size
-      public: void SetWindowSize(const unsigned int _n);
+    /// \brief Set window size
+    /// \param[in] _n new desired window size
+    public: void SetWindowSize(const unsigned int _n);
 
-      /// \brief Get the window size.
-      /// \return The size of the moving window.
-      public: unsigned int WindowSize() const;
+    /// \brief Get the window size.
+    /// \return The size of the moving window.
+    public: unsigned int WindowSize() const;
 
-      /// \brief Get whether the window has been filled.
-      /// \return True if the window has been filled.
-      public: bool WindowFilled() const;
+    /// \brief Get whether the window has been filled.
+    /// \return True if the window has been filled.
+    public: bool WindowFilled() const;
 
-      /// \brief Get filtered result
-      /// \return Latest filtered value
-      public: T Value() const;
+    /// \brief Get filtered result
+    /// \return Latest filtered value
+    public: T Value() const;
 
-      /// \brief For moving window smoothed value
-      public: unsigned int valWindowSize = 4;
+    /// \brief For moving window smoothed value
+    public: unsigned int valWindowSize = 4;
 
-      /// \brief keep track of number of elements
-      public: unsigned int samples = 0;
+    /// \brief keep track of number of elements
+    public: unsigned int samples = 0;
 
-      GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
-      /// \brief buffer history of raw values
-      public: std::vector<T> valHistory;
+    GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+    /// \brief buffer history of raw values
+    public: std::vector<T> valHistory;
 
-      /// \brief iterator pointing to current value in buffer
-      public: typename std::vector<T>::iterator valIter;
+    /// \brief iterator pointing to current value in buffer
+    public: typename std::vector<T>::iterator valIter;
 
-      /// \brief keep track of running sum
-      public: T sum;
-      GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
-    };
+    /// \brief keep track of running sum
+    public: T sum;
+    GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+  };
 
-    using MovingWindowFilteri = MovingWindowFilter<int>;
-    using MovingWindowFilterf = MovingWindowFilter<float>;
-    using MovingWindowFilterd = MovingWindowFilter<double>;
-    using MovingWindowFilterVector3i = MovingWindowFilter<Vector3i>;
-    using MovingWindowFilterVector3f = MovingWindowFilter<Vector3f>;
-    using MovingWindowFilterVector3d = MovingWindowFilter<Vector3d>;
-  }
+  using MovingWindowFilteri = MovingWindowFilter<int>;
+  using MovingWindowFilterf = MovingWindowFilter<float>;
+  using MovingWindowFilterd = MovingWindowFilter<double>;
+  using MovingWindowFilterVector3i = MovingWindowFilter<Vector3i>;
+  using MovingWindowFilterVector3f = MovingWindowFilter<Vector3f>;
+  using MovingWindowFilterVector3d = MovingWindowFilter<Vector3d>;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
 
   //////////////////////////////////////////////////
   template<typename T>
@@ -203,6 +201,5 @@ namespace gz
       return T(value);
     }
   }
-  }  // namespace math
-}  // namespace gz
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_MOVINGWINDOWFILTER_HH_
diff --git a/include/gz/math/OrientedBox.hh b/include/gz/math/OrientedBox.hh
index 367643943..cc0ff4583 100644
--- a/include/gz/math/OrientedBox.hh
+++ b/include/gz/math/OrientedBox.hh
@@ -26,249 +26,246 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \brief Mathematical representation of a box which can be arbitrarily
+  /// positioned and rotated.
+  template<typename T>
+  class OrientedBox
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \brief Mathematical representation of a box which can be arbitrarily
-    /// positioned and rotated.
-    template<typename T>
-    class OrientedBox
+    /// \brief Default constructor
+    public: OrientedBox() : size(Vector3<T>::Zero), pose(Pose3<T>::Zero)
     {
-      /// \brief Default constructor
-      public: OrientedBox() : size(Vector3<T>::Zero), pose(Pose3<T>::Zero)
-      {
-      }
-
-      /// \brief Constructor which takes size and pose.
-      /// \param[in] _size Box size, in its own coordinate frame. Its absolute
-      /// value will be taken, so the size is non-negative.
-      /// \param[in] _pose Box pose.
-      public: OrientedBox(const Vector3<T> &_size, const Pose3<T> &_pose)
-          : size(_size.Abs()), pose(_pose)
-      {
-      }
-
-      /// \brief Constructor which takes size, pose, and material.
-      /// \param[in] _size Box size, in its own coordinate frame. Its absolute
-      /// value will be taken, so the size is non-negative.
-      /// \param[in] _pose Box pose.
-      /// \param[in] _mat Material property for the box.
-      public: OrientedBox(const Vector3<T> &_size, const Pose3<T> &_pose,
-                  const Material &_mat)
-          : size(_size.Abs()), pose(_pose), material(_mat)
-      {
-      }
-
-      /// \brief Constructor which takes only the size.
-      /// \param[in] _size Box size, in its own coordinate frame. Its absolute
-      /// value will be taken, so the size is non-negative.
-      public: explicit OrientedBox(const Vector3<T> &_size)
-          : size(_size.Abs()), pose(Pose3<T>::Zero)
-      {
-      }
-
-      /// \brief Constructor which takes only the size.
-      /// \param[in] _size Box size, in its own coordinate frame. Its absolute
-      /// value will be taken, so the size is non-negative.
-      /// \param[in] _mat Material property for the box.
-      public: explicit OrientedBox(const Vector3<T> &_size,
-                                   const Material &_mat)
-          : size(_size.Abs()), pose(Pose3<T>::Zero), material(_mat)
-      {
-      }
-
-      /// \brief Copy constructor.
-      /// \param[in] _b OrientedBox to copy.
-      public: OrientedBox(const OrientedBox<T> &_b) = default;
-
-      /// \brief Destructor
-      public: ~OrientedBox() = default;
-
-      /// \brief Get the length along the x dimension
-      /// \return Value of the length in the x dimension
-      public: T XLength() const
-      {
-        return this->size.X();
-      }
-
-      /// \brief Get the length along the y dimension
-      /// \return Value of the length in the y dimension
-      public: T YLength() const
-      {
-        return this->size.Y();
-      }
-
-      /// \brief Get the length along the z dimension
-      /// \return Value of the length in the z dimension
-      public: T ZLength() const
-      {
-        return this->size.Z();
-      }
-
-      /// \brief Get the size of the box
-      /// \return Size of the box
-      public: const Vector3<T> &Size() const
-      {
-        return this->size;
-      }
-
-      /// \brief Get the box pose, which is the pose of its center.
-      /// \return The pose of the box.
-      public: const Pose3<T> &Pose() const
-      {
-        return this->pose;
-      }
-
-      /// \brief Set the box size.
-      /// \param[in] _size Box size, in its own coordinate frame. Its absolute
-      /// value will be taken, so the size is non-negative.
-      public: void Size(const Vector3<T> &_size)
-      {
-        // Enforce non-negative size
-        this->size = _size.Abs();
-      }
-
-      /// \brief Set the box pose.
-      /// \param[in] _pose Box pose.
-      public: void Pose(const Pose3<T> &_pose)
-      {
-        this->pose = _pose;
-      }
-
-      /// \brief Assignment operator. Set this box to the parameter
-      /// \param[in]  _b OrientedBox to copy
-      /// \return The new box.
-      public: OrientedBox &operator=(const OrientedBox<T> &_b) = default;
-
-      /// \brief Equality test operator
-      /// \param[in] _b OrientedBox to test
-      /// \return True if equal
-      public: bool operator==(const OrientedBox<T> &_b) const
-      {
-        return this->size == _b.size && this->pose == _b.pose &&
-               this->material == _b.material;
-      }
-
-      /// \brief Inequality test operator
-      /// \param[in] _b OrientedBox to test
-      /// \return True if not equal
-      public: bool operator!=(const OrientedBox<T> &_b) const
-      {
-        return this->size != _b.size || this->pose != _b.pose ||
-               this->material != _b.material;
-      }
-
-      /// \brief Output operator
-      /// \param[in] _out Output stream
-      /// \param[in] _b OrientedBox to output to the stream
-      /// \return The stream
-      public: friend std::ostream &operator<<(std::ostream &_out,
-                                              const OrientedBox<T> &_b)
-      {
-        _out << "Size[" << _b.Size() << "] Pose[" << _b.Pose() << "] "
-          << "Material[" << _b.Material().Name() << "]";
-        return _out;
-      }
-
-      /// \brief Check if a point lies inside the box.
-      /// \param[in] _p Point to check.
-      /// \return True if the point is inside the box.
-      public: bool Contains(const Vector3d &_p) const
-      {
-        // Move point to box frame
-        auto t = Matrix4<T>(this->pose).Inverse();
-        auto p = t *_p;
-
-        return p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 &&
-               p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 &&
-               p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5;
-      }
-
-      /// \brief Get the material associated with this box.
-      /// \return The material assigned to this box.
-      public: const gz::math::Material &Material() const
-      {
-        return this->material;
-      }
-
-      /// \brief Set the material associated with this box.
-      /// \param[in] _mat The material assigned to this box.
-      public: void SetMaterial(const gz::math::Material &_mat)
-      {
-        this->material = _mat;
-      }
-
-      /// \brief Get the volume of the box in m^3.
-      /// \return Volume of the box in m^3.
-      public: T Volume() const
-      {
-        return this->size.X() * this->size.Y() * this->size.Z();
-      }
-
-      /// \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
-      /// values greater than zero. The Material of the box is ignored.
-      /// \param[in] _mass Mass of the box, in kg. This value should be
-      /// greater than zero.
-      /// \return Density of the box in kg/m^3. A negative value is
-      /// returned if the size or _mass is <= 0.
-      public: T DensityFromMass(const T _mass) const
-      {
-        if (this->size.Min() <= 0|| _mass <= 0)
-          return -1.0;
-
-        return _mass / this->Volume();
-      }
-
-      /// \brief Set the density of this box based on a mass value.
-      /// Density is computed using
-      /// double DensityFromMass(const double _mass) const. The
-      /// box is assumed to be solid with uniform density. This
-      /// function requires the box's size to be set to
-      /// values greater than zero. The existing Material density value is
-      /// overwritten only if the return value from this true.
-      /// \param[in] _mass Mass of the box, in kg. This value should be
-      /// greater than zero.
-      /// \return True if the density was set. False is returned if the
-      /// box's size or the _mass value are <= 0.
-      /// \sa double DensityFromMass(const double _mass) const
-      public: bool SetDensityFromMass(const T _mass)
-      {
-        T newDensity = this->DensityFromMass(_mass);
-        if (newDensity > 0)
-          this->material.SetDensity(newDensity);
-        return newDensity > 0;
-      }
-
-      /// \brief Get the mass matrix for this box. This function
-      /// is only meaningful if the box's size and material
-      /// have been set.
-      /// \param[out] _massMat The computed mass matrix will be stored here.
-      /// \return False if computation of the mass matrix failed, which
-      /// could be due to an invalid size (<=0) or density (<=0).
-      public: bool MassMatrix(MassMatrix3<T> &_massMat) const
-      {
-        return _massMat.SetFromBox(this->material, this->size);
-      }
-
-      /// \brief The size of the box in its local frame.
-      private: Vector3<T> size;
-
-      /// \brief The pose of the center of the box.
-      private: Pose3<T> pose;
-
-      /// \brief The box's material.
-      private: gz::math::Material material;
-    };
-
-    typedef OrientedBox<double> OrientedBoxd;
-    typedef OrientedBox<float> OrientedBoxf;
     }
-  }
-}
-#endif
+
+    /// \brief Constructor which takes size and pose.
+    /// \param[in] _size Box size, in its own coordinate frame. Its absolute
+    /// value will be taken, so the size is non-negative.
+    /// \param[in] _pose Box pose.
+    public: OrientedBox(const Vector3<T> &_size, const Pose3<T> &_pose)
+        : size(_size.Abs()), pose(_pose)
+    {
+    }
+
+    /// \brief Constructor which takes size, pose, and material.
+    /// \param[in] _size Box size, in its own coordinate frame. Its absolute
+    /// value will be taken, so the size is non-negative.
+    /// \param[in] _pose Box pose.
+    /// \param[in] _mat Material property for the box.
+    public: OrientedBox(const Vector3<T> &_size, const Pose3<T> &_pose,
+                const Material &_mat)
+        : size(_size.Abs()), pose(_pose), material(_mat)
+    {
+    }
+
+    /// \brief Constructor which takes only the size.
+    /// \param[in] _size Box size, in its own coordinate frame. Its absolute
+    /// value will be taken, so the size is non-negative.
+    public: explicit OrientedBox(const Vector3<T> &_size)
+        : size(_size.Abs()), pose(Pose3<T>::Zero)
+    {
+    }
+
+    /// \brief Constructor which takes only the size.
+    /// \param[in] _size Box size, in its own coordinate frame. Its absolute
+    /// value will be taken, so the size is non-negative.
+    /// \param[in] _mat Material property for the box.
+    public: explicit OrientedBox(const Vector3<T> &_size,
+                                 const Material &_mat)
+        : size(_size.Abs()), pose(Pose3<T>::Zero), material(_mat)
+    {
+    }
+
+    /// \brief Copy constructor.
+    /// \param[in] _b OrientedBox to copy.
+    public: OrientedBox(const OrientedBox<T> &_b) = default;
+
+    /// \brief Destructor
+    public: ~OrientedBox() = default;
+
+    /// \brief Get the length along the x dimension
+    /// \return Value of the length in the x dimension
+    public: T XLength() const
+    {
+      return this->size.X();
+    }
+
+    /// \brief Get the length along the y dimension
+    /// \return Value of the length in the y dimension
+    public: T YLength() const
+    {
+      return this->size.Y();
+    }
+
+    /// \brief Get the length along the z dimension
+    /// \return Value of the length in the z dimension
+    public: T ZLength() const
+    {
+      return this->size.Z();
+    }
+
+    /// \brief Get the size of the box
+    /// \return Size of the box
+    public: const Vector3<T> &Size() const
+    {
+      return this->size;
+    }
+
+    /// \brief Get the box pose, which is the pose of its center.
+    /// \return The pose of the box.
+    public: const Pose3<T> &Pose() const
+    {
+      return this->pose;
+    }
+
+    /// \brief Set the box size.
+    /// \param[in] _size Box size, in its own coordinate frame. Its absolute
+    /// value will be taken, so the size is non-negative.
+    public: void Size(const Vector3<T> &_size)
+    {
+      // Enforce non-negative size
+      this->size = _size.Abs();
+    }
+
+    /// \brief Set the box pose.
+    /// \param[in] _pose Box pose.
+    public: void Pose(const Pose3<T> &_pose)
+    {
+      this->pose = _pose;
+    }
+
+    /// \brief Assignment operator. Set this box to the parameter
+    /// \param[in]  _b OrientedBox to copy
+    /// \return The new box.
+    public: OrientedBox &operator=(const OrientedBox<T> &_b) = default;
+
+    /// \brief Equality test operator
+    /// \param[in] _b OrientedBox to test
+    /// \return True if equal
+    public: bool operator==(const OrientedBox<T> &_b) const
+    {
+      return this->size == _b.size && this->pose == _b.pose &&
+             this->material == _b.material;
+    }
+
+    /// \brief Inequality test operator
+    /// \param[in] _b OrientedBox to test
+    /// \return True if not equal
+    public: bool operator!=(const OrientedBox<T> &_b) const
+    {
+      return this->size != _b.size || this->pose != _b.pose ||
+             this->material != _b.material;
+    }
+
+    /// \brief Output operator
+    /// \param[in] _out Output stream
+    /// \param[in] _b OrientedBox to output to the stream
+    /// \return The stream
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                                            const OrientedBox<T> &_b)
+    {
+      _out << "Size[" << _b.Size() << "] Pose[" << _b.Pose() << "] "
+        << "Material[" << _b.Material().Name() << "]";
+      return _out;
+    }
+
+    /// \brief Check if a point lies inside the box.
+    /// \param[in] _p Point to check.
+    /// \return True if the point is inside the box.
+    public: bool Contains(const Vector3d &_p) const
+    {
+      // Move point to box frame
+      auto t = Matrix4<T>(this->pose).Inverse();
+      auto p = t *_p;
+
+      return p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 &&
+             p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 &&
+             p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5;
+    }
+
+    /// \brief Get the material associated with this box.
+    /// \return The material assigned to this box.
+    public: const gz::math::Material &Material() const
+    {
+      return this->material;
+    }
+
+    /// \brief Set the material associated with this box.
+    /// \param[in] _mat The material assigned to this box.
+    public: void SetMaterial(const gz::math::Material &_mat)
+    {
+      this->material = _mat;
+    }
+
+    /// \brief Get the volume of the box in m^3.
+    /// \return Volume of the box in m^3.
+    public: T Volume() const
+    {
+      return this->size.X() * this->size.Y() * this->size.Z();
+    }
+
+    /// \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
+    /// values greater than zero. The Material of the box is ignored.
+    /// \param[in] _mass Mass of the box, in kg. This value should be
+    /// greater than zero.
+    /// \return Density of the box in kg/m^3. A negative value is
+    /// returned if the size or _mass is <= 0.
+    public: T DensityFromMass(const T _mass) const
+    {
+      if (this->size.Min() <= 0|| _mass <= 0)
+        return -1.0;
+
+      return _mass / this->Volume();
+    }
+
+    /// \brief Set the density of this box based on a mass value.
+    /// Density is computed using
+    /// double DensityFromMass(const double _mass) const. The
+    /// box is assumed to be solid with uniform density. This
+    /// function requires the box's size to be set to
+    /// values greater than zero. The existing Material density value is
+    /// overwritten only if the return value from this true.
+    /// \param[in] _mass Mass of the box, in kg. This value should be
+    /// greater than zero.
+    /// \return True if the density was set. False is returned if the
+    /// box's size or the _mass value are <= 0.
+    /// \sa double DensityFromMass(const double _mass) const
+    public: bool SetDensityFromMass(const T _mass)
+    {
+      T newDensity = this->DensityFromMass(_mass);
+      if (newDensity > 0)
+        this->material.SetDensity(newDensity);
+      return newDensity > 0;
+    }
+
+    /// \brief Get the mass matrix for this box. This function
+    /// is only meaningful if the box's size and material
+    /// have been set.
+    /// \param[out] _massMat The computed mass matrix will be stored here.
+    /// \return False if computation of the mass matrix failed, which
+    /// could be due to an invalid size (<=0) or density (<=0).
+    public: bool MassMatrix(MassMatrix3<T> &_massMat) const
+    {
+      return _massMat.SetFromBox(this->material, this->size);
+    }
+
+    /// \brief The size of the box in its local frame.
+    private: Vector3<T> size;
+
+    /// \brief The pose of the center of the box.
+    private: Pose3<T> pose;
+
+    /// \brief The box's material.
+    private: gz::math::Material material;
+  };
+
+  typedef OrientedBox<double> OrientedBoxd;
+  typedef OrientedBox<float> OrientedBoxf;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_ORIENTEDBOX_HH_
diff --git a/include/gz/math/PID.hh b/include/gz/math/PID.hh
index ce52c5559..34e5ab172 100644
--- a/include/gz/math/PID.hh
+++ b/include/gz/math/PID.hh
@@ -22,185 +22,182 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class PID PID.hh gz/math/PID.hh
+  /// \brief Generic PID controller class.
+  /// Generic proportional-integral-derivative controller class that
+  /// keeps track of PID-error states and control inputs given
+  /// the state of a system and a user specified target state.
+  /// It includes a user-adjustable command offset term (feed-forward).
+  // cppcheck-suppress class_X_Y
+  class GZ_MATH_VISIBLE PID
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class PID PID.hh gz/math/PID.hh
-    /// \brief Generic PID controller class.
-    /// Generic proportional-integral-derivative controller class that
-    /// keeps track of PID-error states and control inputs given
-    /// the state of a system and a user specified target state.
-    /// It includes a user-adjustable command offset term (feed-forward).
-    // cppcheck-suppress class_X_Y
-    class GZ_MATH_VISIBLE PID
-    {
-      /// \brief Constructor, zeros out Pid values when created and
-      /// initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
-      ///
-      /// Disable command clamping by setting _cmdMin to a value larger
-      /// than _cmdMax. Command clamping is disabled by default.
-      ///
-      /// Disable integral clamping by setting _iMin to a value larger
-      /// than _iMax. Integral clamping is disabled by default.
-      ///
-      /// \param[in] _p  The proportional gain.
-      /// \param[in] _i  The integral gain.
-      /// \param[in] _d  The derivative gain.
-      /// \param[in] _imax The integral upper limit.
-      /// \param[in] _imin The integral lower limit.
-      /// \param[in] _cmdMax Output max value.
-      /// \param[in] _cmdMin Output min value.
-      /// \param[in] _cmdOffset Command offset (feed-forward).
-      public: PID(const double _p = 0.0,
-                  const double _i = 0.0,
-                  const double _d = 0.0,
-                  const double _imax = -1.0,
-                  const double _imin = 0.0,
-                  const double _cmdMax = -1.0,
-                  const double _cmdMin = 0.0,
-                  const double _cmdOffset = 0.0);
-
-      /// \brief Initialize PID-gains and integral term
-      ///        limits:[iMax:iMin]-[I1:I2].
-      ///
-      /// Disable command clamping by setting _cmdMin to a value larger
-      /// than _cmdMax. Command clamping is disabled by default.
-      ///
-      /// Disable integral clamping by setting _iMin to a value larger
-      /// than _iMax. Integral clamping is disabled by default.
-      ///
-      /// \param[in] _p  The proportional gain.
-      /// \param[in] _i  The integral gain.
-      /// \param[in] _d  The derivative gain.
-      /// \param[in] _imax The integral upper limit.
-      /// \param[in] _imin The integral lower limit.
-      /// \param[in] _cmdMax Output max value.
-      /// \param[in] _cmdMin Output min value.
-      /// \param[in] _cmdOffset Command offset (feed-forward).
-      public: void Init(const double _p = 0.0,
-                        const double _i = 0.0,
-                        const double _d = 0.0,
-                        const double _imax = -1.0,
-                        const double _imin = 0.0,
-                        const double _cmdMax = -1.0,
-                        const double _cmdMin = 0.0,
-                        const double _cmdOffset = 0.0);
-
-      /// \brief Set the proportional Gain.
-      /// \param[in] _p proportional gain value
-      public: void SetPGain(const double _p);
-
-      /// \brief Set the integral Gain.
-      /// \param[in] _i integral gain value
-      public: void SetIGain(const double _i);
-
-      /// \brief Set the derivative Gain.
-      /// \param[in] _d derivative gain value
-      public: void SetDGain(const double _d);
-
-      /// \brief Set the integral upper limit.
-      /// \param[in] _i integral upper limit value
-      public: void SetIMax(const double _i);
-
-      /// \brief Set the integral lower limit.
-      /// \param[in] _i integral lower limit value
-      public: void SetIMin(const double _i);
-
-      /// \brief Set the maximum value for the command.
-      /// \param[in] _c The maximum value
-      public: void SetCmdMax(const double _c);
-
-      /// \brief Set the minimum value for the command.
-      /// \param[in] _c The minimum value
-      public: void SetCmdMin(const double _c);
-
-      /// \brief Set the offset value for the command,
-      /// which is added to the result of the PID controller.
-      /// \param[in] _c The offset value
-      public: void SetCmdOffset(const double _c);
-
-      /// \brief Get the proportional Gain.
-      /// \return The proportional gain value
-      public: double PGain() const;
-
-      /// \brief Get the integral Gain.
-      /// \return The integral gain value
-      public: double IGain() const;
-
-      /// \brief Get the derivative Gain.
-      /// \return The derivative gain value
-      public: double DGain() const;
-
-      /// \brief Get the integral upper limit.
-      /// \return The integral upper limit value
-      public: double IMax() const;
-
-      /// \brief Get the integral lower limit.
-      /// \return The integral lower limit value
-      public: double IMin() const;
-
-      /// \brief Get the maximum value for the command.
-      /// \return The maximum value
-      public: double CmdMax() const;
-
-      /// \brief Get the minimun value for the command.
-      /// \return The maximum value
-      public: double CmdMin() const;
-
-      /// \brief Get the offset value for the command.
-      /// \return The offset value
-      public: double CmdOffset() const;
-
-      /// \brief Update the Pid loop with nonuniform time step size.
-      /// \param[in] _error  Error since last call (p_state - p_target).
-      /// \param[in] _errorRate Estimate of error rate, that can be used
-      /// when a smoother estimate is available than the finite difference
-      /// used by Update(const double _error,
-      /// const std::chrono::duration<double> &_dt)
-      /// \param[in] _dt Change in time since last update call.
-      /// Normally, this is called at every time step,
-      /// The return value is an updated command to be passed
-      /// to the object being controlled.
-      /// \return the command value
-      public: double Update(const double _error,
-                            double _errorRate,
-                            const std::chrono::duration<double> &_dt);
-
-      /// \brief Update the Pid loop with nonuniform time step size.
-      /// \param[in] _error  Error since last call (p_state - p_target).
-      /// \param[in] _dt Change in time since last update call.
-      /// Normally, this is called at every time step,
-      /// The return value is an updated command to be passed
-      /// to the object being controlled.
-      /// \return the command value
-      public: double Update(const double _error,
-                            const std::chrono::duration<double> &_dt);
-
-      /// \brief Set current target command for this PID controller.
-      /// \param[in] _cmd New command
-      public: void SetCmd(const double _cmd);
-
-      /// \brief Return current command for this PID controller.
-      /// \return the command value
-      public: double Cmd() const;
-
-      /// \brief Return PID error terms for the controller.
-      /// \param[in] _pe  The proportional error.
-      /// \param[in] _ie  The integral of gain times error.
-      /// \param[in] _de  The derivative error.
-      public: void Errors(double &_pe, double &_ie, double &_de) const;
-
-      /// \brief Reset the errors and command.
-      public: void Reset();
-
-      /// \brief Pointer to private data.
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
-#endif
+    /// \brief Constructor, zeros out Pid values when created and
+    /// initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
+    ///
+    /// Disable command clamping by setting _cmdMin to a value larger
+    /// than _cmdMax. Command clamping is disabled by default.
+    ///
+    /// Disable integral clamping by setting _iMin to a value larger
+    /// than _iMax. Integral clamping is disabled by default.
+    ///
+    /// \param[in] _p  The proportional gain.
+    /// \param[in] _i  The integral gain.
+    /// \param[in] _d  The derivative gain.
+    /// \param[in] _imax The integral upper limit.
+    /// \param[in] _imin The integral lower limit.
+    /// \param[in] _cmdMax Output max value.
+    /// \param[in] _cmdMin Output min value.
+    /// \param[in] _cmdOffset Command offset (feed-forward).
+    public: PID(const double _p = 0.0,
+                const double _i = 0.0,
+                const double _d = 0.0,
+                const double _imax = -1.0,
+                const double _imin = 0.0,
+                const double _cmdMax = -1.0,
+                const double _cmdMin = 0.0,
+                const double _cmdOffset = 0.0);
+
+    /// \brief Initialize PID-gains and integral term
+    ///        limits:[iMax:iMin]-[I1:I2].
+    ///
+    /// Disable command clamping by setting _cmdMin to a value larger
+    /// than _cmdMax. Command clamping is disabled by default.
+    ///
+    /// Disable integral clamping by setting _iMin to a value larger
+    /// than _iMax. Integral clamping is disabled by default.
+    ///
+    /// \param[in] _p  The proportional gain.
+    /// \param[in] _i  The integral gain.
+    /// \param[in] _d  The derivative gain.
+    /// \param[in] _imax The integral upper limit.
+    /// \param[in] _imin The integral lower limit.
+    /// \param[in] _cmdMax Output max value.
+    /// \param[in] _cmdMin Output min value.
+    /// \param[in] _cmdOffset Command offset (feed-forward).
+    public: void Init(const double _p = 0.0,
+                      const double _i = 0.0,
+                      const double _d = 0.0,
+                      const double _imax = -1.0,
+                      const double _imin = 0.0,
+                      const double _cmdMax = -1.0,
+                      const double _cmdMin = 0.0,
+                      const double _cmdOffset = 0.0);
+
+    /// \brief Set the proportional Gain.
+    /// \param[in] _p proportional gain value
+    public: void SetPGain(const double _p);
+
+    /// \brief Set the integral Gain.
+    /// \param[in] _i integral gain value
+    public: void SetIGain(const double _i);
+
+    /// \brief Set the derivative Gain.
+    /// \param[in] _d derivative gain value
+    public: void SetDGain(const double _d);
+
+    /// \brief Set the integral upper limit.
+    /// \param[in] _i integral upper limit value
+    public: void SetIMax(const double _i);
+
+    /// \brief Set the integral lower limit.
+    /// \param[in] _i integral lower limit value
+    public: void SetIMin(const double _i);
+
+    /// \brief Set the maximum value for the command.
+    /// \param[in] _c The maximum value
+    public: void SetCmdMax(const double _c);
+
+    /// \brief Set the minimum value for the command.
+    /// \param[in] _c The minimum value
+    public: void SetCmdMin(const double _c);
+
+    /// \brief Set the offset value for the command,
+    /// which is added to the result of the PID controller.
+    /// \param[in] _c The offset value
+    public: void SetCmdOffset(const double _c);
+
+    /// \brief Get the proportional Gain.
+    /// \return The proportional gain value
+    public: double PGain() const;
+
+    /// \brief Get the integral Gain.
+    /// \return The integral gain value
+    public: double IGain() const;
+
+    /// \brief Get the derivative Gain.
+    /// \return The derivative gain value
+    public: double DGain() const;
+
+    /// \brief Get the integral upper limit.
+    /// \return The integral upper limit value
+    public: double IMax() const;
+
+    /// \brief Get the integral lower limit.
+    /// \return The integral lower limit value
+    public: double IMin() const;
+
+    /// \brief Get the maximum value for the command.
+    /// \return The maximum value
+    public: double CmdMax() const;
+
+    /// \brief Get the minimun value for the command.
+    /// \return The maximum value
+    public: double CmdMin() const;
+
+    /// \brief Get the offset value for the command.
+    /// \return The offset value
+    public: double CmdOffset() const;
+
+    /// \brief Update the Pid loop with nonuniform time step size.
+    /// \param[in] _error  Error since last call (p_state - p_target).
+    /// \param[in] _errorRate Estimate of error rate, that can be used
+    /// when a smoother estimate is available than the finite difference
+    /// used by Update(const double _error,
+    /// const std::chrono::duration<double> &_dt)
+    /// \param[in] _dt Change in time since last update call.
+    /// Normally, this is called at every time step,
+    /// The return value is an updated command to be passed
+    /// to the object being controlled.
+    /// \return the command value
+    public: double Update(const double _error,
+                          double _errorRate,
+                          const std::chrono::duration<double> &_dt);
+
+    /// \brief Update the Pid loop with nonuniform time step size.
+    /// \param[in] _error  Error since last call (p_state - p_target).
+    /// \param[in] _dt Change in time since last update call.
+    /// Normally, this is called at every time step,
+    /// The return value is an updated command to be passed
+    /// to the object being controlled.
+    /// \return the command value
+    public: double Update(const double _error,
+                          const std::chrono::duration<double> &_dt);
+
+    /// \brief Set current target command for this PID controller.
+    /// \param[in] _cmd New command
+    public: void SetCmd(const double _cmd);
+
+    /// \brief Return current command for this PID controller.
+    /// \return the command value
+    public: double Cmd() const;
+
+    /// \brief Return PID error terms for the controller.
+    /// \param[in] _pe  The proportional error.
+    /// \param[in] _ie  The integral of gain times error.
+    /// \param[in] _de  The derivative error.
+    public: void Errors(double &_pe, double &_ie, double &_de) const;
+
+    /// \brief Reset the errors and command.
+    public: void Reset();
+
+    /// \brief Pointer to private data.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACEH
+}  // namespace gz::math
+#endif  // GZ_MATH_PID_HH_
diff --git a/include/gz/math/PiecewiseScalarField3.hh b/include/gz/math/PiecewiseScalarField3.hh
index 993d87e3f..e5e4cac3f 100644
--- a/include/gz/math/PiecewiseScalarField3.hh
+++ b/include/gz/math/PiecewiseScalarField3.hh
@@ -27,193 +27,189 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /** \class PiecewiseScalarField3 PiecewiseScalarField3.hh\
+   * gz/math/PiecewiseScalarField3.hh
+   */
+  /// \brief The PiecewiseScalarField3 class constructs a scalar field F
+  /// in R^3 as a union of scalar fields Pn, defined over regions Rn i.e.
+  /// piecewise.
+  ///
+  /// \tparam ScalarField3T a callable type taking a single Vector3<ScalarT>
+  ///   value as argument and returning a ScalarT value. Additionally:
+  ///   - for PiecewiseScalarField3 to have a stream operator overload,
+  ///     ScalarField3T must support stream operator overload;
+  ///   - for PiecewiseScalarField3::Minimum to be callable, ScalarField3T
+  ///     must implement a
+  ///     ScalarT Minimum(const Region3<ScalarT> &, Vector3<ScalarT> &)
+  ///     method that computes its minimum in the given region and returns
+  ///     an argument value that yields said minimum.
+  /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits
+  ///   have been specialized.
+  ///
+  /// ## Example
+  ///
+  /// \snippet examples/piecewise_scalar_field3_example.cc complete
+  template<typename ScalarField3T, typename ScalarT>
+  class PiecewiseScalarField3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /** \class PiecewiseScalarField3 PiecewiseScalarField3.hh\
-     * gz/math/PiecewiseScalarField3.hh
-     */
-    /// \brief The PiecewiseScalarField3 class constructs a scalar field F
-    /// in R^3 as a union of scalar fields Pn, defined over regions Rn i.e.
-    /// piecewise.
-    ///
-    /// \tparam ScalarField3T a callable type taking a single Vector3<ScalarT>
-    ///   value as argument and returning a ScalarT value. Additionally:
-    ///   - for PiecewiseScalarField3 to have a stream operator overload,
-    ///     ScalarField3T must support stream operator overload;
-    ///   - for PiecewiseScalarField3::Minimum to be callable, ScalarField3T
-    ///     must implement a
-    ///     ScalarT Minimum(const Region3<ScalarT> &, Vector3<ScalarT> &)
-    ///     method that computes its minimum in the given region and returns
-    ///     an argument value that yields said minimum.
-    /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits
-    ///   have been specialized.
-    ///
-    /// ## Example
-    ///
-    /// \snippet examples/piecewise_scalar_field3_example.cc complete
-    template<typename ScalarField3T, typename ScalarT>
-    class PiecewiseScalarField3
-    {
-      /// \brief A scalar field P in R^3 and
-      /// the region R in which it is defined
-      public: struct Piece {
-        Region3<ScalarT> region;
-        ScalarField3T field;
-      };
+    /// \brief A scalar field P in R^3 and
+    /// the region R in which it is defined
+    public: struct Piece {
+      Region3<ScalarT> region;
+      ScalarField3T field;
+    };
 
-      /// \brief Constructor
-      public: PiecewiseScalarField3() = default;
+    /// \brief Constructor
+    public: PiecewiseScalarField3() = default;
 
-      /// \brief Constructor
-      /// \param[in] _pieces scalar fields Pn and the regions Rn
-      ///   in which these are defined. Regions should not overlap.
-      public: explicit PiecewiseScalarField3(const std::vector<Piece> &_pieces)
-      : pieces(_pieces)
+    /// \brief Constructor
+    /// \param[in] _pieces scalar fields Pn and the regions Rn
+    ///   in which these are defined. Regions should not overlap.
+    public: explicit PiecewiseScalarField3(const std::vector<Piece> &_pieces)
+    : pieces(_pieces)
+    {
+      for (size_t i = 0; i < pieces.size(); ++i)
       {
-        for (size_t i = 0; i < pieces.size(); ++i)
+        if (pieces[i].region.Empty())
+        {
+          std::cerr << "Region #" << i << " (" << pieces[i].region
+                    << ") in piecewise scalar field definition is empty."
+                    << std::endl;
+        }
+        for (size_t j = i + 1; j < pieces.size(); ++j)
         {
-          if (pieces[i].region.Empty())
+          if (pieces[i].region.Intersects(pieces[j].region))
           {
-            std::cerr << "Region #" << i << " (" << pieces[i].region
-                      << ") in piecewise scalar field definition is empty."
+            std::cerr << "Detected overlap between regions in "
+                      << "piecewise scalar field definition: "
+                      << "region #" << i << " (" << pieces[i].region
+                      << ") overlaps with region #" << j << " ("
+                      << pieces[j].region << "). Region #" << i
+                      << " will take precedence when overlapping."
                       << std::endl;
           }
-          for (size_t j = i + 1; j < pieces.size(); ++j)
-          {
-            if (pieces[i].region.Intersects(pieces[j].region))
-            {
-              std::cerr << "Detected overlap between regions in "
-                        << "piecewise scalar field definition: "
-                        << "region #" << i << " (" << pieces[i].region
-                        << ") overlaps with region #" << j << " ("
-                        << pieces[j].region << "). Region #" << i
-                        << " will take precedence when overlapping."
-                        << std::endl;
-            }
-          }
         }
       }
+    }
 
-      /// \brief Define piecewise scalar field as `_field` throughout R^3 space
-      /// \param[in] _field a scalar field in R^3
-      /// \return `_field` as a piecewise scalar field
-      public: static PiecewiseScalarField3 Throughout(ScalarField3T _field)
-      {
-        return PiecewiseScalarField3<ScalarField3T, ScalarT>({
-            {Region3<ScalarT>::Unbounded, std::move(_field)}});
-      }
+    /// \brief Define piecewise scalar field as `_field` throughout R^3 space
+    /// \param[in] _field a scalar field in R^3
+    /// \return `_field` as a piecewise scalar field
+    public: static PiecewiseScalarField3 Throughout(ScalarField3T _field)
+    {
+      return PiecewiseScalarField3<ScalarField3T, ScalarT>({
+          {Region3<ScalarT>::Unbounded, std::move(_field)}});
+    }
 
-      /// \brief Evaluate the piecewise scalar field at `_p`
-      /// \param[in] _p piecewise scalar field argument
-      /// \return the result of evaluating `F(_p)`, or NaN
-      ///   if the scalar field is not defined at `_p`
-      public: ScalarT Evaluate(const Vector3<ScalarT> &_p) const
+    /// \brief Evaluate the piecewise scalar field at `_p`
+    /// \param[in] _p piecewise scalar field argument
+    /// \return the result of evaluating `F(_p)`, or NaN
+    ///   if the scalar field is not defined at `_p`
+    public: ScalarT Evaluate(const Vector3<ScalarT> &_p) const
+    {
+      auto it = std::find_if(
+          this->pieces.begin(), this->pieces.end(),
+          [&](const Piece &piece)
+          {
+            return piece.region.Contains(_p);
+          });
+      if (it == this->pieces.end())
       {
-        auto it = std::find_if(
-            this->pieces.begin(), this->pieces.end(),
-            [&](const Piece &piece)
-            {
-              return piece.region.Contains(_p);
-            });
-        if (it == this->pieces.end())
-        {
-          return std::numeric_limits<ScalarT>::quiet_NaN();
-        }
-        return it->field(_p);
+        return std::numeric_limits<ScalarT>::quiet_NaN();
       }
+      return it->field(_p);
+    }
+
+    /// \brief Call operator overload
+    /// \see PiecewiseScalarField3::Evaluate()
+    /// \param[in] _p piecewise scalar field argument
+    /// \return the result of evaluating `F(_p)`, or NaN
+    ///   if the scalar field is not defined at `_p`
+    public: ScalarT operator()(const Vector3<ScalarT> &_p) const
+    {
+      return this->Evaluate(_p);
+    }
 
-      /// \brief Call operator overload
-      /// \see PiecewiseScalarField3::Evaluate()
-      /// \param[in] _p piecewise scalar field argument
-      /// \return the result of evaluating `F(_p)`, or NaN
-      ///   if the scalar field is not defined at `_p`
-      public: ScalarT operator()(const Vector3<ScalarT> &_p) const
+    /// \brief Compute the piecewise scalar field minimum
+    /// Note that, since this method computes the minimum
+    /// for each region independently, it implicitly assumes
+    /// continuity in the boundaries between regions, if any.
+    /// \param[out] _pMin scalar field argument that yields
+    ///   the minimum, or NaN if the scalar field is not
+    ///   defined anywhere (i.e. default constructed)
+    /// \return the scalar field minimum, or NaN if the
+    ///   scalar field is not defined anywhere (i.e. default
+    ///   constructed)
+    public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
+    {
+      if (this->pieces.empty())
       {
-        return this->Evaluate(_p);
+        _pMin = Vector3<ScalarT>::NaN;
+        return std::numeric_limits<ScalarT>::quiet_NaN();
       }
-
-      /// \brief Compute the piecewise scalar field minimum
-      /// Note that, since this method computes the minimum
-      /// for each region independently, it implicitly assumes
-      /// continuity in the boundaries between regions, if any.
-      /// \param[out] _pMin scalar field argument that yields
-      ///   the minimum, or NaN if the scalar field is not
-      ///   defined anywhere (i.e. default constructed)
-      /// \return the scalar field minimum, or NaN if the
-      ///   scalar field is not defined anywhere (i.e. default
-      ///   constructed)
-      public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
+      ScalarT yMin = std::numeric_limits<ScalarT>::infinity();
+      for (const Piece &piece : this->pieces)
       {
-        if (this->pieces.empty())
-        {
-          _pMin = Vector3<ScalarT>::NaN;
-          return std::numeric_limits<ScalarT>::quiet_NaN();
-        }
-        ScalarT yMin = std::numeric_limits<ScalarT>::infinity();
-        for (const Piece &piece : this->pieces)
+        if (!piece.region.Empty())
         {
-          if (!piece.region.Empty())
+          Vector3<ScalarT> p;
+          const ScalarT y = piece.field.Minimum(piece.region, p);
+          if (y < yMin)
           {
-            Vector3<ScalarT> p;
-            const ScalarT y = piece.field.Minimum(piece.region, p);
-            if (y < yMin)
-            {
-              _pMin = p;
-              yMin = y;
-            }
+            _pMin = p;
+            yMin = y;
           }
         }
-        return yMin;
       }
+      return yMin;
+    }
 
-      /// \brief Compute the piecewise scalar field minimum
-      /// \return the scalar field minimum, or NaN if the
-      ///   scalar field is not defined anywhere (i.e. default
-      ///   constructed)
-      public: ScalarT Minimum() const
+    /// \brief Compute the piecewise scalar field minimum
+    /// \return the scalar field minimum, or NaN if the
+    ///   scalar field is not defined anywhere (i.e. default
+    ///   constructed)
+    public: ScalarT Minimum() const
+    {
+      Vector3<ScalarT> pMin;
+      return this->Minimum(pMin);
+    }
+
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _field SeparableScalarField3 to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+        std::ostream &_out,
+        const gz::math::PiecewiseScalarField3<
+        ScalarField3T, ScalarT> &_field)
+    {
+      if (_field.pieces.empty())
       {
-        Vector3<ScalarT> pMin;
-        return this->Minimum(pMin);
+        return _out << "undefined";
       }
-
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _field SeparableScalarField3 to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-          std::ostream &_out,
-          const gz::math::PiecewiseScalarField3<
-          ScalarField3T, ScalarT> &_field)
+      for (size_t i = 0; i < _field.pieces.size() - 1; ++i)
       {
-        if (_field.pieces.empty())
-        {
-          return _out << "undefined";
-        }
-        for (size_t i = 0; i < _field.pieces.size() - 1; ++i)
-        {
-          _out << _field.pieces[i].field << " if (x, y, z) in "
-               << _field.pieces[i].region << "; ";
-        }
-        return _out << _field.pieces.back().field
-                    << " if (x, y, z) in "
-                    << _field.pieces.back().region;
+        _out << _field.pieces[i].field << " if (x, y, z) in "
+             << _field.pieces[i].region << "; ";
       }
-
-      /// \brief Scalar fields Pn and the regions Rn in which these are defined
-      private: std::vector<Piece> pieces;
-    };
-
-    template<typename ScalarField3T>
-    using PiecewiseScalarField3f = PiecewiseScalarField3<ScalarField3T, float>;
-    template<typename ScalarField3T>
-    using PiecewiseScalarField3d = PiecewiseScalarField3<ScalarField3T, double>;
+      return _out << _field.pieces.back().field
+                  << " if (x, y, z) in "
+                  << _field.pieces.back().region;
     }
-  }
-}
 
-#endif
+    /// \brief Scalar fields Pn and the regions Rn in which these are defined
+    private: std::vector<Piece> pieces;
+  };
+
+  template<typename ScalarField3T>
+  using PiecewiseScalarField3f = PiecewiseScalarField3<ScalarField3T, float>;
+  template<typename ScalarField3T>
+  using PiecewiseScalarField3d = PiecewiseScalarField3<ScalarField3T, double>;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_PIECEWISE_SCALAR_FIELD3_HH_
diff --git a/include/gz/math/Plane.hh b/include/gz/math/Plane.hh
index 7e5893464..12b95471f 100644
--- a/include/gz/math/Plane.hh
+++ b/include/gz/math/Plane.hh
@@ -25,258 +25,254 @@
 #include <gz/math/Quaternion.hh>
 #include <optional>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Plane Plane.hh gz/math/Plane.hh
+  /// \brief A plane and related functions.
+  template<typename T>
+  class Plane
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Plane Plane.hh gz/math/Plane.hh
-    /// \brief A plane and related functions.
-    template<typename T>
-    class Plane
+    /// \brief Enum used to indicate a side of the plane, no side, or both
+    /// sides for entities on the plane.
+    /// \sa Side
+    public: enum PlaneSide
     {
-      /// \brief Enum used to indicate a side of the plane, no side, or both
-      /// sides for entities on the plane.
-      /// \sa Side
-      public: enum PlaneSide
-      {
-        /// \brief Negative side of the plane. This is the side that is
-        /// opposite the normal.
-        NEGATIVE_SIDE = 0,
+      /// \brief Negative side of the plane. This is the side that is
+      /// opposite the normal.
+      NEGATIVE_SIDE = 0,
 
-        /// \brief Positive side of the plane. This is the side that has the
-        /// normal vector.
-        POSITIVE_SIDE = 1,
+      /// \brief Positive side of the plane. This is the side that has the
+      /// normal vector.
+      POSITIVE_SIDE = 1,
 
-        /// \brief On the plane.
-        NO_SIDE = 2,
+      /// \brief On the plane.
+      NO_SIDE = 2,
 
-        /// \brief On both sides of the plane.
-        BOTH_SIDE = 3
-      };
+      /// \brief On both sides of the plane.
+      BOTH_SIDE = 3
+    };
 
-      /// \brief Constructor
-      public: Plane()
-      : d(0.0)
-      {
-      }
+    /// \brief Constructor
+    public: Plane()
+    : d(0.0)
+    {
+    }
 
-      /// \brief Constructor from a normal and a distance
-      /// \param[in] _normal The plane normal
-      /// \param[in] _offset Offset along the normal
-      public: explicit Plane(const Vector3<T> &_normal, T _offset = 0.0)
-      : normal(_normal), d(_offset)
-      {
-      }
+    /// \brief Constructor from a normal and a distance
+    /// \param[in] _normal The plane normal
+    /// \param[in] _offset Offset along the normal
+    public: explicit Plane(const Vector3<T> &_normal, T _offset = 0.0)
+    : normal(_normal), d(_offset)
+    {
+    }
 
-      /// \brief Constructor
-      /// \param[in] _normal The plane normal
-      /// \param[in] _size Size of the plane
-      /// \param[in] _offset Offset along the normal
-      public: Plane(const Vector3<T> &_normal, const Vector2<T> &_size,
-                    T _offset)
-      {
-        this->Set(_normal, _size, _offset);
-      }
+    /// \brief Constructor
+    /// \param[in] _normal The plane normal
+    /// \param[in] _size Size of the plane
+    /// \param[in] _offset Offset along the normal
+    public: Plane(const Vector3<T> &_normal, const Vector2<T> &_size,
+                  T _offset)
+    {
+      this->Set(_normal, _size, _offset);
+    }
 
-      /// \brief Copy constructor
-      /// \param[in] _plane Plane to copy
-      public: Plane(const Plane &_plane) = default;
+    /// \brief Copy constructor
+    /// \param[in] _plane Plane to copy
+    public: Plane(const Plane &_plane) = default;
 
-      /// \brief Destructor
-      public: ~Plane() = default;
+    /// \brief Destructor
+    public: ~Plane() = default;
 
-      /// \brief Set the plane
-      /// \param[in] _normal The plane normal
-      /// \param[in] _offset Offset along the normal
-      public: void Set(const Vector3<T> &_normal, T _offset)
-      {
-        this->normal = _normal;
-        this->d = _offset;
-      }
+    /// \brief Set the plane
+    /// \param[in] _normal The plane normal
+    /// \param[in] _offset Offset along the normal
+    public: void Set(const Vector3<T> &_normal, T _offset)
+    {
+      this->normal = _normal;
+      this->d = _offset;
+    }
 
-      /// \brief Set the plane
-      /// \param[in] _normal The plane normal
-      /// \param[in] _size Size of the plane
-      /// \param[in] _offset Offset along the normal
-      public: void Set(const Vector3<T> &_normal, const Vector2<T> &_size,
-                       T _offset)
-      {
-        this->normal = _normal;
-        this->size = _size;
-        this->d = _offset;
-      }
+    /// \brief Set the plane
+    /// \param[in] _normal The plane normal
+    /// \param[in] _size Size of the plane
+    /// \param[in] _offset Offset along the normal
+    public: void Set(const Vector3<T> &_normal, const Vector2<T> &_size,
+                     T _offset)
+    {
+      this->normal = _normal;
+      this->size = _size;
+      this->d = _offset;
+    }
 
-      /// \brief The distance to the plane from the given point. The
-      /// distance can be negative, which indicates the point is on the
-      /// negative side of the plane.
-      /// \param[in] _point 3D point to calculate distance from.
-      /// \return Distance from the point to the plane.
-      /// \sa Side
-      public: T Distance(const Vector3<T> &_point) const
-      {
-        return this->normal.Dot(_point) - this->d;
-      }
+    /// \brief The distance to the plane from the given point. The
+    /// distance can be negative, which indicates the point is on the
+    /// negative side of the plane.
+    /// \param[in] _point 3D point to calculate distance from.
+    /// \return Distance from the point to the plane.
+    /// \sa Side
+    public: T Distance(const Vector3<T> &_point) const
+    {
+      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
+    /// \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)
       {
-        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;
       }
+      auto constant = this->Offset() - this->Normal().Dot(_point);
+      auto param = constant / this->Normal().Dot(_gradient);
+      auto intersection = _point + _gradient*param;
 
-      /// \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
-      /// plane is negative, Plane::POSITIVE_SIDE if the distance from the
-      /// point to the plane is positive, or Plane::NO_SIDE if the
-      /// point is on the plane.
-      public: PlaneSide Side(const Vector3<T> &_point) const
-      {
-        T dist = this->Distance(_point);
+      if (this->Size() == Vector2<T>(0, 0))
+        return intersection;
 
-        if (dist < 0.0)
-          return NEGATIVE_SIDE;
+      // 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);
 
-        if (dist > 0.0)
-          return POSITIVE_SIDE;
+      Vector3<T> rotatedXAxis = rotation * Vector3<T>::UnitX;
+      Vector3<T> rotatedYAxis = rotation * Vector3<T>::UnitY;
 
-        return NO_SIDE;
-      }
+      auto xBasis = rotatedXAxis.Dot(intersection);
+      auto yBasis = rotatedYAxis.Dot(intersection);
 
-      /// \brief The side of the plane a box is on.
-      /// \param[in] _box The 3D box to check.
-      /// \return Plane::NEGATIVE_SIDE if the distance from the box to the
-      /// plane is negative, Plane::POSITIVE_SIDE if the distance from the
-      /// box to the plane is positive, or Plane::BOTH_SIDE if the
-      /// box is on the plane.
-      public: PlaneSide Side(const math::AxisAlignedBox &_box) const
+      if (std::abs(xBasis) < this->Size().X() / 2 &&
+          std::abs(yBasis) < this->Size().Y() / 2)
       {
-        double dist = this->Distance(_box.Center());
-        double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0);
+        return intersection;
+      }
+      return std::nullopt;
+    }
 
-        if (dist < -maxAbsDist)
-          return NEGATIVE_SIDE;
+    /// \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
+    /// plane is negative, Plane::POSITIVE_SIDE if the distance from the
+    /// point to the plane is positive, or Plane::NO_SIDE if the
+    /// point is on the plane.
+    public: PlaneSide Side(const Vector3<T> &_point) const
+    {
+      T dist = this->Distance(_point);
 
-        if (dist > maxAbsDist)
-          return POSITIVE_SIDE;
+      if (dist < 0.0)
+        return NEGATIVE_SIDE;
 
-        return BOTH_SIDE;
-      }
+      if (dist > 0.0)
+        return POSITIVE_SIDE;
 
-      /// \brief Get distance to the plane give an origin and direction
-      /// \param[in] _origin the origin
-      /// \param[in] _dir a direction
-      /// \return the shortest distance
-      public: T Distance(const Vector3<T> &_origin,
-                         const Vector3<T> &_dir) const
-      {
-        T denom = this->normal.Dot(_dir);
-
-        if (std::abs(denom) < 1e-3)
-        {
-          // parallel
-          return 0;
-        }
-        else
-        {
-          T nom = _origin.Dot(this->normal) - this->d;
-          T t = -(nom/denom);
-          return t;
-        }
-      }
+      return NO_SIDE;
+    }
 
-      /// \brief Get the plane size
-      public: inline const Vector2<T> &Size() const
-      {
-        return this->size;
-      }
+    /// \brief The side of the plane a box is on.
+    /// \param[in] _box The 3D box to check.
+    /// \return Plane::NEGATIVE_SIDE if the distance from the box to the
+    /// plane is negative, Plane::POSITIVE_SIDE if the distance from the
+    /// box to the plane is positive, or Plane::BOTH_SIDE if the
+    /// box is on the plane.
+    public: PlaneSide Side(const math::AxisAlignedBox &_box) const
+    {
+      double dist = this->Distance(_box.Center());
+      double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0);
 
-      /// \brief Get the plane size
-      public: inline Vector2<T> &Size()
-      {
-        return this->size;
-      }
+      if (dist < -maxAbsDist)
+        return NEGATIVE_SIDE;
 
-      /// \brief Get the plane normal
-      public: inline const Vector3<T> &Normal() const
-      {
-        return this->normal;
-      }
+      if (dist > maxAbsDist)
+        return POSITIVE_SIDE;
+
+      return BOTH_SIDE;
+    }
+
+    /// \brief Get distance to the plane give an origin and direction
+    /// \param[in] _origin the origin
+    /// \param[in] _dir a direction
+    /// \return the shortest distance
+    public: T Distance(const Vector3<T> &_origin,
+                       const Vector3<T> &_dir) const
+    {
+      T denom = this->normal.Dot(_dir);
 
-      /// \brief Get the plane normal
-      public: inline Vector3<T> &Normal()
+      if (std::abs(denom) < 1e-3)
       {
-        return this->normal;
+        // parallel
+        return 0;
       }
-
-      /// \brief Get the plane offset
-      public: inline T Offset() const
+      else
       {
-        return this->d;
+        T nom = _origin.Dot(this->normal) - this->d;
+        T t = -(nom/denom);
+        return t;
       }
+    }
 
-      /// \brief Equal operator
-      /// \param _p another plane
-      /// \return itself
-      public: Plane<T> &operator=(const Plane<T> &_p) = default;
+    /// \brief Get the plane size
+    public: inline const Vector2<T> &Size() const
+    {
+      return this->size;
+    }
 
-      /// \brief Plane normal
-      private: Vector3<T> normal;
+    /// \brief Get the plane size
+    public: inline Vector2<T> &Size()
+    {
+      return this->size;
+    }
 
-      /// \brief Plane size
-      private: Vector2<T> size;
+    /// \brief Get the plane normal
+    public: inline const Vector3<T> &Normal() const
+    {
+      return this->normal;
+    }
 
-      /// \brief Plane offset
-      private: T d;
-    };
+    /// \brief Get the plane normal
+    public: inline Vector3<T> &Normal()
+    {
+      return this->normal;
+    }
 
-    typedef Plane<int> Planei;
-    typedef Plane<double> Planed;
-    typedef Plane<float> Planef;
+    /// \brief Get the plane offset
+    public: inline T Offset() const
+    {
+      return this->d;
     }
-  }
-}
 
-#endif
+    /// \brief Equal operator
+    /// \param _p another plane
+    /// \return itself
+    public: Plane<T> &operator=(const Plane<T> &_p) = default;
+
+    /// \brief Plane normal
+    private: Vector3<T> normal;
+
+    /// \brief Plane size
+    private: Vector2<T> size;
+
+    /// \brief Plane offset
+    private: T d;
+  };
+
+  typedef Plane<int> Planei;
+  typedef Plane<double> Planed;
+  typedef Plane<float> Planef;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_PLANE_HH_
diff --git a/include/gz/math/Polynomial3.hh b/include/gz/math/Polynomial3.hh
index 43971bcfb..4cd110516 100644
--- a/include/gz/math/Polynomial3.hh
+++ b/include/gz/math/Polynomial3.hh
@@ -27,269 +27,265 @@
 #include <gz/math/Vector4.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Polynomial3 Polynomial3.hh gz/math/Polynomial3.hh
+  /// \brief The Polynomial3 class represents a cubic polynomial
+  /// with real coefficients p(x) = c0 x^3 + c1 x^2 + c2 x + c3.
+  /// ## Example
+  ///
+  /// \snippet examples/polynomial3_example.cc complete
+  template <typename T>
+  class Polynomial3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Polynomial3 Polynomial3.hh gz/math/Polynomial3.hh
-    /// \brief The Polynomial3 class represents a cubic polynomial
-    /// with real coefficients p(x) = c0 x^3 + c1 x^2 + c2 x + c3.
-    /// ## Example
-    ///
-    /// \snippet examples/polynomial3_example.cc complete
-    template <typename T>
-    class Polynomial3
+    /// \brief Constructor
+    public: Polynomial3() = default;
+
+    /// \brief Constructor
+    /// \param[in] _coeffs coefficients c0 through c3, left to right
+    public: explicit Polynomial3(Vector4<T> _coeffs)
+    : coeffs(std::move(_coeffs))
     {
-      /// \brief Constructor
-      public: Polynomial3() = default;
+    }
 
-      /// \brief Constructor
-      /// \param[in] _coeffs coefficients c0 through c3, left to right
-      public: explicit Polynomial3(Vector4<T> _coeffs)
-      : coeffs(std::move(_coeffs))
-      {
-      }
+    /// \brief Make a constant polynomial
+    /// \return a p(x) = `_value` polynomial
+    public: static Polynomial3 Constant(T _value)
+    {
+      return Polynomial3(Vector4<T>(0., 0., 0., _value));
+    }
+
+    /// \brief Get the polynomial coefficients
+    /// \return this polynomial coefficients
+    public: const Vector4<T> &Coeffs() const { return this->coeffs; }
 
-      /// \brief Make a constant polynomial
-      /// \return a p(x) = `_value` polynomial
-      public: static Polynomial3 Constant(T _value)
+    /// \brief Evaluate the polynomial at `_x`
+    /// For non-finite `_x`, this function
+    /// computes p(z) as z tends to `_x`.
+    /// \param[in] _x polynomial argument
+    /// \return the result of evaluating p(`_x`)
+    public: T Evaluate(const T &_x) const
+    {
+      using std::isnan, std::isfinite;
+      if (isnan(_x))
       {
-        return Polynomial3(Vector4<T>(0., 0., 0., _value));
+        return _x;
       }
-
-      /// \brief Get the polynomial coefficients
-      /// \return this polynomial coefficients
-      public: const Vector4<T> &Coeffs() const { return this->coeffs; }
-
-      /// \brief Evaluate the polynomial at `_x`
-      /// For non-finite `_x`, this function
-      /// computes p(z) as z tends to `_x`.
-      /// \param[in] _x polynomial argument
-      /// \return the result of evaluating p(`_x`)
-      public: T Evaluate(const T &_x) const
+      if (!isfinite(_x))
       {
-        using std::isnan, std::isfinite;
-        if (isnan(_x))
+        using std::abs, std::copysign;
+        const T epsilon =
+            std::numeric_limits<T>::epsilon();
+        if (abs(this->coeffs[0]) >= epsilon)
         {
-          return _x;
+          return _x * copysign(T(1.), this->coeffs[0]);
         }
-        if (!isfinite(_x))
+        if (abs(this->coeffs[1]) >= epsilon)
         {
-          using std::abs, std::copysign;
-          const T epsilon =
-              std::numeric_limits<T>::epsilon();
-          if (abs(this->coeffs[0]) >= epsilon)
-          {
-            return _x * copysign(T(1.), this->coeffs[0]);
-          }
-          if (abs(this->coeffs[1]) >= epsilon)
-          {
-            return copysign(_x, this->coeffs[1]);
-          }
-          if (abs(this->coeffs[2]) >= epsilon)
-          {
-            return _x * copysign(T(1.), this->coeffs[2]);
-          }
-          return this->coeffs[3];
+          return copysign(_x, this->coeffs[1]);
         }
-        const T _x2 = _x * _x;
-        const T _x3 = _x2 * _x;
-
-        return (this->coeffs[0] * _x3 + this->coeffs[1] * _x2 +
-                this->coeffs[2] * _x + this->coeffs[3]);
+        if (abs(this->coeffs[2]) >= epsilon)
+        {
+          return _x * copysign(T(1.), this->coeffs[2]);
+        }
+        return this->coeffs[3];
       }
+      const T _x2 = _x * _x;
+      const T _x3 = _x2 * _x;
+
+      return (this->coeffs[0] * _x3 + this->coeffs[1] * _x2 +
+              this->coeffs[2] * _x + this->coeffs[3]);
+    }
 
-      /// \brief Call operator overload
-      /// \see Polynomial3::Evaluate()
-      public: T operator()(const T &_x) const
+    /// \brief Call operator overload
+    /// \see Polynomial3::Evaluate()
+    public: T operator()(const T &_x) const
+    {
+      return this->Evaluate(_x);
+    }
+
+    /// \brief Compute polynomial minimum in an `_interval`
+    /// \param[in] _interval polynomial argument interval to check
+    /// \param[out] _xMin polynomial argument that yields minimum,
+    ///   or NaN if the interval is empty
+    /// \return the polynomial minimum in the given interval,
+    ///   or NaN if the interval is empty
+    public: T Minimum(const Interval<T> &_interval, T &_xMin) const
+    {
+      if (_interval.Empty())
       {
-        return this->Evaluate(_x);
+        _xMin = std::numeric_limits<T>::quiet_NaN();
+        return std::numeric_limits<T>::quiet_NaN();
       }
-
-      /// \brief Compute polynomial minimum in an `_interval`
-      /// \param[in] _interval polynomial argument interval to check
-      /// \param[out] _xMin polynomial argument that yields minimum,
-      ///   or NaN if the interval is empty
-      /// \return the polynomial minimum in the given interval,
-      ///   or NaN if the interval is empty
-      public: T Minimum(const Interval<T> &_interval, T &_xMin) const
+      T yMin;
+      // For open intervals, assume continuity in the limit
+      const T &xLeft = _interval.LeftValue();
+      const T &xRight = _interval.RightValue();
+      const T yLeft = this->Evaluate(xLeft);
+      const T yRight = this->Evaluate(xRight);
+      if (yLeft <= yRight)
       {
-        if (_interval.Empty())
-        {
-          _xMin = std::numeric_limits<T>::quiet_NaN();
-          return std::numeric_limits<T>::quiet_NaN();
-        }
-        T yMin;
-        // For open intervals, assume continuity in the limit
-        const T &xLeft = _interval.LeftValue();
-        const T &xRight = _interval.RightValue();
-        const T yLeft = this->Evaluate(xLeft);
-        const T yRight = this->Evaluate(xRight);
-        if (yLeft <= yRight)
-        {
-          yMin = yLeft;
-          _xMin = xLeft;
-        }
-        else
-        {
-          yMin = yRight;
-          _xMin = xRight;
-        }
-        using std::abs, std::sqrt;  // enable ADL
-        constexpr T epsilon = std::numeric_limits<T>::epsilon();
-        if (abs(this->coeffs[0]) >= epsilon)
-        {
-          // Polynomial function p(x) is cubic, look
-          // for local minima within the given interval
+        yMin = yLeft;
+        _xMin = xLeft;
+      }
+      else
+      {
+        yMin = yRight;
+        _xMin = xRight;
+      }
+      using std::abs, std::sqrt;  // enable ADL
+      constexpr T epsilon = std::numeric_limits<T>::epsilon();
+      if (abs(this->coeffs[0]) >= epsilon)
+      {
+        // Polynomial function p(x) is cubic, look
+        // for local minima within the given interval
 
-          // Find local extrema by computing the roots
-          // of p'(x), a quadratic polynomial function
-          const T a = this->coeffs[0] * T(3.);
-          const T b = this->coeffs[1] * T(2.);
-          const T c = this->coeffs[2];
+        // Find local extrema by computing the roots
+        // of p'(x), a quadratic polynomial function
+        const T a = this->coeffs[0] * T(3.);
+        const T b = this->coeffs[1] * T(2.);
+        const T c = this->coeffs[2];
 
-          const T discriminant = b * b - T(4.) * a * c;
-          if (discriminant >= T(0.))
+        const T discriminant = b * b - T(4.) * a * c;
+        if (discriminant >= T(0.))
+        {
+          // Roots of p'(x) are real, check local minima
+          const T x = (-b + sqrt(discriminant)) / (T(2.) * a);
+          if (_interval.Contains(x))
           {
-            // Roots of p'(x) are real, check local minima
-            const T x = (-b + sqrt(discriminant)) / (T(2.) * a);
-            if (_interval.Contains(x))
+            const T y = this->Evaluate(x);
+            if (y < yMin)
             {
-              const T y = this->Evaluate(x);
-              if (y < yMin)
-              {
-                _xMin = x;
-                yMin = y;
-              }
+              _xMin = x;
+              yMin = y;
             }
           }
         }
-        else if (abs(this->coeffs[1]) >= epsilon)
+      }
+      else if (abs(this->coeffs[1]) >= epsilon)
+      {
+        // Polynomial function p(x) is quadratic,
+        // look for global minima if concave
+        const T a = this->coeffs[1];
+        const T b = this->coeffs[2];
+        if (a > T(0.))
         {
-          // Polynomial function p(x) is quadratic,
-          // look for global minima if concave
-          const T a = this->coeffs[1];
-          const T b = this->coeffs[2];
-          if (a > T(0.))
+          const T x = -b / (T(2.) * a);
+          if (_interval.Contains(x))
           {
-            const T x = -b / (T(2.) * a);
-            if (_interval.Contains(x))
+            const T y = this->Evaluate(x);
+            if (y < yMin)
             {
-              const T y = this->Evaluate(x);
-              if (y < yMin)
-              {
-                _xMin = x;
-                yMin = y;
-              }
+              _xMin = x;
+              yMin = y;
             }
           }
         }
-        return yMin;
       }
+      return yMin;
+    }
 
-      /// \brief Compute polynomial minimum in an `_interval`
-      /// \param[in] _interval polynomial argument interval to check
-      /// \return the polynomial minimum in the given interval (may
-      ///   not be finite), or NaN if the interval is empty
-      public: T Minimum(const Interval<T> &_interval) const
-      {
-        T xMin;
-        return this->Minimum(_interval, xMin);
-      }
+    /// \brief Compute polynomial minimum in an `_interval`
+    /// \param[in] _interval polynomial argument interval to check
+    /// \return the polynomial minimum in the given interval (may
+    ///   not be finite), or NaN if the interval is empty
+    public: T Minimum(const Interval<T> &_interval) const
+    {
+      T xMin;
+      return this->Minimum(_interval, xMin);
+    }
 
-      /// \brief Compute polynomial minimum
-      /// \param[out] _xMin polynomial argument that yields minimum
-      /// \return the polynomial minimum (may not be finite)
-      public: T Minimum(T &_xMin) const
-      {
-        return this->Minimum(Interval<T>::Unbounded, _xMin);
-      }
+    /// \brief Compute polynomial minimum
+    /// \param[out] _xMin polynomial argument that yields minimum
+    /// \return the polynomial minimum (may not be finite)
+    public: T Minimum(T &_xMin) const
+    {
+      return this->Minimum(Interval<T>::Unbounded, _xMin);
+    }
 
-      /// \brief Compute polynomial minimum
-      /// \return the polynomial minimum (may not be finite)
-      public: T Minimum() const
-      {
-        T xMin;
-        return this->Minimum(Interval<T>::Unbounded, xMin);
-      }
+    /// \brief Compute polynomial minimum
+    /// \return the polynomial minimum (may not be finite)
+    public: T Minimum() const
+    {
+      T xMin;
+      return this->Minimum(Interval<T>::Unbounded, xMin);
+    }
 
-      /// \brief Prints polynomial as p(`_x`) to `_out` stream
-      /// \param[in] _out Output stream to print to
-      /// \param[in] _x Argument name to be used
-      public: void Print(std::ostream &_out, const std::string &_x = "x") const
+    /// \brief Prints polynomial as p(`_x`) to `_out` stream
+    /// \param[in] _out Output stream to print to
+    /// \param[in] _x Argument name to be used
+    public: void Print(std::ostream &_out, const std::string &_x = "x") const
+    {
+      constexpr T epsilon =
+          std::numeric_limits<T>::epsilon();
+      bool streamStarted = false;
+      for (size_t i = 0; i < 4; ++i)
       {
-        constexpr T epsilon =
-            std::numeric_limits<T>::epsilon();
-        bool streamStarted = false;
-        for (size_t i = 0; i < 4; ++i)
+        using std::abs;  // enable ADL
+        const T magnitude = abs(this->coeffs[i]);
+        const bool sign = this->coeffs[i] < T(0);
+        const int exponent = 3 - static_cast<int>(i);
+        if (magnitude >= epsilon)
         {
-          using std::abs;  // enable ADL
-          const T magnitude = abs(this->coeffs[i]);
-          const bool sign = this->coeffs[i] < T(0);
-          const int exponent = 3 - static_cast<int>(i);
-          if (magnitude >= epsilon)
+          if (streamStarted)
           {
-            if (streamStarted)
+            if (sign)
             {
-              if (sign)
-              {
-                _out << " - ";
-              }
-              else
-              {
-                _out << " + ";
-              }
+              _out << " - ";
             }
-            else if (sign)
+            else
             {
-              _out << "-";
+              _out << " + ";
             }
-            if (exponent > 0)
+          }
+          else if (sign)
+          {
+            _out << "-";
+          }
+          if (exponent > 0)
+          {
+            if ((magnitude - T(1)) > epsilon)
             {
-              if ((magnitude - T(1)) > epsilon)
-              {
-                _out << magnitude << " ";
-              }
-              _out <<  _x;
-              if (exponent > 1)
-              {
-                _out << "^" << exponent;
-              }
+              _out << magnitude << " ";
             }
-            else
+            _out <<  _x;
+            if (exponent > 1)
             {
-              _out << magnitude;
+              _out << "^" << exponent;
             }
-            streamStarted = true;
           }
-        }
-        if (!streamStarted)
-        {
-          _out << this->coeffs[3];
+          else
+          {
+            _out << magnitude;
+          }
+          streamStarted = true;
         }
       }
-
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _p Polynomial3 to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-        std::ostream &_out, const gz::math::Polynomial3<T> &_p)
+      if (!streamStarted)
       {
-        _p.Print(_out, "x");
-        return _out;
+        _out << this->coeffs[3];
       }
+    }
 
-      /// \brief Polynomial coefficients
-      private: Vector4<T> coeffs;
-    };
-    using Polynomial3f = Polynomial3<float>;
-    using Polynomial3d = Polynomial3<double>;
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _p Polynomial3 to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+      std::ostream &_out, const gz::math::Polynomial3<T> &_p)
+    {
+      _p.Print(_out, "x");
+      return _out;
     }
-  }
-}
 
-#endif
+    /// \brief Polynomial coefficients
+    private: Vector4<T> coeffs;
+  };
+  using Polynomial3f = Polynomial3<float>;
+  using Polynomial3d = Polynomial3<double>;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_POLYNOMIAL3_HH_
diff --git a/include/gz/math/Pose3.hh b/include/gz/math/Pose3.hh
index c6e8935e3..7b6903a19 100644
--- a/include/gz/math/Pose3.hh
+++ b/include/gz/math/Pose3.hh
@@ -21,478 +21,475 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Pose3 Pose3.hh gz/math/Pose3.hh
+  /// \brief The Pose3 class represents a 3D position and rotation. The
+  /// position  component is a Vector3, and the rotation is a Quaternion.
+  ///
+  /// The following two type definitions are provided:
+  ///
+  /// * \ref Pose3f
+  /// * \ref Pose3d
+  /// ## Examples
+  ///
+  /// * C++
+  ///
+  /// \snippet examples/pose3_example.cc complete
+  ///
+  /// * Ruby
+  ///
+  /// \code{.rb}
+  /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
+  /// #
+  /// require 'gz/math'
+  ///
+  /// # Construct a default Pose3d.
+  /// p = Gz::Math::Pose3d.new
+  /// printf("A default Pose3d has the following values\n" +
+  ///        "%f %f %f %f %f %f\n", p.Pos().X(), p.Pos().Y(), p.Pos().Z(),
+  ///        p.Rot().Euler().X(), p.Rot().Euler().Y(), p.Rot().Euler().Z())
+  ///
+  /// # Construct a pose at position 1, 2, 3 with a yaw of PI radians.
+  /// p1 = Gz::Math::Pose3d.new(1, 2, 3, 0, 0, Math::PI)
+  /// printf("A pose3d(1, 2, 3, 0, 0, GZ_PI) has the following values\n" +
+  ///        "%f %f %f %f %f %f\n", p1.Pos().X(), p1.Pos().Y(), p1.Pos().Z(),
+  ///        p1.Rot().Euler().X(), p1.Rot().Euler().Y(), p1.Rot().Euler().Z())
+  ///
+  /// # Set the position of a pose to 10, 20, 30
+  /// p.Pos().Set(10, 20, 30)
+  ///
+  /// p3 = p * p1
+  /// printf("Result of combining two poses is\n"+
+  ///         "%f %f %f %f %f %f\n", p3.Pos().X(), p3.Pos().Y(), p3.Pos().Z(),
+  ///        p3.Rot().Euler().X(), p3.Rot().Euler().Y(), p3.Rot().Euler().Z())
+  /// \endcode
+  template<typename T>
+  class Pose3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Pose3 Pose3.hh gz/math/Pose3.hh
-    /// \brief The Pose3 class represents a 3D position and rotation. The
-    /// position  component is a Vector3, and the rotation is a Quaternion.
-    ///
-    /// The following two type definitions are provided:
-    ///
-    /// * \ref Pose3f
-    /// * \ref Pose3d
-    /// ## Examples
-    ///
-    /// * C++
-    ///
-    /// \snippet examples/pose3_example.cc complete
-    ///
-    /// * Ruby
-    ///
-    /// \code{.rb}
-    /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
-    /// #
-    /// require 'gz/math'
-    ///
-    /// # Construct a default Pose3d.
-    /// p = Gz::Math::Pose3d.new
-    /// printf("A default Pose3d has the following values\n" +
-    ///        "%f %f %f %f %f %f\n", p.Pos().X(), p.Pos().Y(), p.Pos().Z(),
-    ///        p.Rot().Euler().X(), p.Rot().Euler().Y(), p.Rot().Euler().Z())
-    ///
-    /// # Construct a pose at position 1, 2, 3 with a yaw of PI radians.
-    /// p1 = Gz::Math::Pose3d.new(1, 2, 3, 0, 0, Math::PI)
-    /// printf("A pose3d(1, 2, 3, 0, 0, GZ_PI) has the following values\n" +
-    ///        "%f %f %f %f %f %f\n", p1.Pos().X(), p1.Pos().Y(), p1.Pos().Z(),
-    ///        p1.Rot().Euler().X(), p1.Rot().Euler().Y(), p1.Rot().Euler().Z())
-    ///
-    /// # Set the position of a pose to 10, 20, 30
-    /// p.Pos().Set(10, 20, 30)
-    ///
-    /// p3 = p * p1
-    /// printf("Result of combining two poses is\n"+
-    ///         "%f %f %f %f %f %f\n", p3.Pos().X(), p3.Pos().Y(), p3.Pos().Z(),
-    ///        p3.Rot().Euler().X(), p3.Rot().Euler().Y(), p3.Rot().Euler().Z())
-    /// \endcode
-    template<typename T>
-    class Pose3
-    {
-      /// \brief A Pose3 initialized to zero.
-      /// This is equivalent to math::Pose3<T>(0, 0, 0, 0, 0, 0).
-      public: static const Pose3<T> &Zero;
-
-      /// \brief Default constructor. This initializes the position
-      /// component to zero and the quaternion to identity.
-      public: Pose3() = default;
-
-      /// \brief Create a Pose3 based on a position and rotation.
-      /// \param[in] _pos The position
-      /// \param[in] _rot The rotation
-      public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
-      : p(_pos), q(_rot)
-      {
-      }
-
-      /// \brief Create a Pose3 using a 6-tuple consisting of
-      ///  x, y, z, roll, pitch, and yaw.
-      /// \param[in] _x x position in meters.
-      /// \param[in] _y y position in meters.
-      /// \param[in] _z z position in meters.
-      /// \param[in] _roll Roll (rotation about X-axis) in radians.
-      /// \param[in] _pitch Pitch (rotation about y-axis) in radians.
-      /// \param[in] _yaw Yaw (rotation about z-axis) in radians.
-      public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
-      : p(_x, _y, _z), q(_roll, _pitch, _yaw)
-      {
-      }
-
-      /// \brief Create a Pose3 using a 7-tuple consisting of
-      /// x, y, z, qw, qx, qy, qz. The first three values are the position
-      /// and the last four the rotation represented as a quaternion.
-      /// \param[in] _x x position in meters.
-      /// \param[in] _y y position in meters.
-      /// \param[in] _z z position in meters.
-      /// \param[in] _qw Quaternion w value.
-      /// \param[in] _qx Quaternion x value.
-      /// \param[in] _qy Quaternion y value.
-      /// \param[in] _qz Quaternion z value.
-      public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
-      : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
-      {
-      }
-
-      /// \brief Copy constructor.
-      /// \param[in] _pose Pose3<T> to copy
-      public: Pose3(const Pose3<T> &_pose) = default;
-
-      /// \brief Destructor.
-      public: ~Pose3() = default;
-
-      /// \brief Set the pose from a Vector3<T> and a Quaternion<T>
-      /// \param[in] _pos The position.
-      /// \param[in] _rot The rotation.
-      public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
-      {
-        this->p = _pos;
-        this->q = _rot;
-      }
-
-      /// \brief Set the pose from a position and Euler angles.
-      /// \param[in] _pos The position.
-      /// \param[in] _rpy The rotation expressed as Euler angles.
-      public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
-      {
-        this->p = _pos;
-        this->q.SetFromEuler(_rpy);
-      }
-
-      /// \brief Set the pose from a six tuple consisting of
-      ///  x, y, z, roll, pitch, and yaw.
-      /// \param[in] _x x position in meters.
-      /// \param[in] _y y position in meters.
-      /// \param[in] _z z position in meters.
-      /// \param[in] _roll Roll (rotation about X-axis) in radians.
-      /// \param[in] _pitch Pitch (rotation about y-axis) in radians.
-      /// \param[in] _yaw Pitch (rotation about z-axis) in radians.
-      public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
-      {
-        this->p.Set(_x, _y, _z);
-        this->q.SetFromEuler(math::Vector3<T>(_roll, _pitch, _yaw));
-      }
-
-      /// \brief See if a pose is finite (e.g., not nan)
-      /// \return True if this pose is finite.
-      public: bool IsFinite() const
-      {
-        return this->p.IsFinite() && this->q.IsFinite();
-      }
-
-      /// \brief Fix any nan values.
-      public: inline void Correct()
-      {
-        this->p.Correct();
-        this->q.Correct();
-      }
-
-      /// \brief Get the inverse of this pose.
-      /// \return The inverse pose.
-      public: Pose3<T> Inverse() const
-      {
-        Quaternion<T> inv = this->q.Inverse();
-        return Pose3<T>(inv * (this->p*-1), inv);
-      }
-
-      /// \brief Equality operator.
-      /// \param[in] _pose Pose3<T> for comparison.
-      /// \return True if this pose is equal to the given pose.
-      public: bool operator==(const Pose3<T> &_pose) const
-      {
-        return this->p == _pose.p && this->q == _pose.q;
-      }
-
-      /// \brief Inequality operator.
-      /// \param[in] _pose Pose3<T> for comparison.
-      /// \return True if this pose is not equal to the  given pose.
-      public: bool operator!=(const Pose3<T> &_pose) const
-      {
-        return this->p != _pose.p || this->q != _pose.q;
-      }
-
-      /// \brief Multiplication operator.
-      /// Given X_OP (frame P relative to O) and X_PQ (frame Q relative to P)
-      /// then X_OQ = X_OP * X_PQ (frame Q relative to O).
-      /// \param[in] _pose The pose to multiply by.
-      /// \return The resulting pose.
-      public: Pose3<T> operator*(const Pose3<T> &_pose) const
-      {
-        return Pose3<T>(_pose.CoordPositionAdd(*this),  this->q * _pose.q);
-      }
-
-      /// \brief Multiplication assignment operator. This pose will become
-      /// equal to this * _pose.
-      /// \param[in] _pose Pose3<T> to multiply to this pose
-      /// \sa operator*(const Pose3<T> &_pose) const
-      /// \return The resulting pose
-      public: const Pose3<T> &operator*=(const Pose3<T> &_pose)
-      {
-        *this = *this * _pose;
-        return *this;
-      }
-
-      /// \brief Assignment operator
-      /// \param[in] _pose Pose3<T> to copy
-      public: Pose3<T> &operator=(const Pose3<T> &_pose) = default;
-
-      /// \brief Add one point to a vector: result = this + pos.
-      /// \param[in] _pos Position to add to this pose
-      /// \return The resulting position.
-      public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
-      {
-        Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
-
-        // result = pose.q + pose.q * this->p * pose.q!
-        tmp = this->q * (tmp * this->q.Inverse());
-
-        return Vector3<T>(this->p.X() + tmp.X(),
-                          this->p.Y() + tmp.Y(),
-                          this->p.Z() + tmp.Z());
-      }
-
-      /// \brief Add one pose to another: result = this + pose.
-      /// \param[in] _pose The Pose3<T> to add.
-      /// \return The resulting position.
-      public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
-      {
-        Quaternion<T> tmp(static_cast<T>(0),
-            this->p.X(), this->p.Y(), this->p.Z());
-
-        // result = _pose.q + _pose.q * this->p * _pose.q!
-        tmp = _pose.q * (tmp * _pose.q.Inverse());
-
-        return Vector3<T>(_pose.p.X() + tmp.X(),
-                          _pose.p.Y() + tmp.Y(),
-                          _pose.p.Z() + tmp.Z());
-      }
-
-      /// \brief Subtract one position from another: result = this - pose
-      /// \param[in] _pose Pose3<T> to subtract
-      /// \return The resulting position
-      public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
-      {
-        Quaternion<T> tmp(0,
-            this->p.X() - _pose.p.X(),
-            this->p.Y() - _pose.p.Y(),
-            this->p.Z() - _pose.p.Z());
-
-        tmp = _pose.q.Inverse() * (tmp * _pose.q);
-        return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
-      }
-
-      /// \brief Add one rotation to another: result =  this->q + rot.
-      /// \param[in] _rot Rotation to add.
-      /// \return The resulting rotation.
-      public: Quaternion<T> CoordRotationAdd(const Quaternion<T> &_rot) const
-      {
-        return Quaternion<T>(_rot * this->q);
-      }
-
-      /// \brief Subtract one rotation from another: result = this->q - rot.
-      /// \param[in] _rot The rotation to subtract.
-      /// \return The resulting rotation.
-      public: inline Quaternion<T> CoordRotationSub(
-                  const Quaternion<T> &_rot) const
-      {
-        Quaternion<T> result(_rot.Inverse() * this->q);
-        result.Normalize();
-        return result;
-      }
-
-      /// \brief Find the inverse of a pose; i.e., if b = this + a, given b and
-      /// this, find a.
-      /// \param[in] _b the other pose.
-      // \return The inverse pose.
-      public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
-      {
-        Quaternion<T> qt;
-        Pose3<T> a;
-
-        a.q = this->q.Inverse() * _b.q;
-        qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
-        qt = qt * a.q.Inverse();
-        a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
-
-        return a;
-      }
-
-      /// \brief Reset the pose. This sets the position to zero and the
-      /// rotation to identify.
-      public: void Reset()
-      {
-        // set the position to zero
-        this->p.Set();
-        this->q = Quaternion<T>::Identity;
-      }
-
-      /// \brief Rotate the vector part of a pose about the origin.
-      /// \param[in] _q rotation.
-      /// \return The rotated pose.
-      public: Pose3<T> RotatePositionAboutOrigin(const Quaternion<T> &_q) const
-      {
-        Pose3<T> a = *this;
-        a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
-                +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
-                +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
-        a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
-                +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
-                +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
-        a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
-                +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
-                +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
-        return a;
-      }
-
-      /// \brief Round all values to _precision decimal places.
-      /// \param[in] _precision Number of decimal places.
-      public: void Round(int _precision)
-      {
-        this->q.Round(_precision);
-        this->p.Round(_precision);
-      }
-
-      /// \brief Get the position.
-      /// \return Origin of the pose.
-      public: inline const Vector3<T> &Pos() const
-      {
-        return this->p;
-      }
-
-      /// \brief Get a mutable reference to the position.
-      /// \return Origin of the pose.
-      public: inline Vector3<T> &Pos()
-      {
-        return this->p;
-      }
-
-      /// \brief Get the X value of the position.
-      /// \return Value X of the origin of the pose.
-      /// \note The return is made by value since
-      /// Vector3<T>.X() is already a reference.
-      public: inline const T X() const
-      {
-        return this->p.X();
-      }
-
-      /// \brief Set X value of the position.
-      public: inline void SetX(T x)
-      {
-       this->p.X() = x;
-      }
-
-      /// \brief Get the Y value of the position.
-      /// \return Value Y of the origin of the pose.
-      /// \note The return is made by value since
-      /// Vector3<T>.Y() is already a reference.
-      public: inline const T Y() const
-      {
-        return this->p.Y();
-      }
-
-      /// \brief Set the Y value of the position.
-      public: inline void SetY(T y)
-      {
-        this->p.Y() = y;
-      }
-
-      /// \brief Get the Z value of the position.
-      /// \return Value Z of the origin of the pose.
-      /// \note The return is made by value since
-      /// Vector3<T>.Z() is already a reference.
-      public: inline const T Z() const
-      {
-        return this->p.Z();
-      }
-
-      /// \brief Set the Z value of the position.
-      public: inline void SetZ(T z)
-      {
-        this->p.Z() = z;
-      }
-
-      /// \brief Get the rotation.
-      /// \return Quaternion representation of the rotation.
-      public: inline const Quaternion<T> &Rot() const
-      {
-        return this->q;
-      }
-
-      /// \brief Get a mutable reference to the rotation.
-      /// \return Quaternion representation of the rotation.
-      public: inline Quaternion<T> &Rot()
-      {
-        return this->q;
-      }
-
-      /// \brief Get the Roll value of the rotation.
-      /// \return Roll value of the orientation.
-      /// \note The return is made by value since
-      ///  Quaternion<T>.Roll() is already a reference.
-      public: inline const T Roll() const
-      {
-        return this->q.Roll();
-      }
-
-      /// \brief Get the Pitch value of the rotation.
-      /// \return Pitch value of the orientation.
-      /// \note The return is made by value since
-      ///  Quaternion<T>.Pitch() is already a reference.
-      public: inline const T Pitch() const
-      {
-        return this->q.Pitch();
-      }
-
-      /// \brief Get the Yaw value of the rotation.
-      /// \return Yaw value of the orientation.
-      /// \note The return is made by value since
-      ///  Quaternion<T>.Yaw() is already a reference.
-      public: inline const T Yaw() const
-      {
-        return this->q.Yaw();
-      }
-
-      /// \brief Stream insertion operator
-      /// \param[in] _out output stream
-      /// \param[in] _pose pose to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-                  std::ostream &_out, const gz::math::Pose3<T> &_pose)
-      {
-        _out << _pose.Pos() << " " << _pose.Rot();
-        return _out;
-      }
-
-      /// \brief Stream extraction operator
-      /// \param[in] _in the input stream
-      /// \param[in] _pose the pose
-      /// \return the stream
-      public: friend std::istream &operator>>(
-                  std::istream &_in, gz::math::Pose3<T> &_pose)
-      {
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        Vector3<T> pos;
-        Quaternion<T> rot;
-        _in >> pos >> rot;
-        _pose.Set(pos, rot);
-        return _in;
-      }
-
-      /// \brief Equality test with tolerance.
-      /// \param[in] _p The pose to compare this against. Both the position
-      /// Vector3 and rotation Quaternion are compared.
-      /// \param[in] _tol Equality tolerance.
-      /// \return True if the position and orientation of the poses are equal
-      /// within the tolerence specified by _tol.
-      public: bool Equal(const Pose3 &_p, const T &_tol) const
-      {
-        return this->p.Equal(_p.p, _tol) && this->q.Equal(_p.q, _tol);
-      }
-
-      /// \brief The position
-      private: Vector3<T> p;
-
-      /// \brief The rotation
-      private: Quaternion<T> q;
-    };
-
-    namespace detail {
-
-      template<typename T> constexpr Pose3<T> gPose3Zero{};
-
-    }  // namespace detail
-
-    template<typename T> const Pose3<T> &Pose3<T>::Zero = detail::gPose3Zero<T>;
-
-    /// typedef Pose3<double> as Pose3d.
-    typedef Pose3<double> Pose3d;
-
-    /// typedef Pose3<float> as Pose3f.
-    typedef Pose3<float> Pose3f;
-    }
-  }
-}
-#endif
+    /// \brief A Pose3 initialized to zero.
+    /// This is equivalent to math::Pose3<T>(0, 0, 0, 0, 0, 0).
+    public: static const Pose3<T> &Zero;
+
+    /// \brief Default constructor. This initializes the position
+    /// component to zero and the quaternion to identity.
+    public: Pose3() = default;
+
+    /// \brief Create a Pose3 based on a position and rotation.
+    /// \param[in] _pos The position
+    /// \param[in] _rot The rotation
+    public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
+    : p(_pos), q(_rot)
+    {
+    }
+
+    /// \brief Create a Pose3 using a 6-tuple consisting of
+    ///  x, y, z, roll, pitch, and yaw.
+    /// \param[in] _x x position in meters.
+    /// \param[in] _y y position in meters.
+    /// \param[in] _z z position in meters.
+    /// \param[in] _roll Roll (rotation about X-axis) in radians.
+    /// \param[in] _pitch Pitch (rotation about y-axis) in radians.
+    /// \param[in] _yaw Yaw (rotation about z-axis) in radians.
+    public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
+    : p(_x, _y, _z), q(_roll, _pitch, _yaw)
+    {
+    }
+
+    /// \brief Create a Pose3 using a 7-tuple consisting of
+    /// x, y, z, qw, qx, qy, qz. The first three values are the position
+    /// and the last four the rotation represented as a quaternion.
+    /// \param[in] _x x position in meters.
+    /// \param[in] _y y position in meters.
+    /// \param[in] _z z position in meters.
+    /// \param[in] _qw Quaternion w value.
+    /// \param[in] _qx Quaternion x value.
+    /// \param[in] _qy Quaternion y value.
+    /// \param[in] _qz Quaternion z value.
+    public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
+    : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
+    {
+    }
+
+    /// \brief Copy constructor.
+    /// \param[in] _pose Pose3<T> to copy
+    public: Pose3(const Pose3<T> &_pose) = default;
+
+    /// \brief Destructor.
+    public: ~Pose3() = default;
+
+    /// \brief Set the pose from a Vector3<T> and a Quaternion<T>
+    /// \param[in] _pos The position.
+    /// \param[in] _rot The rotation.
+    public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
+    {
+      this->p = _pos;
+      this->q = _rot;
+    }
+
+    /// \brief Set the pose from a position and Euler angles.
+    /// \param[in] _pos The position.
+    /// \param[in] _rpy The rotation expressed as Euler angles.
+    public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
+    {
+      this->p = _pos;
+      this->q.SetFromEuler(_rpy);
+    }
+
+    /// \brief Set the pose from a six tuple consisting of
+    ///  x, y, z, roll, pitch, and yaw.
+    /// \param[in] _x x position in meters.
+    /// \param[in] _y y position in meters.
+    /// \param[in] _z z position in meters.
+    /// \param[in] _roll Roll (rotation about X-axis) in radians.
+    /// \param[in] _pitch Pitch (rotation about y-axis) in radians.
+    /// \param[in] _yaw Pitch (rotation about z-axis) in radians.
+    public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
+    {
+      this->p.Set(_x, _y, _z);
+      this->q.SetFromEuler(math::Vector3<T>(_roll, _pitch, _yaw));
+    }
+
+    /// \brief See if a pose is finite (e.g., not nan)
+    /// \return True if this pose is finite.
+    public: bool IsFinite() const
+    {
+      return this->p.IsFinite() && this->q.IsFinite();
+    }
+
+    /// \brief Fix any nan values.
+    public: inline void Correct()
+    {
+      this->p.Correct();
+      this->q.Correct();
+    }
+
+    /// \brief Get the inverse of this pose.
+    /// \return The inverse pose.
+    public: Pose3<T> Inverse() const
+    {
+      Quaternion<T> inv = this->q.Inverse();
+      return Pose3<T>(inv * (this->p*-1), inv);
+    }
+
+    /// \brief Equality operator.
+    /// \param[in] _pose Pose3<T> for comparison.
+    /// \return True if this pose is equal to the given pose.
+    public: bool operator==(const Pose3<T> &_pose) const
+    {
+      return this->p == _pose.p && this->q == _pose.q;
+    }
+
+    /// \brief Inequality operator.
+    /// \param[in] _pose Pose3<T> for comparison.
+    /// \return True if this pose is not equal to the  given pose.
+    public: bool operator!=(const Pose3<T> &_pose) const
+    {
+      return this->p != _pose.p || this->q != _pose.q;
+    }
+
+    /// \brief Multiplication operator.
+    /// Given X_OP (frame P relative to O) and X_PQ (frame Q relative to P)
+    /// then X_OQ = X_OP * X_PQ (frame Q relative to O).
+    /// \param[in] _pose The pose to multiply by.
+    /// \return The resulting pose.
+    public: Pose3<T> operator*(const Pose3<T> &_pose) const
+    {
+      return Pose3<T>(_pose.CoordPositionAdd(*this),  this->q * _pose.q);
+    }
+
+    /// \brief Multiplication assignment operator. This pose will become
+    /// equal to this * _pose.
+    /// \param[in] _pose Pose3<T> to multiply to this pose
+    /// \sa operator*(const Pose3<T> &_pose) const
+    /// \return The resulting pose
+    public: const Pose3<T> &operator*=(const Pose3<T> &_pose)
+    {
+      *this = *this * _pose;
+      return *this;
+    }
+
+    /// \brief Assignment operator
+    /// \param[in] _pose Pose3<T> to copy
+    public: Pose3<T> &operator=(const Pose3<T> &_pose) = default;
+
+    /// \brief Add one point to a vector: result = this + pos.
+    /// \param[in] _pos Position to add to this pose
+    /// \return The resulting position.
+    public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
+    {
+      Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
+
+      // result = pose.q + pose.q * this->p * pose.q!
+      tmp = this->q * (tmp * this->q.Inverse());
+
+      return Vector3<T>(this->p.X() + tmp.X(),
+                        this->p.Y() + tmp.Y(),
+                        this->p.Z() + tmp.Z());
+    }
+
+    /// \brief Add one pose to another: result = this + pose.
+    /// \param[in] _pose The Pose3<T> to add.
+    /// \return The resulting position.
+    public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
+    {
+      Quaternion<T> tmp(static_cast<T>(0),
+          this->p.X(), this->p.Y(), this->p.Z());
+
+      // result = _pose.q + _pose.q * this->p * _pose.q!
+      tmp = _pose.q * (tmp * _pose.q.Inverse());
+
+      return Vector3<T>(_pose.p.X() + tmp.X(),
+                        _pose.p.Y() + tmp.Y(),
+                        _pose.p.Z() + tmp.Z());
+    }
+
+    /// \brief Subtract one position from another: result = this - pose
+    /// \param[in] _pose Pose3<T> to subtract
+    /// \return The resulting position
+    public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
+    {
+      Quaternion<T> tmp(0,
+          this->p.X() - _pose.p.X(),
+          this->p.Y() - _pose.p.Y(),
+          this->p.Z() - _pose.p.Z());
+
+      tmp = _pose.q.Inverse() * (tmp * _pose.q);
+      return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
+    }
+
+    /// \brief Add one rotation to another: result =  this->q + rot.
+    /// \param[in] _rot Rotation to add.
+    /// \return The resulting rotation.
+    public: Quaternion<T> CoordRotationAdd(const Quaternion<T> &_rot) const
+    {
+      return Quaternion<T>(_rot * this->q);
+    }
+
+    /// \brief Subtract one rotation from another: result = this->q - rot.
+    /// \param[in] _rot The rotation to subtract.
+    /// \return The resulting rotation.
+    public: inline Quaternion<T> CoordRotationSub(
+                const Quaternion<T> &_rot) const
+    {
+      Quaternion<T> result(_rot.Inverse() * this->q);
+      result.Normalize();
+      return result;
+    }
+
+    /// \brief Find the inverse of a pose; i.e., if b = this + a, given b and
+    /// this, find a.
+    /// \param[in] _b the other pose.
+    // \return The inverse pose.
+    public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
+    {
+      Quaternion<T> qt;
+      Pose3<T> a;
+
+      a.q = this->q.Inverse() * _b.q;
+      qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
+      qt = qt * a.q.Inverse();
+      a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
+
+      return a;
+    }
+
+    /// \brief Reset the pose. This sets the position to zero and the
+    /// rotation to identify.
+    public: void Reset()
+    {
+      // set the position to zero
+      this->p.Set();
+      this->q = Quaternion<T>::Identity;
+    }
+
+    /// \brief Rotate the vector part of a pose about the origin.
+    /// \param[in] _q rotation.
+    /// \return The rotated pose.
+    public: Pose3<T> RotatePositionAboutOrigin(const Quaternion<T> &_q) const
+    {
+      Pose3<T> a = *this;
+      a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
+              +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
+              +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
+      a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
+              +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
+              +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
+      a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
+              +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
+              +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
+      return a;
+    }
+
+    /// \brief Round all values to _precision decimal places.
+    /// \param[in] _precision Number of decimal places.
+    public: void Round(int _precision)
+    {
+      this->q.Round(_precision);
+      this->p.Round(_precision);
+    }
+
+    /// \brief Get the position.
+    /// \return Origin of the pose.
+    public: inline const Vector3<T> &Pos() const
+    {
+      return this->p;
+    }
+
+    /// \brief Get a mutable reference to the position.
+    /// \return Origin of the pose.
+    public: inline Vector3<T> &Pos()
+    {
+      return this->p;
+    }
+
+    /// \brief Get the X value of the position.
+    /// \return Value X of the origin of the pose.
+    /// \note The return is made by value since
+    /// Vector3<T>.X() is already a reference.
+    public: inline const T X() const
+    {
+      return this->p.X();
+    }
+
+    /// \brief Set X value of the position.
+    public: inline void SetX(T x)
+    {
+     this->p.X() = x;
+    }
+
+    /// \brief Get the Y value of the position.
+    /// \return Value Y of the origin of the pose.
+    /// \note The return is made by value since
+    /// Vector3<T>.Y() is already a reference.
+    public: inline const T Y() const
+    {
+      return this->p.Y();
+    }
+
+    /// \brief Set the Y value of the position.
+    public: inline void SetY(T y)
+    {
+      this->p.Y() = y;
+    }
+
+    /// \brief Get the Z value of the position.
+    /// \return Value Z of the origin of the pose.
+    /// \note The return is made by value since
+    /// Vector3<T>.Z() is already a reference.
+    public: inline const T Z() const
+    {
+      return this->p.Z();
+    }
+
+    /// \brief Set the Z value of the position.
+    public: inline void SetZ(T z)
+    {
+      this->p.Z() = z;
+    }
+
+    /// \brief Get the rotation.
+    /// \return Quaternion representation of the rotation.
+    public: inline const Quaternion<T> &Rot() const
+    {
+      return this->q;
+    }
+
+    /// \brief Get a mutable reference to the rotation.
+    /// \return Quaternion representation of the rotation.
+    public: inline Quaternion<T> &Rot()
+    {
+      return this->q;
+    }
+
+    /// \brief Get the Roll value of the rotation.
+    /// \return Roll value of the orientation.
+    /// \note The return is made by value since
+    ///  Quaternion<T>.Roll() is already a reference.
+    public: inline const T Roll() const
+    {
+      return this->q.Roll();
+    }
+
+    /// \brief Get the Pitch value of the rotation.
+    /// \return Pitch value of the orientation.
+    /// \note The return is made by value since
+    ///  Quaternion<T>.Pitch() is already a reference.
+    public: inline const T Pitch() const
+    {
+      return this->q.Pitch();
+    }
+
+    /// \brief Get the Yaw value of the rotation.
+    /// \return Yaw value of the orientation.
+    /// \note The return is made by value since
+    ///  Quaternion<T>.Yaw() is already a reference.
+    public: inline const T Yaw() const
+    {
+      return this->q.Yaw();
+    }
+
+    /// \brief Stream insertion operator
+    /// \param[in] _out output stream
+    /// \param[in] _pose pose to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+                std::ostream &_out, const gz::math::Pose3<T> &_pose)
+    {
+      _out << _pose.Pos() << " " << _pose.Rot();
+      return _out;
+    }
+
+    /// \brief Stream extraction operator
+    /// \param[in] _in the input stream
+    /// \param[in] _pose the pose
+    /// \return the stream
+    public: friend std::istream &operator>>(
+                std::istream &_in, gz::math::Pose3<T> &_pose)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      Vector3<T> pos;
+      Quaternion<T> rot;
+      _in >> pos >> rot;
+      _pose.Set(pos, rot);
+      return _in;
+    }
+
+    /// \brief Equality test with tolerance.
+    /// \param[in] _p The pose to compare this against. Both the position
+    /// Vector3 and rotation Quaternion are compared.
+    /// \param[in] _tol Equality tolerance.
+    /// \return True if the position and orientation of the poses are equal
+    /// within the tolerence specified by _tol.
+    public: bool Equal(const Pose3 &_p, const T &_tol) const
+    {
+      return this->p.Equal(_p.p, _tol) && this->q.Equal(_p.q, _tol);
+    }
+
+    /// \brief The position
+    private: Vector3<T> p;
+
+    /// \brief The rotation
+    private: Quaternion<T> q;
+  };
+
+  namespace detail {
+
+    template<typename T> constexpr Pose3<T> gPose3Zero{};
+
+  }  // namespace detail
+
+  template<typename T> const Pose3<T> &Pose3<T>::Zero = detail::gPose3Zero<T>;
+
+  /// typedef Pose3<double> as Pose3d.
+  typedef Pose3<double> Pose3d;
+
+  /// typedef Pose3<float> as Pose3f.
+  typedef Pose3<float> Pose3f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_POSE_HH_
diff --git a/include/gz/math/Quaternion.hh b/include/gz/math/Quaternion.hh
index 724d9afcd..ac7fb920a 100644
--- a/include/gz/math/Quaternion.hh
+++ b/include/gz/math/Quaternion.hh
@@ -23,1147 +23,1144 @@
 #include <gz/math/Matrix3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  template <typename T> class Matrix3;
+
+  /// \class Quaternion Quaternion.hh gz/math/Quaternion.hh
+  /// \brief A quaternion class that represents 3D rotations and
+  /// orientations. Four scalar values, [w,x,y,z], are used represent
+  /// orientations and rotations.
+  ///
+  /// The following two type definitions are provided:
+  ///
+  /// * \ref Quaternionf
+  /// * \ref Quaterniond
+  ///
+  /// ## Examples
+  ///
+  /// * C++
+  ///
+  /// \snippet examples/quaternion_example.cc complete
+  ///
+  /// * Ruby
+  ///
+  /// \code{.rb}
+  /// # Modify the RUBYLIB environment variable to include the Gazebo Math
+  /// # library install path. For example, if you install to /user:
+  /// #
+  /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
+  /// #
+  /// require 'gz/math'
+  ///
+  /// q = Gz::Math::Quaterniond.new
+  /// printf("A default quaternion has the following values\n"+
+  ///        "\tW=%f X=%f Y=%f Z=%f\n", q.W(), q.X(), q.Y(), q.Z())
+  ///
+  /// q = Gz::Math::Quaterniond.Identity
+  /// printf("The identity quaternion has the following values\n" +
+  ///        "\tW=%f X=%f Y=%f Z=%f\n", q.W(), q.X(), q.Y(), q.Z())
+  ///
+  /// q2 = Gz::Math::Quaterniond.new(0, 0, 3.14)
+  /// printf("A quaternion initialized from roll=0, pitch=0, and yaw=3.14 " +
+  ///        "has the following values\n" +
+  ///        "\tW=%f X=%f Y=%f Z=%f\n", q2.W(), q2.X(), q2.Y(), q2.Z())
+  ///
+  /// euler = q2.Euler()
+  /// printf("Getting back the euler angles from the quaternion\n" +
+  ///        "\troll=%f pitch=%f yaw=%f\n", euler.X(), euler.Y(), euler.Z())
+  ///
+  /// \endcode
+  template<typename T>
+  class Quaternion
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    template <typename T> class Matrix3;
-
-    /// \class Quaternion Quaternion.hh gz/math/Quaternion.hh
-    /// \brief A quaternion class that represents 3D rotations and
-    /// orientations. Four scalar values, [w,x,y,z], are used represent
-    /// orientations and rotations.
-    ///
-    /// The following two type definitions are provided:
-    ///
-    /// * \ref Quaternionf
-    /// * \ref Quaterniond
-    ///
-    /// ## Examples
-    ///
-    /// * C++
-    ///
-    /// \snippet examples/quaternion_example.cc complete
-    ///
-    /// * Ruby
-    ///
-    /// \code{.rb}
-    /// # Modify the RUBYLIB environment variable to include the Gazebo Math
-    /// # library install path. For example, if you install to /user:
-    /// #
-    /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
-    /// #
-    /// require 'gz/math'
-    ///
-    /// q = Gz::Math::Quaterniond.new
-    /// printf("A default quaternion has the following values\n"+
-    ///        "\tW=%f X=%f Y=%f Z=%f\n", q.W(), q.X(), q.Y(), q.Z())
-    ///
-    /// q = Gz::Math::Quaterniond.Identity
-    /// printf("The identity quaternion has the following values\n" +
-    ///        "\tW=%f X=%f Y=%f Z=%f\n", q.W(), q.X(), q.Y(), q.Z())
-    ///
-    /// q2 = Gz::Math::Quaterniond.new(0, 0, 3.14)
-    /// printf("A quaternion initialized from roll=0, pitch=0, and yaw=3.14 " +
-    ///        "has the following values\n" +
-    ///        "\tW=%f X=%f Y=%f Z=%f\n", q2.W(), q2.X(), q2.Y(), q2.Z())
-    ///
-    /// euler = q2.Euler()
-    /// printf("Getting back the euler angles from the quaternion\n" +
-    ///        "\troll=%f pitch=%f yaw=%f\n", euler.X(), euler.Y(), euler.Z())
-    ///
-    /// \endcode
-    template<typename T>
-    class Quaternion
-    {
-      /// \brief A Quaternion initialized to identity.
-      /// This is equivalent to math::Quaternion<T>(1, 0, 0, 0)
-      public: static const Quaternion &Identity;
+    /// \brief A Quaternion initialized to identity.
+    /// This is equivalent to math::Quaternion<T>(1, 0, 0, 0)
+    public: static const Quaternion &Identity;
 
-      /// \brief A Quaternion initialized to zero.
-      /// This is equivalent to math::Quaternion<T>(0, 0, 0, 0)
-      public: static const Quaternion &Zero;
+    /// \brief A Quaternion initialized to zero.
+    /// This is equivalent to math::Quaternion<T>(0, 0, 0, 0)
+    public: static const Quaternion &Zero;
 
-      /// \brief Default Constructor
-      public: constexpr Quaternion()
-      : qw(1), qx(0), qy(0), qz(0)
-      {
-        // quaternion not normalized, because that breaks
-        // Pose::CoordPositionAdd(...)
-      }
+    /// \brief Default Constructor
+    public: constexpr Quaternion()
+    : qw(1), qx(0), qy(0), qz(0)
+    {
+      // quaternion not normalized, because that breaks
+      // Pose::CoordPositionAdd(...)
+    }
 
-      /// \brief Constructor that initializes each value, [w, x, y, z], of
-      /// the quaternion. This constructor does not normalize the
-      /// quaternion.
-      /// \param[in] _w W param
-      /// \param[in] _x X param
-      /// \param[in] _y Y param
-      /// \param[in] _z Z param
-      public: constexpr Quaternion(const T &_w, const T &_x, const T &_y,
-                                   const T &_z)
-      : qw(_w), qx(_x), qy(_y), qz(_z)
-      {}
-
-      /// \brief Construct a Quaternion from Euler angles, in radians. This
-      /// constructor normalizes the quaternion.
-      /// \param[in] _roll  Roll radians.
-      /// \param[in] _pitch Pitch radians.
-      /// \param[in] _yaw   Yaw radians.
-      /// \sa SetFromEuler(T, T, T)
-      public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
-      {
-        this->SetFromEuler(Vector3<T>(_roll, _pitch, _yaw));
-      }
+    /// \brief Constructor that initializes each value, [w, x, y, z], of
+    /// the quaternion. This constructor does not normalize the
+    /// quaternion.
+    /// \param[in] _w W param
+    /// \param[in] _x X param
+    /// \param[in] _y Y param
+    /// \param[in] _z Z param
+    public: constexpr Quaternion(const T &_w, const T &_x, const T &_y,
+                                 const T &_z)
+    : qw(_w), qx(_x), qy(_y), qz(_z)
+    {}
+
+    /// \brief Construct a Quaternion from Euler angles, in radians. This
+    /// constructor normalizes the quaternion.
+    /// \param[in] _roll  Roll radians.
+    /// \param[in] _pitch Pitch radians.
+    /// \param[in] _yaw   Yaw radians.
+    /// \sa SetFromEuler(T, T, T)
+    public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
+    {
+      this->SetFromEuler(Vector3<T>(_roll, _pitch, _yaw));
+    }
 
-      /// \brief Constructor from an axis and angle. This constructor
-      /// normalizes the quaternion.
-      /// \param[in] _axis The rotation axis.
-      /// \param[in] _angle The rotation angle in radians.
-      public: Quaternion(const Vector3<T> &_axis, const T &_angle)
-      {
-        this->SetFromAxisAngle(_axis, _angle);
-      }
+    /// \brief Constructor from an axis and angle. This constructor
+    /// normalizes the quaternion.
+    /// \param[in] _axis The rotation axis.
+    /// \param[in] _angle The rotation angle in radians.
+    public: Quaternion(const Vector3<T> &_axis, const T &_angle)
+    {
+      this->SetFromAxisAngle(_axis, _angle);
+    }
 
-      /// \brief Construct a Quaternion from Euler angles, in radians. This
-      /// constructor normalizes the quaternion.
-      /// \param[in] _rpy Euler angles in radians.
-      public: explicit Quaternion(const Vector3<T> &_rpy)
-      {
-        this->SetFromEuler(_rpy);
-      }
+    /// \brief Construct a Quaternion from Euler angles, in radians. This
+    /// constructor normalizes the quaternion.
+    /// \param[in] _rpy Euler angles in radians.
+    public: explicit Quaternion(const Vector3<T> &_rpy)
+    {
+      this->SetFromEuler(_rpy);
+    }
 
-      /// \brief Construct from rotation matrix. This constructor does not
-      /// normalize the quaternion.
-      /// \param[in] _mat Rotation matrix (must be orthogonal, the function
-      ///                 doesn't check it)
-      public: explicit Quaternion(const Matrix3<T> &_mat)
-      {
-        this->SetFromMatrix(_mat);
-      }
+    /// \brief Construct from rotation matrix. This constructor does not
+    /// normalize the quaternion.
+    /// \param[in] _mat Rotation matrix (must be orthogonal, the function
+    ///                 doesn't check it)
+    public: explicit Quaternion(const Matrix3<T> &_mat)
+    {
+      this->SetFromMatrix(_mat);
+    }
 
-      /// \brief Copy constructor. This constructor does not normalize the
-      /// quaternion.
-      /// \param[in] _qt Quaternion<T> to copy
-      public: Quaternion(const Quaternion<T> &_qt) = default;
+    /// \brief Copy constructor. This constructor does not normalize the
+    /// quaternion.
+    /// \param[in] _qt Quaternion<T> to copy
+    public: Quaternion(const Quaternion<T> &_qt) = default;
 
-      /// \brief Destructor
-      public: ~Quaternion() = default;
+    /// \brief Destructor
+    public: ~Quaternion() = default;
 
-      /// \brief Assignment operator
-      /// \param[in] _qt Quaternion<T> to copy
-      public: Quaternion<T> &operator=(const Quaternion<T> &_qt) = default;
+    /// \brief Assignment operator
+    /// \param[in] _qt Quaternion<T> to copy
+    public: Quaternion<T> &operator=(const Quaternion<T> &_qt) = default;
 
-      /// \brief Invert the quaternion. The quaternion is first normalized,
-      /// then inverted.
-      public: void Invert()
-      {
-        this->Normalize();
-        // this->qw = this->qw;
-        this->qx = -this->qx;
-        this->qy = -this->qy;
-        this->qz = -this->qz;
-      }
+    /// \brief Invert the quaternion. The quaternion is first normalized,
+    /// then inverted.
+    public: void Invert()
+    {
+      this->Normalize();
+      // this->qw = this->qw;
+      this->qx = -this->qx;
+      this->qy = -this->qy;
+      this->qz = -this->qz;
+    }
 
-      /// \brief Get the inverse of this quaternion
-      /// \return Inverse quaternion
-      public: inline Quaternion<T> Inverse() const
-      {
-        T s = 0;
-        Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
+    /// \brief Get the inverse of this quaternion
+    /// \return Inverse quaternion
+    public: inline Quaternion<T> Inverse() const
+    {
+      T s = 0;
+      Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
 
-        // use s to test if quaternion is valid
-        s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
+      // use s to test if quaternion is valid
+      s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
 
-        if (equal<T>(s, static_cast<T>(0)))
-        {
-          q.qw = 1.0;
-          q.qx = 0.0;
-          q.qy = 0.0;
-          q.qz = 0.0;
-        }
-        else
-        {
-          // deal with non-normalized quaternion
-          // div by s so q * qinv = identity
-          q.qw =  q.qw / s;
-          q.qx = -q.qx / s;
-          q.qy = -q.qy / s;
-          q.qz = -q.qz / s;
-        }
-        return q;
+      if (equal<T>(s, static_cast<T>(0)))
+      {
+        q.qw = 1.0;
+        q.qx = 0.0;
+        q.qy = 0.0;
+        q.qz = 0.0;
       }
-
-      /// \brief Return the logarithm
-      ///
-      /// If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x, y, z) is unit length,
-      /// then log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
-      /// sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
-      ///
-      /// \return The log.
-      public: Quaternion<T> Log() const
+      else
       {
-        Quaternion<T> result;
-        result.qw = 0.0;
-
-        if (std::abs(this->qw) < 1.0)
-        {
-          T fAngle = acos(this->qw);
-          T fSin = sin(fAngle);
-          if (std::abs(fSin) >= 1e-3)
-          {
-            T fCoeff = fAngle/fSin;
-            result.qx = fCoeff*this->qx;
-            result.qy = fCoeff*this->qy;
-            result.qz = fCoeff*this->qz;
-            return result;
-          }
-        }
-
-        result.qx = this->qx;
-        result.qy = this->qy;
-        result.qz = this->qz;
-
-        return result;
+        // deal with non-normalized quaternion
+        // div by s so q * qinv = identity
+        q.qw =  q.qw / s;
+        q.qx = -q.qx / s;
+        q.qy = -q.qy / s;
+        q.qz = -q.qz / s;
       }
+      return q;
+    }
+
+    /// \brief Return the logarithm
+    ///
+    /// If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x, y, z) is unit length,
+    /// then log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
+    /// sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
+    ///
+    /// \return The log.
+    public: Quaternion<T> Log() const
+    {
+      Quaternion<T> result;
+      result.qw = 0.0;
 
-      /// \brief Return the exponent.
-      ///
-      /// If q = A*(x*i+y*j+z*k) where (x, y, z) is unit length, then
-      /// exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
-      /// use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
-      ///
-      /// \return The exponent.
-      public: Quaternion<T> Exp() const
+      if (std::abs(this->qw) < 1.0)
       {
-        T fAngle = sqrt(this->qx*this->qx+
-            this->qy*this->qy+this->qz*this->qz);
+        T fAngle = acos(this->qw);
         T fSin = sin(fAngle);
-
-        Quaternion<T> result;
-        result.qw = cos(fAngle);
-
         if (std::abs(fSin) >= 1e-3)
         {
-          T fCoeff = fSin/fAngle;
+          T fCoeff = fAngle/fSin;
           result.qx = fCoeff*this->qx;
           result.qy = fCoeff*this->qy;
           result.qz = fCoeff*this->qz;
+          return result;
         }
-        else
-        {
-          result.qx = this->qx;
-          result.qy = this->qy;
-          result.qz = this->qz;
-        }
-
-        return result;
       }
 
-      /// \brief Normalize the quaternion.
-      public: void Normalize()
-      {
-        T s = 0;
+      result.qx = this->qx;
+      result.qy = this->qy;
+      result.qz = this->qz;
 
-        s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
-            this->qy * this->qy + this->qz * this->qz));
+      return result;
+    }
 
-        if (equal<T>(s, static_cast<T>(0)))
-        {
-          this->qw = T(1.0);
-          this->qx = T(0.0);
-          this->qy = T(0.0);
-          this->qz = T(0.0);
-        }
-        else
-        {
-          this->qw /= s;
-          this->qx /= s;
-          this->qy /= s;
-          this->qz /= s;
-        }
-      }
+    /// \brief Return the exponent.
+    ///
+    /// If q = A*(x*i+y*j+z*k) where (x, y, z) is unit length, then
+    /// exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
+    /// use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
+    ///
+    /// \return The exponent.
+    public: Quaternion<T> Exp() const
+    {
+      T fAngle = sqrt(this->qx*this->qx+
+          this->qy*this->qy+this->qz*this->qz);
+      T fSin = sin(fAngle);
 
-      /// \brief Gets a normalized version of this quaternion
-      /// \return a normalized quaternion
-      public: Quaternion<T> Normalized() const
+      Quaternion<T> result;
+      result.qw = cos(fAngle);
+
+      if (std::abs(fSin) >= 1e-3)
       {
-        Quaternion<T> result = *this;
-        result.Normalize();
-        return result;
+        T fCoeff = fSin/fAngle;
+        result.qx = fCoeff*this->qx;
+        result.qy = fCoeff*this->qy;
+        result.qz = fCoeff*this->qz;
       }
-
-      /// \brief Set the quaternion from an axis and angle.
-      /// \param[in] _ax X axis
-      /// \param[in] _ay Y axis
-      /// \param[in] _az Z axis
-      /// \param[in] _aa Angle in radians
-      public: void SetFromAxisAngle(T _ax, T _ay, T _az, T _aa)
+      else
       {
-        T l;
+        result.qx = this->qx;
+        result.qy = this->qy;
+        result.qz = this->qz;
+      }
 
-        l = _ax * _ax + _ay * _ay + _az * _az;
+      return result;
+    }
 
-        if (equal<T>(l, static_cast<T>(0)))
-        {
-          this->qw = 1;
-          this->qx = 0;
-          this->qy = 0;
-          this->qz = 0;
-        }
-        else
-        {
-          _aa *= 0.5;
-          l = sin(_aa) / sqrt(l);
-          this->qw = cos(_aa);
-          this->qx = _ax * l;
-          this->qy = _ay * l;
-          this->qz = _az * l;
-        }
+    /// \brief Normalize the quaternion.
+    public: void Normalize()
+    {
+      T s = 0;
 
-        this->Normalize();
-      }
+      s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
+          this->qy * this->qy + this->qz * this->qz));
 
-      /// \brief Set the quaternion from an axis and angle
-      /// \param[in] _axis Axis
-      /// \param[in] _a Angle in radians
-      public: void SetFromAxisAngle(const Vector3<T> &_axis, T _a)
+      if (equal<T>(s, static_cast<T>(0)))
       {
-        this->SetFromAxisAngle(_axis.X(), _axis.Y(), _axis.Z(), _a);
+        this->qw = T(1.0);
+        this->qx = T(0.0);
+        this->qy = T(0.0);
+        this->qz = T(0.0);
       }
-
-      /// \brief Set this quaternion from 4 floating numbers
-      /// \param[in] _w w
-      /// \param[in] _x x
-      /// \param[in] _y y
-      /// \param[in] _z z
-      public: void Set(T _w, T _x, T _y, T _z)
+      else
       {
-        this->qw = _w;
-        this->qx = _x;
-        this->qy = _y;
-        this->qz = _z;
+        this->qw /= s;
+        this->qx /= s;
+        this->qy /= s;
+        this->qz /= s;
       }
+    }
+
+    /// \brief Gets a normalized version of this quaternion
+    /// \return a normalized quaternion
+    public: Quaternion<T> Normalized() const
+    {
+      Quaternion<T> result = *this;
+      result.Normalize();
+      return result;
+    }
 
-      /// \brief Set the quaternion from Euler angles. The order of operations
-      /// is roll, pitch, yaw around a fixed body frame axis
-      /// (the original frame of the object before rotation is applied).
-      /// Roll is a rotation about x, pitch is about y, yaw is about z.
-      /// \param[in] _vec Euler angles in radians.
-      public: void SetFromEuler(const Vector3<T> &_vec)
+    /// \brief Set the quaternion from an axis and angle.
+    /// \param[in] _ax X axis
+    /// \param[in] _ay Y axis
+    /// \param[in] _az Z axis
+    /// \param[in] _aa Angle in radians
+    public: void SetFromAxisAngle(T _ax, T _ay, T _az, T _aa)
+    {
+      T l;
+
+      l = _ax * _ax + _ay * _ay + _az * _az;
+
+      if (equal<T>(l, static_cast<T>(0)))
       {
-        this->SetFromEuler(_vec.X(), _vec.Y(), _vec.Z());
+        this->qw = 1;
+        this->qx = 0;
+        this->qy = 0;
+        this->qz = 0;
       }
-
-      /// \brief Set the quaternion from Euler angles.
-      /// \param[in] _roll Roll angle in radians.
-      /// \param[in] _pitch Pitch angle in radians.
-      /// \param[in] _yaw Yaw angle in radians.
-      public: void SetFromEuler(T _roll, T _pitch, T _yaw)
+      else
       {
-        T phi, the, psi;
-
-        phi = _roll / T(2.0);
-        the = _pitch / T(2.0);
-        psi = _yaw / T(2.0);
+        _aa *= 0.5;
+        l = sin(_aa) / sqrt(l);
+        this->qw = cos(_aa);
+        this->qx = _ax * l;
+        this->qy = _ay * l;
+        this->qz = _az * l;
+      }
 
-        this->qw = T(cos(phi) * cos(the) * cos(psi) +
-          sin(phi) * sin(the) * sin(psi));
-        this->qx = T(sin(phi) * cos(the) * cos(psi) -
-          cos(phi) * sin(the) * sin(psi));
-        this->qy = T(cos(phi) * sin(the) * cos(psi) +
-          sin(phi) * cos(the) * sin(psi));
-        this->qz = T(cos(phi) * cos(the) * sin(psi) -
-          sin(phi) * sin(the) * cos(psi));
+      this->Normalize();
+    }
 
-        this->Normalize();
-      }
+    /// \brief Set the quaternion from an axis and angle
+    /// \param[in] _axis Axis
+    /// \param[in] _a Angle in radians
+    public: void SetFromAxisAngle(const Vector3<T> &_axis, T _a)
+    {
+      this->SetFromAxisAngle(_axis.X(), _axis.Y(), _axis.Z(), _a);
+    }
 
-      /// \brief Return the rotation in Euler angles, in radians.
-      /// \return This quaternion as Euler angles.
-      public: Vector3<T> Euler() const
-      {
-        Vector3<T> vec;
+    /// \brief Set this quaternion from 4 floating numbers
+    /// \param[in] _w w
+    /// \param[in] _x x
+    /// \param[in] _y y
+    /// \param[in] _z z
+    public: void Set(T _w, T _x, T _y, T _z)
+    {
+      this->qw = _w;
+      this->qx = _x;
+      this->qy = _y;
+      this->qz = _z;
+    }
 
-        T tol = static_cast<T>(1e-15);
+    /// \brief Set the quaternion from Euler angles. The order of operations
+    /// is roll, pitch, yaw around a fixed body frame axis
+    /// (the original frame of the object before rotation is applied).
+    /// Roll is a rotation about x, pitch is about y, yaw is about z.
+    /// \param[in] _vec Euler angles in radians.
+    public: void SetFromEuler(const Vector3<T> &_vec)
+    {
+      this->SetFromEuler(_vec.X(), _vec.Y(), _vec.Z());
+    }
 
-        Quaternion<T> copy = *this;
-        T squ;
-        T sqx;
-        T sqy;
-        T sqz;
+    /// \brief Set the quaternion from Euler angles.
+    /// \param[in] _roll Roll angle in radians.
+    /// \param[in] _pitch Pitch angle in radians.
+    /// \param[in] _yaw Yaw angle in radians.
+    public: void SetFromEuler(T _roll, T _pitch, T _yaw)
+    {
+      T phi, the, psi;
+
+      phi = _roll / T(2.0);
+      the = _pitch / T(2.0);
+      psi = _yaw / T(2.0);
+
+      this->qw = T(cos(phi) * cos(the) * cos(psi) +
+        sin(phi) * sin(the) * sin(psi));
+      this->qx = T(sin(phi) * cos(the) * cos(psi) -
+        cos(phi) * sin(the) * sin(psi));
+      this->qy = T(cos(phi) * sin(the) * cos(psi) +
+        sin(phi) * cos(the) * sin(psi));
+      this->qz = T(cos(phi) * cos(the) * sin(psi) -
+        sin(phi) * sin(the) * cos(psi));
+
+      this->Normalize();
+    }
 
-        copy.Normalize();
+    /// \brief Return the rotation in Euler angles, in radians.
+    /// \return This quaternion as Euler angles.
+    public: Vector3<T> Euler() const
+    {
+      Vector3<T> vec;
 
-        squ = copy.qw * copy.qw;
-        sqx = copy.qx * copy.qx;
-        sqy = copy.qy * copy.qy;
-        sqz = copy.qz * copy.qz;
+      T tol = static_cast<T>(1e-15);
 
-        // Pitch
-        T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
-        if (sarg <= T(-1.0))
-        {
-          vec.Y(T(-0.5*GZ_PI));
-        }
-        else if (sarg >= T(1.0))
-        {
-          vec.Y(T(0.5*GZ_PI));
-        }
-        else
-        {
-          vec.Y(T(asin(sarg)));
-        }
+      Quaternion<T> copy = *this;
+      T squ;
+      T sqx;
+      T sqy;
+      T sqz;
 
-        // If the pitch angle is PI/2 or -PI/2, we can only compute
-        // the sum roll + yaw.  However, any combination that gives
-        // the right sum will produce the correct orientation, so we
-        // set yaw = 0 and compute roll.
-        // pitch angle is PI/2
-        if (std::abs(sarg - 1) < tol)
-        {
-          vec.Z(0);
-          vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
-                      squ - sqx + sqy - sqz)));
-        }
-        // pitch angle is -PI/2
-        else if (std::abs(sarg + 1) < tol)
-        {
-          vec.Z(0);
-          vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
-                       squ - sqx + sqy - sqz)));
-        }
-        else
-        {
-          // Roll
-          vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
-                      squ - sqx - sqy + sqz)));
+      copy.Normalize();
 
-          // Yaw
-          vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
-                      squ + sqx - sqy - sqz)));
-        }
+      squ = copy.qw * copy.qw;
+      sqx = copy.qx * copy.qx;
+      sqy = copy.qy * copy.qy;
+      sqz = copy.qz * copy.qz;
 
-        return vec;
+      // Pitch
+      T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
+      if (sarg <= T(-1.0))
+      {
+        vec.Y(T(-0.5*GZ_PI));
       }
-
-      /// \brief Convert Euler angles to a quaternion.
-      /// \param[in] _vec The vector of angles, in radians, to convert.
-      /// \return The resulting quaternion
-      public: static Quaternion<T> EulerToQuaternion(const Vector3<T> &_vec)
+      else if (sarg >= T(1.0))
       {
-        Quaternion<T> result;
-        result.SetFromEuler(_vec);
-        return result;
+        vec.Y(T(0.5*GZ_PI));
       }
-
-      /// \brief Convert Euler angles, in radians, to a quaternion.
-      /// \param[in] _x rotation along x in radians
-      /// \param[in] _y rotation along y in radians
-      /// \param[in] _z rotation along z in radians
-      /// \return The resulting quaternion.
-      public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
+      else
       {
-        return EulerToQuaternion(Vector3<T>(_x, _y, _z));
+        vec.Y(T(asin(sarg)));
       }
 
-      /// \brief Get the Euler roll angle in radians.
-      /// \return The roll component.
-      public: T Roll() const
+      // If the pitch angle is PI/2 or -PI/2, we can only compute
+      // the sum roll + yaw.  However, any combination that gives
+      // the right sum will produce the correct orientation, so we
+      // set yaw = 0 and compute roll.
+      // pitch angle is PI/2
+      if (std::abs(sarg - 1) < tol)
       {
-        return this->Euler().X();
+        vec.Z(0);
+        vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
+                    squ - sqx + sqy - sqz)));
       }
-
-      /// \brief Get the Euler pitch angle in radians.
-      /// \return The pitch component.
-      public: T Pitch() const
+      // pitch angle is -PI/2
+      else if (std::abs(sarg + 1) < tol)
       {
-        return this->Euler().Y();
+        vec.Z(0);
+        vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
+                     squ - sqx + sqy - sqz)));
       }
-
-      /// \brief Get the Euler yaw angle in radians.
-      /// \return The yaw component.
-      public: T Yaw() const
+      else
       {
-        return this->Euler().Z();
+        // Roll
+        vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
+                    squ - sqx - sqy + sqz)));
+
+        // Yaw
+        vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
+                    squ + sqx - sqy - sqz)));
       }
 
-      /// \brief Convert this quaternion to an axis and angle.
-      /// \param[out] _axis Rotation axis.
-      /// \param[out] _angle CCW angle in radians.
-      public: void AxisAngle(Vector3<T> &_axis, T &_angle) const
+      return vec;
+    }
+
+    /// \brief Convert Euler angles to a quaternion.
+    /// \param[in] _vec The vector of angles, in radians, to convert.
+    /// \return The resulting quaternion
+    public: static Quaternion<T> EulerToQuaternion(const Vector3<T> &_vec)
+    {
+      Quaternion<T> result;
+      result.SetFromEuler(_vec);
+      return result;
+    }
+
+    /// \brief Convert Euler angles, in radians, to a quaternion.
+    /// \param[in] _x rotation along x in radians
+    /// \param[in] _y rotation along y in radians
+    /// \param[in] _z rotation along z in radians
+    /// \return The resulting quaternion.
+    public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
+    {
+      return EulerToQuaternion(Vector3<T>(_x, _y, _z));
+    }
+
+    /// \brief Get the Euler roll angle in radians.
+    /// \return The roll component.
+    public: T Roll() const
+    {
+      return this->Euler().X();
+    }
+
+    /// \brief Get the Euler pitch angle in radians.
+    /// \return The pitch component.
+    public: T Pitch() const
+    {
+      return this->Euler().Y();
+    }
+
+    /// \brief Get the Euler yaw angle in radians.
+    /// \return The yaw component.
+    public: T Yaw() const
+    {
+      return this->Euler().Z();
+    }
+
+    /// \brief Convert this quaternion to an axis and angle.
+    /// \param[out] _axis Rotation axis.
+    /// \param[out] _angle CCW angle in radians.
+    public: void AxisAngle(Vector3<T> &_axis, T &_angle) const
+    {
+      T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
+      if (equal<T>(len, static_cast<T>(0)))
       {
-        T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
-        if (equal<T>(len, static_cast<T>(0)))
-        {
-          _angle = 0.0;
-          _axis.Set(1, 0, 0);
-        }
-        else
-        {
-          _angle = 2.0 * acos(this->qw);
-          T invLen =  1.0 / sqrt(len);
-          _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
-        }
+        _angle = 0.0;
+        _axis.Set(1, 0, 0);
+      }
+      else
+      {
+        _angle = 2.0 * acos(this->qw);
+        T invLen =  1.0 / sqrt(len);
+        _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
       }
+    }
 
-      /// \brief Set from a rotation matrix.
-      /// \param[in] _mat Rotation matrix (must be orthogonal, the function
-      ///                 doesn't check it).
-      ///
-      /// Implementation inspired by
-      /// http://www.euclideanspace.com/maths/geometry/rotations/
-      /// conversions/matrixToQuaternion/
-      public: void SetFromMatrix(const Matrix3<T> &_mat)
+    /// \brief Set from a rotation matrix.
+    /// \param[in] _mat Rotation matrix (must be orthogonal, the function
+    ///                 doesn't check it).
+    ///
+    /// Implementation inspired by
+    /// http://www.euclideanspace.com/maths/geometry/rotations/
+    /// conversions/matrixToQuaternion/
+    public: void SetFromMatrix(const Matrix3<T> &_mat)
+    {
+      const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
+      if (trace > 0.0000001)
       {
-        const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
-        if (trace > 0.0000001)
-        {
-          qw = sqrt(1 + trace) / 2;
-          const T s = 1.0 / (4 * qw);
-          qx = (_mat(2, 1) - _mat(1, 2)) * s;
-          qy = (_mat(0, 2) - _mat(2, 0)) * s;
-          qz = (_mat(1, 0) - _mat(0, 1)) * s;
-        }
-        else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
-        {
-          qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
-          const T s = 1.0 / (4 * qx);
-          qw = (_mat(2, 1) - _mat(1, 2)) * s;
-          qy = (_mat(1, 0) + _mat(0, 1)) * s;
-          qz = (_mat(0, 2) + _mat(2, 0)) * s;
-        }
-        else if (_mat(1, 1) > _mat(2, 2))
-        {
-          qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
-          const T s = 1.0 / (4 * qy);
-          qw = (_mat(0, 2) - _mat(2, 0)) * s;
-          qx = (_mat(0, 1) + _mat(1, 0)) * s;
-          qz = (_mat(1, 2) + _mat(2, 1)) * s;
-        }
-        else
-        {
-          qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
-          const T s = 1.0 / (4 * qz);
-          qw = (_mat(1, 0) - _mat(0, 1)) * s;
-          qx = (_mat(0, 2) + _mat(2, 0)) * s;
-          qy = (_mat(1, 2) + _mat(2, 1)) * s;
-        }
+        qw = sqrt(1 + trace) / 2;
+        const T s = 1.0 / (4 * qw);
+        qx = (_mat(2, 1) - _mat(1, 2)) * s;
+        qy = (_mat(0, 2) - _mat(2, 0)) * s;
+        qz = (_mat(1, 0) - _mat(0, 1)) * s;
       }
+      else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
+      {
+        qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
+        const T s = 1.0 / (4 * qx);
+        qw = (_mat(2, 1) - _mat(1, 2)) * s;
+        qy = (_mat(1, 0) + _mat(0, 1)) * s;
+        qz = (_mat(0, 2) + _mat(2, 0)) * s;
+      }
+      else if (_mat(1, 1) > _mat(2, 2))
+      {
+        qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
+        const T s = 1.0 / (4 * qy);
+        qw = (_mat(0, 2) - _mat(2, 0)) * s;
+        qx = (_mat(0, 1) + _mat(1, 0)) * s;
+        qz = (_mat(1, 2) + _mat(2, 1)) * s;
+      }
+      else
+      {
+        qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
+        const T s = 1.0 / (4 * qz);
+        qw = (_mat(1, 0) - _mat(0, 1)) * s;
+        qx = (_mat(0, 2) + _mat(2, 0)) * s;
+        qy = (_mat(1, 2) + _mat(2, 1)) * s;
+      }
+    }
 
-      /// \brief Set this quaternion to represent rotation from
-      /// vector _v1 to vector _v2, so that
-      /// _v2.Normalize() == this * _v1.Normalize() holds.
-      ///
-      /// \param[in] _v1 The first vector.
-      /// \param[in] _v2 The second vector.
-      ///
-      /// Implementation inspired by
-      /// http://stackoverflow.com/a/11741520/1076564
-      public: void SetFrom2Axes(const Vector3<T> &_v1,
-                  const Vector3<T> &_v2)
+    /// \brief Set this quaternion to represent rotation from
+    /// vector _v1 to vector _v2, so that
+    /// _v2.Normalize() == this * _v1.Normalize() holds.
+    ///
+    /// \param[in] _v1 The first vector.
+    /// \param[in] _v2 The second vector.
+    ///
+    /// Implementation inspired by
+    /// http://stackoverflow.com/a/11741520/1076564
+    public: void SetFrom2Axes(const Vector3<T> &_v1,
+                const Vector3<T> &_v2)
+    {
+      // generally, we utilize the fact that a quat (w, x, y, z) represents
+      // rotation of angle 2*w about axis (x, y, z)
+      //
+      // so we want to take get a vector half-way between no rotation and the
+      // double rotation, which is
+      // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
+      // if _v1 and _v2 are unit quaternions
+      //
+      // since we normalize the result anyway, we can omit the division,
+      // getting the result:
+      // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
+      //
+      // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
+      // is multiplied by k = norm(_v1)*norm(_v2)
+
+      const T kCosTheta = _v1.Dot(_v2);
+      const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
+
+      if (fabs(kCosTheta/k + 1) < 1e-6)
       {
-        // generally, we utilize the fact that a quat (w, x, y, z) represents
-        // rotation of angle 2*w about axis (x, y, z)
-        //
-        // so we want to take get a vector half-way between no rotation and the
-        // double rotation, which is
-        // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
-        // if _v1 and _v2 are unit quaternions
-        //
-        // since we normalize the result anyway, we can omit the division,
-        // getting the result:
-        // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
-        //
-        // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
-        // is multiplied by k = norm(_v1)*norm(_v2)
-
-        const T kCosTheta = _v1.Dot(_v2);
-        const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
-
-        if (fabs(kCosTheta/k + 1) < 1e-6)
+        // the vectors are opposite
+        // any vector orthogonal to _v1
+        Vector3<T> other;
         {
-          // the vectors are opposite
-          // any vector orthogonal to _v1
-          Vector3<T> other;
+          const Vector3<T> _v1Abs(_v1.Abs());
+          if (_v1Abs.X() < _v1Abs.Y())
           {
-            const Vector3<T> _v1Abs(_v1.Abs());
-            if (_v1Abs.X() < _v1Abs.Y())
+            if (_v1Abs.X() < _v1Abs.Z())
             {
-              if (_v1Abs.X() < _v1Abs.Z())
-              {
-                other.Set(1, 0, 0);
-              }
-              else
-              {
-                other.Set(0, 0, 1);
-              }
+              other.Set(1, 0, 0);
             }
             else
             {
-              if (_v1Abs.Y() < _v1Abs.Z())
-              {
-                other.Set(0, 1, 0);
-              }
-              else
-              {
-                other.Set(0, 0, 1);
-              }
+              other.Set(0, 0, 1);
+            }
+          }
+          else
+          {
+            if (_v1Abs.Y() < _v1Abs.Z())
+            {
+              other.Set(0, 1, 0);
+            }
+            else
+            {
+              other.Set(0, 0, 1);
             }
           }
-
-          const Vector3<T> axis(_v1.Cross(other).Normalize());
-
-          qw = 0;
-          qx = axis.X();
-          qy = axis.Y();
-          qz = axis.Z();
-        }
-        else
-        {
-          // the vectors are in general position
-          const Vector3<T> axis(_v1.Cross(_v2));
-          qw = kCosTheta + k;
-          qx = axis.X();
-          qy = axis.Y();
-          qz = axis.Z();
-          this->Normalize();
         }
-      }
 
-      /// \brief Scale this quaternion.
-      /// \param[in] _scale Amount to scale this quaternion
-      public: void Scale(T _scale)
-      {
-        Vector3<T> axis;
-        T angle;
+        const Vector3<T> axis(_v1.Cross(other).Normalize());
 
-        // Convert to axis-and-angle
-        this->AxisAngle(axis, angle);
-        angle *= _scale;
-
-        this->SetFromAxisAngle(axis.X(), axis.Y(), axis.Z(), angle);
+        qw = 0;
+        qx = axis.X();
+        qy = axis.Y();
+        qz = axis.Z();
       }
-
-      /// \brief Addition operator.
-      /// \param[in] _qt Quaternion for addition.
-      /// \return This quaternion + _qt.
-      public: Quaternion<T> operator+(const Quaternion<T> &_qt) const
+      else
       {
-        Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
-                             this->qy + _qt.qy, this->qz + _qt.qz);
-        return result;
+        // the vectors are in general position
+        const Vector3<T> axis(_v1.Cross(_v2));
+        qw = kCosTheta + k;
+        qx = axis.X();
+        qy = axis.Y();
+        qz = axis.Z();
+        this->Normalize();
       }
+    }
 
-      /// \brief Addition set operator.
-      /// \param[in] _qt Quaternion for addition.
-      /// \return This quaternion + qt.
-      public: Quaternion<T> operator+=(const Quaternion<T> &_qt)
-      {
-        *this = *this + _qt;
+    /// \brief Scale this quaternion.
+    /// \param[in] _scale Amount to scale this quaternion
+    public: void Scale(T _scale)
+    {
+      Vector3<T> axis;
+      T angle;
 
-        return *this;
-      }
+      // Convert to axis-and-angle
+      this->AxisAngle(axis, angle);
+      angle *= _scale;
 
-      /// \brief Subtraction operator.
-      /// \param[in] _qt Quaternion to subtract.
-      /// \return This quaternion - _qt
-      public: Quaternion<T> operator-(const Quaternion<T> &_qt) const
-      {
-        Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
-                       this->qy - _qt.qy, this->qz - _qt.qz);
-        return result;
-      }
+      this->SetFromAxisAngle(axis.X(), axis.Y(), axis.Z(), angle);
+    }
 
-      /// \brief Subtraction set operator.
-      /// \param[in] _qt Quaternion for subtraction.
-      /// \return This quaternion - qt.
-      public: Quaternion<T> operator-=(const Quaternion<T> &_qt)
-      {
-        *this = *this - _qt;
-        return *this;
-      }
+    /// \brief Addition operator.
+    /// \param[in] _qt Quaternion for addition.
+    /// \return This quaternion + _qt.
+    public: Quaternion<T> operator+(const Quaternion<T> &_qt) const
+    {
+      Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
+                           this->qy + _qt.qy, this->qz + _qt.qz);
+      return result;
+    }
 
-      /// \brief Multiplication operator.
-      /// \param[in] _q Quaternion for multiplication.
-      /// \return This quaternion multiplied by the parameter.
-      public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
-              {
-                return Quaternion<T>(
-                  this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
-                  this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
-                  this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
-                  this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
-              }
-
-      /// \brief Multiplication operator by a scalar.
-      /// \param[in] _f Factor.
-      /// \return Quaternion multiplied by the scalar.
-      public: Quaternion<T> operator*(const T &_f) const
-      {
-        return Quaternion<T>(this->qw*_f, this->qx*_f,
-                             this->qy*_f, this->qz*_f);
-      }
+    /// \brief Addition set operator.
+    /// \param[in] _qt Quaternion for addition.
+    /// \return This quaternion + qt.
+    public: Quaternion<T> operator+=(const Quaternion<T> &_qt)
+    {
+      *this = *this + _qt;
 
-      /// \brief Multiplication set operator.
-      /// \param[in] _qt Quaternion<T> for multiplication.
-      /// \return This quaternion multiplied by the parameter.
-      public: Quaternion<T> operator*=(const Quaternion<T> &_qt)
-      {
-        *this = *this * _qt;
-        return *this;
-      }
+      return *this;
+    }
 
-      /// \brief Vector3 multiplication operator.
-      /// \param[in] _v vector to multiply.
-      /// \return The result of the vector multiplication.
-      public: Vector3<T> operator*(const Vector3<T> &_v) const
-      {
-        Vector3<T> uv, uuv;
-        Vector3<T> qvec(this->qx, this->qy, this->qz);
-        uv = qvec.Cross(_v);
-        uuv = qvec.Cross(uv);
-        uv *= (2.0f * this->qw);
-        uuv *= 2.0f;
-
-        return _v + uv + uuv;
-      }
+    /// \brief Subtraction operator.
+    /// \param[in] _qt Quaternion to subtract.
+    /// \return This quaternion - _qt
+    public: Quaternion<T> operator-(const Quaternion<T> &_qt) const
+    {
+      Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
+                     this->qy - _qt.qy, this->qz - _qt.qz);
+      return result;
+    }
 
-      /// \brief Equality comparison operator. A tolerance of 0.001 is used
-      /// with the gz::math::equal function for each component of the
-      /// quaternions.
-      /// \param[in] _qt Quaternion<T> for comparison.
-      /// \return True if each component of both quaternions is within the
-      /// tolerance of 0.001 of its counterpart.
-      public: bool operator==(const Quaternion<T> &_qt) const
-      {
-        return this->Equal(_qt, static_cast<T>(0.001));
-      }
+    /// \brief Subtraction set operator.
+    /// \param[in] _qt Quaternion for subtraction.
+    /// \return This quaternion - qt.
+    public: Quaternion<T> operator-=(const Quaternion<T> &_qt)
+    {
+      *this = *this - _qt;
+      return *this;
+    }
 
-      /// \brief Not equal to operator. A tolerance of 0.001 is used
-      /// with the gz::math::equal function for each component of the
-      /// quaternions.
-      /// \param[in] _qt Quaternion for comparison.
-      /// \return True if any component of both quaternions is not within
-      /// the tolerance of 0.001 of its counterpart.
-      public: bool operator!=(const Quaternion<T> &_qt) const
-      {
-        return !(*this == _qt);
-      }
+    /// \brief Multiplication operator.
+    /// \param[in] _q Quaternion for multiplication.
+    /// \return This quaternion multiplied by the parameter.
+    public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
+            {
+              return Quaternion<T>(
+                this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
+                this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
+                this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
+                this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
+            }
 
-      /// \brief Unary minus operator.
-      /// \return Negation of each component of this quaternion.
-      public: Quaternion<T> operator-() const
-      {
-        return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
-      }
+    /// \brief Multiplication operator by a scalar.
+    /// \param[in] _f Factor.
+    /// \return Quaternion multiplied by the scalar.
+    public: Quaternion<T> operator*(const T &_f) const
+    {
+      return Quaternion<T>(this->qw*_f, this->qx*_f,
+                           this->qy*_f, this->qz*_f);
+    }
 
-      /// \brief Rotate a vector using the quaternion.
-      /// \param[in] _vec Vector to rotate.
-      /// \return The rotated vector.
-      public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
-      {
-        Quaternion<T> tmp(static_cast<T>(0),
-            _vec.X(), _vec.Y(), _vec.Z());
-        tmp = (*this) * (tmp * this->Inverse());
-        return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
-      }
+    /// \brief Multiplication set operator.
+    /// \param[in] _qt Quaternion<T> for multiplication.
+    /// \return This quaternion multiplied by the parameter.
+    public: Quaternion<T> operator*=(const Quaternion<T> &_qt)
+    {
+      *this = *this * _qt;
+      return *this;
+    }
 
-      /// \brief Get the reverse rotation of a vector by this quaternion.
-      /// \param[in] _vec The vector.
-      /// \return The reversed vector.
-      public: Vector3<T> RotateVectorReverse(const Vector3<T> &_vec) const
-      {
-        Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
+    /// \brief Vector3 multiplication operator.
+    /// \param[in] _v vector to multiply.
+    /// \return The result of the vector multiplication.
+    public: Vector3<T> operator*(const Vector3<T> &_v) const
+    {
+      Vector3<T> uv, uuv;
+      Vector3<T> qvec(this->qx, this->qy, this->qz);
+      uv = qvec.Cross(_v);
+      uuv = qvec.Cross(uv);
+      uv *= (2.0f * this->qw);
+      uuv *= 2.0f;
+
+      return _v + uv + uuv;
+    }
 
-        tmp =  this->Inverse() * (tmp * (*this));
+    /// \brief Equality comparison operator. A tolerance of 0.001 is used
+    /// with the gz::math::equal function for each component of the
+    /// quaternions.
+    /// \param[in] _qt Quaternion<T> for comparison.
+    /// \return True if each component of both quaternions is within the
+    /// tolerance of 0.001 of its counterpart.
+    public: bool operator==(const Quaternion<T> &_qt) const
+    {
+      return this->Equal(_qt, static_cast<T>(0.001));
+    }
 
-        return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
-      }
+    /// \brief Not equal to operator. A tolerance of 0.001 is used
+    /// with the gz::math::equal function for each component of the
+    /// quaternions.
+    /// \param[in] _qt Quaternion for comparison.
+    /// \return True if any component of both quaternions is not within
+    /// the tolerance of 0.001 of its counterpart.
+    public: bool operator!=(const Quaternion<T> &_qt) const
+    {
+      return !(*this == _qt);
+    }
 
-      /// \brief See if a quaternion is finite (e.g., not nan).
-      /// \return True if quaternion is finite.
-      public: bool IsFinite() const
-      {
-        // std::isfinite works with floating point values, need to explicit
-        // cast to avoid ambiguity in vc++.
-        return std::isfinite(static_cast<double>(this->qw)) &&
-               std::isfinite(static_cast<double>(this->qx)) &&
-               std::isfinite(static_cast<double>(this->qy)) &&
-               std::isfinite(static_cast<double>(this->qz));
-      }
+    /// \brief Unary minus operator.
+    /// \return Negation of each component of this quaternion.
+    public: Quaternion<T> operator-() const
+    {
+      return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
+    }
 
-      /// \brief Correct any nan values in this quaternion.
-      public: inline void Correct()
-      {
-        // std::isfinite works with floating point values, need to explicit
-        // cast to avoid ambiguity in vc++.
-        if (!std::isfinite(static_cast<double>(this->qx)))
-          this->qx = 0;
-        if (!std::isfinite(static_cast<double>(this->qy)))
-          this->qy = 0;
-        if (!std::isfinite(static_cast<double>(this->qz)))
-          this->qz = 0;
-        if (!std::isfinite(static_cast<double>(this->qw)))
-          this->qw = 1;
-
-        if (equal(this->qw, static_cast<T>(0)) &&
-            equal(this->qx, static_cast<T>(0)) &&
-            equal(this->qy, static_cast<T>(0)) &&
-            equal(this->qz, static_cast<T>(0)))
-        {
-          this->qw = 1;
-        }
-      }
+    /// \brief Rotate a vector using the quaternion.
+    /// \param[in] _vec Vector to rotate.
+    /// \return The rotated vector.
+    public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
+    {
+      Quaternion<T> tmp(static_cast<T>(0),
+          _vec.X(), _vec.Y(), _vec.Z());
+      tmp = (*this) * (tmp * this->Inverse());
+      return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
+    }
 
-      /// \brief Return the X axis.
-      /// \return the X axis of the vector.
-      public: Vector3<T> XAxis() const
-      {
-        T fTy  = 2.0f*this->qy;
-        T fTz  = 2.0f*this->qz;
+    /// \brief Get the reverse rotation of a vector by this quaternion.
+    /// \param[in] _vec The vector.
+    /// \return The reversed vector.
+    public: Vector3<T> RotateVectorReverse(const Vector3<T> &_vec) const
+    {
+      Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
 
-        T fTwy = fTy*this->qw;
-        T fTwz = fTz*this->qw;
-        T fTxy = fTy*this->qx;
-        T fTxz = fTz*this->qx;
-        T fTyy = fTy*this->qy;
-        T fTzz = fTz*this->qz;
+      tmp =  this->Inverse() * (tmp * (*this));
 
-        return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
-      }
+      return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
+    }
 
-      /// \brief Return the Y axis.
-      /// \return the Y axis of the vector.
-      public: Vector3<T> YAxis() const
-      {
-        T fTx  = 2.0f*this->qx;
-        T fTy  = 2.0f*this->qy;
-        T fTz  = 2.0f*this->qz;
-        T fTwx = fTx*this->qw;
-        T fTwz = fTz*this->qw;
-        T fTxx = fTx*this->qx;
-        T fTxy = fTy*this->qx;
-        T fTyz = fTz*this->qy;
-        T fTzz = fTz*this->qz;
-
-        return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
-      }
+    /// \brief See if a quaternion is finite (e.g., not nan).
+    /// \return True if quaternion is finite.
+    public: bool IsFinite() const
+    {
+      // std::isfinite works with floating point values, need to explicit
+      // cast to avoid ambiguity in vc++.
+      return std::isfinite(static_cast<double>(this->qw)) &&
+             std::isfinite(static_cast<double>(this->qx)) &&
+             std::isfinite(static_cast<double>(this->qy)) &&
+             std::isfinite(static_cast<double>(this->qz));
+    }
 
-      /// \brief Return the Z axis.
-      /// \return the Z axis of the vector.
-      public: Vector3<T> ZAxis() const
+    /// \brief Correct any nan values in this quaternion.
+    public: inline void Correct()
+    {
+      // std::isfinite works with floating point values, need to explicit
+      // cast to avoid ambiguity in vc++.
+      if (!std::isfinite(static_cast<double>(this->qx)))
+        this->qx = 0;
+      if (!std::isfinite(static_cast<double>(this->qy)))
+        this->qy = 0;
+      if (!std::isfinite(static_cast<double>(this->qz)))
+        this->qz = 0;
+      if (!std::isfinite(static_cast<double>(this->qw)))
+        this->qw = 1;
+
+      if (equal(this->qw, static_cast<T>(0)) &&
+          equal(this->qx, static_cast<T>(0)) &&
+          equal(this->qy, static_cast<T>(0)) &&
+          equal(this->qz, static_cast<T>(0)))
       {
-        T fTx  = 2.0f*this->qx;
-        T fTy  = 2.0f*this->qy;
-        T fTz  = 2.0f*this->qz;
-        T fTwx = fTx*this->qw;
-        T fTwy = fTy*this->qw;
-        T fTxx = fTx*this->qx;
-        T fTxz = fTz*this->qx;
-        T fTyy = fTy*this->qy;
-        T fTyz = fTz*this->qy;
-
-        return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
+        this->qw = 1;
       }
+    }
 
-      /// \brief Round all values to _precision decimal places.
-      /// \param[in] _precision the precision.
-      public: void Round(int _precision)
-      {
-        this->qx = precision(this->qx, _precision);
-        this->qy = precision(this->qy, _precision);
-        this->qz = precision(this->qz, _precision);
-        this->qw = precision(this->qw, _precision);
-      }
+    /// \brief Return the X axis.
+    /// \return the X axis of the vector.
+    public: Vector3<T> XAxis() const
+    {
+      T fTy  = 2.0f*this->qy;
+      T fTz  = 2.0f*this->qz;
 
-      /// \brief Get the dot product of this quaternion with the give _q
-      /// quaternion.
-      /// \param[in] _q The other quaternion.
-      /// \return The dot product.
-      public: T Dot(const Quaternion<T> &_q) const
-      {
-        return this->qw*_q.qw + this->qx * _q.qx +
-               this->qy*_q.qy + this->qz*_q.qz;
-      }
+      T fTwy = fTy*this->qw;
+      T fTwz = fTz*this->qw;
+      T fTxy = fTy*this->qx;
+      T fTxz = fTz*this->qx;
+      T fTyy = fTy*this->qy;
+      T fTzz = fTz*this->qz;
 
-      /// \brief Spherical quadratic interpolation
-      /// given the ends and an interpolation parameter between 0 and 1.
-      /// \param[in] _fT the interpolation parameter.
-      /// \param[in] _rkP The beginning quaternion.
-      /// \param[in] _rkA First intermediate quaternion.
-      /// \param[in] _rkB Second intermediate quaternion.
-      /// \param[in] _rkQ The end quaternion.
-      /// \param[in] _shortestPath When true, the rotation may be inverted to
-      /// get to minimize rotation.
-      /// \return The result of the quadratic interpolation.
-      public: static Quaternion<T> Squad(T _fT,
-                  const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
-                  const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
-                  bool _shortestPath = false)
-      {
-        T fSlerpT = 2.0f*_fT*(1.0f-_fT);
-        Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
-        Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
-        return Slerp(fSlerpT, kSlerpP, kSlerpQ);
-      }
+      return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
+    }
 
-      /// \brief Spherical linear interpolation between 2 quaternions,
-      /// given the ends and an interpolation parameter between 0 and 1.
-      /// \param[in] _fT The interpolation parameter.
-      /// \param[in] _rkP The beginning quaternion.
-      /// \param[in] _rkQ The end quaternion.
-      /// \param[in] _shortestPath When true, the rotation may be inverted to
-      /// get to minimize rotation.
-      /// \return The result of the linear interpolation.
-      public: static Quaternion<T> Slerp(T _fT,
-                  const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
-                  bool _shortestPath = false)
-      {
-        T fCos = _rkP.Dot(_rkQ);
-        Quaternion<T> rkT;
+    /// \brief Return the Y axis.
+    /// \return the Y axis of the vector.
+    public: Vector3<T> YAxis() const
+    {
+      T fTx  = 2.0f*this->qx;
+      T fTy  = 2.0f*this->qy;
+      T fTz  = 2.0f*this->qz;
+      T fTwx = fTx*this->qw;
+      T fTwz = fTz*this->qw;
+      T fTxx = fTx*this->qx;
+      T fTxy = fTy*this->qx;
+      T fTyz = fTz*this->qy;
+      T fTzz = fTz*this->qz;
+
+      return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
+    }
 
-        // Do we need to invert rotation?
-        if (fCos < 0.0f && _shortestPath)
-        {
-          fCos = -fCos;
-          rkT = -_rkQ;
-        }
-        else
-        {
-          rkT = _rkQ;
-        }
+    /// \brief Return the Z axis.
+    /// \return the Z axis of the vector.
+    public: Vector3<T> ZAxis() const
+    {
+      T fTx  = 2.0f*this->qx;
+      T fTy  = 2.0f*this->qy;
+      T fTz  = 2.0f*this->qz;
+      T fTwx = fTx*this->qw;
+      T fTwy = fTy*this->qw;
+      T fTxx = fTx*this->qx;
+      T fTxz = fTz*this->qx;
+      T fTyy = fTy*this->qy;
+      T fTyz = fTz*this->qy;
+
+      return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
+    }
 
-        if (std::abs(fCos) < 1 - 1e-03)
-        {
-          // Standard case (slerp)
-          T fSin = sqrt(1 - (fCos*fCos));
-          T fAngle = atan2(fSin, fCos);
-          // FIXME: should check if (std::abs(fSin) >= 1e-3)
-          T fInvSin = 1.0f / fSin;
-          T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
-          T fCoeff1 = sin(_fT * fAngle) * fInvSin;
-          return _rkP * fCoeff0 + rkT * fCoeff1;
-        }
-        else
-        {
-          // There are two situations:
-          // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
-          // so we can do a linear interpolation safely.
-          // 2. "rkP" and "rkQ" are almost inverse of each
-          // other (fCos ~= -1), there
-          // are an infinite number of possibilities interpolation.
-          // but we haven't have method to fix this case, so just use
-          // linear interpolation here.
-          Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
-          // taking the complement requires renormalisation
-          t.Normalize();
-          return t;
-        }
-      }
+    /// \brief Round all values to _precision decimal places.
+    /// \param[in] _precision the precision.
+    public: void Round(int _precision)
+    {
+      this->qx = precision(this->qx, _precision);
+      this->qy = precision(this->qy, _precision);
+      this->qz = precision(this->qz, _precision);
+      this->qw = precision(this->qw, _precision);
+    }
 
-      /// \brief Integrate quaternion for constant angular velocity vector
-      /// along specified interval `_deltaT`.
-      /// Implementation based on:
-      /// http://physicsforgames.blogspot.com/2010/02/quaternions.html
-      /// \param[in] _angularVelocity Angular velocity vector, specified in
-      /// same reference frame as base of this quaternion.
-      /// \param[in] _deltaT Time interval in seconds to integrate over.
-      /// \return Quaternion at integrated configuration.
-      public: Quaternion<T> Integrate(const Vector3<T> &_angularVelocity,
-                                      const T _deltaT) const
-      {
-        Quaternion<T> deltaQ;
-        Vector3<T> theta = _angularVelocity * _deltaT / 2;
-        T thetaMagSq = theta.SquaredLength();
-        T s;
-        if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
-        {
-          deltaQ.W() = 1.0 - thetaMagSq / 2.0;
-          s = 1.0 - thetaMagSq / 6.0;
-        }
-        else
-        {
-          double thetaMag = sqrt(thetaMagSq);
-          deltaQ.W() = cos(thetaMag);
-          s = sin(thetaMag) / thetaMag;
-        }
-        deltaQ.X() = theta.X() * s;
-        deltaQ.Y() = theta.Y() * s;
-        deltaQ.Z() = theta.Z() * s;
-        return deltaQ * (*this);
-      }
+    /// \brief Get the dot product of this quaternion with the give _q
+    /// quaternion.
+    /// \param[in] _q The other quaternion.
+    /// \return The dot product.
+    public: T Dot(const Quaternion<T> &_q) const
+    {
+      return this->qw*_q.qw + this->qx * _q.qx +
+             this->qy*_q.qy + this->qz*_q.qz;
+    }
 
-      /// \brief Get the w component.
-      /// \return The w quaternion component.
-      public: inline T W() const
-      {
-        return this->qw;
-      }
+    /// \brief Spherical quadratic interpolation
+    /// given the ends and an interpolation parameter between 0 and 1.
+    /// \param[in] _fT the interpolation parameter.
+    /// \param[in] _rkP The beginning quaternion.
+    /// \param[in] _rkA First intermediate quaternion.
+    /// \param[in] _rkB Second intermediate quaternion.
+    /// \param[in] _rkQ The end quaternion.
+    /// \param[in] _shortestPath When true, the rotation may be inverted to
+    /// get to minimize rotation.
+    /// \return The result of the quadratic interpolation.
+    public: static Quaternion<T> Squad(T _fT,
+                const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
+                const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
+                bool _shortestPath = false)
+    {
+      T fSlerpT = 2.0f*_fT*(1.0f-_fT);
+      Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
+      Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
+      return Slerp(fSlerpT, kSlerpP, kSlerpQ);
+    }
 
-      /// \brief Get the x component.
-      /// \return The x quaternion component.
-      public: inline T X() const
-      {
-        return this->qx;
-      }
+    /// \brief Spherical linear interpolation between 2 quaternions,
+    /// given the ends and an interpolation parameter between 0 and 1.
+    /// \param[in] _fT The interpolation parameter.
+    /// \param[in] _rkP The beginning quaternion.
+    /// \param[in] _rkQ The end quaternion.
+    /// \param[in] _shortestPath When true, the rotation may be inverted to
+    /// get to minimize rotation.
+    /// \return The result of the linear interpolation.
+    public: static Quaternion<T> Slerp(T _fT,
+                const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
+                bool _shortestPath = false)
+    {
+      T fCos = _rkP.Dot(_rkQ);
+      Quaternion<T> rkT;
 
-      /// \brief Get the y component.
-      /// \return The y quaternion component.
-      public: inline T Y() const
+      // Do we need to invert rotation?
+      if (fCos < 0.0f && _shortestPath)
       {
-        return this->qy;
+        fCos = -fCos;
+        rkT = -_rkQ;
       }
-
-      /// \brief Get the z component.
-      /// \return The z quaternion component.
-      public: inline T Z() const
+      else
       {
-        return this->qz;
+        rkT = _rkQ;
       }
 
-      /// \brief Get a mutable w component.
-      /// \return The w quaternion component.
-      public: inline T &W()
+      if (std::abs(fCos) < 1 - 1e-03)
       {
-        return this->qw;
+        // Standard case (slerp)
+        T fSin = sqrt(1 - (fCos*fCos));
+        T fAngle = atan2(fSin, fCos);
+        // FIXME: should check if (std::abs(fSin) >= 1e-3)
+        T fInvSin = 1.0f / fSin;
+        T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
+        T fCoeff1 = sin(_fT * fAngle) * fInvSin;
+        return _rkP * fCoeff0 + rkT * fCoeff1;
       }
-
-      /// \brief Get a mutable x component.
-      /// \return The x quaternion component.
-      public: inline T &X()
+      else
       {
-        return this->qx;
+        // There are two situations:
+        // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
+        // so we can do a linear interpolation safely.
+        // 2. "rkP" and "rkQ" are almost inverse of each
+        // other (fCos ~= -1), there
+        // are an infinite number of possibilities interpolation.
+        // but we haven't have method to fix this case, so just use
+        // linear interpolation here.
+        Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
+        // taking the complement requires renormalisation
+        t.Normalize();
+        return t;
       }
+    }
 
-      /// \brief Get a mutable y component.
-      /// \return The y quaternion component.
-      public: inline T &Y()
+    /// \brief Integrate quaternion for constant angular velocity vector
+    /// along specified interval `_deltaT`.
+    /// Implementation based on:
+    /// http://physicsforgames.blogspot.com/2010/02/quaternions.html
+    /// \param[in] _angularVelocity Angular velocity vector, specified in
+    /// same reference frame as base of this quaternion.
+    /// \param[in] _deltaT Time interval in seconds to integrate over.
+    /// \return Quaternion at integrated configuration.
+    public: Quaternion<T> Integrate(const Vector3<T> &_angularVelocity,
+                                    const T _deltaT) const
+    {
+      Quaternion<T> deltaQ;
+      Vector3<T> theta = _angularVelocity * _deltaT / 2;
+      T thetaMagSq = theta.SquaredLength();
+      T s;
+      if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
       {
-        return this->qy;
+        deltaQ.W() = 1.0 - thetaMagSq / 2.0;
+        s = 1.0 - thetaMagSq / 6.0;
       }
-
-      /// \brief Get a mutable z component.
-      /// \return The z quaternion component.
-      public: inline T &Z()
+      else
       {
-        return this->qz;
+        double thetaMag = sqrt(thetaMagSq);
+        deltaQ.W() = cos(thetaMag);
+        s = sin(thetaMag) / thetaMag;
       }
+      deltaQ.X() = theta.X() * s;
+      deltaQ.Y() = theta.Y() * s;
+      deltaQ.Z() = theta.Z() * s;
+      return deltaQ * (*this);
+    }
 
-      /// \brief Set the x component.
-      /// \param[in] _v The new value for the x quaternion component.
-      public: inline void SetX(T _v)
-      {
-        this->qx = _v;
-      }
+    /// \brief Get the w component.
+    /// \return The w quaternion component.
+    public: inline T W() const
+    {
+      return this->qw;
+    }
 
-      /// \brief Set the y component.
-      /// \param[in] _v The new value for the y quaternion component.
-      public: inline void SetY(T _v)
-      {
-        this->qy = _v;
-      }
+    /// \brief Get the x component.
+    /// \return The x quaternion component.
+    public: inline T X() const
+    {
+      return this->qx;
+    }
 
+    /// \brief Get the y component.
+    /// \return The y quaternion component.
+    public: inline T Y() const
+    {
+      return this->qy;
+    }
 
-      /// \brief Set the z component.
-      /// \param[in] _v The new value for the z quaternion component.
-      public: inline void SetZ(T _v)
-      {
-        this->qz = _v;
-      }
+    /// \brief Get the z component.
+    /// \return The z quaternion component.
+    public: inline T Z() const
+    {
+      return this->qz;
+    }
 
-      /// \brief Set the w component.
-      /// \param[in] _v The new value for the w quaternion component.
-      public: inline void SetW(T _v)
-      {
-        this->qw = _v;
-      }
+    /// \brief Get a mutable w component.
+    /// \return The w quaternion component.
+    public: inline T &W()
+    {
+      return this->qw;
+    }
 
-      /// \brief Stream insertion operator
-      /// \param[in, out] _out output stream
-      /// \param[in] _q quaternion to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(std::ostream &_out,
-                  const gz::math::Quaternion<T> &_q)
-      {
-        Vector3<T> v(_q.Euler());
-        _out << v;
-        return _out;
-      }
+    /// \brief Get a mutable x component.
+    /// \return The x quaternion component.
+    public: inline T &X()
+    {
+      return this->qx;
+    }
 
-      /// \brief Stream extraction operator
-      /// \param[in, out] _in input stream
-      /// \param[out] _q Quaternion<T> to read values into
-      /// \return The istream
-      public: friend std::istream &operator>>(std::istream &_in,
-          gz::math::Quaternion<T> &_q)
-      {
-        Angle roll, pitch, yaw;
+    /// \brief Get a mutable y component.
+    /// \return The y quaternion component.
+    public: inline T &Y()
+    {
+      return this->qy;
+    }
 
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        _in >> roll >> pitch >> yaw;
+    /// \brief Get a mutable z component.
+    /// \return The z quaternion component.
+    public: inline T &Z()
+    {
+      return this->qz;
+    }
 
-        if (!_in.fail())
-        {
-          _q.SetFromEuler(Vector3<T>(*roll, *pitch, *yaw));
-        }
+    /// \brief Set the x component.
+    /// \param[in] _v The new value for the x quaternion component.
+    public: inline void SetX(T _v)
+    {
+      this->qx = _v;
+    }
 
-        return _in;
-      }
+    /// \brief Set the y component.
+    /// \param[in] _v The new value for the y quaternion component.
+    public: inline void SetY(T _v)
+    {
+      this->qy = _v;
+    }
+
+
+    /// \brief Set the z component.
+    /// \param[in] _v The new value for the z quaternion component.
+    public: inline void SetZ(T _v)
+    {
+      this->qz = _v;
+    }
+
+    /// \brief Set the w component.
+    /// \param[in] _v The new value for the w quaternion component.
+    public: inline void SetW(T _v)
+    {
+      this->qw = _v;
+    }
+
+    /// \brief Stream insertion operator
+    /// \param[in, out] _out output stream
+    /// \param[in] _q quaternion to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                const gz::math::Quaternion<T> &_q)
+    {
+      Vector3<T> v(_q.Euler());
+      _out << v;
+      return _out;
+    }
+
+    /// \brief Stream extraction operator
+    /// \param[in, out] _in input stream
+    /// \param[out] _q Quaternion<T> to read values into
+    /// \return The istream
+    public: friend std::istream &operator>>(std::istream &_in,
+        gz::math::Quaternion<T> &_q)
+    {
+      Angle roll, pitch, yaw;
+
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      _in >> roll >> pitch >> yaw;
 
-      /// \brief Equality comparison test with a tolerance parameter.
-      /// The tolerance is used with the gz::math::equal function for
-      /// each component of the quaternions.
-      /// \param[in] _q The quaternion to compare against.
-      /// \param[in] _tol equality tolerance.
-      /// \return True if the elements of the quaternions are equal within
-      /// the tolerence specified by _tol.
-      public: bool Equal(const Quaternion &_q, const T &_tol) const
+      if (!_in.fail())
       {
-        return equal(this->qx, _q.qx, _tol) &&
-               equal(this->qy, _q.qy, _tol) &&
-               equal(this->qz, _q.qz, _tol) &&
-               equal(this->qw, _q.qw, _tol);
+        _q.SetFromEuler(Vector3<T>(*roll, *pitch, *yaw));
       }
 
-      /// \brief w value of the quaternion
-      private: T qw;
+      return _in;
+    }
 
-      /// \brief x value of the quaternion
-      private: T qx;
+    /// \brief Equality comparison test with a tolerance parameter.
+    /// The tolerance is used with the gz::math::equal function for
+    /// each component of the quaternions.
+    /// \param[in] _q The quaternion to compare against.
+    /// \param[in] _tol equality tolerance.
+    /// \return True if the elements of the quaternions are equal within
+    /// the tolerence specified by _tol.
+    public: bool Equal(const Quaternion &_q, const T &_tol) const
+    {
+      return equal(this->qx, _q.qx, _tol) &&
+             equal(this->qy, _q.qy, _tol) &&
+             equal(this->qz, _q.qz, _tol) &&
+             equal(this->qw, _q.qw, _tol);
+    }
 
-      /// \brief y value of the quaternion
-      private: T qy;
+    /// \brief w value of the quaternion
+    private: T qw;
 
-      /// \brief z value of the quaternion
-      private: T qz;
-    };
+    /// \brief x value of the quaternion
+    private: T qx;
 
-    namespace detail {
+    /// \brief y value of the quaternion
+    private: T qy;
 
-      template<typename T> constexpr Quaternion<T>
-      gQuaternionIdentity(1, 0, 0, 0);
+    /// \brief z value of the quaternion
+    private: T qz;
+  };
 
-      template<typename T> constexpr Quaternion<T>
-      gQuaternionZero(0, 0, 0, 0);
+  namespace detail {
 
-    }  // namespace detail
+    template<typename T> constexpr Quaternion<T>
+    gQuaternionIdentity(1, 0, 0, 0);
 
-    template<typename T> const Quaternion<T>
-    &Quaternion<T>::Identity = detail::gQuaternionIdentity<T>;
+    template<typename T> constexpr Quaternion<T>
+    gQuaternionZero(0, 0, 0, 0);
 
-    template<typename T> const Quaternion<T>
-    &Quaternion<T>::Zero = detail::gQuaternionZero<T>;
+  }  // namespace detail
 
-    /// typedef Quaternion<double> as Quaterniond
-    typedef Quaternion<double> Quaterniond;
+  template<typename T> const Quaternion<T>
+  &Quaternion<T>::Identity = detail::gQuaternionIdentity<T>;
 
-    /// typedef Quaternion<float> as Quaternionf
-    typedef Quaternion<float> Quaternionf;
-    }
-  }
-}
-#endif
+  template<typename T> const Quaternion<T>
+  &Quaternion<T>::Zero = detail::gQuaternionZero<T>;
+
+  /// typedef Quaternion<double> as Quaterniond
+  typedef Quaternion<double> Quaterniond;
+
+  /// typedef Quaternion<float> as Quaternionf
+  typedef Quaternion<float> Quaternionf;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_QUATERNION_HH_
diff --git a/include/gz/math/Rand.hh b/include/gz/math/Rand.hh
index f94824b96..045fb0f8d 100644
--- a/include/gz/math/Rand.hh
+++ b/include/gz/math/Rand.hh
@@ -23,69 +23,66 @@
 #include <gz/math/Helpers.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \def GeneratorType
-    /// \brief std::mt19937
-    typedef std::mt19937 GeneratorType;
-    /// \def UniformRealDist
-    /// \brief std::uniform_real_distribution<double>
-    typedef std::uniform_real_distribution<double> UniformRealDist;
-    /// \def NormalRealDist
-    /// \brief std::normal_distribution<double>
-    typedef std::normal_distribution<double> NormalRealDist;
-    /// \def UniformIntDist
-    /// \brief std::uniform_int<int>
-    typedef std::uniform_int_distribution<int32_t> UniformIntDist;
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \def GeneratorType
+  /// \brief std::mt19937
+  typedef std::mt19937 GeneratorType;
+  /// \def UniformRealDist
+  /// \brief std::uniform_real_distribution<double>
+  typedef std::uniform_real_distribution<double> UniformRealDist;
+  /// \def NormalRealDist
+  /// \brief std::normal_distribution<double>
+  typedef std::normal_distribution<double> NormalRealDist;
+  /// \def UniformIntDist
+  /// \brief std::uniform_int<int>
+  typedef std::uniform_int_distribution<int32_t> UniformIntDist;
 
-    /// \class Rand Rand.hh gz/math/Rand.hh
-    /// \brief Random number generator class
-    class GZ_MATH_VISIBLE Rand
-    {
-      /// \brief Set the seed value.
-      /// \param[in] _seed The seed used to initialize the randon number
-      /// generator.
-      public: static void Seed(unsigned int _seed);
+  /// \class Rand Rand.hh gz/math/Rand.hh
+  /// \brief Random number generator class
+  class GZ_MATH_VISIBLE Rand
+  {
+    /// \brief Set the seed value.
+    /// \param[in] _seed The seed used to initialize the randon number
+    /// generator.
+    public: static void Seed(unsigned int _seed);
 
-      /// \brief Get the seed value.
-      /// \return The seed value used to initialize the random number
-      /// generator.
-      public: static unsigned int Seed();
+    /// \brief Get the seed value.
+    /// \return The seed value used to initialize the random number
+    /// generator.
+    public: static unsigned int Seed();
 
-      /// \brief Get a double from a uniform distribution
-      /// \param[in] _min Minimum bound for the random number
-      /// \param[in] _max Maximum bound for the random number
-      public: static double DblUniform(double _min = 0, double _max = 1);
+    /// \brief Get a double from a uniform distribution
+    /// \param[in] _min Minimum bound for the random number
+    /// \param[in] _max Maximum bound for the random number
+    public: static double DblUniform(double _min = 0, double _max = 1);
 
-      /// \brief Get a double from a normal distribution
-      /// \param[in] _mean Mean value for the distribution
-      /// \param[in] _sigma Sigma value for the distribution
-      public: static double DblNormal(double _mean = 0, double _sigma = 1);
+    /// \brief Get a double from a normal distribution
+    /// \param[in] _mean Mean value for the distribution
+    /// \param[in] _sigma Sigma value for the distribution
+    public: static double DblNormal(double _mean = 0, double _sigma = 1);
 
-      /// \brief Get an integer from a uniform distribution
-      /// \param[in] _min Minimum bound for the random number
-      /// \param[in] _max Maximum bound for the random number
-      public: static int32_t IntUniform(int _min, int _max);
+    /// \brief Get an integer from a uniform distribution
+    /// \param[in] _min Minimum bound for the random number
+    /// \param[in] _max Maximum bound for the random number
+    public: static int32_t IntUniform(int _min, int _max);
 
-      /// \brief Get an integer from a normal distribution
-      /// \param[in] _mean Mean value for the distribution
-      /// \param[in] _sigma Sigma value for the distribution
-      public: static int32_t IntNormal(int _mean, int _sigma);
+    /// \brief Get an integer from a normal distribution
+    /// \param[in] _mean Mean value for the distribution
+    /// \param[in] _sigma Sigma value for the distribution
+    public: static int32_t IntNormal(int _mean, int _sigma);
 
-      /// \brief Get a mutable reference to the seed (create the static
-      /// member if it hasn't been created yet).
-      private: static uint32_t &SeedMutable();
+    /// \brief Get a mutable reference to the seed (create the static
+    /// member if it hasn't been created yet).
+    private: static uint32_t &SeedMutable();
 
-      /// \brief Get a mutable reference to the random generator (create the
-      /// static member if it hasn't been created yet).
-      private: static GeneratorType &RandGenerator();
-    };
-    }
-  }
-}
-#endif
+    /// \brief Get a mutable reference to the random generator (create the
+    /// static member if it hasn't been created yet).
+    private: static GeneratorType &RandGenerator();
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_RAND_HH_
diff --git a/include/gz/math/Region3.hh b/include/gz/math/Region3.hh
index 1236b9a74..e53374f72 100644
--- a/include/gz/math/Region3.hh
+++ b/include/gz/math/Region3.hh
@@ -26,184 +26,180 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Region3 Region3.hh gz/math/Region3.hh
+  /// \brief The Region3 class represents the cartesian product
+  /// of intervals Ix ✕ Iy ✕ Iz, one per axis, yielding an
+  /// axis-aligned region of R^3 space. It can be thought of as
+  /// an intersection of halfspaces. Regions may be open or
+  /// closed in their boundaries, if any.
+  ///
+  /// Note that the Region3 class is essentially a set R ⊆ R^3.
+  /// For 3D solid box semantics, use the `AxisAlignedBox` class
+  /// instead.
+  ///
+  /// ## Example
+  ///
+  /// \snippet examples/region3_example.cc complete
+  template <typename T>
+  class Region3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Region3 Region3.hh gz/math/Region3.hh
-    /// \brief The Region3 class represents the cartesian product
-    /// of intervals Ix ✕ Iy ✕ Iz, one per axis, yielding an
-    /// axis-aligned region of R^3 space. It can be thought of as
-    /// an intersection of halfspaces. Regions may be open or
-    /// closed in their boundaries, if any.
-    ///
-    /// Note that the Region3 class is essentially a set R ⊆ R^3.
-    /// For 3D solid box semantics, use the `AxisAlignedBox` class
-    /// instead.
-    ///
-    /// ## Example
-    ///
-    /// \snippet examples/region3_example.cc complete
-    template <typename T>
-    class Region3
+    /// \brief An unbounded region (-∞, ∞) ✕ (-∞, ∞) ✕ (-∞, ∞)
+    public: static const Region3<T> &Unbounded;
+
+    /// \brief Constructor
+    public: Region3() = default;
+
+    /// \brief Constructor
+    /// \param[in] _ix x-axis interval
+    /// \param[in] _iy y-axis interval
+    /// \param[in] _iz z-axis interval
+    public: constexpr Region3(
+        Interval<T> _ix, Interval<T> _iy, Interval<T> _iz)
+    : ix(std::move(_ix)), iy(std::move(_iy)), iz(std::move(_iz))
     {
-      /// \brief An unbounded region (-∞, ∞) ✕ (-∞, ∞) ✕ (-∞, ∞)
-      public: static const Region3<T> &Unbounded;
-
-      /// \brief Constructor
-      public: Region3() = default;
-
-      /// \brief Constructor
-      /// \param[in] _ix x-axis interval
-      /// \param[in] _iy y-axis interval
-      /// \param[in] _iz z-axis interval
-      public: constexpr Region3(
-          Interval<T> _ix, Interval<T> _iy, Interval<T> _iz)
-      : ix(std::move(_ix)), iy(std::move(_iy)), iz(std::move(_iz))
-      {
-      }
-
-      /// \brief Make an open region
-      /// \param[in] _xLeft leftmost x-axis interval value
-      /// \param[in] _xRight righmost x-axis interval value
-      /// \param[in] _yLeft leftmost y-axis interval value
-      /// \param[in] _yRight righmost y-axis interval value
-      /// \param[in] _zLeft leftmost z-axis interval value
-      /// \param[in] _zRight righmost z-axis interval value
-      /// \return the (`_xLeft`, `_xRight`) ✕ (`_yLeft`, `_yRight`)
-      ///  ✕ (`_zLeft`, `_zRight`) open region
-      public: static constexpr Region3<T> Open(
-          T _xLeft, T _yLeft, T _zLeft,
-          T _xRight, T _yRight, T _zRight)
-      {
-        return Region3<T>(Interval<T>::Open(_xLeft, _xRight),
-                          Interval<T>::Open(_yLeft, _yRight),
-                          Interval<T>::Open(_zLeft, _zRight));
-      }
-
-      /// \brief Make a closed region
-      /// \param[in] _xLeft leftmost x-axis interval value
-      /// \param[in] _xRight righmost x-axis interval value
-      /// \param[in] _yLeft leftmost y-axis interval value
-      /// \param[in] _yRight righmost y-axis interval value
-      /// \param[in] _zLeft leftmost z-axis interval value
-      /// \param[in] _zRight righmost z-axis interval value
-      /// \return the [`_xLeft`, `_xRight`] ✕ [`_yLeft`, `_yRight`]
-      ///  ✕ [`_zLeft`, `_zRight`] closed region
-      public: static constexpr Region3<T> Closed(
-          T _xLeft, T _yLeft, T _zLeft,
-          T _xRight, T _yRight, T _zRight)
-      {
-        return Region3<T>(Interval<T>::Closed(_xLeft, _xRight),
-                          Interval<T>::Closed(_yLeft, _yRight),
-                          Interval<T>::Closed(_zLeft, _zRight));
-      }
-
-      /// \brief Get the x-axis interval for the region
-      /// \return the x-axis interval
-      public: const Interval<T> &Ix() const { return this->ix; }
-
-      /// \brief Get the y-axis interval for the region
-      /// \return the y-axis interval
-      public: const Interval<T> &Iy() const { return this->iy; }
-
-      /// \brief Get the z-axis interval for the region
-      /// \return the z-axis interval
-      public: const Interval<T> &Iz() const { return this->iz; }
-
-      /// \brief Check if the region is empty
-      /// A region is empty if any of the intervals
-      /// it is defined with (i.e. Ix, Iy, Iz) are.
-      /// \return true if it is empty, false otherwise
-      public: bool Empty() const
-      {
-        return this->ix.Empty() || this->iy.Empty() || this->iz.Empty();
-      }
-
-      /// \brief Check if the region contains `_point`
-      /// \param[in] _point point to check for membership
-      /// \return true if it is contained, false otherwise
-      public: bool Contains(const Vector3<T> &_point) const
-      {
-        return (this->ix.Contains(_point.X()) &&
-                this->iy.Contains(_point.Y()) &&
-                this->iz.Contains(_point.Z()));
-      }
-
-      /// \brief Check if the region contains `_other` region
-      /// \param[in] _other region to check for membership
-      /// \return true if it is contained, false otherwise
-      public: bool Contains(const Region3<T> &_other) const
-      {
-        return (this->ix.Contains(_other.ix) &&
-                this->iy.Contains(_other.iy) &&
-                this->iz.Contains(_other.iz));
-      }
-
-      /// \brief Check if the region intersects `_other` region
-      /// \param[in] _other region to check for intersection
-      /// \return true if it is contained, false otherwise
-      public: bool Intersects(const Region3<T>& _other) const
-      {
-        return (this->ix.Intersects(_other.ix) &&
-                this->iy.Intersects(_other.iy) &&
-                this->iz.Intersects(_other.iz));
-      }
-
-      /// \brief Equality test operator
-      /// \param _other region to check for equality
-      /// \return true if regions are equal, false otherwise
-      public: bool operator==(const Region3<T> &_other) const
-      {
-        return this->Contains(_other) && _other.Contains(*this);
-      }
-
-      /// \brief Inequality test operator
-      /// \param _other region to check for inequality
-      /// \return true if regions are unequal, false otherwise
-      public: bool operator!=(const Region3<T> &_other) const
-      {
-        return !this->Contains(_other) || !_other.Contains(*this);
-      }
-
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _r Region3 to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-        std::ostream &_out, const gz::math::Region3<T> &_r)
-      {
-        return _out <<_r.ix << " x " << _r.iy << " x " << _r.iz;
-      }
-
-      /// \brief The x-axis interval
-      private: Interval<T> ix;
-      /// \brief The y-axis interval
-      private: Interval<T> iy;
-      /// \brief The z-axis interval
-      private: Interval<T> iz;
-    };
-
-    namespace detail {
-       template<typename T>
-       constexpr Region3<T> gUnboundedRegion3(
-           Interval<T>::Open(-std::numeric_limits<T>::infinity(),
-                             std::numeric_limits<T>::infinity()),
-           Interval<T>::Open(-std::numeric_limits<T>::infinity(),
-                             std::numeric_limits<T>::infinity()),
-           Interval<T>::Open(-std::numeric_limits<T>::infinity(),
-                             std::numeric_limits<T>::infinity()));
     }
-    template<typename T>
-    const Region3<T> &Region3<T>::Unbounded = detail::gUnboundedRegion3<T>;
 
-    using Region3f = Region3<float>;
-    using Region3d = Region3<double>;
+    /// \brief Make an open region
+    /// \param[in] _xLeft leftmost x-axis interval value
+    /// \param[in] _xRight righmost x-axis interval value
+    /// \param[in] _yLeft leftmost y-axis interval value
+    /// \param[in] _yRight righmost y-axis interval value
+    /// \param[in] _zLeft leftmost z-axis interval value
+    /// \param[in] _zRight righmost z-axis interval value
+    /// \return the (`_xLeft`, `_xRight`) ✕ (`_yLeft`, `_yRight`)
+    ///  ✕ (`_zLeft`, `_zRight`) open region
+    public: static constexpr Region3<T> Open(
+        T _xLeft, T _yLeft, T _zLeft,
+        T _xRight, T _yRight, T _zRight)
+    {
+      return Region3<T>(Interval<T>::Open(_xLeft, _xRight),
+                        Interval<T>::Open(_yLeft, _yRight),
+                        Interval<T>::Open(_zLeft, _zRight));
+    }
+
+    /// \brief Make a closed region
+    /// \param[in] _xLeft leftmost x-axis interval value
+    /// \param[in] _xRight righmost x-axis interval value
+    /// \param[in] _yLeft leftmost y-axis interval value
+    /// \param[in] _yRight righmost y-axis interval value
+    /// \param[in] _zLeft leftmost z-axis interval value
+    /// \param[in] _zRight righmost z-axis interval value
+    /// \return the [`_xLeft`, `_xRight`] ✕ [`_yLeft`, `_yRight`]
+    ///  ✕ [`_zLeft`, `_zRight`] closed region
+    public: static constexpr Region3<T> Closed(
+        T _xLeft, T _yLeft, T _zLeft,
+        T _xRight, T _yRight, T _zRight)
+    {
+      return Region3<T>(Interval<T>::Closed(_xLeft, _xRight),
+                        Interval<T>::Closed(_yLeft, _yRight),
+                        Interval<T>::Closed(_zLeft, _zRight));
+    }
+
+    /// \brief Get the x-axis interval for the region
+    /// \return the x-axis interval
+    public: const Interval<T> &Ix() const { return this->ix; }
+
+    /// \brief Get the y-axis interval for the region
+    /// \return the y-axis interval
+    public: const Interval<T> &Iy() const { return this->iy; }
+
+    /// \brief Get the z-axis interval for the region
+    /// \return the z-axis interval
+    public: const Interval<T> &Iz() const { return this->iz; }
+
+    /// \brief Check if the region is empty
+    /// A region is empty if any of the intervals
+    /// it is defined with (i.e. Ix, Iy, Iz) are.
+    /// \return true if it is empty, false otherwise
+    public: bool Empty() const
+    {
+      return this->ix.Empty() || this->iy.Empty() || this->iz.Empty();
+    }
+
+    /// \brief Check if the region contains `_point`
+    /// \param[in] _point point to check for membership
+    /// \return true if it is contained, false otherwise
+    public: bool Contains(const Vector3<T> &_point) const
+    {
+      return (this->ix.Contains(_point.X()) &&
+              this->iy.Contains(_point.Y()) &&
+              this->iz.Contains(_point.Z()));
+    }
+
+    /// \brief Check if the region contains `_other` region
+    /// \param[in] _other region to check for membership
+    /// \return true if it is contained, false otherwise
+    public: bool Contains(const Region3<T> &_other) const
+    {
+      return (this->ix.Contains(_other.ix) &&
+              this->iy.Contains(_other.iy) &&
+              this->iz.Contains(_other.iz));
+    }
+
+    /// \brief Check if the region intersects `_other` region
+    /// \param[in] _other region to check for intersection
+    /// \return true if it is contained, false otherwise
+    public: bool Intersects(const Region3<T>& _other) const
+    {
+      return (this->ix.Intersects(_other.ix) &&
+              this->iy.Intersects(_other.iy) &&
+              this->iz.Intersects(_other.iz));
+    }
+
+    /// \brief Equality test operator
+    /// \param _other region to check for equality
+    /// \return true if regions are equal, false otherwise
+    public: bool operator==(const Region3<T> &_other) const
+    {
+      return this->Contains(_other) && _other.Contains(*this);
     }
-  }
-}
 
-#endif
+    /// \brief Inequality test operator
+    /// \param _other region to check for inequality
+    /// \return true if regions are unequal, false otherwise
+    public: bool operator!=(const Region3<T> &_other) const
+    {
+      return !this->Contains(_other) || !_other.Contains(*this);
+    }
+
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _r Region3 to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+      std::ostream &_out, const gz::math::Region3<T> &_r)
+    {
+      return _out <<_r.ix << " x " << _r.iy << " x " << _r.iz;
+    }
+
+    /// \brief The x-axis interval
+    private: Interval<T> ix;
+    /// \brief The y-axis interval
+    private: Interval<T> iy;
+    /// \brief The z-axis interval
+    private: Interval<T> iz;
+  };
+
+  namespace detail {
+     template<typename T>
+     constexpr Region3<T> gUnboundedRegion3(
+         Interval<T>::Open(-std::numeric_limits<T>::infinity(),
+                           std::numeric_limits<T>::infinity()),
+         Interval<T>::Open(-std::numeric_limits<T>::infinity(),
+                           std::numeric_limits<T>::infinity()),
+         Interval<T>::Open(-std::numeric_limits<T>::infinity(),
+                           std::numeric_limits<T>::infinity()));
+  }
+  template<typename T>
+  const Region3<T> &Region3<T>::Unbounded = detail::gUnboundedRegion3<T>;
+
+  using Region3f = Region3<float>;
+  using Region3d = Region3<double>;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_REGION3_HH_
diff --git a/include/gz/math/RollingMean.hh b/include/gz/math/RollingMean.hh
index ac15da710..29b31a588 100644
--- a/include/gz/math/RollingMean.hh
+++ b/include/gz/math/RollingMean.hh
@@ -21,54 +21,50 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \brief A class that computes the mean over a series of data points.
+  /// The window size determines the maximum number of data points. The
+  /// oldest value is popped off when the window size is reached and
+  /// a new value is pushed in.
+  class GZ_MATH_VISIBLE RollingMean
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \brief A class that computes the mean over a series of data points.
-    /// The window size determines the maximum number of data points. The
-    /// oldest value is popped off when the window size is reached and
-    /// a new value is pushed in.
-    class GZ_MATH_VISIBLE RollingMean
-    {
-      /// \brief Constructor
-      /// \param[in] _windowSize The window size to use. This value will be
-      /// ignored if it is equal to zero.
-      public: explicit RollingMean(size_t _windowSize = 10);
+    /// \brief Constructor
+    /// \param[in] _windowSize The window size to use. This value will be
+    /// ignored if it is equal to zero.
+    public: explicit RollingMean(size_t _windowSize = 10);
 
-      /// \brief Get the mean value.
-      /// \return The current mean value, or
-      /// std::numeric_limits<double>::quiet_NaN() if data points are not
-      /// present.
-      public: double Mean() const;
+    /// \brief Get the mean value.
+    /// \return The current mean value, or
+    /// std::numeric_limits<double>::quiet_NaN() if data points are not
+    /// present.
+    public: double Mean() const;
 
-      /// \brief Get the number of data points.
-      /// \return The number of datapoints.
-      public: size_t Count() const;
+    /// \brief Get the number of data points.
+    /// \return The number of datapoints.
+    public: size_t Count() const;
 
-      /// \brief Insert a new value.
-      /// \param[in] _value New value to insert.
-      public: void Push(double _value);
+    /// \brief Insert a new value.
+    /// \param[in] _value New value to insert.
+    public: void Push(double _value);
 
-      /// \brief Remove all the pushed values.
-      public: void Clear();
+    /// \brief Remove all the pushed values.
+    public: void Clear();
 
-      /// \brief Set the new window size. This will also clear the data.
-      /// Nothing happens if the _windowSize is zero.
-      /// \param[in] _windowSize The window size to use.
-      public: void SetWindowSize(size_t _windowSize);
+    /// \brief Set the new window size. This will also clear the data.
+    /// Nothing happens if the _windowSize is zero.
+    /// \param[in] _windowSize The window size to use.
+    public: void SetWindowSize(size_t _windowSize);
 
-      /// \brief Get the window size.
-      /// \return The window size.
-      public: size_t WindowSize() const;
+    /// \brief Get the window size.
+    /// \return The window size.
+    public: size_t WindowSize() const;
 
-      /// \brief Private data pointer.
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
-
-#endif
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_ROLLINGMEAN_HH_
diff --git a/include/gz/math/RotationSpline.hh b/include/gz/math/RotationSpline.hh
index 643280a89..3844e2fb9 100644
--- a/include/gz/math/RotationSpline.hh
+++ b/include/gz/math/RotationSpline.hh
@@ -21,100 +21,96 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \class RotationSpline RotationSpline.hh gz/math/RotationSpline.hh
+  /// \brief Spline for rotations
+  class GZ_MATH_VISIBLE  RotationSpline
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \class RotationSpline RotationSpline.hh gz/math/RotationSpline.hh
-    /// \brief Spline for rotations
-    class GZ_MATH_VISIBLE  RotationSpline
-    {
-      /// \brief Constructor. Sets the autoCalc to true
-      public: RotationSpline();
+    /// \brief Constructor. Sets the autoCalc to true
+    public: RotationSpline();
 
-      /// \brief Adds a control point to the end of the spline.
-      /// \param[in] _p control point
-      public: void AddPoint(const Quaterniond &_p);
+    /// \brief Adds a control point to the end of the spline.
+    /// \param[in] _p control point
+    public: void AddPoint(const Quaterniond &_p);
 
-      /// \brief Gets the detail of one of the control points of the spline.
-      /// \param[in] _index the index of the control point. _index is
-      /// clamped to [0, PointCount()-1].
-      /// \remarks This point must already exist in the spline.
-      /// \return The quaternion at the specified point.
-      /// If there are no points, then a Quaterniond with a value of
-      /// [INF, INF, INF, INF] is returned.
-      public: const Quaterniond &Point(const unsigned int _index) const;
+    /// \brief Gets the detail of one of the control points of the spline.
+    /// \param[in] _index the index of the control point. _index is
+    /// clamped to [0, PointCount()-1].
+    /// \remarks This point must already exist in the spline.
+    /// \return The quaternion at the specified point.
+    /// If there are no points, then a Quaterniond with a value of
+    /// [INF, INF, INF, INF] is returned.
+    public: const Quaterniond &Point(const unsigned int _index) const;
 
-      /// \brief Gets the number of control points in the spline.
-      /// \return the count
-      public: unsigned int PointCount() const;
+    /// \brief Gets the number of control points in the spline.
+    /// \return the count
+    public: unsigned int PointCount() const;
 
-      /// \brief Clears all the points in the spline.
-      public: void Clear();
+    /// \brief Clears all the points in the spline.
+    public: void Clear();
 
-      /// \brief Updates a single point in the spline.
-      /// \remarks This point must already exist in the spline.
-      /// \param[in] _index index
-      /// \param[in] _value the new control point value
-      /// \return True on success, false if _index is larger or equal than
-      /// PointCount().
-      public: bool UpdatePoint(const unsigned int _index,
-                               const Quaterniond &_value);
+    /// \brief Updates a single point in the spline.
+    /// \remarks This point must already exist in the spline.
+    /// \param[in] _index index
+    /// \param[in] _value the new control point value
+    /// \return True on success, false if _index is larger or equal than
+    /// PointCount().
+    public: bool UpdatePoint(const unsigned int _index,
+                             const Quaterniond &_value);
 
-      /// \brief Returns an interpolated point based on a parametric
-      ///        value over the whole series.
-      /// \remarks Given a t value between 0 and 1 representing the
-      ///          parametric distance along the whole length of the spline,
-      ///          this method returns an interpolated point.
-      /// \param[in] _t Parametric value.
-      /// \param[in] _useShortestPath Defines if rotation should take the
-      ///        shortest possible path
-      /// \return The rotation, or [INF, INF, INF, INF] on error. Use
-      /// Quateriond::IsFinite() to check for an error
-      public: Quaterniond Interpolate(double _t,
-                                      const bool _useShortestPath = true);
+    /// \brief Returns an interpolated point based on a parametric
+    ///        value over the whole series.
+    /// \remarks Given a t value between 0 and 1 representing the
+    ///          parametric distance along the whole length of the spline,
+    ///          this method returns an interpolated point.
+    /// \param[in] _t Parametric value.
+    /// \param[in] _useShortestPath Defines if rotation should take the
+    ///        shortest possible path
+    /// \return The rotation, or [INF, INF, INF, INF] on error. Use
+    /// Quateriond::IsFinite() to check for an error
+    public: Quaterniond Interpolate(double _t,
+                                    const bool _useShortestPath = true);
 
-      /// \brief Interpolates a single segment of the spline
-      ///        given a parametric value.
-      /// \param[in] _fromIndex The point index to treat as t = 0.
-      ///        _fromIndex + 1 is deemed to be t = 1
-      /// \param[in] _t Parametric value
-      /// \param[in] _useShortestPath Defines if rotation should take the
-      ///         shortest possible path
-      /// \return the rotation, or [INF, INF, INF, INF] on error. Use
-      /// Quateriond::IsFinite() to check for an error
-      public: Quaterniond Interpolate(const unsigned int _fromIndex,
-                  const double _t, const bool _useShortestPath = true);
+    /// \brief Interpolates a single segment of the spline
+    ///        given a parametric value.
+    /// \param[in] _fromIndex The point index to treat as t = 0.
+    ///        _fromIndex + 1 is deemed to be t = 1
+    /// \param[in] _t Parametric value
+    /// \param[in] _useShortestPath Defines if rotation should take the
+    ///         shortest possible path
+    /// \return the rotation, or [INF, INF, INF, INF] on error. Use
+    /// Quateriond::IsFinite() to check for an error
+    public: Quaterniond Interpolate(const unsigned int _fromIndex,
+                const double _t, const bool _useShortestPath = true);
 
-      /// \brief Tells the spline whether it should automatically calculate
-      ///        tangents on demand as points are added.
-      /// \remarks The spline calculates tangents at each point automatically
-      ///          based on the input points.  Normally it does this every
-      ///          time a point changes. However, if you have a lot of points
-      ///          to add in one go, you probably don't want to incur this
-      ///          overhead and would prefer to defer the calculation until
-      ///          you are finished setting all the points. You can do this
-      ///          by calling this method with a parameter of 'false'. Just
-      ///          remember to manually call the recalcTangents method when
-      ///          you are done.
-      /// \param[in] _autoCalc If true, tangents are calculated for you
-      /// whenever a point changes. If false, you must call reclacTangents to
-      /// recalculate them when it best suits.
-      public: void AutoCalculate(bool _autoCalc);
+    /// \brief Tells the spline whether it should automatically calculate
+    ///        tangents on demand as points are added.
+    /// \remarks The spline calculates tangents at each point automatically
+    ///          based on the input points.  Normally it does this every
+    ///          time a point changes. However, if you have a lot of points
+    ///          to add in one go, you probably don't want to incur this
+    ///          overhead and would prefer to defer the calculation until
+    ///          you are finished setting all the points. You can do this
+    ///          by calling this method with a parameter of 'false'. Just
+    ///          remember to manually call the recalcTangents method when
+    ///          you are done.
+    /// \param[in] _autoCalc If true, tangents are calculated for you
+    /// whenever a point changes. If false, you must call reclacTangents to
+    /// recalculate them when it best suits.
+    public: void AutoCalculate(bool _autoCalc);
 
-      /// \brief Recalculates the tangents associated with this spline.
-      /// \remarks If you tell the spline not to update on demand by calling
-      /// setAutoCalculate(false) then you must call this after
-      /// completing your updates to the spline points.
-      public: void RecalcTangents();
+    /// \brief Recalculates the tangents associated with this spline.
+    /// \remarks If you tell the spline not to update on demand by calling
+    /// setAutoCalculate(false) then you must call this after
+    /// completing your updates to the spline points.
+    public: void RecalcTangents();
 
-      /// \brief Private data pointer
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
-
-#endif
+    /// \brief Private data pointer
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_ROTATIONSPLINE_HH_
diff --git a/include/gz/math/SemanticVersion.hh b/include/gz/math/SemanticVersion.hh
index 3eca0c99e..709b4a2f6 100644
--- a/include/gz/math/SemanticVersion.hh
+++ b/include/gz/math/SemanticVersion.hh
@@ -24,116 +24,113 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \class SemanticVersion SemanticVersion.hh
+  ///        gz/math/SemanticVersion.hh
+  /// \brief Version comparison class based on Semantic Versioning 2.0.0
+  /// http://semver.org/
+  /// Compares versions and converts versions from string.
+  class GZ_MATH_VISIBLE SemanticVersion
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \class SemanticVersion SemanticVersion.hh
-    ///        gz/math/SemanticVersion.hh
-    /// \brief Version comparison class based on Semantic Versioning 2.0.0
-    /// http://semver.org/
-    /// Compares versions and converts versions from string.
-    class GZ_MATH_VISIBLE SemanticVersion
+    /// \brief Default constructor. Use the Parse function to populate
+    /// an instance with version information.
+    public: SemanticVersion();
+
+    /// \brief Constructor
+    /// \param[in] _v the string version. ex: "0.3.2"
+    public: explicit SemanticVersion(const std::string &_v);
+
+    /// \brief Constructor
+    /// \param[in] _major The major number
+    /// \param[in] _minor The minor number
+    /// \param[in] _patch The patch number
+    /// \param[in] _prerelease The prerelease string
+    /// \param[in] _build The build metadata string
+    public: explicit SemanticVersion(const unsigned int _major,
+                                     const unsigned int _minor = 0,
+                                     const unsigned int _patch = 0,
+                                     const std::string &_prerelease = "",
+                                     const std::string &_build = "");
+
+    /// \brief Parse a version string and set the major, minor, patch
+    /// numbers, and prerelease and build strings.
+    /// \param[in] _versionStr The version string, such as "1.2.3-pr+123"
+    /// \return True on success.
+    public: bool Parse(const std::string &_versionStr);
+
+    /// \brief Returns the version as a string
+    /// \return The semantic version string
+    public: std::string Version() const;
+
+    /// \brief Get the major number
+    /// \return The major number
+    public: unsigned int Major() const;
+
+    /// \brief Get the minor number
+    /// \return The minor number
+    public: unsigned int Minor() const;
+
+    /// \brief Get the patch number
+    /// \return The patch number
+    public: unsigned int Patch() const;
+
+    /// \brief Get the prerelease string.
+    /// \return Prelrease string, empty if a prerelease string was not
+    /// specified.
+    public: std::string Prerelease() const;
+
+    /// \brief Get the build metadata string. Build meta data is not used
+    /// when determining precedence.
+    /// \return Build metadata string, empty if a build metadata string was
+    /// not specified.
+    public: std::string Build() const;
+
+    /// \brief Less than comparison operator
+    /// \param[in] _other The other version to compare to
+    /// \return True if _other version is newer
+    public: bool operator<(const SemanticVersion &_other) const;
+
+    /// \brief Less than or equal comparison operator
+    /// \param[in] _other The other version to compare to
+    /// \return True if _other version is newer or equal
+    public: bool operator<=(const SemanticVersion &_other) const;
+
+    /// \brief Greater than comparison operator
+    /// \param[in] _other The other version to compare to
+    /// \return True if _other version is older
+    public: bool operator>(const SemanticVersion &_other) const;
+
+    /// \brief Greater than or equal comparison operator
+    /// \param[in] _other The other version to compare to
+    /// \return True if _other version is older or the same
+    public: bool operator>=(const SemanticVersion &_other) const;
+
+    /// \brief Equality comparison operator
+    /// \param[in] _other The other version to compare to
+    /// \return True if _other version is the same
+    public: bool operator==(const SemanticVersion &_other) const;
+
+    /// \brief Inequality comparison operator
+    /// \param[in] _other The other version to compare to
+    /// \return True if _other version is different
+    public: bool operator!=(const SemanticVersion &_other) const;
+
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _v Semantic version to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                                            const SemanticVersion &_v)
     {
-      /// \brief Default constructor. Use the Parse function to populate
-      /// an instance with version information.
-      public: SemanticVersion();
-
-      /// \brief Constructor
-      /// \param[in] _v the string version. ex: "0.3.2"
-      public: explicit SemanticVersion(const std::string &_v);
-
-      /// \brief Constructor
-      /// \param[in] _major The major number
-      /// \param[in] _minor The minor number
-      /// \param[in] _patch The patch number
-      /// \param[in] _prerelease The prerelease string
-      /// \param[in] _build The build metadata string
-      public: explicit SemanticVersion(const unsigned int _major,
-                                       const unsigned int _minor = 0,
-                                       const unsigned int _patch = 0,
-                                       const std::string &_prerelease = "",
-                                       const std::string &_build = "");
-
-      /// \brief Parse a version string and set the major, minor, patch
-      /// numbers, and prerelease and build strings.
-      /// \param[in] _versionStr The version string, such as "1.2.3-pr+123"
-      /// \return True on success.
-      public: bool Parse(const std::string &_versionStr);
-
-      /// \brief Returns the version as a string
-      /// \return The semantic version string
-      public: std::string Version() const;
-
-      /// \brief Get the major number
-      /// \return The major number
-      public: unsigned int Major() const;
-
-      /// \brief Get the minor number
-      /// \return The minor number
-      public: unsigned int Minor() const;
-
-      /// \brief Get the patch number
-      /// \return The patch number
-      public: unsigned int Patch() const;
-
-      /// \brief Get the prerelease string.
-      /// \return Prelrease string, empty if a prerelease string was not
-      /// specified.
-      public: std::string Prerelease() const;
-
-      /// \brief Get the build metadata string. Build meta data is not used
-      /// when determining precedence.
-      /// \return Build metadata string, empty if a build metadata string was
-      /// not specified.
-      public: std::string Build() const;
-
-      /// \brief Less than comparison operator
-      /// \param[in] _other The other version to compare to
-      /// \return True if _other version is newer
-      public: bool operator<(const SemanticVersion &_other) const;
-
-      /// \brief Less than or equal comparison operator
-      /// \param[in] _other The other version to compare to
-      /// \return True if _other version is newer or equal
-      public: bool operator<=(const SemanticVersion &_other) const;
-
-      /// \brief Greater than comparison operator
-      /// \param[in] _other The other version to compare to
-      /// \return True if _other version is older
-      public: bool operator>(const SemanticVersion &_other) const;
-
-      /// \brief Greater than or equal comparison operator
-      /// \param[in] _other The other version to compare to
-      /// \return True if _other version is older or the same
-      public: bool operator>=(const SemanticVersion &_other) const;
-
-      /// \brief Equality comparison operator
-      /// \param[in] _other The other version to compare to
-      /// \return True if _other version is the same
-      public: bool operator==(const SemanticVersion &_other) const;
-
-      /// \brief Inequality comparison operator
-      /// \param[in] _other The other version to compare to
-      /// \return True if _other version is different
-      public: bool operator!=(const SemanticVersion &_other) const;
-
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _v Semantic version to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(std::ostream &_out,
-                                              const SemanticVersion &_v)
-      {
-        _out << _v.Version();
-        return _out;
-      }
-
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
+      _out << _v.Version();
+      return _out;
     }
-  }
-}
-#endif
+
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_SEMANTICVERSION_HH_
diff --git a/include/gz/math/SignalStats.hh b/include/gz/math/SignalStats.hh
index be6dc19af..ec7af1404 100644
--- a/include/gz/math/SignalStats.hh
+++ b/include/gz/math/SignalStats.hh
@@ -23,48 +23,46 @@
 #include <gz/math/Helpers.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \brief Forward declare private data class.
+  class SignalStatisticPrivate;
+
+  /// \class SignalStatistic SignalStats.hh gz/math/SignalStats.hh
+  /// \brief Statistical properties of a discrete time scalar signal.
+  class GZ_MATH_VISIBLE SignalStatistic
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \brief Forward declare private data class.
-    class SignalStatisticPrivate;
+    /// \brief Constructor
+    public: SignalStatistic();
 
-    /// \class SignalStatistic SignalStats.hh gz/math/SignalStats.hh
-    /// \brief Statistical properties of a discrete time scalar signal.
-    class GZ_MATH_VISIBLE SignalStatistic
-    {
-      /// \brief Constructor
-      public: SignalStatistic();
+    /// \brief Destructor
+    public: virtual ~SignalStatistic();
 
-      /// \brief Destructor
-      public: virtual ~SignalStatistic();
+    /// \brief Copy constructor
+    /// \param[in] _ss SignalStatistic to copy
+    public: SignalStatistic(const SignalStatistic &_ss);
 
-      /// \brief Copy constructor
-      /// \param[in] _ss SignalStatistic to copy
-      public: SignalStatistic(const SignalStatistic &_ss);
+    /// \brief Get the current value of the statistical measure.
+    /// \return Current value of the statistical measure.
+    public: virtual double Value() const = 0;
 
-      /// \brief Get the current value of the statistical measure.
-      /// \return Current value of the statistical measure.
-      public: virtual double Value() const = 0;
+    /// \brief Get a short version of the name of this statistical measure.
+    /// \return Short name of the statistical measure.
+    public: virtual std::string ShortName() const = 0;
 
-      /// \brief Get a short version of the name of this statistical measure.
-      /// \return Short name of the statistical measure.
-      public: virtual std::string ShortName() const = 0;
+    /// \brief Get number of data points in measurement.
+    /// \return Number of data points in measurement.
+    public: virtual size_t Count() const;
 
-      /// \brief Get number of data points in measurement.
-      /// \return Number of data points in measurement.
-      public: virtual size_t Count() const;
+    /// \brief Add a new sample to the statistical measure.
+    /// \param[in] _data New signal data point.
+    public: virtual void InsertData(const double _data) = 0;
 
-      /// \brief Add a new sample to the statistical measure.
-      /// \param[in] _data New signal data point.
-      public: virtual void InsertData(const double _data) = 0;
-
-      /// \brief Forget all previous data.
-      public: virtual void Reset();
+    /// \brief Forget all previous data.
+    public: virtual void Reset();
 
 #ifdef _WIN32
 // Disable warning C4251 which is triggered by
@@ -72,169 +70,169 @@ namespace gz
 #pragma warning(push)
 #pragma warning(disable: 4251)
 #endif
-      /// \brief Pointer to private data.
-      protected: std::unique_ptr<SignalStatisticPrivate> dataPtr;
+    /// \brief Pointer to private data.
+    protected: std::unique_ptr<SignalStatisticPrivate> dataPtr;
 #ifdef _WIN32
 #pragma warning(pop)
 #endif
-    };
-
-    /// \class SignalMaximum SignalStats.hh gz/math/SignalStats.hh
-    /// \brief Computing the maximum value of a discretely sampled signal.
-    class GZ_MATH_VISIBLE SignalMaximum : public SignalStatistic
-    {
-      // Documentation inherited.
-      public: virtual double Value() const override;
-
-      /// \brief Get a short version of the name of this statistical measure.
-      /// \return "max"
-      public: virtual std::string ShortName() const override;
-
-      // Documentation inherited.
-      public: virtual void InsertData(const double _data) override;
-    };
-
-    /// \class SignalMean SignalStats.hh gz/math/SignalStats.hh
-    /// \brief Computing the mean value of a discretely sampled signal.
-    class GZ_MATH_VISIBLE SignalMean : public SignalStatistic
-    {
-      // Documentation inherited.
-      public: virtual double Value() const override;
-
-      /// \brief Get a short version of the name of this statistical measure.
-      /// \return "mean"
-      public: virtual std::string ShortName() const override;
-
-      // Documentation inherited.
-      public: virtual void InsertData(const double _data) override;
-    };
-
-    /// \class SignalMinimum SignalStats.hh gz/math/SignalStats.hh
-    /// \brief Computing the minimum value of a discretely sampled signal.
-    class GZ_MATH_VISIBLE SignalMinimum : public SignalStatistic
-    {
-      // Documentation inherited.
-      public: virtual double Value() const override;
-
-      /// \brief Get a short version of the name of this statistical measure.
-      /// \return "min"
-      public: virtual std::string ShortName() const override;
-
-      // Documentation inherited.
-      public: virtual void InsertData(const double _data) override;
-    };
-
-    /// \class SignalRootMeanSquare SignalStats.hh gz/math/SignalStats.hh
-    /// \brief Computing the square root of the mean squared value
-    /// of a discretely sampled signal.
-    class GZ_MATH_VISIBLE SignalRootMeanSquare : public SignalStatistic
-    {
-      // Documentation inherited.
-      public: virtual double Value() const override;
-
-      /// \brief Get a short version of the name of this statistical measure.
-      /// \return "rms"
-      public: virtual std::string ShortName() const override;
-
-      // Documentation inherited.
-      public: virtual void InsertData(const double _data) override;
-    };
-
-    /// \class SignalMaxAbsoluteValue SignalStats.hh
-    /// gz/math/SignalStats.hh
-    /// \brief Computing the maximum of the absolute value
-    /// of a discretely sampled signal.
-    /// Also known as the maximum norm, infinity norm, or supremum norm.
-    class GZ_MATH_VISIBLE SignalMaxAbsoluteValue : public SignalStatistic
-    {
-      // Documentation inherited.
-      public: virtual double Value() const override;
-
-      /// \brief Get a short version of the name of this statistical measure.
-      /// \return "maxAbs"
-      public: virtual std::string ShortName() const override;
-
-      // Documentation inherited.
-      public: virtual void InsertData(const double _data) override;
-    };
-
-    /// \class SignalVariance SignalStats.hh gz/math/SignalStats.hh
-    /// \brief Computing the incremental variance
-    /// of a discretely sampled signal.
-    class GZ_MATH_VISIBLE SignalVariance : public SignalStatistic
-    {
-      // Documentation inherited.
-      public: virtual double Value() const override;
-
-      /// \brief Get a short version of the name of this statistical measure.
-      /// \return "var"
-      public: virtual std::string ShortName() const override;
-
-      // Documentation inherited.
-      public: virtual void InsertData(const double _data) override;
-    };
-
-    /// \brief Forward declare private data class.
-    class SignalStatsPrivate;
-
-    /// \class SignalStats SignalStats.hh gz/math/SignalStats.hh
-    /// \brief Collection of statistics for a scalar signal.
-    class GZ_MATH_VISIBLE SignalStats
-    {
-      /// \brief Constructor
-      public: SignalStats();
-
-      /// \brief Destructor
-      public: ~SignalStats();
-
-      /// \brief Copy constructor
-      /// \param[in] _ss SignalStats to copy
-      public: SignalStats(const SignalStats &_ss);
-
-      /// \brief Get number of data points in first statistic.
-      /// Technically you can have different numbers of data points
-      /// in each statistic if you call InsertStatistic after InsertData,
-      /// but this is not a recommended use case.
-      /// \return Number of data points in first statistic.
-      public: size_t Count() const;
-
-      /// \brief Get the current values of each statistical measure,
-      /// stored in a map using the short name as the key.
-      /// \return Map with short name of each statistic as key
-      /// and value of statistic as the value.
-      public: std::map<std::string, double> Map() const;
-
-      /// \brief Add a new sample to the statistical measures.
-      /// \param[in] _data New signal data point.
-      public: void InsertData(const double _data);
-
-      /// \brief Add a new type of statistic.
-      /// \param[in] _name Short name of new statistic.
-      /// Valid values include:
-      ///  "maxAbs"
-      ///  "mean"
-      ///  "rms"
-      /// \return True if statistic was successfully added,
-      /// false if name was not recognized or had already
-      /// been inserted.
-      public: bool InsertStatistic(const std::string &_name);
-
-      /// \brief Add multiple statistics.
-      /// \param[in] _names Comma-separated list of new statistics.
-      /// For example, all statistics could be added with:
-      ///  "maxAbs,mean,rms"
-      /// \return True if all statistics were successfully added,
-      /// false if any names were not recognized or had already
-      /// been inserted.
-      public: bool InsertStatistics(const std::string &_names);
-
-      /// \brief Forget all previous data.
-      public: void Reset();
-
-      /// \brief Assignment operator
-      /// \param[in] _s A SignalStats to copy
-      /// \return this
-      public: SignalStats &operator=(const SignalStats &_s);
+  };
+
+  /// \class SignalMaximum SignalStats.hh gz/math/SignalStats.hh
+  /// \brief Computing the maximum value of a discretely sampled signal.
+  class GZ_MATH_VISIBLE SignalMaximum : public SignalStatistic
+  {
+    // Documentation inherited.
+    public: virtual double Value() const override;
+
+    /// \brief Get a short version of the name of this statistical measure.
+    /// \return "max"
+    public: virtual std::string ShortName() const override;
+
+    // Documentation inherited.
+    public: virtual void InsertData(const double _data) override;
+  };
+
+  /// \class SignalMean SignalStats.hh gz/math/SignalStats.hh
+  /// \brief Computing the mean value of a discretely sampled signal.
+  class GZ_MATH_VISIBLE SignalMean : public SignalStatistic
+  {
+    // Documentation inherited.
+    public: virtual double Value() const override;
+
+    /// \brief Get a short version of the name of this statistical measure.
+    /// \return "mean"
+    public: virtual std::string ShortName() const override;
+
+    // Documentation inherited.
+    public: virtual void InsertData(const double _data) override;
+  };
+
+  /// \class SignalMinimum SignalStats.hh gz/math/SignalStats.hh
+  /// \brief Computing the minimum value of a discretely sampled signal.
+  class GZ_MATH_VISIBLE SignalMinimum : public SignalStatistic
+  {
+    // Documentation inherited.
+    public: virtual double Value() const override;
+
+    /// \brief Get a short version of the name of this statistical measure.
+    /// \return "min"
+    public: virtual std::string ShortName() const override;
+
+    // Documentation inherited.
+    public: virtual void InsertData(const double _data) override;
+  };
+
+  /// \class SignalRootMeanSquare SignalStats.hh gz/math/SignalStats.hh
+  /// \brief Computing the square root of the mean squared value
+  /// of a discretely sampled signal.
+  class GZ_MATH_VISIBLE SignalRootMeanSquare : public SignalStatistic
+  {
+    // Documentation inherited.
+    public: virtual double Value() const override;
+
+    /// \brief Get a short version of the name of this statistical measure.
+    /// \return "rms"
+    public: virtual std::string ShortName() const override;
+
+    // Documentation inherited.
+    public: virtual void InsertData(const double _data) override;
+  };
+
+  /// \class SignalMaxAbsoluteValue SignalStats.hh
+  /// gz/math/SignalStats.hh
+  /// \brief Computing the maximum of the absolute value
+  /// of a discretely sampled signal.
+  /// Also known as the maximum norm, infinity norm, or supremum norm.
+  class GZ_MATH_VISIBLE SignalMaxAbsoluteValue : public SignalStatistic
+  {
+    // Documentation inherited.
+    public: virtual double Value() const override;
+
+    /// \brief Get a short version of the name of this statistical measure.
+    /// \return "maxAbs"
+    public: virtual std::string ShortName() const override;
+
+    // Documentation inherited.
+    public: virtual void InsertData(const double _data) override;
+  };
+
+  /// \class SignalVariance SignalStats.hh gz/math/SignalStats.hh
+  /// \brief Computing the incremental variance
+  /// of a discretely sampled signal.
+  class GZ_MATH_VISIBLE SignalVariance : public SignalStatistic
+  {
+    // Documentation inherited.
+    public: virtual double Value() const override;
+
+    /// \brief Get a short version of the name of this statistical measure.
+    /// \return "var"
+    public: virtual std::string ShortName() const override;
+
+    // Documentation inherited.
+    public: virtual void InsertData(const double _data) override;
+  };
+
+  /// \brief Forward declare private data class.
+  class SignalStatsPrivate;
+
+  /// \class SignalStats SignalStats.hh gz/math/SignalStats.hh
+  /// \brief Collection of statistics for a scalar signal.
+  class GZ_MATH_VISIBLE SignalStats
+  {
+    /// \brief Constructor
+    public: SignalStats();
+
+    /// \brief Destructor
+    public: ~SignalStats();
+
+    /// \brief Copy constructor
+    /// \param[in] _ss SignalStats to copy
+    public: SignalStats(const SignalStats &_ss);
+
+    /// \brief Get number of data points in first statistic.
+    /// Technically you can have different numbers of data points
+    /// in each statistic if you call InsertStatistic after InsertData,
+    /// but this is not a recommended use case.
+    /// \return Number of data points in first statistic.
+    public: size_t Count() const;
+
+    /// \brief Get the current values of each statistical measure,
+    /// stored in a map using the short name as the key.
+    /// \return Map with short name of each statistic as key
+    /// and value of statistic as the value.
+    public: std::map<std::string, double> Map() const;
+
+    /// \brief Add a new sample to the statistical measures.
+    /// \param[in] _data New signal data point.
+    public: void InsertData(const double _data);
+
+    /// \brief Add a new type of statistic.
+    /// \param[in] _name Short name of new statistic.
+    /// Valid values include:
+    ///  "maxAbs"
+    ///  "mean"
+    ///  "rms"
+    /// \return True if statistic was successfully added,
+    /// false if name was not recognized or had already
+    /// been inserted.
+    public: bool InsertStatistic(const std::string &_name);
+
+    /// \brief Add multiple statistics.
+    /// \param[in] _names Comma-separated list of new statistics.
+    /// For example, all statistics could be added with:
+    ///  "maxAbs,mean,rms"
+    /// \return True if all statistics were successfully added,
+    /// false if any names were not recognized or had already
+    /// been inserted.
+    public: bool InsertStatistics(const std::string &_names);
+
+    /// \brief Forget all previous data.
+    public: void Reset();
+
+    /// \brief Assignment operator
+    /// \param[in] _s A SignalStats to copy
+    /// \return this
+    public: SignalStats &operator=(const SignalStats &_s);
 
 #ifdef _WIN32
 // Disable warning C4251 which is triggered by
@@ -242,13 +240,12 @@ namespace gz
 #pragma warning(push)
 #pragma warning(disable: 4251)
 #endif
-      /// \brief Pointer to private data.
-      private: std::unique_ptr<SignalStatsPrivate> dataPtr;
+    /// \brief Pointer to private data.
+    private: std::unique_ptr<SignalStatsPrivate> dataPtr;
 #ifdef _WIN32
 #pragma warning(pop)
 #endif
-    };
-    }
-  }
-}
-#endif
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_SIGNALSTATS_HH_
diff --git a/include/gz/math/SpeedLimiter.hh b/include/gz/math/SpeedLimiter.hh
index db25f4f27..65c300569 100644
--- a/include/gz/math/SpeedLimiter.hh
+++ b/include/gz/math/SpeedLimiter.hh
@@ -23,12 +23,10 @@
 #include "gz/math/Helpers.hh"
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-namespace math
-{
-// Inline bracket to help doxygen filtering.
-inline namespace GZ_MATH_VERSION_NAMESPACE {
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
   /// \brief Class to limit velocity, acceleration and jerk.
   class GZ_MATH_VISIBLE SpeedLimiter
   {
@@ -127,8 +125,6 @@ inline namespace GZ_MATH_VERSION_NAMESPACE {
     /// \brief Private data pointer.
     GZ_UTILS_IMPL_PTR(dataPtr)
   };
-  }
-}
-}
-
-#endif
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_SYSTEMS_SPEEDLIMITER_HH_
diff --git a/include/gz/math/Sphere.hh b/include/gz/math/Sphere.hh
index 2b7dacaca..c75b70acb 100644
--- a/include/gz/math/Sphere.hh
+++ b/include/gz/math/Sphere.hh
@@ -23,139 +23,135 @@
 #include "gz/math/Quaternion.hh"
 #include "gz/math/Plane.hh"
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Sphere Sphere.hh gz/math/Sphere.hh
+  /// \brief A representation of a sphere.
+  ///
+  /// The sphere class supports defining a sphere with a radius and
+  /// material properties. Radius is in meters.
+  /// See Material for more on material properties.
+  template<typename Precision>
+  class Sphere
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Sphere Sphere.hh gz/math/Sphere.hh
-    /// \brief A representation of a sphere.
-    ///
-    /// The sphere class supports defining a sphere with a radius and
-    /// material properties. Radius is in meters.
-    /// See Material for more on material properties.
-    template<typename Precision>
-    class Sphere
-    {
-      /// \brief Default constructor. The default radius is zero.
-      public: Sphere() = default;
-
-      /// \brief Construct a sphere with a radius.
-      /// \param[in] _radius Radius of the sphere.
-      public: explicit Sphere(const Precision _radius);
-
-      /// \brief Construct a sphere with a radius, material
-      /// \param[in] _radius Radius of the sphere.
-      /// \param[in] _mat Material property for the sphere.
-      public: Sphere(const Precision _radius, const Material &_mat);
-
-      /// \brief Get the radius in meters.
-      /// \return The radius of the sphere in meters.
-      public: Precision Radius() const;
-
-      /// \brief Set the radius in meters.
-      /// \param[in] _radius The radius of the sphere in meters.
-      public: void SetRadius(const Precision _radius);
-
-      /// \brief Get the material associated with this sphere.
-      /// \return The material assigned to this sphere
-      public: const gz::math::Material &Material() const;
-
-      /// \brief Set the material associated with this sphere.
-      /// \param[in] _mat The material assigned to this sphere
-      public: void SetMaterial(const gz::math::Material &_mat);
-
-      /// \brief Get the mass matrix for this sphere. This function
-      /// is only meaningful if the sphere's radius and material have been set.
-      /// \param[out] _massMat The computed mass matrix will be stored
-      /// here.
-      /// \return False if computation of the mass matrix failed, which
-      /// could be due to an invalid radius (<=0) or density (<=0).
-      public: bool MassMatrix(MassMatrix3d &_massMat) const;
-
-      /// \brief Get the mass matrix for this sphere. This function
-      /// is only meaningful if the sphere's radius and material have been set.
-      /// \return The computed mass matrix if parameters are valid
-      /// (radius > 0) and (density > 0). Otherwise
-      /// std::nullopt is returned.
-      public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
-
-      /// \brief Check if this sphere is equal to the provided sphere.
-      /// Radius and material properties will be checked.
-      public: bool operator==(const Sphere &_sphere) const;
-
-      /// \brief Check if this sphere is not equal to the provided sphere.
-      /// Radius and material properties will be checked.
-      public: bool operator!=(const Sphere &_sphere) const;
-
-      /// \brief Get the volume of the sphere in m^3.
-      /// \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
-      /// value greater than zero. The Material of the sphere is ignored.
-      /// \param[in] _mass Mass of the sphere, in kg. This value should be
-      /// greater than zero.
-      /// \return Density of the sphere in kg/m^3. A negative value is
-      /// returned if radius or _mass is <= 0.
-      public: Precision DensityFromMass(const Precision _mass) const;
-
-      /// \brief Set the density of this sphere based on a mass value.
-      /// Density is computed using
-      /// Precision DensityFromMass(const Precision _mass) const. The
-      /// sphere is assumed to be solid with uniform density. This
-      /// function requires the sphere's radius to be set to a
-      /// value greater than zero. The existing Material density value is
-      /// overwritten only if the return value from this true.
-      /// \param[in] _mass Mass of the sphere, in kg. This value should be
-      /// greater than zero.
-      /// \return True if the density was set. False is returned if the
-      /// sphere's radius or the _mass value are <= 0.
-      /// \sa Precision DensityFromMass(const Precision _mass) const
-      public: bool SetDensityFromMass(const Precision _mass);
-
-      /// \brief Radius of the sphere.
-      private: Precision radius = 0.0;
-
-      /// \brief the sphere's material.
-      private: gz::math::Material material;
-    };
-
-    /// \typedef Sphere<int> Spherei
-    /// \brief Sphere with integer precision.
-    typedef Sphere<int> Spherei;
-
-    /// \typedef Sphere<double> Sphered
-    /// \brief Sphere with double precision.
-    typedef Sphere<double> Sphered;
-
-    /// \typedef Sphere<float> Spheref
-    /// \brief Sphere with float precision.
-    typedef Sphere<float> Spheref;
-    }
-  }
-}
+    /// \brief Default constructor. The default radius is zero.
+    public: Sphere() = default;
+
+    /// \brief Construct a sphere with a radius.
+    /// \param[in] _radius Radius of the sphere.
+    public: explicit Sphere(const Precision _radius);
+
+    /// \brief Construct a sphere with a radius, material
+    /// \param[in] _radius Radius of the sphere.
+    /// \param[in] _mat Material property for the sphere.
+    public: Sphere(const Precision _radius, const Material &_mat);
+
+    /// \brief Get the radius in meters.
+    /// \return The radius of the sphere in meters.
+    public: Precision Radius() const;
+
+    /// \brief Set the radius in meters.
+    /// \param[in] _radius The radius of the sphere in meters.
+    public: void SetRadius(const Precision _radius);
+
+    /// \brief Get the material associated with this sphere.
+    /// \return The material assigned to this sphere
+    public: const gz::math::Material &Material() const;
+
+    /// \brief Set the material associated with this sphere.
+    /// \param[in] _mat The material assigned to this sphere
+    public: void SetMaterial(const gz::math::Material &_mat);
+
+    /// \brief Get the mass matrix for this sphere. This function
+    /// is only meaningful if the sphere's radius and material have been set.
+    /// \param[out] _massMat The computed mass matrix will be stored
+    /// here.
+    /// \return False if computation of the mass matrix failed, which
+    /// could be due to an invalid radius (<=0) or density (<=0).
+    public: bool MassMatrix(MassMatrix3d &_massMat) const;
+
+    /// \brief Get the mass matrix for this sphere. This function
+    /// is only meaningful if the sphere's radius and material have been set.
+    /// \return The computed mass matrix if parameters are valid
+    /// (radius > 0) and (density > 0). Otherwise
+    /// std::nullopt is returned.
+    public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
+
+    /// \brief Check if this sphere is equal to the provided sphere.
+    /// Radius and material properties will be checked.
+    public: bool operator==(const Sphere &_sphere) const;
+
+    /// \brief Check if this sphere is not equal to the provided sphere.
+    /// Radius and material properties will be checked.
+    public: bool operator!=(const Sphere &_sphere) const;
+
+    /// \brief Get the volume of the sphere in m^3.
+    /// \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
+    /// value greater than zero. The Material of the sphere is ignored.
+    /// \param[in] _mass Mass of the sphere, in kg. This value should be
+    /// greater than zero.
+    /// \return Density of the sphere in kg/m^3. A negative value is
+    /// returned if radius or _mass is <= 0.
+    public: Precision DensityFromMass(const Precision _mass) const;
+
+    /// \brief Set the density of this sphere based on a mass value.
+    /// Density is computed using
+    /// Precision DensityFromMass(const Precision _mass) const. The
+    /// sphere is assumed to be solid with uniform density. This
+    /// function requires the sphere's radius to be set to a
+    /// value greater than zero. The existing Material density value is
+    /// overwritten only if the return value from this true.
+    /// \param[in] _mass Mass of the sphere, in kg. This value should be
+    /// greater than zero.
+    /// \return True if the density was set. False is returned if the
+    /// sphere's radius or the _mass value are <= 0.
+    /// \sa Precision DensityFromMass(const Precision _mass) const
+    public: bool SetDensityFromMass(const Precision _mass);
+
+    /// \brief Radius of the sphere.
+    private: Precision radius = 0.0;
+
+    /// \brief the sphere's material.
+    private: gz::math::Material material;
+  };
+
+  /// \typedef Sphere<int> Spherei
+  /// \brief Sphere with integer precision.
+  typedef Sphere<int> Spherei;
+
+  /// \typedef Sphere<double> Sphered
+  /// \brief Sphere with double precision.
+  typedef Sphere<double> Sphered;
+
+  /// \typedef Sphere<float> Spheref
+  /// \brief Sphere with float precision.
+  typedef Sphere<float> Spheref;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
 #include "gz/math/detail/Sphere.hh"
-
-#endif
+#endif  // GZ_MATH_SPHERE_HH_
diff --git a/include/gz/math/SphericalCoordinates.hh b/include/gz/math/SphericalCoordinates.hh
index b8fba584a..8cd36244e 100644
--- a/include/gz/math/SphericalCoordinates.hh
+++ b/include/gz/math/SphericalCoordinates.hh
@@ -25,279 +25,276 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+
+  /// \brief Convert spherical coordinates for planetary surfaces.
+  class GZ_MATH_VISIBLE SphericalCoordinates
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-
-    /// \brief Convert spherical coordinates for planetary surfaces.
-    class GZ_MATH_VISIBLE SphericalCoordinates
-    {
-      /// \enum SurfaceType
-      /// \brief Unique identifiers for planetary surface models.
-      public: enum SurfaceType
-              {
-                /// \brief Model of reference ellipsoid for earth, based on
-                /// WGS 84 standard. see wikipedia: World_Geodetic_System
-                EARTH_WGS84 = 1,
-
-                /// \brief Model of the moon, based on the Selenographic
-                /// coordinate system, see wikipedia: Selenographic
-                /// Coordinate System.
-                MOON_SCS = 2,
-
-                /// \brief Custom surface type
-                CUSTOM_SURFACE = 10
-              };
-
-      /// \enum CoordinateType
-      /// \brief Unique identifiers for coordinate types.
-      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
-              };
-
-      /// \brief Constructor.
-      public: SphericalCoordinates();
-
-      /// \brief Constructor with surface type input.
-      /// \param[in] _type SurfaceType specification.
-      public: explicit SphericalCoordinates(const SurfaceType _type);
-
-      /// \brief Constructor with surface type input and properties
-      /// input. To be used for CUSTOM_SURFACE.
-      /// \param[in] _type SurfaceType specification.
-      /// \param[in] _axisEquatorial Semi major axis of the surface.
-      /// \param[in] _axisPolar Semi minor axis of the surface.
-      public: SphericalCoordinates(
-            const SurfaceType _type,
-            const double _axisEquatorial,
-            const double _axisPolar);
-
-      /// \brief Constructor with surface type, angle, and elevation inputs.
-      /// \param[in] _type SurfaceType specification.
-      /// \param[in] _latitude Reference latitude.
-      /// \param[in] _longitude Reference longitude.
-      /// \param[in] _elevation Reference elevation.
-      /// \param[in] _heading Heading offset.
-      public: SphericalCoordinates(const SurfaceType _type,
-                                   const gz::math::Angle &_latitude,
-                                   const gz::math::Angle &_longitude,
-                                   const double _elevation,
-                                   const gz::math::Angle &_heading);
-
-      /// \brief Convert a Cartesian position vector to geodetic coordinates.
-      /// This performs a `PositionTransform` from LOCAL to SPHERICAL.
-      ///
-      /// There's a known bug with this computation that can't be fixed on
-      /// version 6 to avoid behaviour changes. Directly call
-      /// `PositionTransform(_xyz, LOCAL2, SPHERICAL)` for correct results.
-      /// Note that `PositionTransform` returns spherical coordinates in
-      /// radians.
-      ///
-      /// \param[in] _xyz Cartesian position vector in the heading-adjusted
-      /// world frame.
-      /// \return Cooordinates: geodetic latitude (deg), longitude (deg),
-      ///         altitude above sea level (m).
-      public: gz::math::Vector3d SphericalFromLocalPosition(
-                  const gz::math::Vector3d &_xyz) const;
-
-      /// \brief Convert a Cartesian velocity vector in the local frame
-      ///        to a global Cartesian frame with components East, North, Up.
-      /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)`
-      ///
-      /// There's a known bug with this computation that can't be fixed on
-      /// version 6 to avoid behaviour changes. Directly call
-      /// `VelocityTransform(_xyz, LOCAL2, GLOBAL)` for correct results.
-      ///
-      /// \param[in] _xyz Cartesian velocity vector in the heading-adjusted
-      /// world frame.
-      /// \return Rotated vector with components (x,y,z): (East, North, Up).
-      public: gz::math::Vector3d GlobalFromLocalVelocity(
-                  const gz::math::Vector3d &_xyz) const;
-
-      /// \brief Convert a string to a SurfaceType.
-      /// Allowed values: ["EARTH_WGS84"].
-      /// \param[in] _str String to convert.
-      /// \return Conversion to SurfaceType.
-      public: static SurfaceType Convert(const std::string &_str);
-
-      /// \brief Convert a SurfaceType to a string.
-      /// \param[in] _type Surface type
-      /// \return Type as string
-      public: static std::string Convert(SurfaceType _type);
-
-      /// \brief Get the distance between two points expressed in geographic
-      /// latitude and longitude. It assumes that both points are at sea level.
-      /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents
-      /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W.
-      /// This method assumes that the surface model is EARTH_WGS84.
-      /// \param[in] _latA Latitude of point A.
-      /// \param[in] _lonA Longitude of point A.
-      /// \param[in] _latB Latitude of point B.
-      /// \param[in] _lonB Longitude of point B.
-      /// \return Distance in meters.
-      public: static double DistanceWGS84(
-                                     const gz::math::Angle &_latA,
-                                     const gz::math::Angle &_lonA,
-                                     const gz::math::Angle &_latB,
-                                     const gz::math::Angle &_lonB);
-
-      /// \brief Get the distance between two points expressed in geographic
-      /// latitude and longitude. It assumes that both points are at sea level.
-      /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents
-      /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W.
-      /// This is different from the deprecated static Distance() method as it
-      /// takes into account the set surface's radius.
-      /// \param[in] _latA Latitude of point A.
-      /// \param[in] _lonA Longitude of point A.
-      /// \param[in] _latB Latitude of point B.
-      /// \param[in] _lonB Longitude of point B.
-      /// \return Distance in meters.
-      public: double DistanceBetweenPoints(
-                  const gz::math::Angle &_latA,
-                  const gz::math::Angle &_lonA,
-                  const gz::math::Angle &_latB,
-                  const gz::math::Angle &_lonB);
-
-      /// \brief Get SurfaceType currently in use.
-      /// \return Current SurfaceType value.
-      public: SurfaceType Surface() const;
-
-      /// \brief Get the radius of the surface.
-      /// \return radius of the surface in use.
-      public: double SurfaceRadius() const;
-
-      /// \brief Get the major axis of the surface.
-      /// \return Equatorial axis of the surface in use.
-      public: double SurfaceAxisEquatorial() const;
-
-      /// \brief Get the minor axis of the surface.
-      /// \return Polar axis of the surface in use.
-      public: double SurfaceAxisPolar() const;
-
-      /// \brief Get the flattening of the surface.
-      /// \return Flattening parameter of the surface in use.
-      public: double SurfaceFlattening() const;
-
-      /// \brief Get reference geodetic latitude.
-      /// \return Reference geodetic latitude.
-      public: gz::math::Angle LatitudeReference() const;
-
-      /// \brief Get reference longitude.
-      /// \return Reference longitude.
-      public: gz::math::Angle LongitudeReference() const;
-
-      /// \brief Get reference elevation in meters.
-      /// \return Reference elevation.
-      public: double ElevationReference() const;
-
-      /// \brief Get heading offset for the reference frame, expressed as
-      ///        angle from East to x-axis, or equivalently
-      ///        from North to y-axis.
-      /// \return Heading offset of reference frame.
-      public: gz::math::Angle HeadingOffset() const;
-
-      /// \brief Set SurfaceType for planetary surface model.
-      /// \param[in] _type SurfaceType value.
-      public: void SetSurface(const SurfaceType &_type);
-
-      /// \brief Set SurfaceType for planetary surface model with
-      /// custom ellipsoid properties.
-      /// \param[in] _type SurfaceType value.
-      /// \param[in] _axisEquatorial Equatorial axis of the surface.
-      /// \param[in] _axisPolar Polar axis of the surface.
-      public: void SetSurface(
-                  const SurfaceType &_type,
-                  const double _axisEquatorial,
-                  const double _axisPolar);
-
-      /// \brief Set reference geodetic latitude.
-      /// \param[in] _angle Reference geodetic latitude.
-      public: void SetLatitudeReference(const gz::math::Angle &_angle);
-
-      /// \brief Set reference longitude.
-      /// \param[in] _angle Reference longitude.
-      public: void SetLongitudeReference(const gz::math::Angle &_angle);
-
-      /// \brief Set reference elevation above sea level in meters.
-      /// \param[in] _elevation Reference elevation.
-      public: void SetElevationReference(const double _elevation);
-
-      /// \brief Set heading angle offset for the frame.
-      /// \param[in] _angle Heading offset for the frame.
-      public: void SetHeadingOffset(const gz::math::Angle &_angle);
-
-      /// \brief Convert a geodetic position vector to Cartesian coordinates.
-      /// This performs a `PositionTransform` from SPHERICAL to LOCAL.
-      /// \param[in] _latLonEle Geodetic position in the planetary frame of
-      /// reference. X: latitude (deg), Y: longitude (deg), X: altitude.
-      /// \return Cartesian position vector in the heading-adjusted world frame.
-      public: gz::math::Vector3d LocalFromSphericalPosition(
-                  const gz::math::Vector3d &_latLonEle) const;
-
-      /// \brief Convert a Cartesian velocity vector with components East,
-      /// North, Up to a local cartesian frame vector XYZ.
-      /// This is a wrapper around `VelocityTransform(_xyz, GLOBAL, LOCAL)`
-      /// \param[in] _xyz Vector with components (x,y,z): (East, North, Up).
-      /// \return Cartesian vector in the world frame.
-      public: gz::math::Vector3d LocalFromGlobalVelocity(
-                  const gz::math::Vector3d &_xyz) const;
-
-      /// \brief Update coordinate transformation matrix with reference location
-      public: void UpdateTransformationMatrix();
-
-      /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame
-      /// Spherical coordinates use radians, while the other frames use meters.
-      /// \param[in] _pos Position vector in frame defined by parameter _in
-      /// \param[in] _in  CoordinateType for input
-      /// \param[in] _out CoordinateType for output
-      /// \return Transformed coordinate using cached origin.
-      public: gz::math::Vector3d
-              PositionTransform(const gz::math::Vector3d &_pos,
-                  const CoordinateType &_in, const CoordinateType &_out) const;
-
-      /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame
-      /// Spherical coordinates use radians, while the other frames use meters.
-      /// \param[in] _vel Velocity vector in frame defined by parameter _in
-      /// \param[in] _in  CoordinateType for input
-      /// \param[in] _out CoordinateType for output
-      /// \return Transformed velocity vector
-      public: gz::math::Vector3d VelocityTransform(
-                  const gz::math::Vector3d &_vel,
-                  const CoordinateType &_in, const CoordinateType &_out) const;
-
-      /// \brief Equality operator, result = this == _sc
-      /// \param[in] _sc Spherical coordinates to check for equality
-      /// \return true if this == _sc
-      public: bool operator==(const SphericalCoordinates &_sc) const;
-
-      /// \brief Inequality
-      /// \param[in] _sc Spherical coordinates to check for inequality
-      /// \return true if this != _sc
-      public: bool operator!=(const SphericalCoordinates &_sc) const;
-
-      /// \brief Pointer to the private data
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
-#endif
+    /// \enum SurfaceType
+    /// \brief Unique identifiers for planetary surface models.
+    public: enum SurfaceType
+            {
+              /// \brief Model of reference ellipsoid for earth, based on
+              /// WGS 84 standard. see wikipedia: World_Geodetic_System
+              EARTH_WGS84 = 1,
+
+              /// \brief Model of the moon, based on the Selenographic
+              /// coordinate system, see wikipedia: Selenographic
+              /// Coordinate System.
+              MOON_SCS = 2,
+
+              /// \brief Custom surface type
+              CUSTOM_SURFACE = 10
+            };
+
+    /// \enum CoordinateType
+    /// \brief Unique identifiers for coordinate types.
+    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
+            };
+
+    /// \brief Constructor.
+    public: SphericalCoordinates();
+
+    /// \brief Constructor with surface type input.
+    /// \param[in] _type SurfaceType specification.
+    public: explicit SphericalCoordinates(const SurfaceType _type);
+
+    /// \brief Constructor with surface type input and properties
+    /// input. To be used for CUSTOM_SURFACE.
+    /// \param[in] _type SurfaceType specification.
+    /// \param[in] _axisEquatorial Semi major axis of the surface.
+    /// \param[in] _axisPolar Semi minor axis of the surface.
+    public: SphericalCoordinates(
+          const SurfaceType _type,
+          const double _axisEquatorial,
+          const double _axisPolar);
+
+    /// \brief Constructor with surface type, angle, and elevation inputs.
+    /// \param[in] _type SurfaceType specification.
+    /// \param[in] _latitude Reference latitude.
+    /// \param[in] _longitude Reference longitude.
+    /// \param[in] _elevation Reference elevation.
+    /// \param[in] _heading Heading offset.
+    public: SphericalCoordinates(const SurfaceType _type,
+                                 const gz::math::Angle &_latitude,
+                                 const gz::math::Angle &_longitude,
+                                 const double _elevation,
+                                 const gz::math::Angle &_heading);
+
+    /// \brief Convert a Cartesian position vector to geodetic coordinates.
+    /// This performs a `PositionTransform` from LOCAL to SPHERICAL.
+    ///
+    /// There's a known bug with this computation that can't be fixed on
+    /// version 6 to avoid behaviour changes. Directly call
+    /// `PositionTransform(_xyz, LOCAL2, SPHERICAL)` for correct results.
+    /// Note that `PositionTransform` returns spherical coordinates in
+    /// radians.
+    ///
+    /// \param[in] _xyz Cartesian position vector in the heading-adjusted
+    /// world frame.
+    /// \return Cooordinates: geodetic latitude (deg), longitude (deg),
+    ///         altitude above sea level (m).
+    public: gz::math::Vector3d SphericalFromLocalPosition(
+                const gz::math::Vector3d &_xyz) const;
+
+    /// \brief Convert a Cartesian velocity vector in the local frame
+    ///        to a global Cartesian frame with components East, North, Up.
+    /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)`
+    ///
+    /// There's a known bug with this computation that can't be fixed on
+    /// version 6 to avoid behaviour changes. Directly call
+    /// `VelocityTransform(_xyz, LOCAL2, GLOBAL)` for correct results.
+    ///
+    /// \param[in] _xyz Cartesian velocity vector in the heading-adjusted
+    /// world frame.
+    /// \return Rotated vector with components (x,y,z): (East, North, Up).
+    public: gz::math::Vector3d GlobalFromLocalVelocity(
+                const gz::math::Vector3d &_xyz) const;
+
+    /// \brief Convert a string to a SurfaceType.
+    /// Allowed values: ["EARTH_WGS84"].
+    /// \param[in] _str String to convert.
+    /// \return Conversion to SurfaceType.
+    public: static SurfaceType Convert(const std::string &_str);
+
+    /// \brief Convert a SurfaceType to a string.
+    /// \param[in] _type Surface type
+    /// \return Type as string
+    public: static std::string Convert(SurfaceType _type);
+
+    /// \brief Get the distance between two points expressed in geographic
+    /// latitude and longitude. It assumes that both points are at sea level.
+    /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents
+    /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W.
+    /// This method assumes that the surface model is EARTH_WGS84.
+    /// \param[in] _latA Latitude of point A.
+    /// \param[in] _lonA Longitude of point A.
+    /// \param[in] _latB Latitude of point B.
+    /// \param[in] _lonB Longitude of point B.
+    /// \return Distance in meters.
+    public: static double DistanceWGS84(
+                                   const gz::math::Angle &_latA,
+                                   const gz::math::Angle &_lonA,
+                                   const gz::math::Angle &_latB,
+                                   const gz::math::Angle &_lonB);
+
+    /// \brief Get the distance between two points expressed in geographic
+    /// latitude and longitude. It assumes that both points are at sea level.
+    /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents
+    /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W.
+    /// This is different from the deprecated static Distance() method as it
+    /// takes into account the set surface's radius.
+    /// \param[in] _latA Latitude of point A.
+    /// \param[in] _lonA Longitude of point A.
+    /// \param[in] _latB Latitude of point B.
+    /// \param[in] _lonB Longitude of point B.
+    /// \return Distance in meters.
+    public: double DistanceBetweenPoints(
+                const gz::math::Angle &_latA,
+                const gz::math::Angle &_lonA,
+                const gz::math::Angle &_latB,
+                const gz::math::Angle &_lonB);
+
+    /// \brief Get SurfaceType currently in use.
+    /// \return Current SurfaceType value.
+    public: SurfaceType Surface() const;
+
+    /// \brief Get the radius of the surface.
+    /// \return radius of the surface in use.
+    public: double SurfaceRadius() const;
+
+    /// \brief Get the major axis of the surface.
+    /// \return Equatorial axis of the surface in use.
+    public: double SurfaceAxisEquatorial() const;
+
+    /// \brief Get the minor axis of the surface.
+    /// \return Polar axis of the surface in use.
+    public: double SurfaceAxisPolar() const;
+
+    /// \brief Get the flattening of the surface.
+    /// \return Flattening parameter of the surface in use.
+    public: double SurfaceFlattening() const;
+
+    /// \brief Get reference geodetic latitude.
+    /// \return Reference geodetic latitude.
+    public: gz::math::Angle LatitudeReference() const;
+
+    /// \brief Get reference longitude.
+    /// \return Reference longitude.
+    public: gz::math::Angle LongitudeReference() const;
+
+    /// \brief Get reference elevation in meters.
+    /// \return Reference elevation.
+    public: double ElevationReference() const;
+
+    /// \brief Get heading offset for the reference frame, expressed as
+    ///        angle from East to x-axis, or equivalently
+    ///        from North to y-axis.
+    /// \return Heading offset of reference frame.
+    public: gz::math::Angle HeadingOffset() const;
+
+    /// \brief Set SurfaceType for planetary surface model.
+    /// \param[in] _type SurfaceType value.
+    public: void SetSurface(const SurfaceType &_type);
+
+    /// \brief Set SurfaceType for planetary surface model with
+    /// custom ellipsoid properties.
+    /// \param[in] _type SurfaceType value.
+    /// \param[in] _axisEquatorial Equatorial axis of the surface.
+    /// \param[in] _axisPolar Polar axis of the surface.
+    public: void SetSurface(
+                const SurfaceType &_type,
+                const double _axisEquatorial,
+                const double _axisPolar);
+
+    /// \brief Set reference geodetic latitude.
+    /// \param[in] _angle Reference geodetic latitude.
+    public: void SetLatitudeReference(const gz::math::Angle &_angle);
+
+    /// \brief Set reference longitude.
+    /// \param[in] _angle Reference longitude.
+    public: void SetLongitudeReference(const gz::math::Angle &_angle);
+
+    /// \brief Set reference elevation above sea level in meters.
+    /// \param[in] _elevation Reference elevation.
+    public: void SetElevationReference(const double _elevation);
+
+    /// \brief Set heading angle offset for the frame.
+    /// \param[in] _angle Heading offset for the frame.
+    public: void SetHeadingOffset(const gz::math::Angle &_angle);
+
+    /// \brief Convert a geodetic position vector to Cartesian coordinates.
+    /// This performs a `PositionTransform` from SPHERICAL to LOCAL.
+    /// \param[in] _latLonEle Geodetic position in the planetary frame of
+    /// reference. X: latitude (deg), Y: longitude (deg), X: altitude.
+    /// \return Cartesian position vector in the heading-adjusted world frame.
+    public: gz::math::Vector3d LocalFromSphericalPosition(
+                const gz::math::Vector3d &_latLonEle) const;
+
+    /// \brief Convert a Cartesian velocity vector with components East,
+    /// North, Up to a local cartesian frame vector XYZ.
+    /// This is a wrapper around `VelocityTransform(_xyz, GLOBAL, LOCAL)`
+    /// \param[in] _xyz Vector with components (x,y,z): (East, North, Up).
+    /// \return Cartesian vector in the world frame.
+    public: gz::math::Vector3d LocalFromGlobalVelocity(
+                const gz::math::Vector3d &_xyz) const;
+
+    /// \brief Update coordinate transformation matrix with reference location
+    public: void UpdateTransformationMatrix();
+
+    /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame
+    /// Spherical coordinates use radians, while the other frames use meters.
+    /// \param[in] _pos Position vector in frame defined by parameter _in
+    /// \param[in] _in  CoordinateType for input
+    /// \param[in] _out CoordinateType for output
+    /// \return Transformed coordinate using cached origin.
+    public: gz::math::Vector3d
+            PositionTransform(const gz::math::Vector3d &_pos,
+                const CoordinateType &_in, const CoordinateType &_out) const;
+
+    /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame
+    /// Spherical coordinates use radians, while the other frames use meters.
+    /// \param[in] _vel Velocity vector in frame defined by parameter _in
+    /// \param[in] _in  CoordinateType for input
+    /// \param[in] _out CoordinateType for output
+    /// \return Transformed velocity vector
+    public: gz::math::Vector3d VelocityTransform(
+                const gz::math::Vector3d &_vel,
+                const CoordinateType &_in, const CoordinateType &_out) const;
+
+    /// \brief Equality operator, result = this == _sc
+    /// \param[in] _sc Spherical coordinates to check for equality
+    /// \return true if this == _sc
+    public: bool operator==(const SphericalCoordinates &_sc) const;
+
+    /// \brief Inequality
+    /// \param[in] _sc Spherical coordinates to check for inequality
+    /// \return true if this != _sc
+    public: bool operator!=(const SphericalCoordinates &_sc) const;
+
+    /// \brief Pointer to the private data
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_SPHERICALCOORDINATES_HH_
diff --git a/include/gz/math/Spline.hh b/include/gz/math/Spline.hh
index 9ae1db47e..d4236ea91 100644
--- a/include/gz/math/Spline.hh
+++ b/include/gz/math/Spline.hh
@@ -24,240 +24,237 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    // Forward declare private classes
-    class ControlPoint;
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  // Forward declare private classes
+  class ControlPoint;
 
-    /// \class Spline Spline.hh gz/math/Spline.hh
-    /// \brief Splines
-    class GZ_MATH_VISIBLE Spline
-    {
-      /// \brief constructor
-      public: Spline();
+  /// \class Spline Spline.hh gz/math/Spline.hh
+  /// \brief Splines
+  class GZ_MATH_VISIBLE Spline
+  {
+    /// \brief constructor
+    public: Spline();
 
-      /// \brief Sets the tension parameter.
-      /// \remarks A value of 0 results in a Catmull-Rom
-      /// spline.
-      /// \param[in] _t Tension value between 0.0 and 1.0
-      public: void Tension(double _t);
+    /// \brief Sets the tension parameter.
+    /// \remarks A value of 0 results in a Catmull-Rom
+    /// spline.
+    /// \param[in] _t Tension value between 0.0 and 1.0
+    public: void Tension(double _t);
 
-      /// \brief Gets the tension value.
-      /// \return the value of the tension, which is between 0.0 and 1.0.
-      public: double Tension() const;
+    /// \brief Gets the tension value.
+    /// \return the value of the tension, which is between 0.0 and 1.0.
+    public: double Tension() const;
 
-      /// \brief Gets spline arc length.
-      /// \return arc length or INF on error.
-      public: double ArcLength() const;
+    /// \brief Gets spline arc length.
+    /// \return arc length or INF on error.
+    public: double ArcLength() const;
 
-      /// \brief Sets spline arc length up to
-      /// a given parameter value \p _t.
-      /// \param[in] _t parameter value (range 0 to 1).
-      /// \return arc length up to \p _t or INF on error.
-      public: double ArcLength(const double _t) const;
+    /// \brief Sets spline arc length up to
+    /// a given parameter value \p _t.
+    /// \param[in] _t parameter value (range 0 to 1).
+    /// \return arc length up to \p _t or INF on error.
+    public: double ArcLength(const double _t) const;
 
-      /// \brief Sets a spline segment arc length.
-      /// \param[in] _index of the spline segment.
-      /// \param[in] _t parameter value (range 0 to 1).
-      /// \return arc length of a given segment up to
-      /// \p _t or INF on error.
-      public: double ArcLength(const unsigned int _index,
-                               const double _t) const;
+    /// \brief Sets a spline segment arc length.
+    /// \param[in] _index of the spline segment.
+    /// \param[in] _t parameter value (range 0 to 1).
+    /// \return arc length of a given segment up to
+    /// \p _t or INF on error.
+    public: double ArcLength(const unsigned int _index,
+                             const double _t) const;
 
-      /// \brief Adds a single control point to the
-      /// end of the spline.
-      /// \param[in] _p control point value to add.
-      public: void AddPoint(const Vector3d &_p);
+    /// \brief Adds a single control point to the
+    /// end of the spline.
+    /// \param[in] _p control point value to add.
+    public: void AddPoint(const Vector3d &_p);
 
-      /// \brief Adds a single control point to the end
-      /// of the spline with fixed tangent.
-      /// \param[in] _p control point value to add.
-      /// \param[in] _t tangent at \p _p.
-      public: void AddPoint(const Vector3d &_p, const Vector3d &_t);
+    /// \brief Adds a single control point to the end
+    /// of the spline with fixed tangent.
+    /// \param[in] _p control point value to add.
+    /// \param[in] _t tangent at \p _p.
+    public: void AddPoint(const Vector3d &_p, const Vector3d &_t);
 
-      /// \brief Adds a single control point to the end
-      /// of the spline.
-      /// \param[in] _cp control point to add.
-      /// \param[in] _fixed whether this control point
-      /// should not be subject to tangent recomputation.
-      private: void AddPoint(const ControlPoint &_cp, const bool _fixed);
+    /// \brief Adds a single control point to the end
+    /// of the spline.
+    /// \param[in] _cp control point to add.
+    /// \param[in] _fixed whether this control point
+    /// should not be subject to tangent recomputation.
+    private: void AddPoint(const ControlPoint &_cp, const bool _fixed);
 
-      /// \brief Gets the value for one of the control points
-      /// of the spline.
-      /// \param[in] _index the control point index.
-      /// \return the control point value, or [INF, INF, INF]
-      /// on error. Use Vector3d::IsFinite() to check for an error.
-      public: Vector3d Point(const unsigned int _index) const;
+    /// \brief Gets the value for one of the control points
+    /// of the spline.
+    /// \param[in] _index the control point index.
+    /// \return the control point value, or [INF, INF, INF]
+    /// on error. Use Vector3d::IsFinite() to check for an error.
+    public: Vector3d Point(const unsigned int _index) const;
 
-      /// \brief Gets the tangent value for one of the control points
-      /// of the spline.
-      /// \param[in] _index the control point index.
-      /// \return the control point tangent, or [INF, INF, INF]
-      /// on error. Use Vector3d::IsFinite() to check for an error.
-      public: Vector3d Tangent(const unsigned int _index) const;
+    /// \brief Gets the tangent value for one of the control points
+    /// of the spline.
+    /// \param[in] _index the control point index.
+    /// \return the control point tangent, or [INF, INF, INF]
+    /// on error. Use Vector3d::IsFinite() to check for an error.
+    public: Vector3d Tangent(const unsigned int _index) const;
 
-      /// \brief Gets the mth derivative for one of the control points
-      /// of the spline.
-      /// \param[in] _index the control point index.
-      /// \param[in] _mth derivative order.
-      /// \return the control point mth derivative, or [INF, INF, INF]
-      ///  on error. Use Vector3d::IsFinite() to check for an error.
-      public: Vector3d MthDerivative(const unsigned int _index,
-                                     const unsigned int _mth) const;
+    /// \brief Gets the mth derivative for one of the control points
+    /// of the spline.
+    /// \param[in] _index the control point index.
+    /// \param[in] _mth derivative order.
+    /// \return the control point mth derivative, or [INF, INF, INF]
+    ///  on error. Use Vector3d::IsFinite() to check for an error.
+    public: Vector3d MthDerivative(const unsigned int _index,
+                                   const unsigned int _mth) const;
 
-      /// \brief Gets the number of control points in the spline.
-      /// \return the count
-      public: size_t PointCount() const;
+    /// \brief Gets the number of control points in the spline.
+    /// \return the count
+    public: size_t PointCount() const;
 
-      /// \brief Clears all the points in the spline.
-      public: void Clear();
+    /// \brief Clears all the points in the spline.
+    public: void Clear();
 
-      /// \brief Updates a single control point value in the spline,
-      /// keeping its tangent.
-      /// \param[in] _index the control point index.
-      /// \param[in] _p the new control point value.
-      /// \return True on success.
-      public: bool UpdatePoint(const unsigned int _index,
-                               const Vector3d &_p);
+    /// \brief Updates a single control point value in the spline,
+    /// keeping its tangent.
+    /// \param[in] _index the control point index.
+    /// \param[in] _p the new control point value.
+    /// \return True on success.
+    public: bool UpdatePoint(const unsigned int _index,
+                             const Vector3d &_p);
 
-      /// \brief Updates a single control point in the spline, along
-      /// with its tangent.
-      /// \param[in] _index the control point index.
-      /// \param[in] _p the new control point value.
-      /// \param[in] _t the new control point tangent.
-      /// \return True on success.
-      public: bool UpdatePoint(const unsigned int _index,
-                               const Vector3d &_p,
-                               const Vector3d &_t);
+    /// \brief Updates a single control point in the spline, along
+    /// with its tangent.
+    /// \param[in] _index the control point index.
+    /// \param[in] _p the new control point value.
+    /// \param[in] _t the new control point tangent.
+    /// \return True on success.
+    public: bool UpdatePoint(const unsigned int _index,
+                             const Vector3d &_p,
+                             const Vector3d &_t);
 
-      /// \brief Updates a single control point in the spline.
-      /// \param[in] _index the control point index
-      /// \param[in] _cp the new control point
-      /// \param[in] _fixed whether the new control point should not be
-      /// subject to tangent recomputation
-      /// \return True on success.
-      private: bool UpdatePoint(const unsigned int _index,
-                                const ControlPoint &_cp,
-                                const bool _fixed);
+    /// \brief Updates a single control point in the spline.
+    /// \param[in] _index the control point index
+    /// \param[in] _cp the new control point
+    /// \param[in] _fixed whether the new control point should not be
+    /// subject to tangent recomputation
+    /// \return True on success.
+    private: bool UpdatePoint(const unsigned int _index,
+                              const ControlPoint &_cp,
+                              const bool _fixed);
 
-      /// \brief Interpolates a point on the spline
-      /// at parameter value \p _t.
-      /// \remarks Parameter value is normalized over the
-      /// whole spline arc length. Arc length is assumed
-      /// to be linear with the parameter.
-      /// \param[in] _t parameter value (range 0 to 1).
-      /// \return the interpolated point, or
-      /// [INF, INF, INF] on error. Use
-      /// Vector3d::IsFinite() to check for an error.
-      public: Vector3d Interpolate(const double _t) const;
+    /// \brief Interpolates a point on the spline
+    /// at parameter value \p _t.
+    /// \remarks Parameter value is normalized over the
+    /// whole spline arc length. Arc length is assumed
+    /// to be linear with the parameter.
+    /// \param[in] _t parameter value (range 0 to 1).
+    /// \return the interpolated point, or
+    /// [INF, INF, INF] on error. Use
+    /// Vector3d::IsFinite() to check for an error.
+    public: Vector3d Interpolate(const double _t) const;
 
-      /// \brief Interpolates a point on a segment of the spline
-      /// at parameter value \p _t.
-      /// \remarks Parameter value is normalized over the
-      /// segment arc length. Arc length is assumed
-      /// to be linear with the parameter.
-      /// \param[in] _fromIndex The point index to treat as t = 0.
-      /// fromIndex + 1 is deemed to be t = 1.
-      /// \param[in] _t parameter value (range 0 to 1).
-      /// \return the interpolated point, or [INF, INF, INF] on
-      /// error. Use Vector3d::IsFinite() to check for an error.
-      public: Vector3d Interpolate(const unsigned int _fromIndex,
-                                   const double _t) const;
+    /// \brief Interpolates a point on a segment of the spline
+    /// at parameter value \p _t.
+    /// \remarks Parameter value is normalized over the
+    /// segment arc length. Arc length is assumed
+    /// to be linear with the parameter.
+    /// \param[in] _fromIndex The point index to treat as t = 0.
+    /// fromIndex + 1 is deemed to be t = 1.
+    /// \param[in] _t parameter value (range 0 to 1).
+    /// \return the interpolated point, or [INF, INF, INF] on
+    /// error. Use Vector3d::IsFinite() to check for an error.
+    public: Vector3d Interpolate(const unsigned int _fromIndex,
+                                 const double _t) const;
 
-      /// \brief Interpolates a tangent on the spline at
-      /// parameter value \p _t.
-      /// \remarks Parameter value is normalized over the
-      /// whole spline arc length. Arc length is assumed
-      /// to be linear with the parameter.
-      /// \param[in] _t parameter value (range 0 to 1).
-      /// \return the interpolated point, or [INF, INF, INF]
-      /// on error. Use Vector3d::IsFinte() to check for an error.
-      public: Vector3d InterpolateTangent(const double _t) const;
+    /// \brief Interpolates a tangent on the spline at
+    /// parameter value \p _t.
+    /// \remarks Parameter value is normalized over the
+    /// whole spline arc length. Arc length is assumed
+    /// to be linear with the parameter.
+    /// \param[in] _t parameter value (range 0 to 1).
+    /// \return the interpolated point, or [INF, INF, INF]
+    /// on error. Use Vector3d::IsFinte() to check for an error.
+    public: Vector3d InterpolateTangent(const double _t) const;
 
-      /// \brief Interpolates the tangent on a segment of the spline
-      /// at parameter value \p _t.
-      /// \remarks Parameter value is normalized over the
-      /// segment arc length. Arc length is assumed
-      /// to be linear with the parameter.
-      /// \param[in] _fromIndex the point index to treat as t = 0.
-      /// fromIndex + 1 is deemed to be t = 1.
-      /// \param[in] _t parameter value (range 0 to 1).
-      /// \return the interpolated point, or [INF, INF, INF] on
-      /// error. Use Vector3d::IsFinte() to check for an error.
-      public: Vector3d InterpolateTangent(const unsigned int _fromIndex,
-                                          const double _t) const;
+    /// \brief Interpolates the tangent on a segment of the spline
+    /// at parameter value \p _t.
+    /// \remarks Parameter value is normalized over the
+    /// segment arc length. Arc length is assumed
+    /// to be linear with the parameter.
+    /// \param[in] _fromIndex the point index to treat as t = 0.
+    /// fromIndex + 1 is deemed to be t = 1.
+    /// \param[in] _t parameter value (range 0 to 1).
+    /// \return the interpolated point, or [INF, INF, INF] on
+    /// error. Use Vector3d::IsFinte() to check for an error.
+    public: Vector3d InterpolateTangent(const unsigned int _fromIndex,
+                                        const double _t) const;
 
-      /// \brief Interpolates the mth derivative of the spline at
-      /// parameter value \p _t.
-      /// \param[in] _mth order of curve derivative to interpolate.
-      /// \param[in] _1 parameter value (range 0 to 1).
-      /// \return the interpolated mth derivative, or [INF, INF, INF]
-      /// on error. Use Vector3d::IsFinite() to check for an error.
-      public: Vector3d InterpolateMthDerivative(const unsigned int _mth,
-                                                const double _1) const;
+    /// \brief Interpolates the mth derivative of the spline at
+    /// parameter value \p _t.
+    /// \param[in] _mth order of curve derivative to interpolate.
+    /// \param[in] _1 parameter value (range 0 to 1).
+    /// \return the interpolated mth derivative, or [INF, INF, INF]
+    /// on error. Use Vector3d::IsFinite() to check for an error.
+    public: Vector3d InterpolateMthDerivative(const unsigned int _mth,
+                                              const double _1) const;
 
-      /// \brief Interpolates the mth derivative of a segment of the spline
-      /// at parameter value \p _t.
-      /// \remarks Parameter value is normalized over the segment
-      /// arc length. Arc length is assumed to be linear with the parameter.
-      /// \param[in] _fromIndex point index to treat as t = 0, fromIndex + 1
-      /// is deemed to be t = 1.
-      /// \param[in] _mth order of curve derivative to interpolate.
-      /// \param[in] _s parameter value (range 0 to 1).
-      /// \return the interpolated mth derivative, or [INF, INF, INF] on
-      /// error. Use Vector3d::IsFinite() to check for an error.
-      public: Vector3d InterpolateMthDerivative(const unsigned int _fromIndex,
-                                                const unsigned int _mth,
-                                                const double _s) const;
+    /// \brief Interpolates the mth derivative of a segment of the spline
+    /// at parameter value \p _t.
+    /// \remarks Parameter value is normalized over the segment
+    /// arc length. Arc length is assumed to be linear with the parameter.
+    /// \param[in] _fromIndex point index to treat as t = 0, fromIndex + 1
+    /// is deemed to be t = 1.
+    /// \param[in] _mth order of curve derivative to interpolate.
+    /// \param[in] _s parameter value (range 0 to 1).
+    /// \return the interpolated mth derivative, or [INF, INF, INF] on
+    /// error. Use Vector3d::IsFinite() to check for an error.
+    public: Vector3d InterpolateMthDerivative(const unsigned int _fromIndex,
+                                              const unsigned int _mth,
+                                              const double _s) const;
 
-      /// \brief Tells the spline whether it should automatically
-      ///        calculate tangents on demand as points are added.
-      /// \remarks The spline calculates tangents at each point
-      ///          automatically based on the input points. Normally it
-      ///          does this every time a point changes. However, if you
-      ///          have a lot of points to add in one go, you probably
-      ///          don't want to incur this overhead and would prefer to
-      ///          defer the calculation until you are finished setting all
-      ///          the points. You can do this by calling this method with a
-      ///          parameter of 'false'. Just remember to manually call the
-      ///          recalcTangents method when you are done.
-      /// \param[in] _autoCalc If true, tangents are calculated for you whenever
-      ///        a point changes. If false, you must call RecalcTangents to
-      ///        recalculate them when it best suits.
-      public: void AutoCalculate(bool _autoCalc);
+    /// \brief Tells the spline whether it should automatically
+    ///        calculate tangents on demand as points are added.
+    /// \remarks The spline calculates tangents at each point
+    ///          automatically based on the input points. Normally it
+    ///          does this every time a point changes. However, if you
+    ///          have a lot of points to add in one go, you probably
+    ///          don't want to incur this overhead and would prefer to
+    ///          defer the calculation until you are finished setting all
+    ///          the points. You can do this by calling this method with a
+    ///          parameter of 'false'. Just remember to manually call the
+    ///          recalcTangents method when you are done.
+    /// \param[in] _autoCalc If true, tangents are calculated for you whenever
+    ///        a point changes. If false, you must call RecalcTangents to
+    ///        recalculate them when it best suits.
+    public: void AutoCalculate(bool _autoCalc);
 
-      /// \brief Recalculates the tangents associated with this spline.
-      /// \remarks If you tell the spline not to update on demand by
-      ///          calling setAutoCalculate(false) then you must call this
-      ///          after completing your updates to the spline points.
-      public: void RecalcTangents();
+    /// \brief Recalculates the tangents associated with this spline.
+    /// \remarks If you tell the spline not to update on demand by
+    ///          calling setAutoCalculate(false) then you must call this
+    ///          after completing your updates to the spline points.
+    public: void RecalcTangents();
 
-      /// \brief Rebuilds spline segments.
-      private: void Rebuild();
+    /// \brief Rebuilds spline segments.
+    private: void Rebuild();
 
-      /// \internal
-      /// \brief Maps \p _t parameter value over the whole spline
-      /// to the right segment (starting at point \p _index) with
-      /// the proper parameter value fraction \p _fraction.
-      /// \remarks Arc length is assumed to be linear with the parameter.
-      /// \param[in] _t parameter value over the whole spline (range 0 to 1).
-      /// \param[out] _index point index at which the segment starts.
-      /// \param[out] _fraction parameter value fraction for the given segment.
-      /// \return True on success.
-      private: bool MapToSegment(const double _t,
-                                 unsigned int &_index,
-                                 double &_fraction) const;
+    /// \internal
+    /// \brief Maps \p _t parameter value over the whole spline
+    /// to the right segment (starting at point \p _index) with
+    /// the proper parameter value fraction \p _fraction.
+    /// \remarks Arc length is assumed to be linear with the parameter.
+    /// \param[in] _t parameter value over the whole spline (range 0 to 1).
+    /// \param[out] _index point index at which the segment starts.
+    /// \param[out] _fraction parameter value fraction for the given segment.
+    /// \return True on success.
+    private: bool MapToSegment(const double _t,
+                               unsigned int &_index,
+                               double &_fraction) const;
 
-      /// \internal
-      /// \brief Private data pointer
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
-#endif
+    /// \internal
+    /// \brief Private data pointer
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_SPLINE_HH_
diff --git a/include/gz/math/Stopwatch.hh b/include/gz/math/Stopwatch.hh
index fcfa40be8..e9d4bc08f 100644
--- a/include/gz/math/Stopwatch.hh
+++ b/include/gz/math/Stopwatch.hh
@@ -23,104 +23,101 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Use a steady clock
-    // This alias is now deprecated; please use std::chrono::steady_clock
-    // directly instead.
-    using clock
-        [[deprecated("As of 8.0, use std::chrono::steady_clock directly.")]]
-        = std::chrono::steady_clock;
+  // Use a steady clock
+  // This alias is now deprecated; please use std::chrono::steady_clock
+  // directly instead.
+  using clock
+      [[deprecated("As of 8.0, use std::chrono::steady_clock directly.")]]
+      = std::chrono::steady_clock;
 
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \class Stopwatch Stopwatch.hh gz/math/Stopwatch.hh
-    /// \brief The Stopwatch keeps track of time spent in the run state,
-    /// accessed through ElapsedRunTime(), and time spent in the stop state,
-    /// accessed through ElapsedStopTime(). Elapsed run time starts accumulating
-    /// after the first call to Start(). Elapsed stop time starts
-    /// accumulation after Start() has been called followed by Stop(). The
-    /// stopwatch can be reset with the Reset() function.
-    ///
-    /// # Example usage
-    ///
-    /// ```{.cpp}
-    /// gz::math::Stopwatch watch;
-    /// watch.Start();
-    ///
-    /// // do something...
-    ///
-    /// std::cout << "Elapsed time is "
-    /// << std::chrono::duration_cast<std::chrono::milliseconds>(
-    ///   timeSys.ElapsedRunTime()).count() << " ms\n";
-    /// watch.Stop();
-    /// ```
-    class GZ_MATH_VISIBLE Stopwatch
-    {
-      /// \brief Constructor.
-      public: Stopwatch();
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \class Stopwatch Stopwatch.hh gz/math/Stopwatch.hh
+  /// \brief The Stopwatch keeps track of time spent in the run state,
+  /// accessed through ElapsedRunTime(), and time spent in the stop state,
+  /// accessed through ElapsedStopTime(). Elapsed run time starts accumulating
+  /// after the first call to Start(). Elapsed stop time starts
+  /// accumulation after Start() has been called followed by Stop(). The
+  /// stopwatch can be reset with the Reset() function.
+  ///
+  /// # Example usage
+  ///
+  /// ```{.cpp}
+  /// gz::math::Stopwatch watch;
+  /// watch.Start();
+  ///
+  /// // do something...
+  ///
+  /// std::cout << "Elapsed time is "
+  /// << std::chrono::duration_cast<std::chrono::milliseconds>(
+  ///   timeSys.ElapsedRunTime()).count() << " ms\n";
+  /// watch.Stop();
+  /// ```
+  class GZ_MATH_VISIBLE Stopwatch
+  {
+    /// \brief Constructor.
+    public: Stopwatch();
 
-      /// \brief Start the stopwatch.
-      /// \param[in] _reset If true the stopwatch is reset first.
-      /// \return True if the the stopwatch was started. This will return
-      /// false if the stopwatch was already running.
-      public: bool Start(const bool _reset = false);
+    /// \brief Start the stopwatch.
+    /// \param[in] _reset If true the stopwatch is reset first.
+    /// \return True if the the stopwatch was started. This will return
+    /// false if the stopwatch was already running.
+    public: bool Start(const bool _reset = false);
 
-      /// \brief Get the time when the stopwatch was started.
-      /// \return The time when stopwatch was started, or
-      /// std::chrono::steady_clock::time_point::min() if the stopwatch
-      /// has not been started.
-      public: std::chrono::steady_clock::time_point StartTime() const;
+    /// \brief Get the time when the stopwatch was started.
+    /// \return The time when stopwatch was started, or
+    /// std::chrono::steady_clock::time_point::min() if the stopwatch
+    /// has not been started.
+    public: std::chrono::steady_clock::time_point StartTime() const;
 
-      /// \brief Stop the stopwatch
-      /// \return True if the stopwatch was stopped. This will return false
-      /// if the stopwatch is not running.
-      public: bool Stop();
+    /// \brief Stop the stopwatch
+    /// \return True if the stopwatch was stopped. This will return false
+    /// if the stopwatch is not running.
+    public: bool Stop();
 
-      /// \brief Get the time when the stopwatch was last stopped.
-      /// \return The time when stopwatch was last stopped, or
-      /// std::chrono::steady_clock::time_point::min() if the stopwatch
-      /// has never been stopped.
-      public: std::chrono::steady_clock::time_point StopTime() const;
+    /// \brief Get the time when the stopwatch was last stopped.
+    /// \return The time when stopwatch was last stopped, or
+    /// std::chrono::steady_clock::time_point::min() if the stopwatch
+    /// has never been stopped.
+    public: std::chrono::steady_clock::time_point StopTime() const;
 
-      /// \brief Get whether the stopwatch is running.
-      /// \return True if the stopwatch is running.
-      public: bool Running() const;
+    /// \brief Get whether the stopwatch is running.
+    /// \return True if the stopwatch is running.
+    public: bool Running() const;
 
-      /// \brief Reset the stopwatch. This resets the start time, stop time,
-      /// elapsed duration and elapsed stop duration.
-      public: void Reset();
+    /// \brief Reset the stopwatch. This resets the start time, stop time,
+    /// elapsed duration and elapsed stop duration.
+    public: void Reset();
 
-      /// \brief Get the amount of time that the stop watch has been
-      /// running. This is the total amount of run time, spannning all start
-      /// and stop calls. The Reset function or passing true to the Start
-      /// function will reset this value.
-      /// \return Total amount of elapsed run time.
-      public: std::chrono::steady_clock::duration ElapsedRunTime() const;
+    /// \brief Get the amount of time that the stop watch has been
+    /// running. This is the total amount of run time, spannning all start
+    /// and stop calls. The Reset function or passing true to the Start
+    /// function will reset this value.
+    /// \return Total amount of elapsed run time.
+    public: std::chrono::steady_clock::duration ElapsedRunTime() const;
 
-      /// \brief Get the amount of time that the stop watch has been
-      /// stopped. This is the total amount of stop time, spannning all start
-      /// and stop calls. The Reset function or passing true to the Start
-      /// function will reset this value.
-      /// \return Total amount of elapsed stop time.
-      public: std::chrono::steady_clock::duration ElapsedStopTime() const;
+    /// \brief Get the amount of time that the stop watch has been
+    /// stopped. This is the total amount of stop time, spannning all start
+    /// and stop calls. The Reset function or passing true to the Start
+    /// function will reset this value.
+    /// \return Total amount of elapsed stop time.
+    public: std::chrono::steady_clock::duration ElapsedStopTime() const;
 
-      /// \brief Equality operator.
-      /// \param[in] _watch The watch to compare.
-      /// \return True if this watch equals the provided watch.
-      public: bool operator==(const Stopwatch &_watch) const;
+    /// \brief Equality operator.
+    /// \param[in] _watch The watch to compare.
+    /// \return True if this watch equals the provided watch.
+    public: bool operator==(const Stopwatch &_watch) const;
 
-      /// \brief Inequality operator.
-      /// \param[in] _watch The watch to compare.
-      /// \return True if this watch does not equal the provided watch.
-      public: bool operator!=(const Stopwatch &_watch) const;
+    /// \brief Inequality operator.
+    /// \param[in] _watch The watch to compare.
+    /// \return True if this watch does not equal the provided watch.
+    public: bool operator!=(const Stopwatch &_watch) const;
 
-      /// \brief Private data pointer.
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-  }
-  }
-}
-#endif
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_STOPWATCH_HH_
diff --git a/include/gz/math/Temperature.hh b/include/gz/math/Temperature.hh
index 9d352486b..94ae49689 100644
--- a/include/gz/math/Temperature.hh
+++ b/include/gz/math/Temperature.hh
@@ -25,346 +25,343 @@
 #include <gz/utils/ImplPtr.hh>
 
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \brief A class that stores temperature information, and allows
+  /// conversion between different units.
+  ///
+  /// This class is mostly for convenience. It can be used to easily
+  /// convert between temperature units and encapsulate temperature values.
+  ///
+  /// The default unit is Kelvin. Most functions that accept a double
+  /// value will assume the double is Kelvin. The exceptions are a few of
+  /// the conversion functions, such as CelsiusToFahrenheit. Similarly,
+  /// most doubles that are returned will be in Kelvin.
+  ///
+  /// ## Examples
+  ///
+  /// * C++
+  /// \snippet examples/temperature_example.cc complete
+  ///
+  /// * Ruby
+  /// \code{.cc}
+  /// # Modify the RUBYLIB environment variable to include the Gazebo Math
+  /// # library install path. For example, if you install to /user:
+  /// #
+  /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
+  /// #
+  /// require 'gz/math'
+  ///
+  /// celsius = Gz::Math::Temperature::KelvinToCelsius(2.5);
+  /// printf("2.5Kelvin to Celsius is %f\n", celsius)
+  ///
+  /// temp = Gz::Math::Temperature.new(123.5)
+  /// printf("Constructed a Temperature object with %f Kelvin\n",
+  ///        temp.Kelvin())
+  ///
+  /// printf("Same temperature in Celsius %f\n", temp.Celsius())
+  ///
+  /// temp += 100.0
+  /// printf("Temperature + 100.0 is %fK", temp.Kelvin())
+  ///
+  /// newTemp = Gz::Math::Temperature.new(temp.Kelvin())
+  /// newTemp += temp + 23.5;
+  /// printf("Copied temp and added 23.5K. The new tempurature is %fF\n",
+  ///     newTemp.Fahrenheit());
+  /// \endcode
+  class GZ_MATH_VISIBLE Temperature
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \brief A class that stores temperature information, and allows
-    /// conversion between different units.
-    ///
-    /// This class is mostly for convenience. It can be used to easily
-    /// convert between temperature units and encapsulate temperature values.
-    ///
-    /// The default unit is Kelvin. Most functions that accept a double
-    /// value will assume the double is Kelvin. The exceptions are a few of
-    /// the conversion functions, such as CelsiusToFahrenheit. Similarly,
-    /// most doubles that are returned will be in Kelvin.
-    ///
-    /// ## Examples
-    ///
-    /// * C++
-    /// \snippet examples/temperature_example.cc complete
-    ///
-    /// * Ruby
-    /// \code{.cc}
-    /// # Modify the RUBYLIB environment variable to include the Gazebo Math
-    /// # library install path. For example, if you install to /user:
-    /// #
-    /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
-    /// #
-    /// require 'gz/math'
-    ///
-    /// celsius = Gz::Math::Temperature::KelvinToCelsius(2.5);
-    /// printf("2.5Kelvin to Celsius is %f\n", celsius)
-    ///
-    /// temp = Gz::Math::Temperature.new(123.5)
-    /// printf("Constructed a Temperature object with %f Kelvin\n",
-    ///        temp.Kelvin())
-    ///
-    /// printf("Same temperature in Celsius %f\n", temp.Celsius())
-    ///
-    /// temp += 100.0
-    /// printf("Temperature + 100.0 is %fK", temp.Kelvin())
-    ///
-    /// newTemp = Gz::Math::Temperature.new(temp.Kelvin())
-    /// newTemp += temp + 23.5;
-    /// printf("Copied temp and added 23.5K. The new tempurature is %fF\n",
-    ///     newTemp.Fahrenheit());
-    /// \endcode
-    class GZ_MATH_VISIBLE Temperature
+    /// \brief Default constructor.
+    public: Temperature();
+
+    /// \brief Kelvin value constructor. This is a conversion constructor,
+    /// and assumes the passed in value is in Kelvin.
+    /// \param[in] _temp Temperature in Kelvin.
+    // cppcheck-suppress noExplicitConstructor
+    public: Temperature(double _temp);
+
+    /// \brief Convert Kelvin to Celsius.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Temperature in Celsius.
+    public: static double KelvinToCelsius(double _temp);
+
+    /// \brief Convert Kelvin to Fahrenheit.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Temperature in Fahrenheit.
+    public: static double KelvinToFahrenheit(double _temp);
+
+    /// \brief Convert Celsius to Fahrenheit.
+    /// \param[in] _temp Temperature in Celsius.
+    /// \return Temperature in Fahrenheit.
+    public: static double CelsiusToFahrenheit(double _temp);
+
+    /// \brief Convert Celsius to Kelvin
+    /// \param[in] _temp Temperature in Celsius
+    /// \return Temperature in Kelvin
+    public: static double CelsiusToKelvin(double _temp);
+
+    /// \brief Convert Fahrenheit to Celsius
+    /// \param[in] _temp Temperature in Fahrenheit
+    /// \return Temperature in Celsius
+    public: static double FahrenheitToCelsius(double _temp);
+
+    /// \brief Convert Fahrenheit to Kelvin
+    /// \param[in] _temp Temperature in Fahrenheit
+    /// \return Temperature in Kelvin
+    public: static double FahrenheitToKelvin(double _temp);
+
+    /// \brief Set the temperature from a Kelvin value.
+    /// \param[in] _temp Temperature in Kelvin.
+    public: void SetKelvin(double _temp);
+
+    /// \brief Set the temperature from a Celsius value.
+    /// \param[in] _temp Temperature in Celsius.
+    public: void SetCelsius(double _temp);
+
+    /// \brief Set the temperature from a Fahrenheit value.
+    /// \param[in] _temp Temperature in Fahrenheit.
+    public: void SetFahrenheit(double _temp);
+
+    /// \brief Get the temperature in Kelvin.
+    /// \return Temperature in Kelvin.
+    public: double Kelvin() const;
+
+    /// \brief Get the temperature in Celsius.
+    /// \return Temperature in Celsius.
+    public: double Celsius() const;
+
+    /// \brief Get the temperature in Fahrenheit.
+    /// \return Temperature in Fahrenheit.
+    public: double Fahrenheit() const;
+
+    /// \brief Accessor operator.
+    /// \return Temperature in Kelvin.
+    /// \sa Kelvin().
+    public: double operator()() const;
+
+    /// \brief Assignment operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Reference to this instance.
+    public: Temperature &operator=(double _temp);
+
+    /// \brief Addition operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Resulting temperature.
+    public: Temperature operator+(double _temp) const;
+
+    /// \brief Addition operator.
+    /// \param[in] _temp Temperature object.
+    /// \return Resulting temperature.
+    public: Temperature operator+(const Temperature &_temp) const;
+
+    /// \brief Addition operator for double type.
+    /// \param[in] _t Temperature in Kelvin.
+    /// \param[in] _temp Temperature object.
+    /// \return Resulting temperature.
+    public: friend Temperature operator+(double _t, const Temperature &_temp)
     {
-      /// \brief Default constructor.
-      public: Temperature();
-
-      /// \brief Kelvin value constructor. This is a conversion constructor,
-      /// and assumes the passed in value is in Kelvin.
-      /// \param[in] _temp Temperature in Kelvin.
-      // cppcheck-suppress noExplicitConstructor
-      public: Temperature(double _temp);
-
-      /// \brief Convert Kelvin to Celsius.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Temperature in Celsius.
-      public: static double KelvinToCelsius(double _temp);
-
-      /// \brief Convert Kelvin to Fahrenheit.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Temperature in Fahrenheit.
-      public: static double KelvinToFahrenheit(double _temp);
-
-      /// \brief Convert Celsius to Fahrenheit.
-      /// \param[in] _temp Temperature in Celsius.
-      /// \return Temperature in Fahrenheit.
-      public: static double CelsiusToFahrenheit(double _temp);
-
-      /// \brief Convert Celsius to Kelvin
-      /// \param[in] _temp Temperature in Celsius
-      /// \return Temperature in Kelvin
-      public: static double CelsiusToKelvin(double _temp);
-
-      /// \brief Convert Fahrenheit to Celsius
-      /// \param[in] _temp Temperature in Fahrenheit
-      /// \return Temperature in Celsius
-      public: static double FahrenheitToCelsius(double _temp);
-
-      /// \brief Convert Fahrenheit to Kelvin
-      /// \param[in] _temp Temperature in Fahrenheit
-      /// \return Temperature in Kelvin
-      public: static double FahrenheitToKelvin(double _temp);
-
-      /// \brief Set the temperature from a Kelvin value.
-      /// \param[in] _temp Temperature in Kelvin.
-      public: void SetKelvin(double _temp);
-
-      /// \brief Set the temperature from a Celsius value.
-      /// \param[in] _temp Temperature in Celsius.
-      public: void SetCelsius(double _temp);
-
-      /// \brief Set the temperature from a Fahrenheit value.
-      /// \param[in] _temp Temperature in Fahrenheit.
-      public: void SetFahrenheit(double _temp);
-
-      /// \brief Get the temperature in Kelvin.
-      /// \return Temperature in Kelvin.
-      public: double Kelvin() const;
-
-      /// \brief Get the temperature in Celsius.
-      /// \return Temperature in Celsius.
-      public: double Celsius() const;
-
-      /// \brief Get the temperature in Fahrenheit.
-      /// \return Temperature in Fahrenheit.
-      public: double Fahrenheit() const;
-
-      /// \brief Accessor operator.
-      /// \return Temperature in Kelvin.
-      /// \sa Kelvin().
-      public: double operator()() const;
-
-      /// \brief Assignment operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Reference to this instance.
-      public: Temperature &operator=(double _temp);
-
-      /// \brief Addition operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Resulting temperature.
-      public: Temperature operator+(double _temp) const;
-
-      /// \brief Addition operator.
-      /// \param[in] _temp Temperature object.
-      /// \return Resulting temperature.
-      public: Temperature operator+(const Temperature &_temp) const;
-
-      /// \brief Addition operator for double type.
-      /// \param[in] _t Temperature in Kelvin.
-      /// \param[in] _temp Temperature object.
-      /// \return Resulting temperature.
-      public: friend Temperature operator+(double _t, const Temperature &_temp)
-      {
-        return _t + _temp.Kelvin();
-      }
-
-      /// \brief Addition assignment operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Reference to this instance.
-      public: const Temperature &operator+=(double _temp);
-
-      /// \brief Addition assignment operator.
-      /// \param[in] _temp Temperature object.
-      /// \return Reference to this instance.
-      public: const Temperature &operator+=(const Temperature &_temp);
-
-      /// \brief Subtraction operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Resulting temperature.
-      public: Temperature operator-(double _temp) const;
-
-      /// \brief Subtraction operator.
-      /// \param[in] _temp Temperature object.
-      /// \return Resulting temperature.
-      public: Temperature operator-(const Temperature &_temp) const;
-
-      /// \brief Subtraction operator for double type.
-      /// \param[in] _t Temperature in Kelvin.
-      /// \param[in] _temp Temperature object.
-      /// \return Resulting temperature.
-      public: friend Temperature operator-(double _t, const Temperature &_temp)
-      {
-        return _t - _temp.Kelvin();
-      }
-
-      /// \brief Subtraction assignment operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Reference to this instance.
-      public: const Temperature &operator-=(double _temp);
-
-      /// \brief Subtraction assignment operator.
-      /// \param[in] _temp Temperature object.
-      /// \return Reference to this instance.
-      public: const Temperature &operator-=(const Temperature &_temp);
-
-      /// \brief Multiplication operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Resulting temperature.
-      public: Temperature operator*(double _temp) const;
-
-      /// \brief Multiplication operator.
-      /// \param[in] _temp Temperature object.
-      /// \return Resulting temperature.
-      public: Temperature operator*(const Temperature &_temp) const;
-
-      /// \brief Multiplication operator for double type.
-      /// \param[in] _t Temperature in Kelvin.
-      /// \param[in] _temp Temperature object.
-      /// \return Resulting temperature.
-      public: friend Temperature operator*(double _t, const Temperature &_temp)
-      {
-        return _t * _temp.Kelvin();
-      }
-
-      /// \brief Multiplication assignment operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Reference to this instance.
-      public: const Temperature &operator*=(double _temp);
-
-      /// \brief Multiplication assignment operator.
-      /// \param[in] _temp Temperature object.
-      /// \return Reference to this instance.
-      public: const Temperature &operator*=(const Temperature &_temp);
-
-      /// \brief Division operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Resulting temperature.
-      public: Temperature operator/(double _temp) const;
-
-      /// \brief Division operator.
-      /// \param[in] _temp Temperature object.
-      /// \return Resulting temperature.
-      public: Temperature operator/(const Temperature &_temp) const;
-
-      /// \brief Division operator for double type.
-      /// \param[in] _t Temperature in Kelvin.
-      /// \param[in] _temp Temperature object.
-      /// \return Resulting temperature.
-      public: friend Temperature operator/(double _t, const Temperature &_temp)
-      {
-        return _t / _temp.Kelvin();
-      }
-
-      /// \brief Division assignment operator.
-      /// \param[in] _temp Temperature in Kelvin.
-      /// \return Reference to this instance.
-      public: const Temperature &operator/=(double _temp);
-
-      /// \brief Division assignment operator.
-      /// \param[in] _temp Temperature object.
-      /// \return Reference to this instance.
-      public: const Temperature &operator/=(const Temperature &_temp);
-
-      /// \brief Equal to operator.
-      /// \param[in] _temp The temperature to compare.
-      /// \return True if the temperatures are the same, false otherwise.
-      public: bool operator==(const Temperature &_temp) const;
-
-      /// \brief Equal to operator, where the value of _temp is assumed to
-      /// be in Kelvin.
-      /// \param[in] _temp The temperature (in Kelvin) to compare.
-      /// \return True if the temperatures are the same, false otherwise.
-      public: bool operator==(double _temp) const;
-
-      /// \brief Inequality to operator.
-      /// \param[in] _temp The temperature to compare.
-      /// \return False if the temperatures are the same, true otherwise.
-      public: bool operator!=(const Temperature &_temp) const;
-
-      /// \brief Inequality to operator, where the value of _temp is assumed to
-      /// be in Kelvin.
-      /// \param[in] _temp The temperature (in Kelvin) to compare.
-      /// \return False if the temperatures are the same, true otherwise.
-      public: bool operator!=(double _temp) const;
-
-      /// \brief Less than to operator.
-      /// \param[in] _temp The temperature to compare.
-      /// \return True if this is less than _temp.
-      public: bool operator<(const Temperature &_temp) const;
-
-      /// \brief Less than operator, where the value of _temp is assumed to
-      /// be in Kelvin.
-      /// \param[in] _temp The temperature (in Kelvin) to compare.
-      /// \return True if this is less than _temp.
-      public: bool operator<(double _temp) const;
-
-      /// \brief Less than or equal to operator.
-      /// \param[in] _temp The temperature to compare.
-      /// \return True if this is less than or equal _temp.
-      public: bool operator<=(const Temperature &_temp) const;
-
-      /// \brief Less than or equal operator,
-      /// where the value of _temp is assumed to be in Kelvin.
-      /// \param[in] _temp The temperature (in Kelvin) to compare.
-      /// \return True if this is less than or equal to _temp.
-      public: bool operator<=(double _temp) const;
-
-      /// \brief Greater than operator.
-      /// \param[in] _temp The temperature to compare.
-      /// \return True if this is greater than _temp.
-      public: bool operator>(const Temperature &_temp) const;
-
-      /// \brief Greater than operator, where the value of _temp is assumed to
-      /// be in Kelvin.
-      /// \param[in] _temp The temperature (in Kelvin) to compare.
-      /// \return True if this is greater than _temp.
-      public: bool operator>(double _temp) const;
-
-      /// \brief Greater than or equal to operator.
-      /// \param[in] _temp The temperature to compare.
-      /// \return True if this is greater than or equal to _temp.
-      public: bool operator>=(const Temperature &_temp) const;
-
-      /// \brief Greater than equal operator,
-      /// where the value of _temp is assumed to be in Kelvin.
-      /// \param[in] _temp The temperature (in Kelvin) to compare.
-      /// \return True if this is greater than or equal to _temp.
-      public: bool operator>=(double _temp) const;
-
-      /// \brief Stream insertion operator.
-      /// \param[in] _out The output stream.
-      /// \param[in] _temp Temperature to write to the stream.
-      /// \return The output stream.
-      public: friend std::ostream &operator<<(std::ostream &_out,
-                  const gz::math::Temperature &_temp)
-              {
-                _out << _temp.Kelvin();
-                return _out;
-              }
-
-      /// \brief Stream extraction operator.
-      /// \param[in] _in the input stream.
-      /// \param[in] _temp Temperature to read from to the stream. Assumes
-      /// temperature value is in Kelvin.
-      /// \return The input stream.
-      public: friend std::istream &operator>>(std::istream &_in,
-                  gz::math::Temperature &_temp)
-              {
-                // Skip white spaces
-                _in.setf(std::ios_base::skipws);
+      return _t + _temp.Kelvin();
+    }
 
-                double kelvin;
-                _in >> kelvin;
+    /// \brief Addition assignment operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Reference to this instance.
+    public: const Temperature &operator+=(double _temp);
+
+    /// \brief Addition assignment operator.
+    /// \param[in] _temp Temperature object.
+    /// \return Reference to this instance.
+    public: const Temperature &operator+=(const Temperature &_temp);
+
+    /// \brief Subtraction operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Resulting temperature.
+    public: Temperature operator-(double _temp) const;
+
+    /// \brief Subtraction operator.
+    /// \param[in] _temp Temperature object.
+    /// \return Resulting temperature.
+    public: Temperature operator-(const Temperature &_temp) const;
+
+    /// \brief Subtraction operator for double type.
+    /// \param[in] _t Temperature in Kelvin.
+    /// \param[in] _temp Temperature object.
+    /// \return Resulting temperature.
+    public: friend Temperature operator-(double _t, const Temperature &_temp)
+    {
+      return _t - _temp.Kelvin();
+    }
 
-                if (!_in.fail())
-                {
-                  _temp.SetKelvin(kelvin);
-                }
-                return _in;
-              }
+    /// \brief Subtraction assignment operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Reference to this instance.
+    public: const Temperature &operator-=(double _temp);
+
+    /// \brief Subtraction assignment operator.
+    /// \param[in] _temp Temperature object.
+    /// \return Reference to this instance.
+    public: const Temperature &operator-=(const Temperature &_temp);
+
+    /// \brief Multiplication operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Resulting temperature.
+    public: Temperature operator*(double _temp) const;
+
+    /// \brief Multiplication operator.
+    /// \param[in] _temp Temperature object.
+    /// \return Resulting temperature.
+    public: Temperature operator*(const Temperature &_temp) const;
+
+    /// \brief Multiplication operator for double type.
+    /// \param[in] _t Temperature in Kelvin.
+    /// \param[in] _temp Temperature object.
+    /// \return Resulting temperature.
+    public: friend Temperature operator*(double _t, const Temperature &_temp)
+    {
+      return _t * _temp.Kelvin();
+    }
 
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
+    /// \brief Multiplication assignment operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Reference to this instance.
+    public: const Temperature &operator*=(double _temp);
+
+    /// \brief Multiplication assignment operator.
+    /// \param[in] _temp Temperature object.
+    /// \return Reference to this instance.
+    public: const Temperature &operator*=(const Temperature &_temp);
+
+    /// \brief Division operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Resulting temperature.
+    public: Temperature operator/(double _temp) const;
+
+    /// \brief Division operator.
+    /// \param[in] _temp Temperature object.
+    /// \return Resulting temperature.
+    public: Temperature operator/(const Temperature &_temp) const;
+
+    /// \brief Division operator for double type.
+    /// \param[in] _t Temperature in Kelvin.
+    /// \param[in] _temp Temperature object.
+    /// \return Resulting temperature.
+    public: friend Temperature operator/(double _t, const Temperature &_temp)
+    {
+      return _t / _temp.Kelvin();
     }
-  }
-}
-#endif
+
+    /// \brief Division assignment operator.
+    /// \param[in] _temp Temperature in Kelvin.
+    /// \return Reference to this instance.
+    public: const Temperature &operator/=(double _temp);
+
+    /// \brief Division assignment operator.
+    /// \param[in] _temp Temperature object.
+    /// \return Reference to this instance.
+    public: const Temperature &operator/=(const Temperature &_temp);
+
+    /// \brief Equal to operator.
+    /// \param[in] _temp The temperature to compare.
+    /// \return True if the temperatures are the same, false otherwise.
+    public: bool operator==(const Temperature &_temp) const;
+
+    /// \brief Equal to operator, where the value of _temp is assumed to
+    /// be in Kelvin.
+    /// \param[in] _temp The temperature (in Kelvin) to compare.
+    /// \return True if the temperatures are the same, false otherwise.
+    public: bool operator==(double _temp) const;
+
+    /// \brief Inequality to operator.
+    /// \param[in] _temp The temperature to compare.
+    /// \return False if the temperatures are the same, true otherwise.
+    public: bool operator!=(const Temperature &_temp) const;
+
+    /// \brief Inequality to operator, where the value of _temp is assumed to
+    /// be in Kelvin.
+    /// \param[in] _temp The temperature (in Kelvin) to compare.
+    /// \return False if the temperatures are the same, true otherwise.
+    public: bool operator!=(double _temp) const;
+
+    /// \brief Less than to operator.
+    /// \param[in] _temp The temperature to compare.
+    /// \return True if this is less than _temp.
+    public: bool operator<(const Temperature &_temp) const;
+
+    /// \brief Less than operator, where the value of _temp is assumed to
+    /// be in Kelvin.
+    /// \param[in] _temp The temperature (in Kelvin) to compare.
+    /// \return True if this is less than _temp.
+    public: bool operator<(double _temp) const;
+
+    /// \brief Less than or equal to operator.
+    /// \param[in] _temp The temperature to compare.
+    /// \return True if this is less than or equal _temp.
+    public: bool operator<=(const Temperature &_temp) const;
+
+    /// \brief Less than or equal operator,
+    /// where the value of _temp is assumed to be in Kelvin.
+    /// \param[in] _temp The temperature (in Kelvin) to compare.
+    /// \return True if this is less than or equal to _temp.
+    public: bool operator<=(double _temp) const;
+
+    /// \brief Greater than operator.
+    /// \param[in] _temp The temperature to compare.
+    /// \return True if this is greater than _temp.
+    public: bool operator>(const Temperature &_temp) const;
+
+    /// \brief Greater than operator, where the value of _temp is assumed to
+    /// be in Kelvin.
+    /// \param[in] _temp The temperature (in Kelvin) to compare.
+    /// \return True if this is greater than _temp.
+    public: bool operator>(double _temp) const;
+
+    /// \brief Greater than or equal to operator.
+    /// \param[in] _temp The temperature to compare.
+    /// \return True if this is greater than or equal to _temp.
+    public: bool operator>=(const Temperature &_temp) const;
+
+    /// \brief Greater than equal operator,
+    /// where the value of _temp is assumed to be in Kelvin.
+    /// \param[in] _temp The temperature (in Kelvin) to compare.
+    /// \return True if this is greater than or equal to _temp.
+    public: bool operator>=(double _temp) const;
+
+    /// \brief Stream insertion operator.
+    /// \param[in] _out The output stream.
+    /// \param[in] _temp Temperature to write to the stream.
+    /// \return The output stream.
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                const gz::math::Temperature &_temp)
+            {
+              _out << _temp.Kelvin();
+              return _out;
+            }
+
+    /// \brief Stream extraction operator.
+    /// \param[in] _in the input stream.
+    /// \param[in] _temp Temperature to read from to the stream. Assumes
+    /// temperature value is in Kelvin.
+    /// \return The input stream.
+    public: friend std::istream &operator>>(std::istream &_in,
+                gz::math::Temperature &_temp)
+            {
+              // Skip white spaces
+              _in.setf(std::ios_base::skipws);
+
+              double kelvin;
+              _in >> kelvin;
+
+              if (!_in.fail())
+              {
+                _temp.SetKelvin(kelvin);
+              }
+              return _in;
+            }
+
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_TEMPERATURE_HH_
diff --git a/include/gz/math/TimeVaryingVolumetricGrid.hh b/include/gz/math/TimeVaryingVolumetricGrid.hh
index 27c737f3c..5d8eb4dd0 100644
--- a/include/gz/math/TimeVaryingVolumetricGrid.hh
+++ b/include/gz/math/TimeVaryingVolumetricGrid.hh
@@ -25,11 +25,8 @@
 #include <utility>
 #include <vector>
 
-namespace gz
+namespace gz::math
 {
-namespace math
-{
-
 /// \brief A grid with interpolation where time can be varied. This class has
 /// no implementation to allow for different strategies.
 /// \sa InMemoryTimeVaryingVolumetricGrid for an example
@@ -179,8 +176,5 @@ class InMemoryTimeVaryingVolumetricGridFactory
   /// Temporary datastore
   private: std::map<T, std::vector<std::pair<Vector3d, V>>> _points;
 };
-
-}
-}
-
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_HH_
diff --git a/include/gz/math/TimeVaryingVolumetricGridLookupField.hh b/include/gz/math/TimeVaryingVolumetricGridLookupField.hh
index 9acb75e25..0fdf4d02c 100644
--- a/include/gz/math/TimeVaryingVolumetricGridLookupField.hh
+++ b/include/gz/math/TimeVaryingVolumetricGridLookupField.hh
@@ -25,293 +25,290 @@
 #include <utility>
 #include <vector>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  /// \brief Lookup table for a time-varying volumetric dataset.
+  /// This is an unimplemented template as the actual methods depend on the
+  /// underlying structure. The key idea is that one uses a session `S` to
+  /// hold a session token. This is so that we don't keep doing O(logn)
+  /// lookups and instead step along the axis.
+  template<typename T, typename V, typename S>
+  class TimeVaryingVolumetricGridLookupField
   {
-    /// \brief Lookup table for a time-varying volumetric dataset.
-    /// This is an unimplemented template as the actual methods depend on the
-    /// underlying structure. The key idea is that one uses a session `S` to
-    /// hold a session token. This is so that we don't keep doing O(logn)
-    /// lookups and instead step along the axis.
-    template<typename T, typename V, typename S>
-    class TimeVaryingVolumetricGridLookupField
-    {
-      /// \brief Constructor
-      public: TimeVaryingVolumetricGridLookupField();
-
-      /// \brief Adds a volumetric grid field.
-      public: void AddVolumetricGridField(
-        const T& time, const VolumetricGridLookupField<V> &_field);
-
-      /// \brief Creates a session for querying
-      public: S CreateSession() const;
-
-      /// \brief Creates a session startingg at time T
-      /// \param[in] _time The time to create the session at.
-      public: S CreateSession(const T &_time) const;
-
-      /// \brief Update session to new time. Returns new session pointer if
-      /// more is available. Otherwise returns nullopt if we have run out of
-      /// time.
-      /// \param[in] _session - The session
-      /// \param[in] _time - Time to step to.
-      public: std::optional<S> StepTo(const S &_session, const T &_time);
-
-      /// \brief Looks up interpolators at a given time step. Use the session
-      /// to set the time step.
-      /// \param[in] _session - The session
-      /// \param[in] _point - The point to query
-      /// \param[in] _tol - Tolerance
-      public: std::vector<InterpolationPoint4D<T, V>>
-      LookUp(const S &_session,
-        const Vector3<V> &_point,
-        const Vector3<V> &_tol) const;
-
-      /// \brief Uses quadrilinear interpolation to estimate value of current
-      /// point. Returns nullopt if query is out of range.
-      /// \param[in] _session - The session
-      /// \param[in] _points - The interpolation points retrieved from
-      ///  `LookUp()`.
-      /// \param[in] _values1 - Value array at timestep 1.
-      /// \param[in] _values2 - Value array at timestep 2.
-      /// \param[in] _default - Value used if there is a hole in the data.
-      /// \returns The estimated value for the point. Nullopt if we are
-      /// outside the field. Default value if in the field but no value is
-      /// in the index.
-      public: template<typename X>
-      std::optional<X> EstimateQuadrilinear(
-        const S &_session,
-        const std::vector<InterpolationPoint4D<T, V>> &_points,
-        const std::vector<X> &_values1,
-        const std::vector<X> &_values2,
-        const X _default = X(0)
-      ) const;
-
-      /// \brief Get the bounds of this grid field.
-      /// \return A pair of vectors.
-      public: std::pair<Vector3<V>, Vector3<V>> Bounds(const S &_session);
-    };
-
-    /// \brief An in-memory session. Loads the whole dataset in memory and
-    /// performs queries.
-    template<typename T, typename V>
-    class InMemorySession
-    {
-      /// \brief Iterator which holds pointer to current state
-      /// TODO(arjo): Use friend to make visible only to InMemorySession
-      /// specialization
-      private:
-        typename std::map<T, VolumetricGridLookupField<V>>::const_iterator iter;
-
-      /// \brief Time of last query
-      public: T time;
-
-      friend class
-        TimeVaryingVolumetricGridLookupField<T, V, InMemorySession<T, V>>;
-    };
-
-    /// \brief Specialized version of `TimeVaryingVolumetricGridLookupField`
-    /// for in-memory lookup. It loads the whole dataset into memory.
-    template<typename T, typename V>
-    class TimeVaryingVolumetricGridLookupField<T, V, InMemorySession<T, V>>
-    {
-      /// \brief Default constructor
-      public: TimeVaryingVolumetricGridLookupField()
-      {}
-
-      /// \brief Documentation inherited
-      public: void AddVolumetricGridField(
-        const T &_time, const VolumetricGridLookupField<V> &_field) {
-        this->gridFields.emplace(_time, _field);
+    /// \brief Constructor
+    public: TimeVaryingVolumetricGridLookupField();
+
+    /// \brief Adds a volumetric grid field.
+    public: void AddVolumetricGridField(
+      const T& time, const VolumetricGridLookupField<V> &_field);
+
+    /// \brief Creates a session for querying
+    public: S CreateSession() const;
+
+    /// \brief Creates a session startingg at time T
+    /// \param[in] _time The time to create the session at.
+    public: S CreateSession(const T &_time) const;
+
+    /// \brief Update session to new time. Returns new session pointer if
+    /// more is available. Otherwise returns nullopt if we have run out of
+    /// time.
+    /// \param[in] _session - The session
+    /// \param[in] _time - Time to step to.
+    public: std::optional<S> StepTo(const S &_session, const T &_time);
+
+    /// \brief Looks up interpolators at a given time step. Use the session
+    /// to set the time step.
+    /// \param[in] _session - The session
+    /// \param[in] _point - The point to query
+    /// \param[in] _tol - Tolerance
+    public: std::vector<InterpolationPoint4D<T, V>>
+    LookUp(const S &_session,
+      const Vector3<V> &_point,
+      const Vector3<V> &_tol) const;
+
+    /// \brief Uses quadrilinear interpolation to estimate value of current
+    /// point. Returns nullopt if query is out of range.
+    /// \param[in] _session - The session
+    /// \param[in] _points - The interpolation points retrieved from
+    ///  `LookUp()`.
+    /// \param[in] _values1 - Value array at timestep 1.
+    /// \param[in] _values2 - Value array at timestep 2.
+    /// \param[in] _default - Value used if there is a hole in the data.
+    /// \returns The estimated value for the point. Nullopt if we are
+    /// outside the field. Default value if in the field but no value is
+    /// in the index.
+    public: template<typename X>
+    std::optional<X> EstimateQuadrilinear(
+      const S &_session,
+      const std::vector<InterpolationPoint4D<T, V>> &_points,
+      const std::vector<X> &_values1,
+      const std::vector<X> &_values2,
+      const X _default = X(0)
+    ) const;
+
+    /// \brief Get the bounds of this grid field.
+    /// \return A pair of vectors.
+    public: std::pair<Vector3<V>, Vector3<V>> Bounds(const S &_session);
+  };
+
+  /// \brief An in-memory session. Loads the whole dataset in memory and
+  /// performs queries.
+  template<typename T, typename V>
+  class InMemorySession
+  {
+    /// \brief Iterator which holds pointer to current state
+    /// TODO(arjo): Use friend to make visible only to InMemorySession
+    /// specialization
+    private:
+      typename std::map<T, VolumetricGridLookupField<V>>::const_iterator iter;
+
+    /// \brief Time of last query
+    public: T time;
+
+    friend class
+      TimeVaryingVolumetricGridLookupField<T, V, InMemorySession<T, V>>;
+  };
+
+  /// \brief Specialized version of `TimeVaryingVolumetricGridLookupField`
+  /// for in-memory lookup. It loads the whole dataset into memory.
+  template<typename T, typename V>
+  class TimeVaryingVolumetricGridLookupField<T, V, InMemorySession<T, V>>
+  {
+    /// \brief Default constructor
+    public: TimeVaryingVolumetricGridLookupField()
+    {}
+
+    /// \brief Documentation inherited
+    public: void AddVolumetricGridField(
+      const T &_time, const VolumetricGridLookupField<V> &_field) {
+      this->gridFields.emplace(_time, _field);
+    }
+
+    /// \brief Documentation inherited
+    public: InMemorySession<T, V> CreateSession() const {
+      InMemorySession<T, V> sess;
+      sess.iter = this->gridFields.begin();
+      sess.time = T(0);
+      return sess;
+    }
+
+    /// \brief Documentation inherited
+    public: InMemorySession<T, V> CreateSession(const T &_time) const {
+      InMemorySession<T, V> sess;
+      sess.iter = this->gridFields.lower_bound(_time);
+      sess.time = _time;
+      return sess;
+    }
+
+    /// \brief In memory session
+    public: bool IsValid(const InMemorySession<T, V> &_session) const {
+      return this->gridFields.end() != _session.iter;
+    }
+
+    /// \brief Documentation inherited
+    public: std::optional<InMemorySession<T, V>> StepTo(
+      const InMemorySession<T, V> &_session, const T &_time) const {
+      if (_session.iter == gridFields.end())
+      {
+        return std::nullopt;
       }
 
-      /// \brief Documentation inherited
-      public: InMemorySession<T, V> CreateSession() const {
-        InMemorySession<T, V> sess;
-        sess.iter = this->gridFields.begin();
-        sess.time = T(0);
-        return sess;
+      InMemorySession<T, V> newSess(_session);
+
+      auto nextTime = std::next(_session.iter);
+      if (nextTime == this->gridFields.end() || _time < _session.iter->first)
+      {
+        return std::nullopt;
       }
 
-      /// \brief Documentation inherited
-      public: InMemorySession<T, V> CreateSession(const T &_time) const {
-        InMemorySession<T, V> sess;
-        sess.iter = this->gridFields.lower_bound(_time);
-        sess.time = _time;
-        return sess;
+      while (nextTime != this->gridFields.end()
+        && nextTime->first <= _time)
+      {
+        newSess.iter = nextTime;
+        nextTime = std::next(nextTime);
       }
+      newSess.time = _time;
+      return newSess;
+    }
+
+    /// \brief Documentation inherited
+    public: std::vector<InterpolationPoint4D<T, V>>
+      LookUp(const InMemorySession<T, V> &_session,
+        const Vector3<V> &_point,
+        const Vector3<V> &_tol = Vector3<V>{1e-6, 1e-6, 1e-6}) const {
+
+      std::vector<InterpolationPoint4D<T, V>> res;
 
-      /// \brief In memory session
-      public: bool IsValid(const InMemorySession<T, V> &_session) const {
-        return this->gridFields.end() != _session.iter;
+      if (_session.iter == this->gridFields.end())
+      {
+        // Out of bounds
+        return res;
       }
 
-      /// \brief Documentation inherited
-      public: std::optional<InMemorySession<T, V>> StepTo(
-        const InMemorySession<T, V> &_session, const T &_time) const {
-        if (_session.iter == gridFields.end())
-        {
-          return std::nullopt;
-        }
-
-        InMemorySession<T, V> newSess(_session);
-
-        auto nextTime = std::next(_session.iter);
-        if (nextTime == this->gridFields.end() || _time < _session.iter->first)
-        {
-          return std::nullopt;
-        }
-
-        while (nextTime != this->gridFields.end()
-          && nextTime->first <= _time)
-        {
-          newSess.iter = nextTime;
-          nextTime = std::next(nextTime);
-        }
-        newSess.time = _time;
-        return newSess;
+      InterpolationPoint4D<T, V> slice1;
+      slice1.timeSlice = _session.iter->second.GetInterpolators(
+          _point, _tol.X(), _tol.Y(), _tol.Z());
+      slice1.time = _session.iter->first;
+      res.push_back(slice1);
+
+      auto nextTime = std::next(_session.iter);
+      if (nextTime != this->gridFields.end())
+      {
+        // Only add next item if field exists
+        InterpolationPoint4D<T, V> slice2;
+        slice2.timeSlice = nextTime->second.GetInterpolators(
+          _point, _tol.X(), _tol.Y(), _tol.Z());
+        slice2.time = nextTime->first;
+        res.push_back(slice2);
+      }
+      return res;
+    }
+
+    /// \brief Uses quadrilinear interpolation to estimate value of current
+    /// point. Returns nullopt if query is out of range.
+    /// \param[in] _session - The session
+    /// \param[in] _interpolators - The interpolation points
+    /// retrieved from `LookUp()`
+    /// \param[in] _values1 - Value array at timestep 1.
+    /// \param[in] _values2 - Value array at timestep 2.
+    /// \param[in] _position - The position to be queried.
+    /// \param[in] _default - Value used if there is a hole in the data.
+    /// \returns The estimated value for the point. Nullopt if we are
+    /// outside the field. Default value if in the field but no value is
+    /// in the index.
+    public: template<typename X>
+    std::optional<X> EstimateQuadrilinear(
+      const InMemorySession<T, V> &_session,
+      const std::vector<InterpolationPoint4D<T, V>> &_interpolators,
+      const std::vector<X> &_values1,
+      const std::vector<X> &_values2,
+      const Vector3<X> &_position,
+      const X _default = X(0)
+    ) const
+    {
+      if (_session.iter == this->gridFields.end())
+      {
+        // Out of bounds
+        return std::nullopt;
       }
 
-      /// \brief Documentation inherited
-      public: std::vector<InterpolationPoint4D<T, V>>
-        LookUp(const InMemorySession<T, V> &_session,
-          const Vector3<V> &_point,
-          const Vector3<V> &_tol = Vector3<V>{1e-6, 1e-6, 1e-6}) const {
-
-        std::vector<InterpolationPoint4D<T, V>> res;
-
-        if (_session.iter == this->gridFields.end())
-        {
-          // Out of bounds
-          return res;
-        }
-
-        InterpolationPoint4D<T, V> slice1;
-        slice1.timeSlice = _session.iter->second.GetInterpolators(
-            _point, _tol.X(), _tol.Y(), _tol.Z());
-        slice1.time = _session.iter->first;
-        res.push_back(slice1);
-
-        auto nextTime = std::next(_session.iter);
-        if (nextTime != this->gridFields.end())
-        {
-          // Only add next item if field exists
-          InterpolationPoint4D<T, V> slice2;
-          slice2.timeSlice = nextTime->second.GetInterpolators(
-            _point, _tol.X(), _tol.Y(), _tol.Z());
-          slice2.time = nextTime->first;
-          res.push_back(slice2);
-        }
-        return res;
+      auto time = _session.time;
+      if (_interpolators.size() == 0) return std::nullopt;
+      if (_interpolators.size() == 1)
+      {
+        // This happens we reach the end of time
+        return _session.iter->second.EstimateValueUsingTrilinear(
+          _position,
+          _values2,
+          _default);
       }
 
-      /// \brief Uses quadrilinear interpolation to estimate value of current
-      /// point. Returns nullopt if query is out of range.
-      /// \param[in] _session - The session
-      /// \param[in] _interpolators - The interpolation points
-      /// retrieved from `LookUp()`
-      /// \param[in] _values1 - Value array at timestep 1.
-      /// \param[in] _values2 - Value array at timestep 2.
-      /// \param[in] _position - The position to be queried.
-      /// \param[in] _default - Value used if there is a hole in the data.
-      /// \returns The estimated value for the point. Nullopt if we are
-      /// outside the field. Default value if in the field but no value is
-      /// in the index.
-      public: template<typename X>
-      std::optional<X> EstimateQuadrilinear(
-        const InMemorySession<T, V> &_session,
-        const std::vector<InterpolationPoint4D<T, V>> &_interpolators,
-        const std::vector<X> &_values1,
-        const std::vector<X> &_values2,
-        const Vector3<X> &_position,
-        const X _default = X(0)
-      ) const
+      /// Got nothing to interpolate. Out of bounds.
+      if (_interpolators[0].timeSlice.size() == 0
+       && _interpolators[1].timeSlice.size() == 0)
+        return std::nullopt;
+
+      /// Only one of the two time-slices has data. Use that slice to guess.
+      auto next = std::next(_session.iter);
+      if (_interpolators[1].timeSlice.size() == 0)
       {
-        if (_session.iter == this->gridFields.end())
-        {
-          // Out of bounds
-          return std::nullopt;
-        }
-
-        auto time = _session.time;
-        if (_interpolators.size() == 0) return std::nullopt;
-        if (_interpolators.size() == 1)
-        {
-          // This happens we reach the end of time
-          return _session.iter->second.EstimateValueUsingTrilinear(
-            _position,
-            _values2,
-            _default);
-        }
-
-        /// Got nothing to interpolate. Out of bounds.
-        if (_interpolators[0].timeSlice.size() == 0
-         && _interpolators[1].timeSlice.size() == 0)
-          return std::nullopt;
-
-        /// Only one of the two time-slices has data. Use that slice to guess.
-        auto next = std::next(_session.iter);
-        if (_interpolators[1].timeSlice.size() == 0)
-        {
-          return _session.iter->second.EstimateValueUsingTrilinear(
-            _position,
-            _values1,
-            _default
-          );
-        }
-        if (_interpolators[0].timeSlice.size() == 0)
-        {
-          return next->second.EstimateValueUsingTrilinear(
-            _position,
-            _values2,
-            _default
-          );
-        }
-
-        /// Default case where both time-slices has interpolation
-        auto res1 = _session.iter->second.EstimateValueUsingTrilinear(
+        return _session.iter->second.EstimateValueUsingTrilinear(
           _position,
           _values1,
           _default
         );
-
-        auto res2 = next->second.EstimateValueUsingTrilinear(
+      }
+      if (_interpolators[0].timeSlice.size() == 0)
+      {
+        return next->second.EstimateValueUsingTrilinear(
           _position,
           _values2,
           _default
         );
-
-        if (res1.has_value() || res2.has_value())
-        {
-          InterpolationPoint1D<T>
-            pt1{_session.iter->first, 0}, pt2{next->first, 1};
-          // If either has value interpolate using default value.
-          std::vector<X> times{
-            res1.value_or(_default), res2.value_or(_default)};
-          return LinearInterpolate(pt1, pt2, times, time);
-        }
-        // Return nullopt if we are out of range
-        return std::nullopt;
       }
 
-      /// \brief Get the bounds of this grid field.
-      /// \return A pair of vectors. All zeros if session is invalid.
-      public: std::pair<Vector3<V>, Vector3<V>> Bounds(
-        const InMemorySession<T, V> &_session) const
+      /// Default case where both time-slices has interpolation
+      auto res1 = _session.iter->second.EstimateValueUsingTrilinear(
+        _position,
+        _values1,
+        _default
+      );
+
+      auto res2 = next->second.EstimateValueUsingTrilinear(
+        _position,
+        _values2,
+        _default
+      );
+
+      if (res1.has_value() || res2.has_value())
       {
-        if (_session.iter == this->gridFields.end())
-        {
-          // Out of bounds
-          return std::pair<Vector3<T>, Vector3<T>>(
-            Vector3<T>{0, 0, 0}, Vector3<T>{0, 0, 0}
-          );
-        }
-        return _session.iter->second.Bounds();
+        InterpolationPoint1D<T>
+          pt1{_session.iter->first, 0}, pt2{next->first, 1};
+        // If either has value interpolate using default value.
+        std::vector<X> times{
+          res1.value_or(_default), res2.value_or(_default)};
+        return LinearInterpolate(pt1, pt2, times, time);
+      }
+      // Return nullopt if we are out of range
+      return std::nullopt;
+    }
+
+    /// \brief Get the bounds of this grid field.
+    /// \return A pair of vectors. All zeros if session is invalid.
+    public: std::pair<Vector3<V>, Vector3<V>> Bounds(
+      const InMemorySession<T, V> &_session) const
+    {
+      if (_session.iter == this->gridFields.end())
+      {
+        // Out of bounds
+        return std::pair<Vector3<T>, Vector3<T>>(
+          Vector3<T>{0, 0, 0}, Vector3<T>{0, 0, 0}
+        );
       }
+      return _session.iter->second.Bounds();
+    }
 
-      private: std::map<T, VolumetricGridLookupField<V>> gridFields;
-    };
-  }
-}
-#endif
+    private: std::map<T, VolumetricGridLookupField<V>> gridFields;
+  };
+}  // namespace gz::math
+#endif  // GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
diff --git a/include/gz/math/Triangle.hh b/include/gz/math/Triangle.hh
index 15e8d19cf..496a23aec 100644
--- a/include/gz/math/Triangle.hh
+++ b/include/gz/math/Triangle.hh
@@ -23,225 +23,222 @@
 #include <gz/math/Vector2.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Triangle Triangle.hh gz/math/Triangle.hh
+  /// \brief Triangle class and related functions.
+  template<typename T>
+  class Triangle
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Triangle Triangle.hh gz/math/Triangle.hh
-    /// \brief Triangle class and related functions.
-    template<typename T>
-    class Triangle
+    /// \brief Default constructor
+    public: Triangle() = default;
+
+    /// \brief Constructor
+    /// \param[in] _pt1 First point that defines the triangle.
+    /// \param[in] _pt2 Second point that defines the triangle.
+    /// \param[in] _pt3 Third point that defines the triangle.
+    public: Triangle(const math::Vector2<T> &_pt1,
+                     const math::Vector2<T> &_pt2,
+                     const math::Vector2<T> &_pt3)
     {
-      /// \brief Default constructor
-      public: Triangle() = default;
-
-      /// \brief Constructor
-      /// \param[in] _pt1 First point that defines the triangle.
-      /// \param[in] _pt2 Second point that defines the triangle.
-      /// \param[in] _pt3 Third point that defines the triangle.
-      public: Triangle(const math::Vector2<T> &_pt1,
-                       const math::Vector2<T> &_pt2,
-                       const math::Vector2<T> &_pt3)
-      {
-        this->Set(_pt1, _pt2, _pt3);
-      }
+      this->Set(_pt1, _pt2, _pt3);
+    }
 
-      /// \brief Set one vertex of the triangle.
-      /// \param[in] _index Index of the point to set, where
-      /// 0 == first vertex, 1 == second vertex, and 2 == third vertex.
-      /// The index is clamped to the range [0, 2].
-      /// \param[in] _pt Value of the point to set.
-      public: void Set(const unsigned int _index, const math::Vector2<T> &_pt)
-      {
-        this->pts[clamp(_index, 0u, 2u)] = _pt;
-      }
+    /// \brief Set one vertex of the triangle.
+    /// \param[in] _index Index of the point to set, where
+    /// 0 == first vertex, 1 == second vertex, and 2 == third vertex.
+    /// The index is clamped to the range [0, 2].
+    /// \param[in] _pt Value of the point to set.
+    public: void Set(const unsigned int _index, const math::Vector2<T> &_pt)
+    {
+      this->pts[clamp(_index, 0u, 2u)] = _pt;
+    }
 
-      /// \brief Set all vertices of the triangle.
-      /// \param[in] _pt1 First point that defines the triangle.
-      /// \param[in] _pt2 Second point that defines the triangle.
-      /// \param[in] _pt3 Third point that defines the triangle.
-      public: void Set(const math::Vector2<T> &_pt1,
-                       const math::Vector2<T> &_pt2,
-                       const math::Vector2<T> &_pt3)
-      {
-        this->pts[0] = _pt1;
-        this->pts[1] = _pt2;
-        this->pts[2] = _pt3;
-      }
+    /// \brief Set all vertices of the triangle.
+    /// \param[in] _pt1 First point that defines the triangle.
+    /// \param[in] _pt2 Second point that defines the triangle.
+    /// \param[in] _pt3 Third point that defines the triangle.
+    public: void Set(const math::Vector2<T> &_pt1,
+                     const math::Vector2<T> &_pt2,
+                     const math::Vector2<T> &_pt3)
+    {
+      this->pts[0] = _pt1;
+      this->pts[1] = _pt2;
+      this->pts[2] = _pt3;
+    }
 
-      /// \brief Get whether this triangle is valid, based on triangle
-      /// inequality: the sum of the lengths of any two sides must be greater
-      /// than the length of the remaining side.
-      /// \return True if the triangle inequality holds
-      public: bool Valid() const
-      {
-        T a = this->Side(0).Length();
-        T b = this->Side(1).Length();
-        T c = this->Side(2).Length();
-        return (a+b) > c && (b+c) > a && (c+a) > b;
-      }
+    /// \brief Get whether this triangle is valid, based on triangle
+    /// inequality: the sum of the lengths of any two sides must be greater
+    /// than the length of the remaining side.
+    /// \return True if the triangle inequality holds
+    public: bool Valid() const
+    {
+      T a = this->Side(0).Length();
+      T b = this->Side(1).Length();
+      T c = this->Side(2).Length();
+      return (a+b) > c && (b+c) > a && (c+a) > b;
+    }
 
-      /// \brief Get a line segment for one side of the triangle.
-      /// \param[in] _index Index of the side to retreive, where
-      /// 0 == Line2(pt1, pt2),
-      /// 1 == Line2(pt2, pt3),
-      /// 2 == Line2(pt3, pt1)
-      /// The index is clamped to the range [0, 2]
-      /// \return Line segment of the requested side.
-      public: Line2<T> Side(const unsigned int _index) const
-      {
-        if (_index == 0)
-          return Line2<T>(this->pts[0], this->pts[1]);
-        else if (_index == 1)
-          return Line2<T>(this->pts[1], this->pts[2]);
-        else
-          return Line2<T>(this->pts[2], this->pts[0]);
-      }
+    /// \brief Get a line segment for one side of the triangle.
+    /// \param[in] _index Index of the side to retreive, where
+    /// 0 == Line2(pt1, pt2),
+    /// 1 == Line2(pt2, pt3),
+    /// 2 == Line2(pt3, pt1)
+    /// The index is clamped to the range [0, 2]
+    /// \return Line segment of the requested side.
+    public: Line2<T> Side(const unsigned int _index) const
+    {
+      if (_index == 0)
+        return Line2<T>(this->pts[0], this->pts[1]);
+      else if (_index == 1)
+        return Line2<T>(this->pts[1], this->pts[2]);
+      else
+        return Line2<T>(this->pts[2], this->pts[0]);
+    }
 
-      /// \brief Check if this triangle completely contains the given line
-      /// segment.
-      /// \param[in] _line Line to check.
-      /// \return True if the line's start and end points are both inside
-      /// this triangle.
-      public: bool Contains(const Line2<T> &_line) const
-      {
-        return this->Contains(_line[0]) && this->Contains(_line[1]);
-      }
+    /// \brief Check if this triangle completely contains the given line
+    /// segment.
+    /// \param[in] _line Line to check.
+    /// \return True if the line's start and end points are both inside
+    /// this triangle.
+    public: bool Contains(const Line2<T> &_line) const
+    {
+      return this->Contains(_line[0]) && this->Contains(_line[1]);
+    }
 
-      /// \brief Get whether this triangle contains the given point.
-      /// \param[in] _pt Point to check.
-      /// \return True if the point is inside or on the triangle.
-      public: bool Contains(const math::Vector2<T> &_pt) const
+    /// \brief Get whether this triangle contains the given point.
+    /// \param[in] _pt Point to check.
+    /// \return True if the point is inside or on the triangle.
+    public: bool Contains(const math::Vector2<T> &_pt) const
+    {
+      // Compute vectors
+      math::Vector2<T> v0 = this->pts[2] -this->pts[0];
+      math::Vector2<T> v1 = this->pts[1] -this->pts[0];
+      math::Vector2<T> v2 = _pt - this->pts[0];
+
+      // Compute dot products
+      double dot00 = v0.Dot(v0);
+      double dot01 = v0.Dot(v1);
+      double dot02 = v0.Dot(v2);
+      double dot11 = v1.Dot(v1);
+      double dot12 = v1.Dot(v2);
+
+      // Compute barycentric coordinates
+      double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
+      double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+      double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+      // Check if point is in triangle
+      return (u >= 0) && (v >= 0) && (u + v <= 1);
+    }
+
+    /// \brief Get whether the given line intersects this triangle.
+    /// \param[in] _line Line to check.
+    /// \param[out] _ipt1 Return value of the first intersection point,
+    /// only valid if the return value of the function is true.
+    /// \param[out] _ipt2 Return value of the second intersection point,
+    /// only valid if the return value of the function is true.
+    /// \return True if the given line intersects this triangle.
+    public: bool Intersects(const Line2<T> &_line,
+                            math::Vector2<T> &_ipt1,
+                            math::Vector2<T> &_ipt2) const
+    {
+      if (this->Contains(_line))
       {
-        // Compute vectors
-        math::Vector2<T> v0 = this->pts[2] -this->pts[0];
-        math::Vector2<T> v1 = this->pts[1] -this->pts[0];
-        math::Vector2<T> v2 = _pt - this->pts[0];
-
-        // Compute dot products
-        double dot00 = v0.Dot(v0);
-        double dot01 = v0.Dot(v1);
-        double dot02 = v0.Dot(v2);
-        double dot11 = v1.Dot(v1);
-        double dot12 = v1.Dot(v2);
-
-        // Compute barycentric coordinates
-        double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
-        double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
-        double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
-
-        // Check if point is in triangle
-        return (u >= 0) && (v >= 0) && (u + v <= 1);
+        _ipt1 = _line[0];
+        _ipt2 = _line[1];
+        return true;
       }
 
-      /// \brief Get whether the given line intersects this triangle.
-      /// \param[in] _line Line to check.
-      /// \param[out] _ipt1 Return value of the first intersection point,
-      /// only valid if the return value of the function is true.
-      /// \param[out] _ipt2 Return value of the second intersection point,
-      /// only valid if the return value of the function is true.
-      /// \return True if the given line intersects this triangle.
-      public: bool Intersects(const Line2<T> &_line,
-                              math::Vector2<T> &_ipt1,
-                              math::Vector2<T> &_ipt2) const
-      {
-        if (this->Contains(_line))
-        {
-          _ipt1 = _line[0];
-          _ipt2 = _line[1];
-          return true;
-        }
+      Line2<T> line1(this->pts[0], this->pts[1]);
+      Line2<T> line2(this->pts[1], this->pts[2]);
+      Line2<T> line3(this->pts[2], this->pts[0]);
 
-        Line2<T> line1(this->pts[0], this->pts[1]);
-        Line2<T> line2(this->pts[1], this->pts[2]);
-        Line2<T> line3(this->pts[2], this->pts[0]);
+      math::Vector2<T> pt;
+      std::set<math::Vector2<T> > points;
 
-        math::Vector2<T> pt;
-        std::set<math::Vector2<T> > points;
+      if (line1.Intersect(_line, pt))
+        points.insert(pt);
 
-        if (line1.Intersect(_line, pt))
-          points.insert(pt);
+      if (line2.Intersect(_line, pt))
+        points.insert(pt);
 
-        if (line2.Intersect(_line, pt))
-          points.insert(pt);
+      if (line3.Intersect(_line, pt))
+        points.insert(pt);
 
-        if (line3.Intersect(_line, pt))
-          points.insert(pt);
+      if (points.empty())
+      {
+        return false;
+      }
+      else if (points.size() == 1)
+      {
+        auto iter = points.begin();
 
-        if (points.empty())
-        {
-          return false;
-        }
-        else if (points.size() == 1)
-        {
-          auto iter = points.begin();
-
-          _ipt1 = *iter;
-          if (this->Contains(_line[0]))
-            _ipt2 = _line[0];
-          else
-          {
-            _ipt2 = _line[1];
-          }
-        }
+        _ipt1 = *iter;
+        if (this->Contains(_line[0]))
+          _ipt2 = _line[0];
         else
         {
-          auto iter = points.begin();
-          _ipt1 = *(iter++);
-          _ipt2 = *iter;
+          _ipt2 = _line[1];
         }
-
-        return true;
       }
-
-      /// \brief Get the length of the triangle's perimeter.
-      /// \return Sum of the triangle's line segments.
-      public: T Perimeter() const
+      else
       {
-        return this->Side(0).Length() + this->Side(1).Length() +
-               this->Side(2).Length();
+        auto iter = points.begin();
+        _ipt1 = *(iter++);
+        _ipt2 = *iter;
       }
 
-      /// \brief Get the area of this triangle.
-      /// \return Triangle's area.
-      public: double Area() const
-      {
-        double s = this->Perimeter() / 2.0;
-        T a = this->Side(0).Length();
-        T b = this->Side(1).Length();
-        T c = this->Side(2).Length();
-
-        // Heron's formula
-        // http://en.wikipedia.org/wiki/Heron%27s_formula
-        return sqrt(s * (s-a) * (s-b) * (s-c));
-      }
+      return true;
+    }
 
-      /// \brief Get one of points that define the triangle.
-      /// \param[in] _index The index, where 0 == first vertex,
-      /// 1 == second vertex, and 2 == third vertex.
-      /// The index is clamped to the range [0, 2]
-      /// \return The point specified by _index.
-      public: math::Vector2<T> operator[](const size_t _index) const
-      {
-        return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
-      }
+    /// \brief Get the length of the triangle's perimeter.
+    /// \return Sum of the triangle's line segments.
+    public: T Perimeter() const
+    {
+      return this->Side(0).Length() + this->Side(1).Length() +
+             this->Side(2).Length();
+    }
 
-      /// The points of the triangle
-      private: math::Vector2<T> pts[3];
-    };
+    /// \brief Get the area of this triangle.
+    /// \return Triangle's area.
+    public: double Area() const
+    {
+      double s = this->Perimeter() / 2.0;
+      T a = this->Side(0).Length();
+      T b = this->Side(1).Length();
+      T c = this->Side(2).Length();
+
+      // Heron's formula
+      // http://en.wikipedia.org/wiki/Heron%27s_formula
+      return sqrt(s * (s-a) * (s-b) * (s-c));
+    }
 
-    /// Integer specialization of the Triangle class.
-    typedef Triangle<int> Trianglei;
+    /// \brief Get one of points that define the triangle.
+    /// \param[in] _index The index, where 0 == first vertex,
+    /// 1 == second vertex, and 2 == third vertex.
+    /// The index is clamped to the range [0, 2]
+    /// \return The point specified by _index.
+    public: math::Vector2<T> operator[](const size_t _index) const
+    {
+      return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
+    }
 
-    /// Double specialization of the Triangle class.
-    typedef Triangle<double> Triangled;
+    /// The points of the triangle
+    private: math::Vector2<T> pts[3];
+  };
 
-    /// Float specialization of the Triangle class.
-    typedef Triangle<float> Trianglef;
-    }
-  }
-}
-#endif
+  /// Integer specialization of the Triangle class.
+  typedef Triangle<int> Trianglei;
+
+  /// Double specialization of the Triangle class.
+  typedef Triangle<double> Triangled;
+
+  /// Float specialization of the Triangle class.
+  typedef Triangle<float> Trianglef;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_TRIANGLE_HH_
diff --git a/include/gz/math/Triangle3.hh b/include/gz/math/Triangle3.hh
index ae90171eb..a81213b1f 100644
--- a/include/gz/math/Triangle3.hh
+++ b/include/gz/math/Triangle3.hh
@@ -23,264 +23,261 @@
 #include <gz/math/Vector3.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Triangle3 Triangle3.hh gz/math/Triangle3.hh
+  /// \brief A 3-dimensional triangle and related functions.
+  template<typename T>
+  class Triangle3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Triangle3 Triangle3.hh gz/math/Triangle3.hh
-    /// \brief A 3-dimensional triangle and related functions.
-    template<typename T>
-    class Triangle3
-    {
-      /// \brief Default constructor
-      public: Triangle3() = default;
+    /// \brief Default constructor
+    public: Triangle3() = default;
 
-      /// \brief Constructor.
-      ///
-      /// Keep in mind that the triangle normal
-      /// is determined by the order of these vertices. Search
-      /// the internet for "triangle winding" for more information.
-      /// \param[in] _pt1 First point that defines the triangle.
-      /// \param[in] _pt2 Second point that defines the triangle.
-      /// \param[in] _pt3 Third point that defines the triangle.
-      public: Triangle3(const Vector3<T> &_pt1,
-                        const Vector3<T> &_pt2,
-                        const Vector3<T> &_pt3)
-      {
-        this->Set(_pt1, _pt2, _pt3);
-      }
+    /// \brief Constructor.
+    ///
+    /// Keep in mind that the triangle normal
+    /// is determined by the order of these vertices. Search
+    /// the internet for "triangle winding" for more information.
+    /// \param[in] _pt1 First point that defines the triangle.
+    /// \param[in] _pt2 Second point that defines the triangle.
+    /// \param[in] _pt3 Third point that defines the triangle.
+    public: Triangle3(const Vector3<T> &_pt1,
+                      const Vector3<T> &_pt2,
+                      const Vector3<T> &_pt3)
+    {
+      this->Set(_pt1, _pt2, _pt3);
+    }
 
-      /// \brief Set one vertex of the triangle.
-      ///
-      /// Keep in mind that the triangle normal
-      /// is determined by the order of these vertices. Search
-      /// the internet for "triangle winding" for more information.
-      ///
-      /// \param[in] _index Index of the point to set. _index is clamped
-      /// to the range [0,2].
-      /// \param[in] _pt Value of the point to set.
-      public: void Set(const unsigned int _index, const Vector3<T> &_pt)
-      {
-        this->pts[clamp(_index, 0u, 2u)] = _pt;
-      }
+    /// \brief Set one vertex of the triangle.
+    ///
+    /// Keep in mind that the triangle normal
+    /// is determined by the order of these vertices. Search
+    /// the internet for "triangle winding" for more information.
+    ///
+    /// \param[in] _index Index of the point to set. _index is clamped
+    /// to the range [0,2].
+    /// \param[in] _pt Value of the point to set.
+    public: void Set(const unsigned int _index, const Vector3<T> &_pt)
+    {
+      this->pts[clamp(_index, 0u, 2u)] = _pt;
+    }
 
-      /// \brief Set all vertices of the triangle.
-      ///
-      /// Keep in mind that the triangle normal
-      /// is determined by the order of these vertices. Search
-      /// the internet for "triangle winding" for more information.
-      ///
-      /// \param[in] _pt1 First point that defines the triangle.
-      /// \param[in] _pt2 Second point that defines the triangle.
-      /// \param[in] _pt3 Third point that defines the triangle.
-      public: void Set(const Vector3<T> &_pt1,
-                       const Vector3<T> &_pt2,
-                       const Vector3<T> &_pt3)
-      {
-        this->pts[0] = _pt1;
-        this->pts[1] = _pt2;
-        this->pts[2] = _pt3;
-      }
+    /// \brief Set all vertices of the triangle.
+    ///
+    /// Keep in mind that the triangle normal
+    /// is determined by the order of these vertices. Search
+    /// the internet for "triangle winding" for more information.
+    ///
+    /// \param[in] _pt1 First point that defines the triangle.
+    /// \param[in] _pt2 Second point that defines the triangle.
+    /// \param[in] _pt3 Third point that defines the triangle.
+    public: void Set(const Vector3<T> &_pt1,
+                     const Vector3<T> &_pt2,
+                     const Vector3<T> &_pt3)
+    {
+      this->pts[0] = _pt1;
+      this->pts[1] = _pt2;
+      this->pts[2] = _pt3;
+    }
 
-      /// \brief Get whether this triangle is valid, based on triangle
-      /// inequality: the sum of the lengths of any two sides must be greater
-      /// than the length of the remaining side.
-      /// \return True if the triangle inequality holds
-      public: bool Valid() const
-      {
-        T a = this->Side(0).Length();
-        T b = this->Side(1).Length();
-        T c = this->Side(2).Length();
-        return (a+b) > c && (b+c) > a && (c+a) > b;
-      }
+    /// \brief Get whether this triangle is valid, based on triangle
+    /// inequality: the sum of the lengths of any two sides must be greater
+    /// than the length of the remaining side.
+    /// \return True if the triangle inequality holds
+    public: bool Valid() const
+    {
+      T a = this->Side(0).Length();
+      T b = this->Side(1).Length();
+      T c = this->Side(2).Length();
+      return (a+b) > c && (b+c) > a && (c+a) > b;
+    }
 
-      /// \brief Get a line segment for one side of the triangle.
-      /// \param[in] _index Index of the side to retrieve, where
-      /// 0 == Line3(pt1, pt2),
-      /// 1 == Line3(pt2, pt3),
-      /// 2 == Line3(pt3, pt1).
-      /// _index is clamped to the range [0,2].
-      /// \return Line segment of the requested side.
-      public: Line3<T> Side(const unsigned int _index) const
-      {
-        if (_index == 0)
-          return Line3<T>(this->pts[0], this->pts[1]);
-        else if (_index == 1)
-          return Line3<T>(this->pts[1], this->pts[2]);
-        else
-          return Line3<T>(this->pts[2], this->pts[0]);
-      }
+    /// \brief Get a line segment for one side of the triangle.
+    /// \param[in] _index Index of the side to retrieve, where
+    /// 0 == Line3(pt1, pt2),
+    /// 1 == Line3(pt2, pt3),
+    /// 2 == Line3(pt3, pt1).
+    /// _index is clamped to the range [0,2].
+    /// \return Line segment of the requested side.
+    public: Line3<T> Side(const unsigned int _index) const
+    {
+      if (_index == 0)
+        return Line3<T>(this->pts[0], this->pts[1]);
+      else if (_index == 1)
+        return Line3<T>(this->pts[1], this->pts[2]);
+      else
+        return Line3<T>(this->pts[2], this->pts[0]);
+    }
 
-      /// \brief Check if this triangle completely contains the given line
-      /// segment.
-      /// \param[in] _line Line to check.
-      /// \return True if the line's start and end points are both inside
-      /// this triangle.
-      public: bool Contains(const Line3<T> &_line) const
-      {
-        return this->Contains(_line[0]) && this->Contains(_line[1]);
-      }
+    /// \brief Check if this triangle completely contains the given line
+    /// segment.
+    /// \param[in] _line Line to check.
+    /// \return True if the line's start and end points are both inside
+    /// this triangle.
+    public: bool Contains(const Line3<T> &_line) const
+    {
+      return this->Contains(_line[0]) && this->Contains(_line[1]);
+    }
 
-      /// \brief Get whether this triangle contains the given point.
-      /// \param[in] _pt Point to check.
-      /// \return True if the point is inside or on the triangle.
-      public: bool Contains(const Vector3<T> &_pt) const
+    /// \brief Get whether this triangle contains the given point.
+    /// \param[in] _pt Point to check.
+    /// \return True if the point is inside or on the triangle.
+    public: bool Contains(const Vector3<T> &_pt) const
+    {
+      // Make sure the point is on the same plane as the triangle
+      if (Planed(this->Normal()).Side(Vector3d(_pt[0], _pt[1], _pt[2]))
+        == Planed::NO_SIDE)
       {
-        // Make sure the point is on the same plane as the triangle
-        if (Planed(this->Normal()).Side(Vector3d(_pt[0], _pt[1], _pt[2]))
-          == Planed::NO_SIDE)
-        {
-          Vector3<T> v0 = this->pts[2] - this->pts[0];
-          Vector3<T> v1 = this->pts[1] - this->pts[0];
-          Vector3<T> v2 = _pt - this->pts[0];
+        Vector3<T> v0 = this->pts[2] - this->pts[0];
+        Vector3<T> v1 = this->pts[1] - this->pts[0];
+        Vector3<T> v2 = _pt - this->pts[0];
 
-          double dot00 = v0.Dot(v0);
-          double dot01 = v0.Dot(v1);
-          double dot02 = v0.Dot(v2);
-          double dot11 = v1.Dot(v1);
-          double dot12 = v1.Dot(v2);
+        double dot00 = v0.Dot(v0);
+        double dot01 = v0.Dot(v1);
+        double dot02 = v0.Dot(v2);
+        double dot11 = v1.Dot(v1);
+        double dot12 = v1.Dot(v2);
 
-          // Compute barycentric coordinates
-          double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
-          double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
-          double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+        // Compute barycentric coordinates
+        double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
+        double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+        double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
 
-          // Check if point is in triangle
-          return (u >= 0) && (v >= 0) && (u + v <= 1);
-        }
-        return false;
+        // Check if point is in triangle
+        return (u >= 0) && (v >= 0) && (u + v <= 1);
       }
+      return false;
+    }
 
-      /// \brief Get the triangle's normal vector.
-      /// \return The normal vector for the triangle.
-      public: Vector3d Normal() const
-      {
-        return math::Vector3d::Normal(
-          Vector3d(this->pts[0][0], this->pts[0][1], this->pts[0][2]),
-          Vector3d(this->pts[1][0], this->pts[1][1], this->pts[1][2]),
-          Vector3d(this->pts[2][0], this->pts[2][1], this->pts[2][2]));
-      }
+    /// \brief Get the triangle's normal vector.
+    /// \return The normal vector for the triangle.
+    public: Vector3d Normal() const
+    {
+      return math::Vector3d::Normal(
+        Vector3d(this->pts[0][0], this->pts[0][1], this->pts[0][2]),
+        Vector3d(this->pts[1][0], this->pts[1][1], this->pts[1][2]),
+        Vector3d(this->pts[2][0], this->pts[2][1], this->pts[2][2]));
+    }
 
-      /// \brief Get whether the given line intersects an edge of this triangle.
-      ///
-      /// The returned intersection point is one of:
-      ///
-      /// * If the line is coplanar with the triangle:
-      ///   * The point on the closest edge of the triangle that the line
-      ///     intersects.
-      ///   OR
-      ///   * The first point on the line, if the line is completely contained
-      /// * If the line is not coplanar, the point on the triangle that the
-      ///   line intersects.
-      ///
-      /// \param[in] _line Line to check.
-      /// \param[out] _ipt1 Return value of the first intersection point,
-      /// only valid if the return value of the function is true.
-      /// \return True if the given line intersects this triangle.
-      public: bool Intersects(
-        const Line3<T> &_line, Vector3<T> &_ipt1) const
-      {
-        // Triangle normal
-        Vector3d norm = this->Normal();
+    /// \brief Get whether the given line intersects an edge of this triangle.
+    ///
+    /// The returned intersection point is one of:
+    ///
+    /// * If the line is coplanar with the triangle:
+    ///   * The point on the closest edge of the triangle that the line
+    ///     intersects.
+    ///   OR
+    ///   * The first point on the line, if the line is completely contained
+    /// * If the line is not coplanar, the point on the triangle that the
+    ///   line intersects.
+    ///
+    /// \param[in] _line Line to check.
+    /// \param[out] _ipt1 Return value of the first intersection point,
+    /// only valid if the return value of the function is true.
+    /// \return True if the given line intersects this triangle.
+    public: bool Intersects(
+      const Line3<T> &_line, Vector3<T> &_ipt1) const
+    {
+      // Triangle normal
+      Vector3d norm = this->Normal();
 
-        // Ray direction to intersect with triangle
-        Vector3<T> dir = (_line[1] - _line[0]).Normalize();
+      // Ray direction to intersect with triangle
+      Vector3<T> dir = (_line[1] - _line[0]).Normalize();
 
-        double denom = norm.Dot(Vector3d(dir[0], dir[1], dir[2]));
+      double denom = norm.Dot(Vector3d(dir[0], dir[1], dir[2]));
 
-        // Handle the case when the line is not co-planar with the triangle
-        if (!math::equal(denom, 0.0))
-        {
-          // Distance from line start to triangle intersection
-          Vector3<T> diff = _line[0] - this->pts[0];
-          double intersection =
-            -norm.Dot(Vector3d(diff[0], diff[1], diff[2])) / denom;
+      // Handle the case when the line is not co-planar with the triangle
+      if (!math::equal(denom, 0.0))
+      {
+        // Distance from line start to triangle intersection
+        Vector3<T> diff = _line[0] - this->pts[0];
+        double intersection =
+          -norm.Dot(Vector3d(diff[0], diff[1], diff[2])) / denom;
 
-          // Make sure the ray intersects the triangle
-          if (intersection < 1.0 || intersection > _line.Length())
-            return false;
+        // Make sure the ray intersects the triangle
+        if (intersection < 1.0 || intersection > _line.Length())
+          return false;
 
-          // Return point of intersection
-          _ipt1 = _line[0] + (dir * intersection);
+        // Return point of intersection
+        _ipt1 = _line[0] + (dir * intersection);
 
+        return true;
+      }
+      // Line co-planar with triangle
+      else
+      {
+        // If the line is completely inside the triangle
+        if (this->Contains(_line))
+        {
+          _ipt1 = _line[0];
           return true;
         }
-        // Line co-planar with triangle
-        else
+        // If the line intersects the first side
+        else if (_line.Intersect(this->Side(0), _ipt1))
         {
-          // If the line is completely inside the triangle
-          if (this->Contains(_line))
-          {
-            _ipt1 = _line[0];
-            return true;
-          }
-          // If the line intersects the first side
-          else if (_line.Intersect(this->Side(0), _ipt1))
-          {
-            return true;
-          }
-          // If the line intersects the second side
-          else if (_line.Intersect(this->Side(1), _ipt1))
-          {
-            return true;
-          }
-          // If the line intersects the third side
-          else if (_line.Intersect(this->Side(2), _ipt1))
-          {
-            return true;
-          }
+          return true;
+        }
+        // If the line intersects the second side
+        else if (_line.Intersect(this->Side(1), _ipt1))
+        {
+          return true;
+        }
+        // If the line intersects the third side
+        else if (_line.Intersect(this->Side(2), _ipt1))
+        {
+          return true;
         }
-
-        return false;
       }
 
-      /// \brief Get the length of the triangle's perimeter.
-      /// \return Sum of the triangle's line segments.
-      public: T Perimeter() const
-      {
-        return this->Side(0).Length() + this->Side(1).Length() +
-               this->Side(2).Length();
-      }
+      return false;
+    }
 
-      /// \brief Get the area of this triangle.
-      /// \return Triangle's area.
-      public: double Area() const
-      {
-        double s = this->Perimeter() / 2.0;
-        T a = this->Side(0).Length();
-        T b = this->Side(1).Length();
-        T c = this->Side(2).Length();
+    /// \brief Get the length of the triangle's perimeter.
+    /// \return Sum of the triangle's line segments.
+    public: T Perimeter() const
+    {
+      return this->Side(0).Length() + this->Side(1).Length() +
+             this->Side(2).Length();
+    }
 
-        // Heron's formula
-        // http://en.wikipedia.org/wiki/Heron%27s_formula
-        return sqrt(s * (s-a) * (s-b) * (s-c));
-      }
+    /// \brief Get the area of this triangle.
+    /// \return Triangle's area.
+    public: double Area() const
+    {
+      double s = this->Perimeter() / 2.0;
+      T a = this->Side(0).Length();
+      T b = this->Side(1).Length();
+      T c = this->Side(2).Length();
 
-      /// \brief Get one of points that define the triangle.
-      /// \param[in] _index: 0, 1, or 2. _index is clamped to the range
-      /// [0,2].
-      /// \return The triangle point at _index.
-      public: Vector3<T> operator[](const size_t _index) const
-      {
-        return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
-      }
+      // Heron's formula
+      // http://en.wikipedia.org/wiki/Heron%27s_formula
+      return sqrt(s * (s-a) * (s-b) * (s-c));
+    }
 
-      /// The points of the triangle
-      private: Vector3<T> pts[3];
-    };
+    /// \brief Get one of points that define the triangle.
+    /// \param[in] _index: 0, 1, or 2. _index is clamped to the range
+    /// [0,2].
+    /// \return The triangle point at _index.
+    public: Vector3<T> operator[](const size_t _index) const
+    {
+      return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
+    }
 
-    /// \brief Integer specialization of the Triangle class.
-    typedef Triangle3<int> Triangle3i;
+    /// The points of the triangle
+    private: Vector3<T> pts[3];
+  };
 
-    /// \brief Double specialization of the Triangle class.
-    typedef Triangle3<double> Triangle3d;
+  /// \brief Integer specialization of the Triangle class.
+  typedef Triangle3<int> Triangle3i;
 
-    /// \brief Float specialization of the Triangle class.
-    typedef Triangle3<float> Triangle3f;
-    }
-  }
-}
-#endif
+  /// \brief Double specialization of the Triangle class.
+  typedef Triangle3<double> Triangle3d;
+
+  /// \brief Float specialization of the Triangle class.
+  typedef Triangle3<float> Triangle3f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_TRIANGLE3_HH_
diff --git a/include/gz/math/Vector2.hh b/include/gz/math/Vector2.hh
index 3fef1be5c..031582bbe 100644
--- a/include/gz/math/Vector2.hh
+++ b/include/gz/math/Vector2.hh
@@ -26,582 +26,579 @@
 #include <gz/math/Helpers.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Vector2 Vector2.hh gz/math/Vector2.hh
+  /// \brief Two dimensional (x, y) vector.
+  template<typename T>
+  class Vector2
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Vector2 Vector2.hh gz/math/Vector2.hh
-    /// \brief Two dimensional (x, y) vector.
-    template<typename T>
-    class Vector2
-    {
-      /// \brief math::Vector2(0, 0)
-      public: static const Vector2<T> &Zero;
+    /// \brief math::Vector2(0, 0)
+    public: static const Vector2<T> &Zero;
 
-      /// \brief math::Vector2(1, 1)
-      public: static const Vector2<T> &One;
+    /// \brief math::Vector2(1, 1)
+    public: static const Vector2<T> &One;
 
-      /// \brief math::Vector2(NaN, NaN, NaN)
-      public: static const Vector2 &NaN;
-
-      /// \brief Default Constructor
-      public: constexpr Vector2()
-      : data{0, 0}
-      {
-      }
+    /// \brief math::Vector2(NaN, NaN, NaN)
+    public: static const Vector2 &NaN;
 
-      /// \brief Constructor
-      /// \param[in] _x value along x
-      /// \param[in] _y value along y
-      public: constexpr Vector2(const T &_x, const T &_y)
-      : data{_x, _y}
-      {
-      }
+    /// \brief Default Constructor
+    public: constexpr Vector2()
+    : data{0, 0}
+    {
+    }
 
-      /// \brief Copy constructor
-      /// \param[in] _v the value
-      public: Vector2(const Vector2<T> &_v) = default;
+    /// \brief Constructor
+    /// \param[in] _x value along x
+    /// \param[in] _y value along y
+    public: constexpr Vector2(const T &_x, const T &_y)
+    : data{_x, _y}
+    {
+    }
 
-      /// \brief Destructor
-      public: ~Vector2() = default;
+    /// \brief Copy constructor
+    /// \param[in] _v the value
+    public: Vector2(const Vector2<T> &_v) = default;
 
-      /// \brief Return the sum of the values
-      /// \return the sum
-      public: T Sum() const
-      {
-        return this->data[0] + this->data[1];
-      }
+    /// \brief Destructor
+    public: ~Vector2() = default;
 
-      /// \brief Calc distance to the given point
-      /// \param[in] _pt The point to measure to
-      /// \return the distance
-      public: double Distance(const Vector2 &_pt) const
-      {
-        return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
-                    (this->data[1]-_pt[1])*(this->data[1]-_pt[1]));
-      }
+    /// \brief Return the sum of the values
+    /// \return the sum
+    public: T Sum() const
+    {
+      return this->data[0] + this->data[1];
+    }
 
-      /// \brief Returns the length (magnitude) of the vector
-      /// \return The length
-      public: T Length() const
-      {
-        return static_cast<T>(sqrt(this->SquaredLength()));
-      }
+    /// \brief Calc distance to the given point
+    /// \param[in] _pt The point to measure to
+    /// \return the distance
+    public: double Distance(const Vector2 &_pt) const
+    {
+      return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
+                  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]));
+    }
 
-      /// \brief Returns the square of the length (magnitude) of the vector
-      /// \return The squared length
-      public: T SquaredLength() const
-      {
-        return
-          this->data[0] * this->data[0] +
-          this->data[1] * this->data[1];
-      }
+    /// \brief Returns the length (magnitude) of the vector
+    /// \return The length
+    public: T Length() const
+    {
+      return static_cast<T>(sqrt(this->SquaredLength()));
+    }
 
-      /// \brief Normalize the vector length
-      public: void Normalize()
-      {
-        T d = this->Length();
+    /// \brief Returns the square of the length (magnitude) of the vector
+    /// \return The squared length
+    public: T SquaredLength() const
+    {
+      return
+        this->data[0] * this->data[0] +
+        this->data[1] * this->data[1];
+    }
 
-        if (!equal<T>(d, static_cast<T>(0.0)))
-        {
-          this->data[0] /= d;
-          this->data[1] /= d;
-        }
-      }
+    /// \brief Normalize the vector length
+    public: void Normalize()
+    {
+      T d = this->Length();
 
-      /// \brief Returns a normalized vector
-      /// \return unit length vector
-      public: Vector2 Normalized() const
+      if (!equal<T>(d, static_cast<T>(0.0)))
       {
-        Vector2<T> result = *this;
-        result.Normalize();
-        return result;
+        this->data[0] /= d;
+        this->data[1] /= d;
       }
+    }
 
-      /// \brief Round to near whole number, return the result.
-      /// \return the result
-      public: Vector2 Round()
-      {
-        this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
-        this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
-        return *this;
-      }
+    /// \brief Returns a normalized vector
+    /// \return unit length vector
+    public: Vector2 Normalized() const
+    {
+      Vector2<T> result = *this;
+      result.Normalize();
+      return result;
+    }
 
-      /// \brief Get a rounded version of this vector
-      /// \return a rounded vector
-      public: Vector2 Rounded() const
-      {
-        Vector2<T> result = *this;
-        result.Round();
-        return result;
-      }
+    /// \brief Round to near whole number, return the result.
+    /// \return the result
+    public: Vector2 Round()
+    {
+      this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
+      this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
+      return *this;
+    }
 
-      /// \brief Set the contents of the vector
-      /// \param[in] _x value along x
-      /// \param[in] _y value along y
-      public: void Set(T _x, T _y)
-      {
-        this->data[0] = _x;
-        this->data[1] = _y;
-      }
+    /// \brief Get a rounded version of this vector
+    /// \return a rounded vector
+    public: Vector2 Rounded() const
+    {
+      Vector2<T> result = *this;
+      result.Round();
+      return result;
+    }
 
-      /// \brief Get the dot product of this vector and _v
-      /// \param[in] _v the vector
-      /// \return The dot product
-      public: T Dot(const Vector2<T> &_v) const
-      {
-        return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
-      }
+    /// \brief Set the contents of the vector
+    /// \param[in] _x value along x
+    /// \param[in] _y value along y
+    public: void Set(T _x, T _y)
+    {
+      this->data[0] = _x;
+      this->data[1] = _y;
+    }
 
-      /// \brief Get the absolute value of the vector
-      /// \return a vector with positive elements
-      public: Vector2 Abs() const
-      {
-        return Vector2(std::abs(this->data[0]),
-                       std::abs(this->data[1]));
-      }
+    /// \brief Get the dot product of this vector and _v
+    /// \param[in] _v the vector
+    /// \return The dot product
+    public: T Dot(const Vector2<T> &_v) const
+    {
+      return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
+    }
 
-      /// \brief Return the absolute dot product of this vector and
-      /// another vector. This is similar to the Dot function, except the
-      /// absolute value of each component of the vector is used.
-      ///
-      /// result = abs(x1 * x2) + abs(y1 * y2)
-      ///
-      /// \param[in] _v The vector
-      /// \return The absolute dot product
-      public: T AbsDot(const Vector2<T> &_v) const
-      {
-        return std::abs(this->data[0] * _v[0]) +
-               std::abs(this->data[1] * _v[1]);
-      }
+    /// \brief Get the absolute value of the vector
+    /// \return a vector with positive elements
+    public: Vector2 Abs() const
+    {
+      return Vector2(std::abs(this->data[0]),
+                     std::abs(this->data[1]));
+    }
 
-       /// \brief Corrects any nan values
-      public: inline void Correct()
-      {
-        // std::isfinite works with floating point values,
-        // need to explicit cast to avoid ambiguity in vc++.
-        if (!std::isfinite(static_cast<double>(this->data[0])))
-          this->data[0] = 0;
-        if (!std::isfinite(static_cast<double>(this->data[1])))
-          this->data[1] = 0;
-      }
+    /// \brief Return the absolute dot product of this vector and
+    /// another vector. This is similar to the Dot function, except the
+    /// absolute value of each component of the vector is used.
+    ///
+    /// result = abs(x1 * x2) + abs(y1 * y2)
+    ///
+    /// \param[in] _v The vector
+    /// \return The absolute dot product
+    public: T AbsDot(const Vector2<T> &_v) const
+    {
+      return std::abs(this->data[0] * _v[0]) +
+             std::abs(this->data[1] * _v[1]);
+    }
 
-      /// \brief Set this vector's components to the maximum of itself and the
-      ///        passed in vector
-      /// \param[in] _v the maximum clamping vector
-      public: void Max(const Vector2<T> &_v)
-      {
-        this->data[0] = std::max(_v[0], this->data[0]);
-        this->data[1] = std::max(_v[1], this->data[1]);
-      }
+     /// \brief Corrects any nan values
+    public: inline void Correct()
+    {
+      // std::isfinite works with floating point values,
+      // need to explicit cast to avoid ambiguity in vc++.
+      if (!std::isfinite(static_cast<double>(this->data[0])))
+        this->data[0] = 0;
+      if (!std::isfinite(static_cast<double>(this->data[1])))
+        this->data[1] = 0;
+    }
 
-      /// \brief Set this vector's components to the minimum of itself and the
-      ///        passed in vector
-      /// \param[in] _v the minimum clamping vector
-      public: void Min(const Vector2<T> &_v)
-      {
-        this->data[0] = std::min(_v[0], this->data[0]);
-        this->data[1] = std::min(_v[1], this->data[1]);
-      }
+    /// \brief Set this vector's components to the maximum of itself and the
+    ///        passed in vector
+    /// \param[in] _v the maximum clamping vector
+    public: void Max(const Vector2<T> &_v)
+    {
+      this->data[0] = std::max(_v[0], this->data[0]);
+      this->data[1] = std::max(_v[1], this->data[1]);
+    }
 
-      /// \brief Get the maximum value in the vector
-      /// \return the maximum element
-      public: T Max() const
-      {
-        return std::max(this->data[0], this->data[1]);
-      }
+    /// \brief Set this vector's components to the minimum of itself and the
+    ///        passed in vector
+    /// \param[in] _v the minimum clamping vector
+    public: void Min(const Vector2<T> &_v)
+    {
+      this->data[0] = std::min(_v[0], this->data[0]);
+      this->data[1] = std::min(_v[1], this->data[1]);
+    }
 
-      /// \brief Get the minimum value in the vector
-      /// \return the minimum element
-      public: T Min() const
-      {
-        return std::min(this->data[0], this->data[1]);
-      }
+    /// \brief Get the maximum value in the vector
+    /// \return the maximum element
+    public: T Max() const
+    {
+      return std::max(this->data[0], this->data[1]);
+    }
 
-      /// \brief Assignment operator
-      /// \param[in] _v a value for x and y element
-      /// \return this
-      public: Vector2 &operator=(const Vector2 &_v) = default;
+    /// \brief Get the minimum value in the vector
+    /// \return the minimum element
+    public: T Min() const
+    {
+      return std::min(this->data[0], this->data[1]);
+    }
 
-      /// \brief Assignment operator
-      /// \param[in] _v the value for x and y element
-      /// \return this
-      public: const Vector2 &operator=(T _v)
-      {
-        this->data[0] = _v;
-        this->data[1] = _v;
+    /// \brief Assignment operator
+    /// \param[in] _v a value for x and y element
+    /// \return this
+    public: Vector2 &operator=(const Vector2 &_v) = default;
 
-        return *this;
-      }
+    /// \brief Assignment operator
+    /// \param[in] _v the value for x and y element
+    /// \return this
+    public: const Vector2 &operator=(T _v)
+    {
+      this->data[0] = _v;
+      this->data[1] = _v;
 
-      /// \brief Addition operator
-      /// \param[in] _v vector to add
-      /// \return sum vector
-      public: Vector2 operator+(const Vector2 &_v) const
-      {
-        return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]);
-      }
+      return *this;
+    }
 
-      /// \brief Addition assignment operator
-      /// \param[in] _v the vector to add
-      // \return this
-      public: const Vector2 &operator+=(const Vector2 &_v)
-      {
-        this->data[0] += _v[0];
-        this->data[1] += _v[1];
+    /// \brief Addition operator
+    /// \param[in] _v vector to add
+    /// \return sum vector
+    public: Vector2 operator+(const Vector2 &_v) const
+    {
+      return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]);
+    }
 
-        return *this;
-      }
+    /// \brief Addition assignment operator
+    /// \param[in] _v the vector to add
+    // \return this
+    public: const Vector2 &operator+=(const Vector2 &_v)
+    {
+      this->data[0] += _v[0];
+      this->data[1] += _v[1];
 
-      /// \brief Addition operators
-      /// \param[in] _s the scalar addend
-      /// \return sum vector
-      public: inline Vector2<T> operator+(const T _s) const
-      {
-        return Vector2<T>(this->data[0] + _s,
-                          this->data[1] + _s);
-      }
+      return *this;
+    }
 
-      /// \brief Addition operators
-      /// \param[in] _s the scalar addend
-      /// \param[in] _v input vector
-      /// \return sum vector
-      public: friend inline Vector2<T> operator+(const T _s,
-                                                 const Vector2<T> &_v)
-      {
-        return _v + _s;
-      }
+    /// \brief Addition operators
+    /// \param[in] _s the scalar addend
+    /// \return sum vector
+    public: inline Vector2<T> operator+(const T _s) const
+    {
+      return Vector2<T>(this->data[0] + _s,
+                        this->data[1] + _s);
+    }
 
-      /// \brief Addition assignment operator
-      /// \param[in] _s scalar addend
-      /// \return this
-      public: const Vector2<T> &operator+=(const T _s)
-      {
-        this->data[0] += _s;
-        this->data[1] += _s;
+    /// \brief Addition operators
+    /// \param[in] _s the scalar addend
+    /// \param[in] _v input vector
+    /// \return sum vector
+    public: friend inline Vector2<T> operator+(const T _s,
+                                               const Vector2<T> &_v)
+    {
+      return _v + _s;
+    }
 
-        return *this;
-      }
+    /// \brief Addition assignment operator
+    /// \param[in] _s scalar addend
+    /// \return this
+    public: const Vector2<T> &operator+=(const T _s)
+    {
+      this->data[0] += _s;
+      this->data[1] += _s;
 
-      /// \brief Negation operator
-      /// \return negative of this vector
-      public: inline Vector2 operator-() const
-      {
-        return Vector2(-this->data[0], -this->data[1]);
-      }
+      return *this;
+    }
 
-      /// \brief Subtraction operator
-      /// \param[in] _v the vector to substract
-      /// \return the subtracted vector
-      public: Vector2 operator-(const Vector2 &_v) const
-      {
-        return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]);
-      }
+    /// \brief Negation operator
+    /// \return negative of this vector
+    public: inline Vector2 operator-() const
+    {
+      return Vector2(-this->data[0], -this->data[1]);
+    }
 
-      /// \brief Subtraction assignment operator
-      /// \param[in] _v the vector to substract
-      /// \return this
-      public: const Vector2 &operator-=(const Vector2 &_v)
-      {
-        this->data[0] -= _v[0];
-        this->data[1] -= _v[1];
+    /// \brief Subtraction operator
+    /// \param[in] _v the vector to substract
+    /// \return the subtracted vector
+    public: Vector2 operator-(const Vector2 &_v) const
+    {
+      return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]);
+    }
 
-        return *this;
-      }
+    /// \brief Subtraction assignment operator
+    /// \param[in] _v the vector to substract
+    /// \return this
+    public: const Vector2 &operator-=(const Vector2 &_v)
+    {
+      this->data[0] -= _v[0];
+      this->data[1] -= _v[1];
 
-      /// \brief Subtraction operators
-      /// \param[in] _s the scalar subtrahend
-      /// \return difference vector
-      public: inline Vector2<T> operator-(const T _s) const
-      {
-        return Vector2<T>(this->data[0] - _s,
-                          this->data[1] - _s);
-      }
+      return *this;
+    }
 
-      /// \brief Subtraction operators
-      /// \param[in] _s the scalar minuend
-      /// \param[in] _v vector subtrahend
-      /// \return difference vector
-      public: friend inline Vector2<T> operator-(const T _s,
-                                                 const Vector2<T> &_v)
-      {
-        return {_s - _v.X(), _s - _v.Y()};
-      }
+    /// \brief Subtraction operators
+    /// \param[in] _s the scalar subtrahend
+    /// \return difference vector
+    public: inline Vector2<T> operator-(const T _s) const
+    {
+      return Vector2<T>(this->data[0] - _s,
+                        this->data[1] - _s);
+    }
 
-      /// \brief Subtraction assignment operator
-      /// \param[in] _s scalar subtrahend
-      /// \return this
-      public: const Vector2<T> &operator-=(T _s)
-      {
-        this->data[0] -= _s;
-        this->data[1] -= _s;
+    /// \brief Subtraction operators
+    /// \param[in] _s the scalar minuend
+    /// \param[in] _v vector subtrahend
+    /// \return difference vector
+    public: friend inline Vector2<T> operator-(const T _s,
+                                               const Vector2<T> &_v)
+    {
+      return {_s - _v.X(), _s - _v.Y()};
+    }
 
-        return *this;
-      }
+    /// \brief Subtraction assignment operator
+    /// \param[in] _s scalar subtrahend
+    /// \return this
+    public: const Vector2<T> &operator-=(T _s)
+    {
+      this->data[0] -= _s;
+      this->data[1] -= _s;
 
-      /// \brief Division operator
-      /// \remarks this is an element wise division
-      /// \param[in] _v a vector
-      /// \result a result
-      public: const Vector2 operator/(const Vector2 &_v) const
-      {
-        return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]);
-      }
+      return *this;
+    }
 
-      /// \brief Division operator
-      /// \remarks this is an element wise division
-      /// \param[in] _v a vector
-      /// \return this
-      public: const Vector2 &operator/=(const Vector2 &_v)
-      {
-        this->data[0] /= _v[0];
-        this->data[1] /= _v[1];
+    /// \brief Division operator
+    /// \remarks this is an element wise division
+    /// \param[in] _v a vector
+    /// \result a result
+    public: const Vector2 operator/(const Vector2 &_v) const
+    {
+      return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]);
+    }
 
-        return *this;
-      }
+    /// \brief Division operator
+    /// \remarks this is an element wise division
+    /// \param[in] _v a vector
+    /// \return this
+    public: const Vector2 &operator/=(const Vector2 &_v)
+    {
+      this->data[0] /= _v[0];
+      this->data[1] /= _v[1];
 
-      /// \brief Division operator
-      /// \param[in] _v the value
-      /// \return a vector
-      public: const Vector2 operator/(T _v) const
-      {
-        return Vector2(this->data[0] / _v, this->data[1] / _v);
-      }
+      return *this;
+    }
 
-      /// \brief Division operator
-      /// \param[in] _v the divisor
-      /// \return a vector
-      public: const Vector2 &operator/=(T _v)
-      {
-        this->data[0] /= _v;
-        this->data[1] /= _v;
+    /// \brief Division operator
+    /// \param[in] _v the value
+    /// \return a vector
+    public: const Vector2 operator/(T _v) const
+    {
+      return Vector2(this->data[0] / _v, this->data[1] / _v);
+    }
 
-        return *this;
-      }
+    /// \brief Division operator
+    /// \param[in] _v the divisor
+    /// \return a vector
+    public: const Vector2 &operator/=(T _v)
+    {
+      this->data[0] /= _v;
+      this->data[1] /= _v;
 
-      /// \brief Multiplication operators
-      /// \param[in] _v the vector
-      /// \return the result
-      public: const Vector2 operator*(const Vector2 &_v) const
-      {
-        return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]);
-      }
+      return *this;
+    }
 
-      /// \brief Multiplication assignment operator
-      /// \remarks this is an element wise multiplication
-      /// \param[in] _v the vector
-      /// \return this
-      public: const Vector2 &operator*=(const Vector2 &_v)
-      {
-        this->data[0] *= _v[0];
-        this->data[1] *= _v[1];
+    /// \brief Multiplication operators
+    /// \param[in] _v the vector
+    /// \return the result
+    public: const Vector2 operator*(const Vector2 &_v) const
+    {
+      return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]);
+    }
 
-        return *this;
-      }
+    /// \brief Multiplication assignment operator
+    /// \remarks this is an element wise multiplication
+    /// \param[in] _v the vector
+    /// \return this
+    public: const Vector2 &operator*=(const Vector2 &_v)
+    {
+      this->data[0] *= _v[0];
+      this->data[1] *= _v[1];
 
-      /// \brief Multiplication operators
-      /// \param[in] _v the scaling factor
-      /// \return a scaled vector
-      public: const Vector2 operator*(T _v) const
-      {
-        return Vector2(this->data[0] * _v, this->data[1] * _v);
-      }
+      return *this;
+    }
 
-      /// \brief Scalar left multiplication operators
-      /// \param[in] _s the scaling factor
-      /// \param[in] _v the vector to scale
-      /// \return a scaled vector
-      public: friend inline const Vector2 operator*(const T _s,
-                                                    const Vector2 &_v)
-      {
-        return Vector2(_v * _s);
-      }
+    /// \brief Multiplication operators
+    /// \param[in] _v the scaling factor
+    /// \return a scaled vector
+    public: const Vector2 operator*(T _v) const
+    {
+      return Vector2(this->data[0] * _v, this->data[1] * _v);
+    }
 
-      /// \brief Multiplication assignment operator
-      /// \param[in] _v the scaling factor
-      /// \return a scaled vector
-      public: const Vector2 &operator*=(T _v)
-      {
-        this->data[0] *= _v;
-        this->data[1] *= _v;
+    /// \brief Scalar left multiplication operators
+    /// \param[in] _s the scaling factor
+    /// \param[in] _v the vector to scale
+    /// \return a scaled vector
+    public: friend inline const Vector2 operator*(const T _s,
+                                                  const Vector2 &_v)
+    {
+      return Vector2(_v * _s);
+    }
 
-        return *this;
-      }
+    /// \brief Multiplication assignment operator
+    /// \param[in] _v the scaling factor
+    /// \return a scaled vector
+    public: const Vector2 &operator*=(T _v)
+    {
+      this->data[0] *= _v;
+      this->data[1] *= _v;
 
-      /// \brief Equality test with tolerance.
-      /// \param[in] _v the vector to compare to
-      /// \param[in] _tol equality tolerance.
-      /// \return true if the elements of the vectors are equal within
-      /// the tolerence specified by _tol.
-      public: bool Equal(const Vector2 &_v, const T &_tol) const
-      {
-        return equal<T>(this->data[0], _v[0], _tol)
-            && equal<T>(this->data[1], _v[1], _tol);
-      }
+      return *this;
+    }
 
-      /// \brief Equal to operator
-      /// \param[in] _v the vector to compare to
-      /// \return true if the elements of the 2 vectors are equal within
-      /// a tolerence (1e-6)
-      public: bool operator==(const Vector2 &_v) const
-      {
-        return this->Equal(_v, static_cast<T>(1e-6));
-      }
+    /// \brief Equality test with tolerance.
+    /// \param[in] _v the vector to compare to
+    /// \param[in] _tol equality tolerance.
+    /// \return true if the elements of the vectors are equal within
+    /// the tolerence specified by _tol.
+    public: bool Equal(const Vector2 &_v, const T &_tol) const
+    {
+      return equal<T>(this->data[0], _v[0], _tol)
+          && equal<T>(this->data[1], _v[1], _tol);
+    }
 
-      /// \brief Not equal to operator
-      /// \return true if elements are of diffent values (tolerence 1e-6)
-      public: bool operator!=(const Vector2 &_v) const
-      {
-        return !(*this == _v);
-      }
+    /// \brief Equal to operator
+    /// \param[in] _v the vector to compare to
+    /// \return true if the elements of the 2 vectors are equal within
+    /// a tolerence (1e-6)
+    public: bool operator==(const Vector2 &_v) const
+    {
+      return this->Equal(_v, static_cast<T>(1e-6));
+    }
 
-      /// \brief See if a point is finite (e.g., not nan)
-      /// \return true if finite, false otherwise
-      public: bool IsFinite() const
-      {
-        // std::isfinite works with floating point values,
-        // need to explicit cast to avoid ambiguity in vc++.
-        return std::isfinite(static_cast<double>(this->data[0])) &&
-               std::isfinite(static_cast<double>(this->data[1]));
-      }
+    /// \brief Not equal to operator
+    /// \return true if elements are of diffent values (tolerence 1e-6)
+    public: bool operator!=(const Vector2 &_v) const
+    {
+      return !(*this == _v);
+    }
 
-      /// \brief Array subscript operator
-      /// \param[in] _index The index, where 0 == x and 1 == y.
-      /// The index is clamped to the range [0,1].
-      public: T &operator[](const std::size_t _index)
-      {
-        return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
-      }
+    /// \brief See if a point is finite (e.g., not nan)
+    /// \return true if finite, false otherwise
+    public: bool IsFinite() const
+    {
+      // std::isfinite works with floating point values,
+      // need to explicit cast to avoid ambiguity in vc++.
+      return std::isfinite(static_cast<double>(this->data[0])) &&
+             std::isfinite(static_cast<double>(this->data[1]));
+    }
 
-      /// \brief Const-qualified array subscript operator
-      /// \param[in] _index The index, where 0 == x and 1 == y.
-      /// The index is clamped to the range [0,1].
-      public: T operator[](const std::size_t _index) const
-      {
-        return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
-      }
+    /// \brief Array subscript operator
+    /// \param[in] _index The index, where 0 == x and 1 == y.
+    /// The index is clamped to the range [0,1].
+    public: T &operator[](const std::size_t _index)
+    {
+      return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
+    }
 
-      /// \brief Return the x value.
-      /// \return Value of the X component.
-      public: inline T X() const
-      {
-        return this->data[0];
-      }
+    /// \brief Const-qualified array subscript operator
+    /// \param[in] _index The index, where 0 == x and 1 == y.
+    /// The index is clamped to the range [0,1].
+    public: T operator[](const std::size_t _index) const
+    {
+      return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
+    }
 
-      /// \brief Return the y value.
-      /// \return Value of the Y component.
-      public: inline T Y() const
-      {
-        return this->data[1];
-      }
+    /// \brief Return the x value.
+    /// \return Value of the X component.
+    public: inline T X() const
+    {
+      return this->data[0];
+    }
 
-      /// \brief Return a mutable x value.
-      /// \return Value of the X component.
-      public: inline T &X()
-      {
-        return this->data[0];
-      }
+    /// \brief Return the y value.
+    /// \return Value of the Y component.
+    public: inline T Y() const
+    {
+      return this->data[1];
+    }
 
-      /// \brief Return a mutable y value.
-      /// \return Value of the Y component.
-      public: inline T &Y()
-      {
-        return this->data[1];
-      }
+    /// \brief Return a mutable x value.
+    /// \return Value of the X component.
+    public: inline T &X()
+    {
+      return this->data[0];
+    }
 
-      /// \brief Set the x value.
-      /// \param[in] _v Value for the x component.
-      public: inline void X(const T &_v)
-      {
-        this->data[0] = _v;
-      }
+    /// \brief Return a mutable y value.
+    /// \return Value of the Y component.
+    public: inline T &Y()
+    {
+      return this->data[1];
+    }
 
-      /// \brief Set the y value.
-      /// \param[in] _v Value for the y component.
-      public: inline void Y(const T &_v)
-      {
-        this->data[1] = _v;
-      }
+    /// \brief Set the x value.
+    /// \param[in] _v Value for the x component.
+    public: inline void X(const T &_v)
+    {
+      this->data[0] = _v;
+    }
 
-      /// \brief Stream extraction operator
-      /// \param[out] _out output stream
-      /// \param[in] _pt Vector2 to output
-      /// \return The stream
-      public: friend std::ostream
-      &operator<<(std::ostream &_out, const Vector2<T> &_pt)
-      {
-        for (auto i : {0, 1})
-        {
-          if (i > 0)
-            _out << " ";
-
-          appendToStream(_out, _pt[i]);
-        }
-        return _out;
-      }
+    /// \brief Set the y value.
+    /// \param[in] _v Value for the y component.
+    public: inline void Y(const T &_v)
+    {
+      this->data[1] = _v;
+    }
 
-      /// \brief Less than operator.
-      /// \param[in] _pt Vector to compare.
-      /// \return True if this vector's first or second value is less than
-      /// the given vector's first or second value.
-      public: bool operator<(const Vector2<T> &_pt) const
+    /// \brief Stream extraction operator
+    /// \param[out] _out output stream
+    /// \param[in] _pt Vector2 to output
+    /// \return The stream
+    public: friend std::ostream
+    &operator<<(std::ostream &_out, const Vector2<T> &_pt)
+    {
+      for (auto i : {0, 1})
       {
-        return this->data[0] < _pt[0] || this->data[1] < _pt[1];
+        if (i > 0)
+          _out << " ";
+
+        appendToStream(_out, _pt[i]);
       }
+      return _out;
+    }
 
-      /// \brief Stream extraction operator
-      /// \param[in] _in input stream
-      /// \param[in] _pt Vector2 to read values into
-      /// \return The stream
-      public: friend std::istream
-      &operator>>(std::istream &_in, Vector2<T> &_pt)
+    /// \brief Less than operator.
+    /// \param[in] _pt Vector to compare.
+    /// \return True if this vector's first or second value is less than
+    /// the given vector's first or second value.
+    public: bool operator<(const Vector2<T> &_pt) const
+    {
+      return this->data[0] < _pt[0] || this->data[1] < _pt[1];
+    }
+
+    /// \brief Stream extraction operator
+    /// \param[in] _in input stream
+    /// \param[in] _pt Vector2 to read values into
+    /// \return The stream
+    public: friend std::istream
+    &operator>>(std::istream &_in, Vector2<T> &_pt)
+    {
+      T x, y;
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      _in >> x >> y;
+      if (!_in.fail())
       {
-        T x, y;
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        _in >> x >> y;
-        if (!_in.fail())
-        {
-          _pt.Set(x, y);
-        }
-        return _in;
+        _pt.Set(x, y);
       }
+      return _in;
+    }
 
-      /// \brief The x and y values.
-      private: T data[2];
-    };
+    /// \brief The x and y values.
+    private: T data[2];
+  };
 
-    namespace detail {
+  namespace detail {
 
-      template<typename T>
-      constexpr Vector2<T> gVector2Zero(0, 0);
+    template<typename T>
+    constexpr Vector2<T> gVector2Zero(0, 0);
 
-      template<typename T>
-      constexpr Vector2<T> gVector2One(1, 1);
+    template<typename T>
+    constexpr Vector2<T> gVector2One(1, 1);
 
-      template<typename T>
-      constexpr Vector2<T> gVector2NaN(
-          std::numeric_limits<T>::quiet_NaN(),
-          std::numeric_limits<T>::quiet_NaN());
+    template<typename T>
+    constexpr Vector2<T> gVector2NaN(
+        std::numeric_limits<T>::quiet_NaN(),
+        std::numeric_limits<T>::quiet_NaN());
 
-    }  // namespace detail
+  }  // namespace detail
 
-    template<typename T>
-    const Vector2<T> &Vector2<T>::Zero = detail::gVector2Zero<T>;
+  template<typename T>
+  const Vector2<T> &Vector2<T>::Zero = detail::gVector2Zero<T>;
 
-    template<typename T>
-    const Vector2<T> &Vector2<T>::One = detail::gVector2One<T>;
+  template<typename T>
+  const Vector2<T> &Vector2<T>::One = detail::gVector2One<T>;
 
-    template<typename T>
-    const Vector2<T> &Vector2<T>::NaN = detail::gVector2NaN<T>;
+  template<typename T>
+  const Vector2<T> &Vector2<T>::NaN = detail::gVector2NaN<T>;
 
-    typedef Vector2<int> Vector2i;
-    typedef Vector2<double> Vector2d;
-    typedef Vector2<float> Vector2f;
-    }
-  }
-}
-#endif
+  typedef Vector2<int> Vector2i;
+  typedef Vector2<double> Vector2d;
+  typedef Vector2<float> Vector2f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_VECTOR2_HH_
diff --git a/include/gz/math/Vector3.hh b/include/gz/math/Vector3.hh
index 2e30191ec..00d58ec8f 100644
--- a/include/gz/math/Vector3.hh
+++ b/include/gz/math/Vector3.hh
@@ -26,780 +26,777 @@
 #include <gz/math/Helpers.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Vector3 Vector3.hh gz/math/Vector3.hh
+  /// \brief The Vector3 class represents the generic vector containing 3
+  /// elements.  Since it's commonly used to keep coordinate system
+  /// related information, its elements are labeled by x, y, z.
+  template<typename T>
+  class Vector3
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Vector3 Vector3.hh gz/math/Vector3.hh
-    /// \brief The Vector3 class represents the generic vector containing 3
-    /// elements.  Since it's commonly used to keep coordinate system
-    /// related information, its elements are labeled by x, y, z.
-    template<typename T>
-    class Vector3
-    {
-      /// \brief math::Vector3(0, 0, 0)
-      public: static const Vector3 &Zero;
+    /// \brief math::Vector3(0, 0, 0)
+    public: static const Vector3 &Zero;
 
-      /// \brief math::Vector3(1, 1, 1)
-      public: static const Vector3 &One;
+    /// \brief math::Vector3(1, 1, 1)
+    public: static const Vector3 &One;
 
-      /// \brief math::Vector3(1, 0, 0)
-      public: static const Vector3 &UnitX;
+    /// \brief math::Vector3(1, 0, 0)
+    public: static const Vector3 &UnitX;
 
-      /// \brief math::Vector3(0, 1, 0)
-      public: static const Vector3 &UnitY;
+    /// \brief math::Vector3(0, 1, 0)
+    public: static const Vector3 &UnitY;
 
-      /// \brief math::Vector3(0, 0, 1)
-      public: static const Vector3 &UnitZ;
+    /// \brief math::Vector3(0, 0, 1)
+    public: static const Vector3 &UnitZ;
 
-      /// \brief math::Vector3(NaN, NaN, NaN)
-      public: static const Vector3 &NaN;
+    /// \brief math::Vector3(NaN, NaN, NaN)
+    public: static const Vector3 &NaN;
 
-      /// \brief Constructor
-      public: constexpr Vector3()
-      : data{0, 0, 0}
-      {
-      }
+    /// \brief Constructor
+    public: constexpr Vector3()
+    : data{0, 0, 0}
+    {
+    }
 
-      /// \brief Constructor
-      /// \param[in] _x value along x
-      /// \param[in] _y value along y
-      /// \param[in] _z value along z
-      public: constexpr Vector3(const T &_x, const T &_y, const T &_z)
-      : data{_x, _y, _z}
-      {
-      }
+    /// \brief Constructor
+    /// \param[in] _x value along x
+    /// \param[in] _y value along y
+    /// \param[in] _z value along z
+    public: constexpr Vector3(const T &_x, const T &_y, const T &_z)
+    : data{_x, _y, _z}
+    {
+    }
 
-      /// \brief Copy constructor
-      /// \param[in] _v a vector
-      public: Vector3(const Vector3<T> &_v) = default;
+    /// \brief Copy constructor
+    /// \param[in] _v a vector
+    public: Vector3(const Vector3<T> &_v) = default;
 
-      /// \brief Destructor
-      public: ~Vector3() = default;
+    /// \brief Destructor
+    public: ~Vector3() = default;
 
-      /// \brief Return the sum of the values
-      /// \return the sum
-      public: T Sum() const
-      {
-        return this->data[0] + this->data[1] + this->data[2];
-      }
+    /// \brief Return the sum of the values
+    /// \return the sum
+    public: T Sum() const
+    {
+      return this->data[0] + this->data[1] + this->data[2];
+    }
 
-      /// \brief Calc distance to the given point
-      /// \param[in] _pt the point
-      /// \return the distance
-      public: T Distance(const Vector3<T> &_pt) const
-      {
-        return static_cast<T>(sqrt(
-                    (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
-                    (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
-                    (this->data[2]-_pt[2])*(this->data[2]-_pt[2])));
-      }
+    /// \brief Calc distance to the given point
+    /// \param[in] _pt the point
+    /// \return the distance
+    public: T Distance(const Vector3<T> &_pt) const
+    {
+      return static_cast<T>(sqrt(
+                  (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
+                  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
+                  (this->data[2]-_pt[2])*(this->data[2]-_pt[2])));
+    }
 
-      /// \brief Calc distance to the given point
-      /// \param[in] _x value along x
-      /// \param[in] _y value along y
-      /// \param[in] _z value along z
-      /// \return the distance
-      public: T Distance(T _x, T _y, T _z) const
-      {
-        return this->Distance(Vector3(_x, _y, _z));
-      }
+    /// \brief Calc distance to the given point
+    /// \param[in] _x value along x
+    /// \param[in] _y value along y
+    /// \param[in] _z value along z
+    /// \return the distance
+    public: T Distance(T _x, T _y, T _z) const
+    {
+      return this->Distance(Vector3(_x, _y, _z));
+    }
 
-      /// \brief Returns the length (magnitude) of the vector
-      /// \return the length
-      public: T Length() const
-      {
-        return static_cast<T>(sqrt(this->SquaredLength()));
-      }
+    /// \brief Returns the length (magnitude) of the vector
+    /// \return the length
+    public: T Length() const
+    {
+      return static_cast<T>(sqrt(this->SquaredLength()));
+    }
 
-      /// \brief Return the square of the length (magnitude) of the vector
-      /// \return the squared length
-      public: T SquaredLength() const
+    /// \brief Return the square of the length (magnitude) of the vector
+    /// \return the squared length
+    public: T SquaredLength() const
+    {
+      return
+        this->data[0] * this->data[0] +
+        this->data[1] * this->data[1] +
+        this->data[2] * this->data[2];
+    }
+
+    /// \brief Normalize the vector length
+    /// \return unit length vector
+    public: Vector3 Normalize()
+    {
+      T d = this->Length();
+
+      if (!equal<T>(d, static_cast<T>(0.0)))
       {
-        return
-          this->data[0] * this->data[0] +
-          this->data[1] * this->data[1] +
-          this->data[2] * this->data[2];
+        this->data[0] /= d;
+        this->data[1] /= d;
+        this->data[2] /= d;
       }
 
-      /// \brief Normalize the vector length
-      /// \return unit length vector
-      public: Vector3 Normalize()
-      {
-        T d = this->Length();
+      return *this;
+    }
+
+    /// \brief Return a normalized vector
+    /// \return unit length vector
+    public: Vector3 Normalized() const
+    {
+      Vector3<T> result = *this;
+      result.Normalize();
+      return result;
+    }
 
-        if (!equal<T>(d, static_cast<T>(0.0)))
-        {
-          this->data[0] /= d;
-          this->data[1] /= d;
-          this->data[2] /= d;
-        }
+    /// \brief Round to near whole number, return the result.
+    /// \return the result
+    public: Vector3 Round()
+    {
+      this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
+      this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
+      this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
+      return *this;
+    }
 
-        return *this;
-      }
+    /// \brief Get a rounded version of this vector
+    /// \return a rounded vector
+    public: Vector3 Rounded() const
+    {
+      Vector3<T> result = *this;
+      result.Round();
+      return result;
+    }
 
-      /// \brief Return a normalized vector
-      /// \return unit length vector
-      public: Vector3 Normalized() const
-      {
-        Vector3<T> result = *this;
-        result.Normalize();
-        return result;
-      }
+    /// \brief Set the contents of the vector
+    /// \param[in] _x value along x
+    /// \param[in] _y value along y
+    /// \param[in] _z value aling z
+    public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
+    {
+      this->data[0] = _x;
+      this->data[1] = _y;
+      this->data[2] = _z;
+    }
 
-      /// \brief Round to near whole number, return the result.
-      /// \return the result
-      public: Vector3 Round()
-      {
-        this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
-        this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
-        this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
-        return *this;
-      }
+    /// \brief Return the cross product of this vector with another vector.
+    /// \param[in] _v a vector
+    /// \return the cross product
+    public: Vector3 Cross(const Vector3<T> &_v) const
+    {
+      return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
+                     this->data[2] * _v[0] - this->data[0] * _v[2],
+                     this->data[0] * _v[1] - this->data[1] * _v[0]);
+    }
 
-      /// \brief Get a rounded version of this vector
-      /// \return a rounded vector
-      public: Vector3 Rounded() const
-      {
-        Vector3<T> result = *this;
-        result.Round();
-        return result;
-      }
+    /// \brief Return the dot product of this vector and another vector
+    /// \param[in] _v the vector
+    /// \return the dot product
+    public: T Dot(const Vector3<T> &_v) const
+    {
+      return this->data[0] * _v[0] +
+             this->data[1] * _v[1] +
+             this->data[2] * _v[2];
+    }
 
-      /// \brief Set the contents of the vector
-      /// \param[in] _x value along x
-      /// \param[in] _y value along y
-      /// \param[in] _z value aling z
-      public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
-      {
-        this->data[0] = _x;
-        this->data[1] = _y;
-        this->data[2] = _z;
-      }
+    /// \brief Return the absolute dot product of this vector and
+    /// another vector. This is similar to the Dot function, except the
+    /// absolute value of each component of the vector is used.
+    ///
+    /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 *z2)
+    ///
+    /// \param[in] _v the vector
+    /// \return The absolute dot product
+    public: T AbsDot(const Vector3<T> &_v) const
+    {
+      return std::abs(this->data[0] * _v[0]) +
+             std::abs(this->data[1] * _v[1]) +
+             std::abs(this->data[2] * _v[2]);
+    }
 
-      /// \brief Return the cross product of this vector with another vector.
-      /// \param[in] _v a vector
-      /// \return the cross product
-      public: Vector3 Cross(const Vector3<T> &_v) const
-      {
-        return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
-                       this->data[2] * _v[0] - this->data[0] * _v[2],
-                       this->data[0] * _v[1] - this->data[1] * _v[0]);
-      }
+    /// \brief Get the absolute value of the vector
+    /// \return a vector with positive elements
+    public: Vector3 Abs() const
+    {
+      return Vector3(std::abs(this->data[0]),
+                     std::abs(this->data[1]),
+                     std::abs(this->data[2]));
+    }
 
-      /// \brief Return the dot product of this vector and another vector
-      /// \param[in] _v the vector
-      /// \return the dot product
-      public: T Dot(const Vector3<T> &_v) const
-      {
-        return this->data[0] * _v[0] +
-               this->data[1] * _v[1] +
-               this->data[2] * _v[2];
-      }
+    /// \brief Return a vector that is perpendicular to this one.
+    /// \return an orthogonal vector
+    public: Vector3 Perpendicular() const
+    {
+      static const T sqrZero = static_cast<T>(1e-06 * 1e-06);
 
-      /// \brief Return the absolute dot product of this vector and
-      /// another vector. This is similar to the Dot function, except the
-      /// absolute value of each component of the vector is used.
-      ///
-      /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 *z2)
-      ///
-      /// \param[in] _v the vector
-      /// \return The absolute dot product
-      public: T AbsDot(const Vector3<T> &_v) const
-      {
-        return std::abs(this->data[0] * _v[0]) +
-               std::abs(this->data[1] * _v[1]) +
-               std::abs(this->data[2] * _v[2]);
-      }
+      Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
 
-      /// \brief Get the absolute value of the vector
-      /// \return a vector with positive elements
-      public: Vector3 Abs() const
+      // Check the length of the vector
+      if (perp.SquaredLength() < sqrZero)
       {
-        return Vector3(std::abs(this->data[0]),
-                       std::abs(this->data[1]),
-                       std::abs(this->data[2]));
+        perp = this->Cross(Vector3(0, 1, 0));
       }
 
-      /// \brief Return a vector that is perpendicular to this one.
-      /// \return an orthogonal vector
-      public: Vector3 Perpendicular() const
-      {
-        static const T sqrZero = static_cast<T>(1e-06 * 1e-06);
+      return perp;
+    }
 
-        Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
+    /// \brief Get a normal vector to a triangle
+    /// \param[in] _v1 first vertex of the triangle
+    /// \param[in] _v2 second vertex
+    /// \param[in] _v3 third vertex
+    /// \return the normal
+    public: static Vector3 Normal(const Vector3<T> &_v1,
+                const Vector3<T> &_v2, const Vector3<T> &_v3)
+    {
+      Vector3<T> a = _v2 - _v1;
+      Vector3<T> b = _v3 - _v1;
+      Vector3<T> n = a.Cross(b);
+      return n.Normalize();
+    }
 
-        // Check the length of the vector
-        if (perp.SquaredLength() < sqrZero)
-        {
-          perp = this->Cross(Vector3(0, 1, 0));
-        }
+    /// \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
+    public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
+    {
+      T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
+      d = d / (_pt2 - _pt1).Length();
+      return d;
+    }
 
-        return perp;
-      }
+    /// \brief Set this vector's components to the maximum of itself and the
+    ///        passed in vector
+    /// \param[in] _v the maximum clamping vector
+    public: void Max(const Vector3<T> &_v)
+    {
+      if (_v[0] > this->data[0])
+        this->data[0] = _v[0];
+      if (_v[1] > this->data[1])
+        this->data[1] = _v[1];
+      if (_v[2] > this->data[2])
+        this->data[2] = _v[2];
+    }
 
-      /// \brief Get a normal vector to a triangle
-      /// \param[in] _v1 first vertex of the triangle
-      /// \param[in] _v2 second vertex
-      /// \param[in] _v3 third vertex
-      /// \return the normal
-      public: static Vector3 Normal(const Vector3<T> &_v1,
-                  const Vector3<T> &_v2, const Vector3<T> &_v3)
-      {
-        Vector3<T> a = _v2 - _v1;
-        Vector3<T> b = _v3 - _v1;
-        Vector3<T> n = a.Cross(b);
-        return n.Normalize();
-      }
+    /// \brief Set this vector's components to the minimum of itself and the
+    ///        passed in vector
+    /// \param[in] _v the minimum clamping vector
+    public: void Min(const Vector3<T> &_v)
+    {
+      if (_v[0] < this->data[0])
+        this->data[0] = _v[0];
+      if (_v[1] < this->data[1])
+        this->data[1] = _v[1];
+      if (_v[2] < this->data[2])
+        this->data[2] = _v[2];
+    }
 
-      /// \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
-      public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
-      {
-        T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
-        d = d / (_pt2 - _pt1).Length();
-        return d;
-      }
+    /// \brief Get the maximum value in the vector
+    /// \return the maximum element
+    public: T Max() const
+    {
+      return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
+    }
 
-      /// \brief Set this vector's components to the maximum of itself and the
-      ///        passed in vector
-      /// \param[in] _v the maximum clamping vector
-      public: void Max(const Vector3<T> &_v)
-      {
-        if (_v[0] > this->data[0])
-          this->data[0] = _v[0];
-        if (_v[1] > this->data[1])
-          this->data[1] = _v[1];
-        if (_v[2] > this->data[2])
-          this->data[2] = _v[2];
-      }
+    /// \brief Get the minimum value in the vector
+    /// \return the minimum element
+    public: T Min() const
+    {
+      return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
+    }
 
-      /// \brief Set this vector's components to the minimum of itself and the
-      ///        passed in vector
-      /// \param[in] _v the minimum clamping vector
-      public: void Min(const Vector3<T> &_v)
-      {
-        if (_v[0] < this->data[0])
-          this->data[0] = _v[0];
-        if (_v[1] < this->data[1])
-          this->data[1] = _v[1];
-        if (_v[2] < this->data[2])
-          this->data[2] = _v[2];
-      }
+    /// \brief Get the number with the maximum absolute value in the vector
+    /// \return the element with maximum absolute value
+    public: T MaxAbs() const
+    {
+      T max = std::max(std::abs(this->data[0]), std::abs(this->data[1]));
+      max = std::max(max, std::abs(this->data[2]));
+      return max;
+    }
 
-      /// \brief Get the maximum value in the vector
-      /// \return the maximum element
-      public: T Max() const
-      {
-        return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
-      }
+    /// \brief Get the number with the maximum absolute value in the vector
+    /// \return the element with minimum absolute value
+    public: T MinAbs() const
+    {
+      T min = std::min(std::abs(this->data[0]), std::abs(this->data[1]));
+      min = std::min(min, std::abs(this->data[2]));
+      return min;
+    }
 
-      /// \brief Get the minimum value in the vector
-      /// \return the minimum element
-      public: T Min() const
-      {
-        return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
-      }
+    /// \brief Assignment operator
+    /// \param[in] _v a new value
+    /// \return this
+    public: Vector3 &operator=(const Vector3<T> &_v) = default;
 
-      /// \brief Get the number with the maximum absolute value in the vector
-      /// \return the element with maximum absolute value
-      public: T MaxAbs() const
-      {
-        T max = std::max(std::abs(this->data[0]), std::abs(this->data[1]));
-        max = std::max(max, std::abs(this->data[2]));
-        return max;
-      }
+    /// \brief Assignment operator
+    /// \param[in] _v assigned to all elements
+    /// \return this
+    public: Vector3 &operator=(T _v)
+    {
+      this->data[0] = _v;
+      this->data[1] = _v;
+      this->data[2] = _v;
 
-      /// \brief Get the number with the maximum absolute value in the vector
-      /// \return the element with minimum absolute value
-      public: T MinAbs() const
-      {
-        T min = std::min(std::abs(this->data[0]), std::abs(this->data[1]));
-        min = std::min(min, std::abs(this->data[2]));
-        return min;
-      }
+      return *this;
+    }
 
-      /// \brief Assignment operator
-      /// \param[in] _v a new value
-      /// \return this
-      public: Vector3 &operator=(const Vector3<T> &_v) = default;
+    /// \brief Addition operator
+    /// \param[in] _v vector to add
+    /// \return the sum vector
+    public: Vector3 operator+(const Vector3<T> &_v) const
+    {
+      return Vector3(this->data[0] + _v[0],
+                     this->data[1] + _v[1],
+                     this->data[2] + _v[2]);
+    }
 
-      /// \brief Assignment operator
-      /// \param[in] _v assigned to all elements
-      /// \return this
-      public: Vector3 &operator=(T _v)
-      {
-        this->data[0] = _v;
-        this->data[1] = _v;
-        this->data[2] = _v;
+    /// \brief Addition assignment operator
+    /// \param[in] _v vector to add
+    /// \return the sum vector
+    public: const Vector3 &operator+=(const Vector3<T> &_v)
+    {
+      this->data[0] += _v[0];
+      this->data[1] += _v[1];
+      this->data[2] += _v[2];
 
-        return *this;
-      }
+      return *this;
+    }
 
-      /// \brief Addition operator
-      /// \param[in] _v vector to add
-      /// \return the sum vector
-      public: Vector3 operator+(const Vector3<T> &_v) const
-      {
-        return Vector3(this->data[0] + _v[0],
-                       this->data[1] + _v[1],
-                       this->data[2] + _v[2]);
-      }
+    /// \brief Addition operators
+    /// \param[in] _s the scalar addend
+    /// \return sum vector
+    public: inline Vector3<T> operator+(const T _s) const
+    {
+      return Vector3<T>(this->data[0] + _s,
+                        this->data[1] + _s,
+                        this->data[2] + _s);
+    }
 
-      /// \brief Addition assignment operator
-      /// \param[in] _v vector to add
-      /// \return the sum vector
-      public: const Vector3 &operator+=(const Vector3<T> &_v)
-      {
-        this->data[0] += _v[0];
-        this->data[1] += _v[1];
-        this->data[2] += _v[2];
+    /// \brief Addition operators
+    /// \param[in] _s the scalar addend
+    /// \param[in] _v input vector
+    /// \return sum vector
+    public: friend inline Vector3<T> operator+(const T _s,
+                                               const Vector3<T> &_v)
+    {
+      return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s};
+    }
 
-        return *this;
-      }
+    /// \brief Addition assignment operator
+    /// \param[in] _s scalar addend
+    /// \return this
+    public: const Vector3<T> &operator+=(const T _s)
+    {
+      this->data[0] += _s;
+      this->data[1] += _s;
+      this->data[2] += _s;
 
-      /// \brief Addition operators
-      /// \param[in] _s the scalar addend
-      /// \return sum vector
-      public: inline Vector3<T> operator+(const T _s) const
-      {
-        return Vector3<T>(this->data[0] + _s,
-                          this->data[1] + _s,
-                          this->data[2] + _s);
-      }
+      return *this;
+    }
 
-      /// \brief Addition operators
-      /// \param[in] _s the scalar addend
-      /// \param[in] _v input vector
-      /// \return sum vector
-      public: friend inline Vector3<T> operator+(const T _s,
-                                                 const Vector3<T> &_v)
-      {
-        return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s};
-      }
+    /// \brief Negation operator
+    /// \return negative of this vector
+    public: inline Vector3 operator-() const
+    {
+      return Vector3(-this->data[0], -this->data[1], -this->data[2]);
+    }
 
-      /// \brief Addition assignment operator
-      /// \param[in] _s scalar addend
-      /// \return this
-      public: const Vector3<T> &operator+=(const T _s)
-      {
-        this->data[0] += _s;
-        this->data[1] += _s;
-        this->data[2] += _s;
+    /// \brief Subtraction operators
+    /// \param[in] _pt a vector to substract
+    /// \return a vector after the substraction
+    public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
+    {
+      return Vector3(this->data[0] - _pt[0],
+                     this->data[1] - _pt[1],
+                     this->data[2] - _pt[2]);
+    }
 
-        return *this;
-      }
+    /// \brief Subtraction assignment operators
+    /// \param[in] _pt subtrahend
+    /// \return a vector after the substraction
+    public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
+    {
+      this->data[0] -= _pt[0];
+      this->data[1] -= _pt[1];
+      this->data[2] -= _pt[2];
 
-      /// \brief Negation operator
-      /// \return negative of this vector
-      public: inline Vector3 operator-() const
-      {
-        return Vector3(-this->data[0], -this->data[1], -this->data[2]);
-      }
+      return *this;
+    }
 
-      /// \brief Subtraction operators
-      /// \param[in] _pt a vector to substract
-      /// \return a vector after the substraction
-      public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
-      {
-        return Vector3(this->data[0] - _pt[0],
-                       this->data[1] - _pt[1],
-                       this->data[2] - _pt[2]);
-      }
+    /// \brief Subtraction operators
+    /// \param[in] _s the scalar subtrahend
+    /// \return difference vector
+    public: inline Vector3<T> operator-(const T _s) const
+    {
+      return Vector3<T>(this->data[0] - _s,
+                        this->data[1] - _s,
+                        this->data[2] - _s);
+    }
 
-      /// \brief Subtraction assignment operators
-      /// \param[in] _pt subtrahend
-      /// \return a vector after the substraction
-      public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
-      {
-        this->data[0] -= _pt[0];
-        this->data[1] -= _pt[1];
-        this->data[2] -= _pt[2];
+    /// \brief Subtraction operators
+    /// \param[in] _s the scalar minuend
+    /// \param[in] _v vector subtrahend
+    /// \return difference vector
+    public: friend inline Vector3<T> operator-(const T _s,
+                                               const Vector3<T> &_v)
+    {
+      return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()};
+    }
 
-        return *this;
-      }
+    /// \brief Subtraction assignment operator
+    /// \param[in] _s scalar subtrahend
+    /// \return this
+    public: const Vector3<T> &operator-=(const T _s)
+    {
+      this->data[0] -= _s;
+      this->data[1] -= _s;
+      this->data[2] -= _s;
 
-      /// \brief Subtraction operators
-      /// \param[in] _s the scalar subtrahend
-      /// \return difference vector
-      public: inline Vector3<T> operator-(const T _s) const
-      {
-        return Vector3<T>(this->data[0] - _s,
-                          this->data[1] - _s,
-                          this->data[2] - _s);
-      }
+      return *this;
+    }
 
-      /// \brief Subtraction operators
-      /// \param[in] _s the scalar minuend
-      /// \param[in] _v vector subtrahend
-      /// \return difference vector
-      public: friend inline Vector3<T> operator-(const T _s,
-                                                 const Vector3<T> &_v)
-      {
-        return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()};
-      }
+    /// \brief Division operator
+    /// \remarks this is an element wise division
+    /// \param[in] _pt the vector divisor
+    /// \return a vector
+    public: const Vector3<T> operator/(const Vector3<T> &_pt) const
+    {
+      return Vector3(this->data[0] / _pt[0],
+                     this->data[1] / _pt[1],
+                     this->data[2] / _pt[2]);
+    }
 
-      /// \brief Subtraction assignment operator
-      /// \param[in] _s scalar subtrahend
-      /// \return this
-      public: const Vector3<T> &operator-=(const T _s)
-      {
-        this->data[0] -= _s;
-        this->data[1] -= _s;
-        this->data[2] -= _s;
+    /// \brief Division assignment operator
+    /// \remarks this is an element wise division
+    /// \param[in] _pt the vector divisor
+    /// \return a vector
+    public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
+    {
+      this->data[0] /= _pt[0];
+      this->data[1] /= _pt[1];
+      this->data[2] /= _pt[2];
 
-        return *this;
-      }
+      return *this;
+    }
 
-      /// \brief Division operator
-      /// \remarks this is an element wise division
-      /// \param[in] _pt the vector divisor
-      /// \return a vector
-      public: const Vector3<T> operator/(const Vector3<T> &_pt) const
-      {
-        return Vector3(this->data[0] / _pt[0],
-                       this->data[1] / _pt[1],
-                       this->data[2] / _pt[2]);
-      }
+    /// \brief Division operator
+    /// \remarks this is an element wise division
+    /// \param[in] _v the divisor
+    /// \return a vector
+    public: const Vector3<T> operator/(T _v) const
+    {
+      return Vector3(this->data[0] / _v,
+                     this->data[1] / _v,
+                     this->data[2] / _v);
+    }
 
-      /// \brief Division assignment operator
-      /// \remarks this is an element wise division
-      /// \param[in] _pt the vector divisor
-      /// \return a vector
-      public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
-      {
-        this->data[0] /= _pt[0];
-        this->data[1] /= _pt[1];
-        this->data[2] /= _pt[2];
+    /// \brief Division assignment operator
+    /// \remarks this is an element wise division
+    /// \param[in] _v the divisor
+    /// \return this
+    public: const Vector3<T> &operator/=(T _v)
+    {
+      this->data[0] /= _v;
+      this->data[1] /= _v;
+      this->data[2] /= _v;
 
-        return *this;
-      }
+      return *this;
+    }
 
-      /// \brief Division operator
-      /// \remarks this is an element wise division
-      /// \param[in] _v the divisor
-      /// \return a vector
-      public: const Vector3<T> operator/(T _v) const
-      {
-        return Vector3(this->data[0] / _v,
-                       this->data[1] / _v,
-                       this->data[2] / _v);
-      }
+    /// \brief Multiplication operator
+    /// \remarks this is an element wise multiplication, not a cross product
+    /// \param[in] _p multiplier operator
+    /// \return a vector
+    public: Vector3<T> operator*(const Vector3<T> &_p) const
+    {
+      return Vector3(this->data[0] * _p[0],
+                     this->data[1] * _p[1],
+                     this->data[2] * _p[2]);
+    }
 
-      /// \brief Division assignment operator
-      /// \remarks this is an element wise division
-      /// \param[in] _v the divisor
-      /// \return this
-      public: const Vector3<T> &operator/=(T _v)
-      {
-        this->data[0] /= _v;
-        this->data[1] /= _v;
-        this->data[2] /= _v;
+    /// \brief Multiplication assignment operators
+    /// \remarks this is an element wise multiplication, not a cross product
+    /// \param[in] _v a vector
+    /// \return this
+    public: const Vector3<T> &operator*=(const Vector3<T> &_v)
+    {
+      this->data[0] *= _v[0];
+      this->data[1] *= _v[1];
+      this->data[2] *= _v[2];
 
-        return *this;
-      }
+      return *this;
+    }
 
-      /// \brief Multiplication operator
-      /// \remarks this is an element wise multiplication, not a cross product
-      /// \param[in] _p multiplier operator
-      /// \return a vector
-      public: Vector3<T> operator*(const Vector3<T> &_p) const
-      {
-        return Vector3(this->data[0] * _p[0],
-                       this->data[1] * _p[1],
-                       this->data[2] * _p[2]);
-      }
+    /// \brief Multiplication operators
+    /// \param[in] _s the scaling factor
+    /// \return a scaled vector
+    public: inline Vector3<T> operator*(T _s) const
+    {
+      return Vector3<T>(this->data[0] * _s,
+                        this->data[1] * _s,
+                        this->data[2] * _s);
+    }
 
-      /// \brief Multiplication assignment operators
-      /// \remarks this is an element wise multiplication, not a cross product
-      /// \param[in] _v a vector
-      /// \return this
-      public: const Vector3<T> &operator*=(const Vector3<T> &_v)
-      {
-        this->data[0] *= _v[0];
-        this->data[1] *= _v[1];
-        this->data[2] *= _v[2];
+    /// \brief Multiplication operators
+    /// \param[in] _s the scaling factor
+    /// \param[in] _v input vector
+    /// \return a scaled vector
+    public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
+    {
+      return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s};
+    }
 
-        return *this;
-      }
+    /// \brief Multiplication operator
+    /// \param[in] _v scaling factor
+    /// \return this
+    public: const Vector3<T> &operator*=(T _v)
+    {
+      this->data[0] *= _v;
+      this->data[1] *= _v;
+      this->data[2] *= _v;
 
-      /// \brief Multiplication operators
-      /// \param[in] _s the scaling factor
-      /// \return a scaled vector
-      public: inline Vector3<T> operator*(T _s) const
-      {
-        return Vector3<T>(this->data[0] * _s,
-                          this->data[1] * _s,
-                          this->data[2] * _s);
-      }
+      return *this;
+    }
 
-      /// \brief Multiplication operators
-      /// \param[in] _s the scaling factor
-      /// \param[in] _v input vector
-      /// \return a scaled vector
-      public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
-      {
-        return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s};
-      }
+    /// \brief Equality test with tolerance.
+    /// \param[in] _v the vector to compare to
+    /// \param[in] _tol equality tolerance.
+    /// \return true if the elements of the vectors are equal within
+    /// the tolerence specified by _tol.
+    public: bool Equal(const Vector3 &_v, const T &_tol) const
+    {
+      return equal<T>(this->data[0], _v[0], _tol)
+          && equal<T>(this->data[1], _v[1], _tol)
+          && equal<T>(this->data[2], _v[2], _tol);
+    }
 
-      /// \brief Multiplication operator
-      /// \param[in] _v scaling factor
-      /// \return this
-      public: const Vector3<T> &operator*=(T _v)
-      {
-        this->data[0] *= _v;
-        this->data[1] *= _v;
-        this->data[2] *= _v;
+    /// \brief Equal to operator
+    /// \param[in] _v The vector to compare against
+    /// \return true if each component is equal within a
+    /// default tolerence (1e-3), false otherwise
+    public: bool operator==(const Vector3<T> &_v) const
+    {
+      return this->Equal(_v, static_cast<T>(1e-3));
+    }
 
-        return *this;
-      }
+    /// \brief Not equal to operator
+    /// \param[in] _v The vector to compare against
+    /// \return false if each component is equal within a
+    /// default tolerence (1e-3), true otherwise
+    public: bool operator!=(const Vector3<T> &_v) const
+    {
+      return !(*this == _v);
+    }
 
-      /// \brief Equality test with tolerance.
-      /// \param[in] _v the vector to compare to
-      /// \param[in] _tol equality tolerance.
-      /// \return true if the elements of the vectors are equal within
-      /// the tolerence specified by _tol.
-      public: bool Equal(const Vector3 &_v, const T &_tol) const
-      {
-        return equal<T>(this->data[0], _v[0], _tol)
-            && equal<T>(this->data[1], _v[1], _tol)
-            && equal<T>(this->data[2], _v[2], _tol);
-      }
+    /// \brief See if a point is finite (e.g., not nan)
+    /// \return true if is finite or false otherwise
+    public: bool IsFinite() const
+    {
+      // std::isfinite works with floating point values,
+      // need to explicit cast to avoid ambiguity in vc++.
+      return std::isfinite(static_cast<double>(this->data[0])) &&
+             std::isfinite(static_cast<double>(this->data[1])) &&
+             std::isfinite(static_cast<double>(this->data[2]));
+    }
 
-      /// \brief Equal to operator
-      /// \param[in] _v The vector to compare against
-      /// \return true if each component is equal within a
-      /// default tolerence (1e-3), false otherwise
-      public: bool operator==(const Vector3<T> &_v) const
-      {
-        return this->Equal(_v, static_cast<T>(1e-3));
-      }
+    /// \brief Corrects any nan values
+    public: inline void Correct()
+    {
+      // std::isfinite works with floating point values,
+      // need to explicit cast to avoid ambiguity in vc++.
+      if (!std::isfinite(static_cast<double>(this->data[0])))
+        this->data[0] = 0;
+      if (!std::isfinite(static_cast<double>(this->data[1])))
+        this->data[1] = 0;
+      if (!std::isfinite(static_cast<double>(this->data[2])))
+        this->data[2] = 0;
+    }
 
-      /// \brief Not equal to operator
-      /// \param[in] _v The vector to compare against
-      /// \return false if each component is equal within a
-      /// default tolerence (1e-3), true otherwise
-      public: bool operator!=(const Vector3<T> &_v) const
-      {
-        return !(*this == _v);
-      }
+    /// \brief Array subscript operator
+    /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z.
+    /// The index is clamped to the range [0,2].
+    /// \return The value.
+    public: T &operator[](const std::size_t _index)
+    {
+      return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
+    }
 
-      /// \brief See if a point is finite (e.g., not nan)
-      /// \return true if is finite or false otherwise
-      public: bool IsFinite() const
-      {
-        // std::isfinite works with floating point values,
-        // need to explicit cast to avoid ambiguity in vc++.
-        return std::isfinite(static_cast<double>(this->data[0])) &&
-               std::isfinite(static_cast<double>(this->data[1])) &&
-               std::isfinite(static_cast<double>(this->data[2]));
-      }
+    /// \brief Const-qualified array subscript operator
+    /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z.
+    /// The index is clamped to the range [0,2].
+    /// \return The value.
+    public: T operator[](const std::size_t _index) const
+    {
+      return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
+    }
 
-      /// \brief Corrects any nan values
-      public: inline void Correct()
-      {
-        // std::isfinite works with floating point values,
-        // need to explicit cast to avoid ambiguity in vc++.
-        if (!std::isfinite(static_cast<double>(this->data[0])))
-          this->data[0] = 0;
-        if (!std::isfinite(static_cast<double>(this->data[1])))
-          this->data[1] = 0;
-        if (!std::isfinite(static_cast<double>(this->data[2])))
-          this->data[2] = 0;
-      }
+    /// \brief Round all values to _precision decimal places
+    /// \param[in] _precision the decimal places
+    public: void Round(int _precision)
+    {
+      this->data[0] = precision(this->data[0], _precision);
+      this->data[1] = precision(this->data[1], _precision);
+      this->data[2] = precision(this->data[2], _precision);
+    }
 
-      /// \brief Array subscript operator
-      /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z.
-      /// The index is clamped to the range [0,2].
-      /// \return The value.
-      public: T &operator[](const std::size_t _index)
-      {
-        return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
-      }
+    /// \brief Equality test
+    /// \remarks This is equivalent to the == operator
+    /// \param[in] _v the other vector
+    /// \return true if the 2 vectors have the same values, false otherwise
+    public: bool Equal(const Vector3<T> &_v) const
+    {
+      return equal<T>(this->data[0], _v[0]) &&
+             equal<T>(this->data[1], _v[1]) &&
+             equal<T>(this->data[2], _v[2]);
+    }
 
-      /// \brief Const-qualified array subscript operator
-      /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z.
-      /// The index is clamped to the range [0,2].
-      /// \return The value.
-      public: T operator[](const std::size_t _index) const
-      {
-        return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
-      }
+    /// \brief Get the x value.
+    /// \return The x component of the vector
+    public: inline T X() const
+    {
+      return this->data[0];
+    }
 
-      /// \brief Round all values to _precision decimal places
-      /// \param[in] _precision the decimal places
-      public: void Round(int _precision)
-      {
-        this->data[0] = precision(this->data[0], _precision);
-        this->data[1] = precision(this->data[1], _precision);
-        this->data[2] = precision(this->data[2], _precision);
-      }
+    /// \brief Get the y value.
+    /// \return The y component of the vector
+    public: inline T Y() const
+    {
+      return this->data[1];
+    }
 
-      /// \brief Equality test
-      /// \remarks This is equivalent to the == operator
-      /// \param[in] _v the other vector
-      /// \return true if the 2 vectors have the same values, false otherwise
-      public: bool Equal(const Vector3<T> &_v) const
-      {
-        return equal<T>(this->data[0], _v[0]) &&
-               equal<T>(this->data[1], _v[1]) &&
-               equal<T>(this->data[2], _v[2]);
-      }
+    /// \brief Get the z value.
+    /// \return The z component of the vector
+    public: inline T Z() const
+    {
+      return this->data[2];
+    }
 
-      /// \brief Get the x value.
-      /// \return The x component of the vector
-      public: inline T X() const
-      {
-        return this->data[0];
-      }
+    /// \brief Get a mutable reference to the x value.
+    /// \return The x component of the vector
+    public: inline T &X()
+    {
+      return this->data[0];
+    }
 
-      /// \brief Get the y value.
-      /// \return The y component of the vector
-      public: inline T Y() const
-      {
-        return this->data[1];
-      }
+    /// \brief Get a mutable reference to the y value.
+    /// \return The y component of the vector
+    public: inline T &Y()
+    {
+      return this->data[1];
+    }
 
-      /// \brief Get the z value.
-      /// \return The z component of the vector
-      public: inline T Z() const
-      {
-        return this->data[2];
-      }
+    /// \brief Get a mutable reference to the z value.
+    /// \return The z component of the vector
+    public: inline T &Z()
+    {
+      return this->data[2];
+    }
 
-      /// \brief Get a mutable reference to the x value.
-      /// \return The x component of the vector
-      public: inline T &X()
-      {
-        return this->data[0];
-      }
+    /// \brief Set the x value.
+    /// \param[in] _v Value for the x component.
+    public: inline void X(const T &_v)
+    {
+      this->data[0] = _v;
+    }
 
-      /// \brief Get a mutable reference to the y value.
-      /// \return The y component of the vector
-      public: inline T &Y()
-      {
-        return this->data[1];
-      }
+    /// \brief Set the y value.
+    /// \param[in] _v Value for the y component.
+    public: inline void Y(const T &_v)
+    {
+      this->data[1] = _v;
+    }
 
-      /// \brief Get a mutable reference to the z value.
-      /// \return The z component of the vector
-      public: inline T &Z()
-      {
-        return this->data[2];
-      }
+    /// \brief Set the z value.
+    /// \param[in] _v Value for the z component.
+    public: inline void Z(const T &_v)
+    {
+      this->data[2] = _v;
+    }
 
-      /// \brief Set the x value.
-      /// \param[in] _v Value for the x component.
-      public: inline void X(const T &_v)
-      {
-        this->data[0] = _v;
-      }
+    /// \brief Less than operator.
+    /// \param[in] _pt Vector to compare.
+    /// \return True if this vector's X(), Y(), or Z() value is less
+    /// than the given vector's corresponding values.
+    public: bool operator<(const Vector3<T> &_pt) const
+    {
+      return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
+             this->data[2] < _pt[2];
+    }
 
-      /// \brief Set the y value.
-      /// \param[in] _v Value for the y component.
-      public: inline void Y(const T &_v)
+    /// \brief Stream insertion operator
+    /// \param _out output stream
+    /// \param _pt Vector3 to output
+    /// \return the stream
+    public: friend std::ostream &operator<<(
+                std::ostream &_out, const gz::math::Vector3<T> &_pt)
+    {
+      for (auto i : {0, 1, 2})
       {
-        this->data[1] = _v;
-      }
+        if (i > 0)
+          _out << " ";
 
-      /// \brief Set the z value.
-      /// \param[in] _v Value for the z component.
-      public: inline void Z(const T &_v)
-      {
-        this->data[2] = _v;
+        appendToStream(_out, _pt[i]);
       }
 
-      /// \brief Less than operator.
-      /// \param[in] _pt Vector to compare.
-      /// \return True if this vector's X(), Y(), or Z() value is less
-      /// than the given vector's corresponding values.
-      public: bool operator<(const Vector3<T> &_pt) const
-      {
-        return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
-               this->data[2] < _pt[2];
-      }
+      return _out;
+    }
 
-      /// \brief Stream insertion operator
-      /// \param _out output stream
-      /// \param _pt Vector3 to output
-      /// \return the stream
-      public: friend std::ostream &operator<<(
-                  std::ostream &_out, const gz::math::Vector3<T> &_pt)
+    /// \brief Stream extraction operator
+    /// \param _in input stream
+    /// \param _pt vector3 to read values into
+    /// \return the stream
+    public: friend std::istream &operator>>(
+                std::istream &_in, gz::math::Vector3<T> &_pt)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      T x, y, z;
+      _in >> x >> y >> z;
+      if (!_in.fail())
       {
-        for (auto i : {0, 1, 2})
-        {
-          if (i > 0)
-            _out << " ";
-
-          appendToStream(_out, _pt[i]);
-        }
-
-        return _out;
+        _pt.Set(x, y, z);
       }
+      return _in;
+    }
 
-      /// \brief Stream extraction operator
-      /// \param _in input stream
-      /// \param _pt vector3 to read values into
-      /// \return the stream
-      public: friend std::istream &operator>>(
-                  std::istream &_in, gz::math::Vector3<T> &_pt)
-      {
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        T x, y, z;
-        _in >> x >> y >> z;
-        if (!_in.fail())
-        {
-          _pt.Set(x, y, z);
-        }
-        return _in;
-      }
+    /// \brief The x, y, and z values
+    private: T data[3];
+  };
 
-      /// \brief The x, y, and z values
-      private: T data[3];
-    };
-
-    namespace detail {
-
-      template<typename T>
-      constexpr Vector3<T> gVector3Zero(0, 0, 0);
-      template<typename T>
-      constexpr Vector3<T> gVector3One(1, 1, 1);
-      template<typename T>
-      constexpr Vector3<T> gVector3UnitX(1, 0, 0);
-      template<typename T>
-      constexpr Vector3<T> gVector3UnitY(0, 1, 0);
-      template<typename T>
-      constexpr Vector3<T> gVector3UnitZ(0, 0, 1);
-      template<typename T>
-      constexpr Vector3<T> gVector3NaN(
-          std::numeric_limits<T>::quiet_NaN(),
-          std::numeric_limits<T>::quiet_NaN(),
-          std::numeric_limits<T>::quiet_NaN());
-    }  // namespace detail
+  namespace detail {
 
     template<typename T>
-    const Vector3<T> &Vector3<T>::Zero = detail::gVector3Zero<T>;
+    constexpr Vector3<T> gVector3Zero(0, 0, 0);
     template<typename T>
-    const Vector3<T> &Vector3<T>::One = detail::gVector3One<T>;
+    constexpr Vector3<T> gVector3One(1, 1, 1);
     template<typename T>
-    const Vector3<T> &Vector3<T>::UnitX = detail::gVector3UnitX<T>;
+    constexpr Vector3<T> gVector3UnitX(1, 0, 0);
     template<typename T>
-    const Vector3<T> &Vector3<T>::UnitY = detail::gVector3UnitY<T>;
+    constexpr Vector3<T> gVector3UnitY(0, 1, 0);
     template<typename T>
-    const Vector3<T> &Vector3<T>::UnitZ = detail::gVector3UnitZ<T>;
+    constexpr Vector3<T> gVector3UnitZ(0, 0, 1);
     template<typename T>
-    const Vector3<T> &Vector3<T>::NaN = detail::gVector3NaN<T>;
-
-    typedef Vector3<int> Vector3i;
-    typedef Vector3<double> Vector3d;
-    typedef Vector3<float> Vector3f;
-    }
-  }
-}
-#endif
+    constexpr Vector3<T> gVector3NaN(
+        std::numeric_limits<T>::quiet_NaN(),
+        std::numeric_limits<T>::quiet_NaN(),
+        std::numeric_limits<T>::quiet_NaN());
+  }  // namespace detail
+
+  template<typename T>
+  const Vector3<T> &Vector3<T>::Zero = detail::gVector3Zero<T>;
+  template<typename T>
+  const Vector3<T> &Vector3<T>::One = detail::gVector3One<T>;
+  template<typename T>
+  const Vector3<T> &Vector3<T>::UnitX = detail::gVector3UnitX<T>;
+  template<typename T>
+  const Vector3<T> &Vector3<T>::UnitY = detail::gVector3UnitY<T>;
+  template<typename T>
+  const Vector3<T> &Vector3<T>::UnitZ = detail::gVector3UnitZ<T>;
+  template<typename T>
+  const Vector3<T> &Vector3<T>::NaN = detail::gVector3NaN<T>;
+
+  typedef Vector3<int> Vector3i;
+  typedef Vector3<double> Vector3d;
+  typedef Vector3<float> Vector3f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_VECTOR3_HH_
diff --git a/include/gz/math/Vector3Stats.hh b/include/gz/math/Vector3Stats.hh
index a9b17dbc9..bde34b172 100644
--- a/include/gz/math/Vector3Stats.hh
+++ b/include/gz/math/Vector3Stats.hh
@@ -24,83 +24,79 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  /// \class Vector3Stats Vector3Stats.hh gz/math/Vector3Stats.hh
+  /// \brief Collection of statistics for a Vector3 signal.
+  class GZ_MATH_VISIBLE Vector3Stats
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    /// \class Vector3Stats Vector3Stats.hh gz/math/Vector3Stats.hh
-    /// \brief Collection of statistics for a Vector3 signal.
-    class GZ_MATH_VISIBLE Vector3Stats
-    {
-      /// \brief Constructor
-      public: Vector3Stats();
-
-      /// \brief Add a new sample to the statistical measures.
-      /// \param[in] _data New signal data point.
-      public: void InsertData(const Vector3d &_data);
-
-      /// \brief Add a new type of statistic.
-      /// \param[in] _name Short name of new statistic.
-      /// Valid values include:
-      ///  "maxAbs"
-      ///  "mean"
-      ///  "rms"
-      /// \return True if statistic was successfully added,
-      /// false if name was not recognized or had already
-      /// been inserted.
-      public: bool InsertStatistic(const std::string &_name);
-
-      /// \brief Add multiple statistics.
-      /// \param[in] _names Comma-separated list of new statistics.
-      /// For example, all statistics could be added with:
-      ///  "maxAbs,mean,rms"
-      /// \return True if all statistics were successfully added,
-      /// false if any names were not recognized or had already
-      /// been inserted.
-      public: bool InsertStatistics(const std::string &_names);
-
-      /// \brief Forget all previous data.
-      public: void Reset();
-
-      /// \brief Get statistics for x component of signal.
-      /// \return Statistics for x component of signal.
-      public: const SignalStats &X() const;
-
-      /// \brief Get statistics for y component of signal.
-      /// \return Statistics for y component of signal.
-      public: const SignalStats &Y() const;
-
-      /// \brief Get statistics for z component of signal.
-      /// \return Statistics for z component of signal.
-      public: const SignalStats &Z() const;
-
-      /// \brief Get statistics for magnitude component of signal.
-      /// \return Statistics for magnitude component of signal.
-      public: const SignalStats &Mag() const;
-
-      /// \brief Get mutable reference to statistics for x component of signal.
-      /// \return Statistics for x component of signal.
-      public: SignalStats &X();
-
-      /// \brief Get mutable reference to statistics for y component of signal.
-      /// \return Statistics for y component of signal.
-      public: SignalStats &Y();
-
-      /// \brief Get mutable reference to statistics for z component of signal.
-      /// \return Statistics for z component of signal.
-      public: SignalStats &Z();
-
-      /// \brief Get mutable reference to statistics for magnitude of signal.
-      /// \return Statistics for magnitude of signal.
-      public: SignalStats &Mag();
-
-      /// \brief Pointer to private data.
-      GZ_UTILS_IMPL_PTR(dataPtr)
-    };
-    }
-  }
-}
-#endif
-
+    /// \brief Constructor
+    public: Vector3Stats();
+
+    /// \brief Add a new sample to the statistical measures.
+    /// \param[in] _data New signal data point.
+    public: void InsertData(const Vector3d &_data);
+
+    /// \brief Add a new type of statistic.
+    /// \param[in] _name Short name of new statistic.
+    /// Valid values include:
+    ///  "maxAbs"
+    ///  "mean"
+    ///  "rms"
+    /// \return True if statistic was successfully added,
+    /// false if name was not recognized or had already
+    /// been inserted.
+    public: bool InsertStatistic(const std::string &_name);
+
+    /// \brief Add multiple statistics.
+    /// \param[in] _names Comma-separated list of new statistics.
+    /// For example, all statistics could be added with:
+    ///  "maxAbs,mean,rms"
+    /// \return True if all statistics were successfully added,
+    /// false if any names were not recognized or had already
+    /// been inserted.
+    public: bool InsertStatistics(const std::string &_names);
+
+    /// \brief Forget all previous data.
+    public: void Reset();
+
+    /// \brief Get statistics for x component of signal.
+    /// \return Statistics for x component of signal.
+    public: const SignalStats &X() const;
+
+    /// \brief Get statistics for y component of signal.
+    /// \return Statistics for y component of signal.
+    public: const SignalStats &Y() const;
+
+    /// \brief Get statistics for z component of signal.
+    /// \return Statistics for z component of signal.
+    public: const SignalStats &Z() const;
+
+    /// \brief Get statistics for magnitude component of signal.
+    /// \return Statistics for magnitude component of signal.
+    public: const SignalStats &Mag() const;
+
+    /// \brief Get mutable reference to statistics for x component of signal.
+    /// \return Statistics for x component of signal.
+    public: SignalStats &X();
+
+    /// \brief Get mutable reference to statistics for y component of signal.
+    /// \return Statistics for y component of signal.
+    public: SignalStats &Y();
+
+    /// \brief Get mutable reference to statistics for z component of signal.
+    /// \return Statistics for z component of signal.
+    public: SignalStats &Z();
+
+    /// \brief Get mutable reference to statistics for magnitude of signal.
+    /// \return Statistics for magnitude of signal.
+    public: SignalStats &Mag();
+
+    /// \brief Pointer to private data.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }   // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_VECTOR3STATS_HH_
diff --git a/include/gz/math/Vector4.hh b/include/gz/math/Vector4.hh
index 4770c2056..c27a370e6 100644
--- a/include/gz/math/Vector4.hh
+++ b/include/gz/math/Vector4.hh
@@ -25,735 +25,732 @@
 #include <gz/math/Helpers.hh>
 #include <gz/math/config.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+  //
+  /// \class Vector4 Vector4.hh gz/math/Vector4.hh
+  /// \brief T Generic x, y, z, w vector
+  template<typename T>
+  class Vector4
   {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-    /// \class Vector4 Vector4.hh gz/math/Vector4.hh
-    /// \brief T Generic x, y, z, w vector
-    template<typename T>
-    class Vector4
+    /// \brief math::Vector4(0, 0, 0, 0)
+    public: static const Vector4<T> &Zero;
+
+    /// \brief math::Vector4(1, 1, 1, 1)
+    public: static const Vector4<T> &One;
+
+    /// \brief math::Vector4(NaN, NaN, NaN, NaN)
+    public: static const Vector4 &NaN;
+
+    /// \brief Constructor
+    public: constexpr Vector4()
+    : data{0, 0, 0, 0}
     {
-      /// \brief math::Vector4(0, 0, 0, 0)
-      public: static const Vector4<T> &Zero;
+    }
 
-      /// \brief math::Vector4(1, 1, 1, 1)
-      public: static const Vector4<T> &One;
+    /// \brief Constructor with component values
+    /// \param[in] _x value along x axis
+    /// \param[in] _y value along y axis
+    /// \param[in] _z value along z axis
+    /// \param[in] _w value along w axis
+    public: constexpr Vector4(const T &_x, const T &_y, const T &_z,
+                              const T &_w)
+    : data{_x, _y, _z, _w}
+    {
+    }
 
-      /// \brief math::Vector4(NaN, NaN, NaN, NaN)
-      public: static const Vector4 &NaN;
+    /// \brief Copy constructor
+    /// \param[in] _v vector
+    public: Vector4(const Vector4<T> &_v) = default;
 
-      /// \brief Constructor
-      public: constexpr Vector4()
-      : data{0, 0, 0, 0}
-      {
-      }
+    /// \brief Destructor
+    public: ~Vector4() = default;
 
-      /// \brief Constructor with component values
-      /// \param[in] _x value along x axis
-      /// \param[in] _y value along y axis
-      /// \param[in] _z value along z axis
-      /// \param[in] _w value along w axis
-      public: constexpr Vector4(const T &_x, const T &_y, const T &_z,
-                                const T &_w)
-      : data{_x, _y, _z, _w}
-      {
-      }
+    /// \brief Calc distance to the given point
+    /// \param[in] _pt the point
+    /// \return the distance
+    public: T Distance(const Vector4<T> &_pt) const
+    {
+      return static_cast<T>(sqrt(
+                  (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
+                  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
+                  (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
+                  (this->data[3]-_pt[3])*(this->data[3]-_pt[3])));
+    }
 
-      /// \brief Copy constructor
-      /// \param[in] _v vector
-      public: Vector4(const Vector4<T> &_v) = default;
+    /// \brief Calc distance to the given point
+    /// \param[in] _x value along x
+    /// \param[in] _y value along y
+    /// \param[in] _z value along z
+    /// \param[in] _w value along w
+    /// \return the distance
+    public: T Distance(T _x, T _y, T _z, T _w) const
+    {
+      return this->Distance(Vector4(_x, _y, _z, _w));
+    }
 
-      /// \brief Destructor
-      public: ~Vector4() = default;
+    /// \brief Returns the length (magnitude) of the vector
+    /// \return The length
+    public: T Length() const
+    {
+      return static_cast<T>(sqrt(this->SquaredLength()));
+    }
 
-      /// \brief Calc distance to the given point
-      /// \param[in] _pt the point
-      /// \return the distance
-      public: T Distance(const Vector4<T> &_pt) const
-      {
-        return static_cast<T>(sqrt(
-                    (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
-                    (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
-                    (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
-                    (this->data[3]-_pt[3])*(this->data[3]-_pt[3])));
-      }
+    /// \brief Return the square of the length (magnitude) of the vector
+    /// \return the length
+    public: T SquaredLength() const
+    {
+      return
+        this->data[0] * this->data[0] +
+        this->data[1] * this->data[1] +
+        this->data[2] * this->data[2] +
+        this->data[3] * this->data[3];
+    }
 
-      /// \brief Calc distance to the given point
-      /// \param[in] _x value along x
-      /// \param[in] _y value along y
-      /// \param[in] _z value along z
-      /// \param[in] _w value along w
-      /// \return the distance
-      public: T Distance(T _x, T _y, T _z, T _w) const
-      {
-        return this->Distance(Vector4(_x, _y, _z, _w));
-      }
+    /// \brief Round to near whole number.
+    public: void Round()
+    {
+      this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
+      this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
+      this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
+      this->data[3] = static_cast<T>(std::nearbyint(this->data[3]));
+    }
 
-      /// \brief Returns the length (magnitude) of the vector
-      /// \return The length
-      public: T Length() const
-      {
-        return static_cast<T>(sqrt(this->SquaredLength()));
-      }
+    /// \brief Get a rounded version of this vector
+    /// \return a rounded vector
+    public: Vector4 Rounded() const
+    {
+      Vector4<T> result = *this;
+      result.Round();
+      return result;
+    }
 
-      /// \brief Return the square of the length (magnitude) of the vector
-      /// \return the length
-      public: T SquaredLength() const
-      {
-        return
-          this->data[0] * this->data[0] +
-          this->data[1] * this->data[1] +
-          this->data[2] * this->data[2] +
-          this->data[3] * this->data[3];
-      }
+    /// \brief Corrects any nan values
+    public: inline void Correct()
+    {
+      // std::isfinite works with floating point values,
+      // need to explicit cast to avoid ambiguity in vc++.
+      if (!std::isfinite(static_cast<double>(this->data[0])))
+        this->data[0] = 0;
+      if (!std::isfinite(static_cast<double>(this->data[1])))
+        this->data[1] = 0;
+      if (!std::isfinite(static_cast<double>(this->data[2])))
+        this->data[2] = 0;
+      if (!std::isfinite(static_cast<double>(this->data[3])))
+        this->data[3] = 0;
+    }
 
-      /// \brief Round to near whole number.
-      public: void Round()
-      {
-        this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
-        this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
-        this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
-        this->data[3] = static_cast<T>(std::nearbyint(this->data[3]));
-      }
+    /// \brief Normalize the vector length
+    public: void Normalize()
+    {
+      T d = this->Length();
 
-      /// \brief Get a rounded version of this vector
-      /// \return a rounded vector
-      public: Vector4 Rounded() const
+      if (!equal<T>(d, static_cast<T>(0.0)))
       {
-        Vector4<T> result = *this;
-        result.Round();
-        return result;
+        this->data[0] /= d;
+        this->data[1] /= d;
+        this->data[2] /= d;
+        this->data[3] /= d;
       }
+    }
 
-      /// \brief Corrects any nan values
-      public: inline void Correct()
-      {
-        // std::isfinite works with floating point values,
-        // need to explicit cast to avoid ambiguity in vc++.
-        if (!std::isfinite(static_cast<double>(this->data[0])))
-          this->data[0] = 0;
-        if (!std::isfinite(static_cast<double>(this->data[1])))
-          this->data[1] = 0;
-        if (!std::isfinite(static_cast<double>(this->data[2])))
-          this->data[2] = 0;
-        if (!std::isfinite(static_cast<double>(this->data[3])))
-          this->data[3] = 0;
-      }
+    /// \brief Return a normalized vector
+    /// \return unit length vector
+    public: Vector4 Normalized() const
+    {
+      Vector4<T> result = *this;
+      result.Normalize();
+      return result;
+    }
 
-      /// \brief Normalize the vector length
-      public: void Normalize()
-      {
-        T d = this->Length();
-
-        if (!equal<T>(d, static_cast<T>(0.0)))
-        {
-          this->data[0] /= d;
-          this->data[1] /= d;
-          this->data[2] /= d;
-          this->data[3] /= d;
-        }
-      }
+    /// \brief Return the dot product of this vector and another vector
+    /// \param[in] _v the vector
+    /// \return the dot product
+    public: T Dot(const Vector4<T> &_v) const
+    {
+      return this->data[0] * _v[0] +
+             this->data[1] * _v[1] +
+             this->data[2] * _v[2] +
+             this->data[3] * _v[3];
+    }
 
-      /// \brief Return a normalized vector
-      /// \return unit length vector
-      public: Vector4 Normalized() const
-      {
-        Vector4<T> result = *this;
-        result.Normalize();
-        return result;
-      }
+    /// \brief Return the absolute dot product of this vector and
+    /// another vector. This is similar to the Dot function, except the
+    /// absolute value of each component of the vector is used.
+    ///
+    /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 * z2) + abs(w1 * w2)
+    ///
+    /// \param[in] _v the vector
+    /// \return The absolute dot product
+    public: T AbsDot(const Vector4<T> &_v) const
+    {
+      return std::abs(this->data[0] * _v[0]) +
+             std::abs(this->data[1] * _v[1]) +
+             std::abs(this->data[2] * _v[2]) +
+             std::abs(this->data[3] * _v[3]);
+    }
 
-      /// \brief Return the dot product of this vector and another vector
-      /// \param[in] _v the vector
-      /// \return the dot product
-      public: T Dot(const Vector4<T> &_v) const
-      {
-        return this->data[0] * _v[0] +
-               this->data[1] * _v[1] +
-               this->data[2] * _v[2] +
-               this->data[3] * _v[3];
-      }
+    /// \brief Get the absolute value of the vector
+    /// \return a vector with positive elements
+    public: Vector4 Abs() const
+    {
+      return Vector4(std::abs(this->data[0]),
+                     std::abs(this->data[1]),
+                     std::abs(this->data[2]),
+                     std::abs(this->data[3]));
+    }
 
-      /// \brief Return the absolute dot product of this vector and
-      /// another vector. This is similar to the Dot function, except the
-      /// absolute value of each component of the vector is used.
-      ///
-      /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 * z2) + abs(w1 * w2)
-      ///
-      /// \param[in] _v the vector
-      /// \return The absolute dot product
-      public: T AbsDot(const Vector4<T> &_v) const
-      {
-        return std::abs(this->data[0] * _v[0]) +
-               std::abs(this->data[1] * _v[1]) +
-               std::abs(this->data[2] * _v[2]) +
-               std::abs(this->data[3] * _v[3]);
-      }
+    /// \brief Set the contents of the vector
+    /// \param[in] _x value along x axis
+    /// \param[in] _y value along y axis
+    /// \param[in] _z value along z axis
+    /// \param[in] _w value along w axis
+    public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
+    {
+      this->data[0] = _x;
+      this->data[1] = _y;
+      this->data[2] = _z;
+      this->data[3] = _w;
+    }
 
-      /// \brief Get the absolute value of the vector
-      /// \return a vector with positive elements
-      public: Vector4 Abs() const
-      {
-        return Vector4(std::abs(this->data[0]),
-                       std::abs(this->data[1]),
-                       std::abs(this->data[2]),
-                       std::abs(this->data[3]));
-      }
+    /// \brief Set this vector's components to the maximum of itself and the
+    ///        passed in vector
+    /// \param[in] _v the maximum clamping vector
+    public: void Max(const Vector4<T> &_v)
+    {
+      this->data[0] = std::max(_v[0], this->data[0]);
+      this->data[1] = std::max(_v[1], this->data[1]);
+      this->data[2] = std::max(_v[2], this->data[2]);
+      this->data[3] = std::max(_v[3], this->data[3]);
+    }
 
-      /// \brief Set the contents of the vector
-      /// \param[in] _x value along x axis
-      /// \param[in] _y value along y axis
-      /// \param[in] _z value along z axis
-      /// \param[in] _w value along w axis
-      public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
-      {
-        this->data[0] = _x;
-        this->data[1] = _y;
-        this->data[2] = _z;
-        this->data[3] = _w;
-      }
+    /// \brief Set this vector's components to the minimum of itself and the
+    ///        passed in vector
+    /// \param[in] _v the minimum clamping vector
+    public: void Min(const Vector4<T> &_v)
+    {
+      this->data[0] = std::min(_v[0], this->data[0]);
+      this->data[1] = std::min(_v[1], this->data[1]);
+      this->data[2] = std::min(_v[2], this->data[2]);
+      this->data[3] = std::min(_v[3], this->data[3]);
+    }
 
-      /// \brief Set this vector's components to the maximum of itself and the
-      ///        passed in vector
-      /// \param[in] _v the maximum clamping vector
-      public: void Max(const Vector4<T> &_v)
-      {
-        this->data[0] = std::max(_v[0], this->data[0]);
-        this->data[1] = std::max(_v[1], this->data[1]);
-        this->data[2] = std::max(_v[2], this->data[2]);
-        this->data[3] = std::max(_v[3], this->data[3]);
-      }
+    /// \brief Get the maximum value in the vector
+    /// \return the maximum element
+    public: T Max() const
+    {
+      return *std::max_element(this->data, this->data+4);
+    }
 
-      /// \brief Set this vector's components to the minimum of itself and the
-      ///        passed in vector
-      /// \param[in] _v the minimum clamping vector
-      public: void Min(const Vector4<T> &_v)
-      {
-        this->data[0] = std::min(_v[0], this->data[0]);
-        this->data[1] = std::min(_v[1], this->data[1]);
-        this->data[2] = std::min(_v[2], this->data[2]);
-        this->data[3] = std::min(_v[3], this->data[3]);
-      }
+    /// \brief Get the minimum value in the vector
+    /// \return the minimum element
+    public: T Min() const
+    {
+      return *std::min_element(this->data, this->data+4);
+    }
 
-      /// \brief Get the maximum value in the vector
-      /// \return the maximum element
-      public: T Max() const
-      {
-        return *std::max_element(this->data, this->data+4);
-      }
+    /// \brief Return the sum of the values
+    /// \return the sum
+    public: T Sum() const
+    {
+      return this->data[0] + this->data[1] + this->data[2] + this->data[3];
+    }
 
-      /// \brief Get the minimum value in the vector
-      /// \return the minimum element
-      public: T Min() const
-      {
-        return *std::min_element(this->data, this->data+4);
-      }
+    /// \brief Assignment operator
+    /// \param[in] _v the vector
+    /// \return a reference to this vector
+    public: Vector4<T> &operator=(const Vector4<T> &_v) = default;
 
-      /// \brief Return the sum of the values
-      /// \return the sum
-      public: T Sum() const
-      {
-        return this->data[0] + this->data[1] + this->data[2] + this->data[3];
-      }
+    /// \brief Assignment operator
+    /// \param[in] _value
+    public: Vector4<T> &operator=(T _value)
+    {
+      this->data[0] = _value;
+      this->data[1] = _value;
+      this->data[2] = _value;
+      this->data[3] = _value;
 
-      /// \brief Assignment operator
-      /// \param[in] _v the vector
-      /// \return a reference to this vector
-      public: Vector4<T> &operator=(const Vector4<T> &_v) = default;
+      return *this;
+    }
 
-      /// \brief Assignment operator
-      /// \param[in] _value
-      public: Vector4<T> &operator=(T _value)
-      {
-        this->data[0] = _value;
-        this->data[1] = _value;
-        this->data[2] = _value;
-        this->data[3] = _value;
+    /// \brief Addition operator
+    /// \param[in] _v the vector to add
+    /// \result a sum vector
+    public: Vector4<T> operator+(const Vector4<T> &_v) const
+    {
+      return Vector4<T>(this->data[0] + _v[0],
+                        this->data[1] + _v[1],
+                        this->data[2] + _v[2],
+                        this->data[3] + _v[3]);
+    }
 
-        return *this;
-      }
+    /// \brief Addition operator
+    /// \param[in] _v the vector to add
+    /// \return this vector
+    public: const Vector4<T> &operator+=(const Vector4<T> &_v)
+    {
+      this->data[0] += _v[0];
+      this->data[1] += _v[1];
+      this->data[2] += _v[2];
+      this->data[3] += _v[3];
 
-      /// \brief Addition operator
-      /// \param[in] _v the vector to add
-      /// \result a sum vector
-      public: Vector4<T> operator+(const Vector4<T> &_v) const
-      {
-        return Vector4<T>(this->data[0] + _v[0],
-                          this->data[1] + _v[1],
-                          this->data[2] + _v[2],
-                          this->data[3] + _v[3]);
-      }
+      return *this;
+    }
 
-      /// \brief Addition operator
-      /// \param[in] _v the vector to add
-      /// \return this vector
-      public: const Vector4<T> &operator+=(const Vector4<T> &_v)
-      {
-        this->data[0] += _v[0];
-        this->data[1] += _v[1];
-        this->data[2] += _v[2];
-        this->data[3] += _v[3];
+    /// \brief Addition operators
+    /// \param[in] _s the scalar addend
+    /// \return sum vector
+    public: inline Vector4<T> operator+(const T _s) const
+    {
+      return Vector4<T>(this->data[0] + _s,
+                        this->data[1] + _s,
+                        this->data[2] + _s,
+                        this->data[3] + _s);
+    }
 
-        return *this;
-      }
+    /// \brief Addition operators
+    /// \param[in] _s the scalar addend
+    /// \param[in] _v input vector
+    /// \return sum vector
+    public: friend inline Vector4<T> operator+(const T _s,
+                                               const Vector4<T> &_v)
+    {
+      return _v + _s;
+    }
 
-      /// \brief Addition operators
-      /// \param[in] _s the scalar addend
-      /// \return sum vector
-      public: inline Vector4<T> operator+(const T _s) const
-      {
-        return Vector4<T>(this->data[0] + _s,
-                          this->data[1] + _s,
-                          this->data[2] + _s,
-                          this->data[3] + _s);
-      }
+    /// \brief Addition assignment operator
+    /// \param[in] _s scalar addend
+    /// \return this
+    public: const Vector4<T> &operator+=(const T _s)
+    {
+      this->data[0] += _s;
+      this->data[1] += _s;
+      this->data[2] += _s;
+      this->data[3] += _s;
 
-      /// \brief Addition operators
-      /// \param[in] _s the scalar addend
-      /// \param[in] _v input vector
-      /// \return sum vector
-      public: friend inline Vector4<T> operator+(const T _s,
-                                                 const Vector4<T> &_v)
-      {
-        return _v + _s;
-      }
+      return *this;
+    }
 
-      /// \brief Addition assignment operator
-      /// \param[in] _s scalar addend
-      /// \return this
-      public: const Vector4<T> &operator+=(const T _s)
-      {
-        this->data[0] += _s;
-        this->data[1] += _s;
-        this->data[2] += _s;
-        this->data[3] += _s;
+    /// \brief Negation operator
+    /// \return negative of this vector
+    public: inline Vector4 operator-() const
+    {
+      return Vector4(-this->data[0], -this->data[1],
+                     -this->data[2], -this->data[3]);
+    }
 
-        return *this;
-      }
+    /// \brief Subtraction operator
+    /// \param[in] _v the vector to substract
+    /// \return a vector
+    public: Vector4<T> operator-(const Vector4<T> &_v) const
+    {
+      return Vector4<T>(this->data[0] - _v[0],
+                        this->data[1] - _v[1],
+                        this->data[2] - _v[2],
+                        this->data[3] - _v[3]);
+    }
 
-      /// \brief Negation operator
-      /// \return negative of this vector
-      public: inline Vector4 operator-() const
-      {
-        return Vector4(-this->data[0], -this->data[1],
-                       -this->data[2], -this->data[3]);
-      }
+    /// \brief Subtraction assigment operators
+    /// \param[in] _v the vector to substract
+    /// \return this vector
+    public: const Vector4<T> &operator-=(const Vector4<T> &_v)
+    {
+      this->data[0] -= _v[0];
+      this->data[1] -= _v[1];
+      this->data[2] -= _v[2];
+      this->data[3] -= _v[3];
 
-      /// \brief Subtraction operator
-      /// \param[in] _v the vector to substract
-      /// \return a vector
-      public: Vector4<T> operator-(const Vector4<T> &_v) const
-      {
-        return Vector4<T>(this->data[0] - _v[0],
-                          this->data[1] - _v[1],
-                          this->data[2] - _v[2],
-                          this->data[3] - _v[3]);
-      }
+      return *this;
+    }
 
-      /// \brief Subtraction assigment operators
-      /// \param[in] _v the vector to substract
-      /// \return this vector
-      public: const Vector4<T> &operator-=(const Vector4<T> &_v)
-      {
-        this->data[0] -= _v[0];
-        this->data[1] -= _v[1];
-        this->data[2] -= _v[2];
-        this->data[3] -= _v[3];
+    /// \brief Subtraction operators
+    /// \param[in] _s the scalar subtrahend
+    /// \return difference vector
+    public: inline Vector4<T> operator-(const T _s) const
+    {
+      return Vector4<T>(this->data[0] - _s,
+                        this->data[1] - _s,
+                        this->data[2] - _s,
+                        this->data[3] - _s);
+    }
 
-        return *this;
-      }
+    /// \brief Subtraction operators
+    /// \param[in] _s the scalar minuend
+    /// \param[in] _v vector subtrahend
+    /// \return difference vector
+    public: friend inline Vector4<T> operator-(const T _s,
+                                               const Vector4<T> &_v)
+    {
+      return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
+    }
 
-      /// \brief Subtraction operators
-      /// \param[in] _s the scalar subtrahend
-      /// \return difference vector
-      public: inline Vector4<T> operator-(const T _s) const
-      {
-        return Vector4<T>(this->data[0] - _s,
-                          this->data[1] - _s,
-                          this->data[2] - _s,
-                          this->data[3] - _s);
-      }
+    /// \brief Subtraction assignment operator
+    /// \param[in] _s scalar subtrahend
+    /// \return this
+    public: const Vector4<T> &operator-=(const T _s)
+    {
+      this->data[0] -= _s;
+      this->data[1] -= _s;
+      this->data[2] -= _s;
+      this->data[3] -= _s;
 
-      /// \brief Subtraction operators
-      /// \param[in] _s the scalar minuend
-      /// \param[in] _v vector subtrahend
-      /// \return difference vector
-      public: friend inline Vector4<T> operator-(const T _s,
-                                                 const Vector4<T> &_v)
-      {
-        return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
-      }
+      return *this;
+    }
 
-      /// \brief Subtraction assignment operator
-      /// \param[in] _s scalar subtrahend
-      /// \return this
-      public: const Vector4<T> &operator-=(const T _s)
-      {
-        this->data[0] -= _s;
-        this->data[1] -= _s;
-        this->data[2] -= _s;
-        this->data[3] -= _s;
+    /// \brief Division assignment operator
+    /// \remarks Performs element wise division,
+    /// which has limited use.
+    /// \param[in] _v the vector to perform element wise division with
+    /// \return a result vector
+    public: const Vector4<T> operator/(const Vector4<T> &_v) const
+    {
+      return Vector4<T>(this->data[0] / _v[0],
+                        this->data[1] / _v[1],
+                        this->data[2] / _v[2],
+                        this->data[3] / _v[3]);
+    }
 
-        return *this;
-      }
+    /// \brief Division assignment operator
+    /// \remarks Performs element wise division,
+    /// which has limited use.
+    /// \param[in] _v the vector to perform element wise division with
+    /// \return this
+    public: const Vector4<T> &operator/=(const Vector4<T> &_v)
+    {
+      this->data[0] /= _v[0];
+      this->data[1] /= _v[1];
+      this->data[2] /= _v[2];
+      this->data[3] /= _v[3];
 
-      /// \brief Division assignment operator
-      /// \remarks Performs element wise division,
-      /// which has limited use.
-      /// \param[in] _v the vector to perform element wise division with
-      /// \return a result vector
-      public: const Vector4<T> operator/(const Vector4<T> &_v) const
-      {
-        return Vector4<T>(this->data[0] / _v[0],
-                          this->data[1] / _v[1],
-                          this->data[2] / _v[2],
-                          this->data[3] / _v[3]);
-      }
+      return *this;
+    }
 
-      /// \brief Division assignment operator
-      /// \remarks Performs element wise division,
-      /// which has limited use.
-      /// \param[in] _v the vector to perform element wise division with
-      /// \return this
-      public: const Vector4<T> &operator/=(const Vector4<T> &_v)
-      {
-        this->data[0] /= _v[0];
-        this->data[1] /= _v[1];
-        this->data[2] /= _v[2];
-        this->data[3] /= _v[3];
+    /// \brief Division assignment operator
+    /// \remarks Performs element wise division,
+    /// which has limited use.
+    /// \param[in] _v another vector
+    /// \return a result vector
+    public: const Vector4<T> operator/(T _v) const
+    {
+      return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
+          this->data[2] / _v, this->data[3] / _v);
+    }
 
-        return *this;
-      }
+    /// \brief Division operator
+    /// \param[in] _v scaling factor
+    /// \return a vector
+    public: const Vector4<T> &operator/=(T _v)
+    {
+      this->data[0] /= _v;
+      this->data[1] /= _v;
+      this->data[2] /= _v;
+      this->data[3] /= _v;
 
-      /// \brief Division assignment operator
-      /// \remarks Performs element wise division,
-      /// which has limited use.
-      /// \param[in] _v another vector
-      /// \return a result vector
-      public: const Vector4<T> operator/(T _v) const
-      {
-        return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
-            this->data[2] / _v, this->data[3] / _v);
-      }
+      return *this;
+    }
 
-      /// \brief Division operator
-      /// \param[in] _v scaling factor
-      /// \return a vector
-      public: const Vector4<T> &operator/=(T _v)
-      {
-        this->data[0] /= _v;
-        this->data[1] /= _v;
-        this->data[2] /= _v;
-        this->data[3] /= _v;
+    /// \brief Multiplication operator.
+    /// \remarks Performs element wise multiplication,
+    /// which has limited use.
+    /// \param[in] _pt another vector
+    /// \return result vector
+    public: const Vector4<T> operator*(const Vector4<T> &_pt) const
+    {
+      return Vector4<T>(this->data[0] * _pt[0],
+                        this->data[1] * _pt[1],
+                        this->data[2] * _pt[2],
+                        this->data[3] * _pt[3]);
+    }
 
-        return *this;
-      }
+    /// \brief Matrix multiplication operator.
+    /// \param[in] _m matrix
+    /// \return the vector multiplied by _m
+    public: const Vector4<T> operator*(const Matrix4<T> &_m) const
+    {
+      return Vector4<T>(
+          this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
+          this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
+          this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
+          this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
+          this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
+          this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
+          this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
+          this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
+    }
 
-      /// \brief Multiplication operator.
-      /// \remarks Performs element wise multiplication,
-      /// which has limited use.
-      /// \param[in] _pt another vector
-      /// \return result vector
-      public: const Vector4<T> operator*(const Vector4<T> &_pt) const
-      {
-        return Vector4<T>(this->data[0] * _pt[0],
-                          this->data[1] * _pt[1],
-                          this->data[2] * _pt[2],
-                          this->data[3] * _pt[3]);
-      }
+    /// \brief Multiplication assignment operator
+    /// \remarks Performs element wise multiplication,
+    /// which has limited use.
+    /// \param[in] _pt a vector
+    /// \return this
+    public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
+    {
+      this->data[0] *= _pt[0];
+      this->data[1] *= _pt[1];
+      this->data[2] *= _pt[2];
+      this->data[3] *= _pt[3];
 
-      /// \brief Matrix multiplication operator.
-      /// \param[in] _m matrix
-      /// \return the vector multiplied by _m
-      public: const Vector4<T> operator*(const Matrix4<T> &_m) const
-      {
-        return Vector4<T>(
-            this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
-            this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
-            this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
-            this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
-            this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
-            this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
-            this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
-            this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
-      }
+      return *this;
+    }
 
-      /// \brief Multiplication assignment operator
-      /// \remarks Performs element wise multiplication,
-      /// which has limited use.
-      /// \param[in] _pt a vector
-      /// \return this
-      public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
-      {
-        this->data[0] *= _pt[0];
-        this->data[1] *= _pt[1];
-        this->data[2] *= _pt[2];
-        this->data[3] *= _pt[3];
+    /// \brief Multiplication operators
+    /// \param[in] _v scaling factor
+    /// \return a  scaled vector
+    public: const Vector4<T> operator*(T _v) const
+    {
+      return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
+          this->data[2] * _v, this->data[3] * _v);
+    }
 
-        return *this;
-      }
+    /// \brief Scalar left multiplication operators
+    /// \param[in] _s the scaling factor
+    /// \param[in] _v the vector to scale
+    /// \return a scaled vector
+    public: friend inline const Vector4 operator*(const T _s,
+                                                  const Vector4 &_v)
+    {
+      return Vector4(_v * _s);
+    }
 
-      /// \brief Multiplication operators
-      /// \param[in] _v scaling factor
-      /// \return a  scaled vector
-      public: const Vector4<T> operator*(T _v) const
-      {
-        return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
-            this->data[2] * _v, this->data[3] * _v);
-      }
+    /// \brief Multiplication assignment operator
+    /// \param[in] _v scaling factor
+    /// \return this
+    public: const Vector4<T> &operator*=(T _v)
+    {
+      this->data[0] *= _v;
+      this->data[1] *= _v;
+      this->data[2] *= _v;
+      this->data[3] *= _v;
 
-      /// \brief Scalar left multiplication operators
-      /// \param[in] _s the scaling factor
-      /// \param[in] _v the vector to scale
-      /// \return a scaled vector
-      public: friend inline const Vector4 operator*(const T _s,
-                                                    const Vector4 &_v)
-      {
-        return Vector4(_v * _s);
-      }
+      return *this;
+    }
 
-      /// \brief Multiplication assignment operator
-      /// \param[in] _v scaling factor
-      /// \return this
-      public: const Vector4<T> &operator*=(T _v)
-      {
-        this->data[0] *= _v;
-        this->data[1] *= _v;
-        this->data[2] *= _v;
-        this->data[3] *= _v;
+    /// \brief Equality test with tolerance.
+    /// \param[in] _v the vector to compare to
+    /// \param[in] _tol equality tolerance.
+    /// \return true if the elements of the vectors are equal within
+    /// the tolerence specified by _tol.
+    public: bool Equal(const Vector4 &_v, const T &_tol) const
+    {
+      return equal<T>(this->data[0], _v[0], _tol)
+          && equal<T>(this->data[1], _v[1], _tol)
+          && equal<T>(this->data[2], _v[2], _tol)
+          && equal<T>(this->data[3], _v[3], _tol);
+    }
 
-        return *this;
-      }
+    /// \brief Equal to operator
+    /// \param[in] _v the other vector
+    /// \return true if each component is equal within a
+    /// default tolerence (1e-6), false otherwise
+    public: bool operator==(const Vector4<T> &_v) const
+    {
+      return this->Equal(_v, static_cast<T>(1e-6));
+    }
 
-      /// \brief Equality test with tolerance.
-      /// \param[in] _v the vector to compare to
-      /// \param[in] _tol equality tolerance.
-      /// \return true if the elements of the vectors are equal within
-      /// the tolerence specified by _tol.
-      public: bool Equal(const Vector4 &_v, const T &_tol) const
-      {
-        return equal<T>(this->data[0], _v[0], _tol)
-            && equal<T>(this->data[1], _v[1], _tol)
-            && equal<T>(this->data[2], _v[2], _tol)
-            && equal<T>(this->data[3], _v[3], _tol);
-      }
+    /// \brief Not equal to operator
+    /// \param[in] _pt the other vector
+    /// \return false if each component is equal within a
+    /// default tolerence (1e-6), true otherwise
+    public: bool operator!=(const Vector4<T> &_pt) const
+    {
+      return !(*this == _pt);
+    }
 
-      /// \brief Equal to operator
-      /// \param[in] _v the other vector
-      /// \return true if each component is equal within a
-      /// default tolerence (1e-6), false otherwise
-      public: bool operator==(const Vector4<T> &_v) const
-      {
-        return this->Equal(_v, static_cast<T>(1e-6));
-      }
+    /// \brief See if a point is finite (e.g., not nan)
+    /// \return true if finite, false otherwise
+    public: bool IsFinite() const
+    {
+      // std::isfinite works with floating point values,
+      // need to explicit cast to avoid ambiguity in vc++.
+      return std::isfinite(static_cast<double>(this->data[0])) &&
+             std::isfinite(static_cast<double>(this->data[1])) &&
+             std::isfinite(static_cast<double>(this->data[2])) &&
+             std::isfinite(static_cast<double>(this->data[3]));
+    }
 
-      /// \brief Not equal to operator
-      /// \param[in] _pt the other vector
-      /// \return false if each component is equal within a
-      /// default tolerence (1e-6), true otherwise
-      public: bool operator!=(const Vector4<T> &_pt) const
-      {
-        return !(*this == _pt);
-      }
+    /// \brief Array subscript operator
+    /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w.
+    /// The index is clamped to the range (0,3).
+    /// \return The value.
+    public: T &operator[](const std::size_t _index)
+    {
+      return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
+    }
 
-      /// \brief See if a point is finite (e.g., not nan)
-      /// \return true if finite, false otherwise
-      public: bool IsFinite() const
-      {
-        // std::isfinite works with floating point values,
-        // need to explicit cast to avoid ambiguity in vc++.
-        return std::isfinite(static_cast<double>(this->data[0])) &&
-               std::isfinite(static_cast<double>(this->data[1])) &&
-               std::isfinite(static_cast<double>(this->data[2])) &&
-               std::isfinite(static_cast<double>(this->data[3]));
-      }
+    /// \brief Const-qualified array subscript operator
+    /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w.
+    /// The index is clamped to the range (0,3).
+    /// \return The value.
+    public: T operator[](const std::size_t _index) const
+    {
+      return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
+    }
 
-      /// \brief Array subscript operator
-      /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w.
-      /// The index is clamped to the range (0,3).
-      /// \return The value.
-      public: T &operator[](const std::size_t _index)
-      {
-        return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
-      }
+    /// \brief Return a mutable x value.
+    /// \return The x component of the vector
+    public: T &X()
+    {
+      return this->data[0];
+    }
 
-      /// \brief Const-qualified array subscript operator
-      /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w.
-      /// The index is clamped to the range (0,3).
-      /// \return The value.
-      public: T operator[](const std::size_t _index) const
-      {
-        return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
-      }
+    /// \brief Return a mutable y value.
+    /// \return The y component of the vector
+    public: T &Y()
+    {
+      return this->data[1];
+    }
 
-      /// \brief Return a mutable x value.
-      /// \return The x component of the vector
-      public: T &X()
-      {
-        return this->data[0];
-      }
+    /// \brief Return a mutable z value.
+    /// \return The z component of the vector
+    public: T &Z()
+    {
+      return this->data[2];
+    }
 
-      /// \brief Return a mutable y value.
-      /// \return The y component of the vector
-      public: T &Y()
-      {
-        return this->data[1];
-      }
+    /// \brief Return a mutable w value.
+    /// \return The w component of the vector
+    public: T &W()
+    {
+      return this->data[3];
+    }
 
-      /// \brief Return a mutable z value.
-      /// \return The z component of the vector
-      public: T &Z()
-      {
-        return this->data[2];
-      }
+    /// \brief Get the x value.
+    /// \return The x component of the vector
+    public: T X() const
+    {
+      return this->data[0];
+    }
 
-      /// \brief Return a mutable w value.
-      /// \return The w component of the vector
-      public: T &W()
-      {
-        return this->data[3];
-      }
+    /// \brief Get the y value.
+    /// \return The y component of the vector
+    public: T Y() const
+    {
+      return this->data[1];
+    }
 
-      /// \brief Get the x value.
-      /// \return The x component of the vector
-      public: T X() const
-      {
-        return this->data[0];
-      }
+    /// \brief Get the z value.
+    /// \return The z component of the vector
+    public: T Z() const
+    {
+      return this->data[2];
+    }
 
-      /// \brief Get the y value.
-      /// \return The y component of the vector
-      public: T Y() const
-      {
-        return this->data[1];
-      }
+    /// \brief Get the w value.
+    /// \return The w component of the vector
+    public: T W() const
+    {
+      return this->data[3];
+    }
 
-      /// \brief Get the z value.
-      /// \return The z component of the vector
-      public: T Z() const
-      {
-        return this->data[2];
-      }
+    /// \brief Set the x value.
+    /// \param[in] _v Value for the x component.
+    public: inline void X(const T &_v)
+    {
+      this->data[0] = _v;
+    }
 
-      /// \brief Get the w value.
-      /// \return The w component of the vector
-      public: T W() const
-      {
-        return this->data[3];
-      }
+    /// \brief Set the y value.
+    /// \param[in] _v Value for the y component.
+    public: inline void Y(const T &_v)
+    {
+      this->data[1] = _v;
+    }
 
-      /// \brief Set the x value.
-      /// \param[in] _v Value for the x component.
-      public: inline void X(const T &_v)
-      {
-        this->data[0] = _v;
-      }
+    /// \brief Set the z value.
+    /// \param[in] _v Value for the z component.
+    public: inline void Z(const T &_v)
+    {
+      this->data[2] = _v;
+    }
 
-      /// \brief Set the y value.
-      /// \param[in] _v Value for the y component.
-      public: inline void Y(const T &_v)
-      {
-        this->data[1] = _v;
-      }
+    /// \brief Set the w value.
+    /// \param[in] _v Value for the w component.
+    public: inline void W(const T &_v)
+    {
+      this->data[3] = _v;
+    }
 
-      /// \brief Set the z value.
-      /// \param[in] _v Value for the z component.
-      public: inline void Z(const T &_v)
-      {
-        this->data[2] = _v;
-      }
+    /// \brief Less than operator.
+    /// \param[in] _pt Vector to compare.
+    /// \return True if this vector's X(), Y(), Z() or W() value is less
+    /// than the given vector's corresponding values.
+    public: bool operator<(const Vector4<T> &_pt) const
+    {
+      return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
+             this->data[2] < _pt[2] || this->data[3] < _pt[3];
+    }
 
-      /// \brief Set the w value.
-      /// \param[in] _v Value for the w component.
-      public: inline void W(const T &_v)
+    /// \brief Stream insertion operator
+    /// \param[in] _out output stream
+    /// \param[in] _pt Vector4 to output
+    /// \return The stream
+    public: friend std::ostream &operator<<(
+                std::ostream &_out, const gz::math::Vector4<T> &_pt)
+    {
+      for (auto i : {0, 1, 2, 3})
       {
-        this->data[3] = _v;
-      }
+        if (i > 0)
+          _out << " ";
 
-      /// \brief Less than operator.
-      /// \param[in] _pt Vector to compare.
-      /// \return True if this vector's X(), Y(), Z() or W() value is less
-      /// than the given vector's corresponding values.
-      public: bool operator<(const Vector4<T> &_pt) const
-      {
-        return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
-               this->data[2] < _pt[2] || this->data[3] < _pt[3];
+        appendToStream(_out, _pt[i]);
       }
+      return _out;
+    }
 
-      /// \brief Stream insertion operator
-      /// \param[in] _out output stream
-      /// \param[in] _pt Vector4 to output
-      /// \return The stream
-      public: friend std::ostream &operator<<(
-                  std::ostream &_out, const gz::math::Vector4<T> &_pt)
-      {
-        for (auto i : {0, 1, 2, 3})
-        {
-          if (i > 0)
-            _out << " ";
-
-          appendToStream(_out, _pt[i]);
-        }
-        return _out;
-      }
+    /// \brief Stream extraction operator
+    /// \param[in] _in input stream
+    /// \param[in] _pt Vector4 to read values into
+    /// \return the stream
+    public: friend std::istream &operator>>(
+                std::istream &_in, gz::math::Vector4<T> &_pt)
+    {
+      T x, y, z, w;
 
-      /// \brief Stream extraction operator
-      /// \param[in] _in input stream
-      /// \param[in] _pt Vector4 to read values into
-      /// \return the stream
-      public: friend std::istream &operator>>(
-                  std::istream &_in, gz::math::Vector4<T> &_pt)
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      _in >> x >> y >> z >> w;
+      if (!_in.fail())
       {
-        T x, y, z, w;
-
-        // Skip white spaces
-        _in.setf(std::ios_base::skipws);
-        _in >> x >> y >> z >> w;
-        if (!_in.fail())
-        {
-          _pt.Set(x, y, z, w);
-        }
-        return _in;
+        _pt.Set(x, y, z, w);
       }
+      return _in;
+    }
 
-      /// \brief Data values, 0==x, 1==y, 2==z, 3==w
-      private: T data[4];
-    };
+    /// \brief Data values, 0==x, 1==y, 2==z, 3==w
+    private: T data[4];
+  };
 
-    namespace detail {
+  namespace detail {
 
-      template<typename T>
-      constexpr Vector4<T> gVector4Zero(0, 0, 0, 0);
+    template<typename T>
+    constexpr Vector4<T> gVector4Zero(0, 0, 0, 0);
 
-      template<typename T>
-      constexpr Vector4<T> gVector4One(1, 1, 1, 1);
+    template<typename T>
+    constexpr Vector4<T> gVector4One(1, 1, 1, 1);
 
-      template<typename T>
-      constexpr Vector4<T> gVector4NaN(
-          std::numeric_limits<T>::quiet_NaN(),
-          std::numeric_limits<T>::quiet_NaN(),
-          std::numeric_limits<T>::quiet_NaN(),
-          std::numeric_limits<T>::quiet_NaN());
+    template<typename T>
+    constexpr Vector4<T> gVector4NaN(
+        std::numeric_limits<T>::quiet_NaN(),
+        std::numeric_limits<T>::quiet_NaN(),
+        std::numeric_limits<T>::quiet_NaN(),
+        std::numeric_limits<T>::quiet_NaN());
 
-    }  // namespace detail
+  }  // namespace detail
 
-    template<typename T>
-    const Vector4<T> &Vector4<T>::Zero = detail::gVector4Zero<T>;
+  template<typename T>
+  const Vector4<T> &Vector4<T>::Zero = detail::gVector4Zero<T>;
 
-    template<typename T>
-    const Vector4<T> &Vector4<T>::One = detail::gVector4One<T>;
+  template<typename T>
+  const Vector4<T> &Vector4<T>::One = detail::gVector4One<T>;
 
-    template<typename T>
-    const Vector4<T> &Vector4<T>::NaN = detail::gVector4NaN<T>;
+  template<typename T>
+  const Vector4<T> &Vector4<T>::NaN = detail::gVector4NaN<T>;
 
-    typedef Vector4<int> Vector4i;
-    typedef Vector4<double> Vector4d;
-    typedef Vector4<float> Vector4f;
-    }
-  }
-}
-#endif
+  typedef Vector4<int> Vector4i;
+  typedef Vector4<double> Vector4d;
+  typedef Vector4<float> Vector4f;
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_VERSION_NAMESPACE
diff --git a/include/gz/math/VolumetricGridLookupField.hh b/include/gz/math/VolumetricGridLookupField.hh
index 7da02503f..81eba1aa2 100644
--- a/include/gz/math/VolumetricGridLookupField.hh
+++ b/include/gz/math/VolumetricGridLookupField.hh
@@ -27,262 +27,258 @@
 
 #include <gz/math/detail/AxisIndex.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
-      template<typename T, typename I = std::size_t>
-      /// \brief Lookup table for a volumetric dataset. This class is used to
-      /// lookup indices for a large dataset that's organized in a grid. This
-      /// class is not meant to be used with non-grid like data sets. The grid
-      /// may be sparse or non uniform and missing data points.
-      class VolumetricGridLookupField
-      {
-        /// Get the index along the given axis
-        private: AxisIndex<T> z_indices_by_depth;
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
+    template<typename T, typename I = std::size_t>
+    /// \brief Lookup table for a volumetric dataset. This class is used to
+    /// lookup indices for a large dataset that's organized in a grid. This
+    /// class is not meant to be used with non-grid like data sets. The grid
+    /// may be sparse or non uniform and missing data points.
+    class VolumetricGridLookupField
+    {
+      /// Get the index along the given axis
+      private: AxisIndex<T> z_indices_by_depth;
 
-        private: AxisIndex<T> x_indices_by_lat;
+      private: AxisIndex<T> x_indices_by_lat;
 
-        private: AxisIndex<T> y_indices_by_lon;
+      private: AxisIndex<T> y_indices_by_lon;
 
-        private: std::vector<
-          std::vector<std::vector<std::optional<I>>>> index_table;
+      private: std::vector<
+        std::vector<std::vector<std::optional<I>>>> index_table;
 
-        /// \brief Constructor
-        /// \param[in] _cloud The cloud of points to use to construct the grid.
-        public: VolumetricGridLookupField(
-          const std::vector<Vector3<T>> &_cloud)
+      /// \brief Constructor
+      /// \param[in] _cloud The cloud of points to use to construct the grid.
+      public: VolumetricGridLookupField(
+        const std::vector<Vector3<T>> &_cloud)
+      {
+        // NOTE: This part of the code assumes an exact grid of points.
+        // The grid may be distorted or the stride between different points
+        // may not be the same, but fundamentally the data is structured still
+        // forms a grid-like structure. It keeps track of the axis indices for
+        // each point in the grid.
+        for(auto pt : _cloud)
         {
-          // NOTE: This part of the code assumes an exact grid of points.
-          // The grid may be distorted or the stride between different points
-          // may not be the same, but fundamentally the data is structured still
-          // forms a grid-like structure. It keeps track of the axis indices for
-          // each point in the grid.
-          for(auto pt : _cloud)
-          {
-            x_indices_by_lat.AddIndexIfNotFound(pt.X());
-            y_indices_by_lon.AddIndexIfNotFound(pt.Y());
-            z_indices_by_depth.AddIndexIfNotFound(pt.Z());
-          }
-
-          int num_x = x_indices_by_lat.GetNumUniqueIndices();
-          int num_y = y_indices_by_lon.GetNumUniqueIndices();
-          int num_z = z_indices_by_depth.GetNumUniqueIndices();
+          x_indices_by_lat.AddIndexIfNotFound(pt.X());
+          y_indices_by_lon.AddIndexIfNotFound(pt.Y());
+          z_indices_by_depth.AddIndexIfNotFound(pt.Z());
+        }
 
-          index_table.resize(num_z);
-          for(int i = 0; i < num_z; ++i)
-          {
-            index_table[i].resize(num_y);
-            for(int j = 0; j < num_y; ++j)
-            {
-              index_table[i][j].resize(num_x);
-            }
-          }
+        int num_x = x_indices_by_lat.GetNumUniqueIndices();
+        int num_y = y_indices_by_lon.GetNumUniqueIndices();
+        int num_z = z_indices_by_depth.GetNumUniqueIndices();
 
-          for(std::size_t i = 0; i < _cloud.size(); ++i)
+        index_table.resize(num_z);
+        for(int i = 0; i < num_z; ++i)
+        {
+          index_table[i].resize(num_y);
+          for(int j = 0; j < num_y; ++j)
           {
-            const auto &pt = _cloud[i];
-            const std::size_t x_index =
-              x_indices_by_lat.GetIndex(pt.X()).value();
-            const std::size_t y_index =
-              y_indices_by_lon.GetIndex(pt.Y()).value();
-            const std::size_t z_index =
-              z_indices_by_depth.GetIndex(pt.Z()).value();
-            index_table[z_index][y_index][x_index] = i;
+            index_table[i][j].resize(num_x);
           }
         }
 
-        /// \brief Retrieves the indices of the points that are to be used for
-        /// interpolation. Separate tolerance values are used for each axis as
-        /// data may have different magnitudes on each axis.
-        /// \param[in] _pt The point to get the interpolators for.
-        /// \param[in] _xTol The tolerance for the x axis.
-        /// \param[in] _yTol The tolerance for the y axis.
-        /// \param[in] _zTol The tolerance for the z axis.
-        /// \returns A list of points which are to be used for interpolation. If
-        /// the point is in the middle of a grid cell then it returns the four
-        /// corners of the grid cell. If its along a plane then it returns the
-        /// four corners, if along an edge two points and if coincident with a
-        /// corner one point. If the data is sparse and missing indices then a
-        /// nullopt will be returned.
-        public: std::vector<InterpolationPoint3D<T>>
-          GetInterpolators(
-            const Vector3<T> &_pt,
-            const double _xTol = 1e-6,
-            const double _yTol = 1e-6,
-            const double _zTol = 1e-6) const
+        for(std::size_t i = 0; i < _cloud.size(); ++i)
         {
-          std::vector<InterpolationPoint3D<T>> interpolators;
+          const auto &pt = _cloud[i];
+          const std::size_t x_index =
+            x_indices_by_lat.GetIndex(pt.X()).value();
+          const std::size_t y_index =
+            y_indices_by_lon.GetIndex(pt.Y()).value();
+          const std::size_t z_index =
+            z_indices_by_depth.GetIndex(pt.Z()).value();
+          index_table[z_index][y_index][x_index] = i;
+        }
+      }
+
+      /// \brief Retrieves the indices of the points that are to be used for
+      /// interpolation. Separate tolerance values are used for each axis as
+      /// data may have different magnitudes on each axis.
+      /// \param[in] _pt The point to get the interpolators for.
+      /// \param[in] _xTol The tolerance for the x axis.
+      /// \param[in] _yTol The tolerance for the y axis.
+      /// \param[in] _zTol The tolerance for the z axis.
+      /// \returns A list of points which are to be used for interpolation. If
+      /// the point is in the middle of a grid cell then it returns the four
+      /// corners of the grid cell. If its along a plane then it returns the
+      /// four corners, if along an edge two points and if coincident with a
+      /// corner one point. If the data is sparse and missing indices then a
+      /// nullopt will be returned.
+      public: std::vector<InterpolationPoint3D<T>>
+        GetInterpolators(
+          const Vector3<T> &_pt,
+          const double _xTol = 1e-6,
+          const double _yTol = 1e-6,
+          const double _zTol = 1e-6) const
+      {
+        std::vector<InterpolationPoint3D<T>> interpolators;
 
-          auto x_indices = x_indices_by_lat.GetInterpolators(_pt.X(), _xTol);
-          auto y_indices = y_indices_by_lon.GetInterpolators(_pt.Y(), _yTol);
-          auto z_indices = z_indices_by_depth.GetInterpolators(_pt.Z(), _zTol);
+        auto x_indices = x_indices_by_lat.GetInterpolators(_pt.X(), _xTol);
+        auto y_indices = y_indices_by_lon.GetInterpolators(_pt.Y(), _yTol);
+        auto z_indices = z_indices_by_depth.GetInterpolators(_pt.Z(), _zTol);
 
-          for(const auto &x_index : x_indices)
+        for(const auto &x_index : x_indices)
+        {
+          for(const auto &y_index : y_indices)
           {
-            for(const auto &y_index : y_indices)
+            for(const auto &z_index : z_indices)
             {
-              for(const auto &z_index : z_indices)
-              {
-                auto index =
-                  index_table[z_index.index][y_index.index][x_index.index];
-                interpolators.push_back(
-                  InterpolationPoint3D<T>{
-                    Vector3<T>(
-                      x_index.position,
-                      y_index.position,
-                      z_index.position
-                    ),
-                    std::optional{index}
-                  });
-              }
+              auto index =
+                index_table[z_index.index][y_index.index][x_index.index];
+              interpolators.push_back(
+                InterpolationPoint3D<T>{
+                  Vector3<T>(
+                    x_index.position,
+                    y_index.position,
+                    z_index.position
+                  ),
+                  std::optional{index}
+                });
             }
           }
-
-          return interpolators;
         }
 
-        /// \brief Constructor
-        /// \param[in] _cloud The cloud of points to use to construct the grid.
-        /// \param[in] _indices A series of indices these points correspond to.
-        public: VolumetricGridLookupField(
-          const std::vector<Vector3<T>> &_cloud,
-          const std::vector<I> &_indices)
-        {
-          assert(_indices.size() == _cloud.size());
-          // NOTE: This part of the code assumes an exact grid of points.
-          // The grid may be distorted or the stride between different points
-          // may not be the same, but fundamentally the data is structured in
-          // a grid-like structure. It keeps track of the axis indices for
-          // each point in the grid.
-          for(auto pt : _cloud)
-          {
-            x_indices_by_lat.AddIndexIfNotFound(pt.X());
-            y_indices_by_lon.AddIndexIfNotFound(pt.Y());
-            z_indices_by_depth.AddIndexIfNotFound(pt.Z());
-          }
+        return interpolators;
+      }
 
-          int num_x = x_indices_by_lat.GetNumUniqueIndices();
-          int num_y = y_indices_by_lon.GetNumUniqueIndices();
-          int num_z = z_indices_by_depth.GetNumUniqueIndices();
+      /// \brief Constructor
+      /// \param[in] _cloud The cloud of points to use to construct the grid.
+      /// \param[in] _indices A series of indices these points correspond to.
+      public: VolumetricGridLookupField(
+        const std::vector<Vector3<T>> &_cloud,
+        const std::vector<I> &_indices)
+      {
+        assert(_indices.size() == _cloud.size());
+        // NOTE: This part of the code assumes an exact grid of points.
+        // The grid may be distorted or the stride between different points
+        // may not be the same, but fundamentally the data is structured in
+        // a grid-like structure. It keeps track of the axis indices for
+        // each point in the grid.
+        for(auto pt : _cloud)
+        {
+          x_indices_by_lat.AddIndexIfNotFound(pt.X());
+          y_indices_by_lon.AddIndexIfNotFound(pt.Y());
+          z_indices_by_depth.AddIndexIfNotFound(pt.Z());
+        }
 
-          index_table.resize(num_z);
-          for(int i = 0; i < num_z; ++i)
-          {
-            index_table[i].resize(num_y);
-            for(int j = 0; j < num_y; ++j)
-            {
-              index_table[i][j].resize(num_x);
-            }
-          }
+        int num_x = x_indices_by_lat.GetNumUniqueIndices();
+        int num_y = y_indices_by_lon.GetNumUniqueIndices();
+        int num_z = z_indices_by_depth.GetNumUniqueIndices();
 
-          for(std::size_t i = 0; i < _cloud.size(); ++i)
+        index_table.resize(num_z);
+        for(int i = 0; i < num_z; ++i)
+        {
+          index_table[i].resize(num_y);
+          for(int j = 0; j < num_y; ++j)
           {
-            const auto &pt = _cloud[i];
-            const std::size_t x_index =
-              x_indices_by_lat.GetIndex(pt.X()).value();
-            const std::size_t y_index =
-              y_indices_by_lon.GetIndex(pt.Y()).value();
-            const std::size_t z_index =
-              z_indices_by_depth.GetIndex(pt.Z()).value();
-            index_table[z_index][y_index][x_index] = _indices[i];
+            index_table[i][j].resize(num_x);
           }
         }
 
-        /// \brief Estimates the values for a grid given a list of values to
-        /// interpolate. This method uses Trilinear interpolation.
-        /// \param[in] _pt The point to estimate for.
-        /// \param[in] _values The values to interpolate.
-        /// \param[in] _default If a value is not found at a specific point then
-        /// this value will be used.
-        /// \returns The estimated value for the point.
-        public: template<typename V>
-        std::optional<V> EstimateValueUsingTrilinear(
-          const Vector3<T> &_pt,
-          const std::vector<V> &_values,
-          const V &_default = V(0)) const
+        for(std::size_t i = 0; i < _cloud.size(); ++i)
         {
-          auto interpolators = GetInterpolators(_pt);
-          return EstimateValueUsingTrilinear(
-            interpolators,
-            _pt,
-            _values,
-            _default);
+          const auto &pt = _cloud[i];
+          const std::size_t x_index =
+            x_indices_by_lat.GetIndex(pt.X()).value();
+          const std::size_t y_index =
+            y_indices_by_lon.GetIndex(pt.Y()).value();
+          const std::size_t z_index =
+            z_indices_by_depth.GetIndex(pt.Z()).value();
+          index_table[z_index][y_index][x_index] = _indices[i];
         }
+      }
 
-        /// \brief Estimates the values for a grid given a list of values to
-        /// interpolate. This method uses Trilinear interpolation.
-        /// \param[in] _interpolators The list of interpolators to use.
-        /// Retrieved by calling GetInterpolators().
-        /// \param[in] _pt The point to estimate for.
-        /// \param[in] _values The values to interpolate.
-        /// \param[in] _default If a value is not found at a specific point then
-        /// this value will be used.
-        /// \returns The estimated value for the point. Nullopt if we are
-        /// outside the field. Default value if in the field but no value is
-        /// in the index.
-        public: template<typename V>
-        std::optional<V> EstimateValueUsingTrilinear(
-          const std::vector<InterpolationPoint3D<T>> _interpolators,
-          const Vector3<T> &_pt,
-          const std::vector<V> &_values,
-          const V &_default = V(0)) const
+      /// \brief Estimates the values for a grid given a list of values to
+      /// interpolate. This method uses Trilinear interpolation.
+      /// \param[in] _pt The point to estimate for.
+      /// \param[in] _values The values to interpolate.
+      /// \param[in] _default If a value is not found at a specific point then
+      /// this value will be used.
+      /// \returns The estimated value for the point.
+      public: template<typename V>
+      std::optional<V> EstimateValueUsingTrilinear(
+        const Vector3<T> &_pt,
+        const std::vector<V> &_values,
+        const V &_default = V(0)) const
+      {
+        auto interpolators = GetInterpolators(_pt);
+        return EstimateValueUsingTrilinear(
+          interpolators,
+          _pt,
+          _values,
+          _default);
+      }
+
+      /// \brief Estimates the values for a grid given a list of values to
+      /// interpolate. This method uses Trilinear interpolation.
+      /// \param[in] _interpolators The list of interpolators to use.
+      /// Retrieved by calling GetInterpolators().
+      /// \param[in] _pt The point to estimate for.
+      /// \param[in] _values The values to interpolate.
+      /// \param[in] _default If a value is not found at a specific point then
+      /// this value will be used.
+      /// \returns The estimated value for the point. Nullopt if we are
+      /// outside the field. Default value if in the field but no value is
+      /// in the index.
+      public: template<typename V>
+      std::optional<V> EstimateValueUsingTrilinear(
+        const std::vector<InterpolationPoint3D<T>> _interpolators,
+        const Vector3<T> &_pt,
+        const std::vector<V> &_values,
+        const V &_default = V(0)) const
+      {
+        if (_interpolators.size() == 0)
         {
-          if (_interpolators.size() == 0)
-          {
-            return std::nullopt;
-          }
-          else if (_interpolators.size() == 1)
-          {
-            if (!_interpolators[0].index.has_value())
-            {
-              return _default;
-            }
-            return _values[_interpolators[0].index.value()];
-          }
-          else if (_interpolators.size() == 2)
-          {
-            return LinearInterpolate(_interpolators[0], _interpolators[1],
-              _values, _pt, _default);
-          }
-          else if (_interpolators.size() == 4)
-          {
-            return BiLinearInterpolate(
-              _interpolators, 0, _values, _pt, _default);
-          }
-          else if (_interpolators.size() == 8)
-          {
-            return TrilinearInterpolate(_interpolators, _values, _pt, _default);
-          }
-          else
+          return std::nullopt;
+        }
+        else if (_interpolators.size() == 1)
+        {
+          if (!_interpolators[0].index.has_value())
           {
-            // should never get here
-            return std::nullopt;
+            return _default;
           }
+          return _values[_interpolators[0].index.value()];
         }
-
-        /// \brief Get the bounds of this grid field.
-        /// \return A pair of vectors.
-        public: std::pair<Vector3<T>, Vector3<T>> Bounds() const
+        else if (_interpolators.size() == 2)
         {
-          return std::pair<Vector3<T>, Vector3<T>>(
-            Vector3<T>{
-              x_indices_by_lat.MinKey(),
-              y_indices_by_lon.MinKey(),
-              z_indices_by_depth.MinKey()
-            },
-            Vector3<T>{
-              x_indices_by_lat.MaxKey(),
-              y_indices_by_lon.MaxKey(),
-              z_indices_by_depth.MaxKey()
-            });
+          return LinearInterpolate(_interpolators[0], _interpolators[1],
+            _values, _pt, _default);
         }
+        else if (_interpolators.size() == 4)
+        {
+          return BiLinearInterpolate(
+            _interpolators, 0, _values, _pt, _default);
+        }
+        else if (_interpolators.size() == 8)
+        {
+          return TrilinearInterpolate(_interpolators, _values, _pt, _default);
+        }
+        else
+        {
+          // should never get here
+          return std::nullopt;
+        }
+      }
 
-      };
-    }
-  }
-}
+      /// \brief Get the bounds of this grid field.
+      /// \return A pair of vectors.
+      public: std::pair<Vector3<T>, Vector3<T>> Bounds() const
+      {
+        return std::pair<Vector3<T>, Vector3<T>>(
+          Vector3<T>{
+            x_indices_by_lat.MinKey(),
+            y_indices_by_lon.MinKey(),
+            z_indices_by_depth.MinKey()
+          },
+          Vector3<T>{
+            x_indices_by_lat.MaxKey(),
+            y_indices_by_lon.MaxKey(),
+            z_indices_by_depth.MaxKey()
+          });
+      }
 
-#endif
+    };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
diff --git a/include/gz/math/detail/AxisIndex.hh b/include/gz/math/detail/AxisIndex.hh
index 3d4dabb1a..2055dea63 100644
--- a/include/gz/math/detail/AxisIndex.hh
+++ b/include/gz/math/detail/AxisIndex.hh
@@ -27,124 +27,121 @@
 
 #include <gz/math/detail/InterpolationPoint.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
-  {
-    // Inline bracket to help doxygen filtering.
-    inline namespace GZ_MATH_VERSION_NAMESPACE {
+  // Inline bracket to help doxygen filtering.
+  inline namespace GZ_MATH_VERSION_NAMESPACE {
 
-      /// \brief Represents a sparse number line which can be searched via
-      /// search for indices.
-      template <typename T>
-      class AxisIndex
-      {
-        /// \brief Binary tree with indices
-        private: std::map<T, std::size_t> axisIndex;
+    /// \brief Represents a sparse number line which can be searched via
+    /// search for indices.
+    template <typename T>
+    class AxisIndex
+    {
+      /// \brief Binary tree with indices
+      private: std::map<T, std::size_t> axisIndex;
 
-        /// \brief Number of indices
-        private: int numIndices{0};
+      /// \brief Number of indices
+      private: int numIndices{0};
 
-        /// \brief Minimum key
-        /// \return the minimum key or zero if the axis is empty.
-        public: T MinKey() const
+      /// \brief Minimum key
+      /// \return the minimum key or zero if the axis is empty.
+      public: T MinKey() const
+      {
+        auto key = axisIndex.begin();
+        if (key == axisIndex.end())
         {
-          auto key = axisIndex.begin();
-          if (key == axisIndex.end())
-          {
-            return T(0);
-          }
-          return key->first;
+          return T(0);
         }
+        return key->first;
+      }
 
-        /// \brief Maximum key
-        /// \return the maximum key or zero if the axis is empty.
-        public: T MaxKey() const
+      /// \brief Maximum key
+      /// \return the maximum key or zero if the axis is empty.
+      public: T MaxKey() const
+      {
+        auto key = axisIndex.rbegin();
+        if (key == axisIndex.rend())
         {
-          auto key = axisIndex.rbegin();
-          if (key == axisIndex.rend())
-          {
-            return T(0);
-          }
-          return key->first;
+          return T(0);
         }
+        return key->first;
+      }
 
-        /// \brief Register the existance of a measurement at _value.
-        /// \param[in] _value The position of the measurement.
-        public: void AddIndexIfNotFound(T _value)
+      /// \brief Register the existance of a measurement at _value.
+      /// \param[in] _value The position of the measurement.
+      public: void AddIndexIfNotFound(T _value)
+      {
+        if (axisIndex.find(_value) == axisIndex.end())
         {
-          if (axisIndex.find(_value) == axisIndex.end())
-          {
-            axisIndex[_value] = numIndices;
-            numIndices++;
-          }
+          axisIndex[_value] = numIndices;
+          numIndices++;
         }
+      }
 
-        /// \brief Get the number of unique indices.
-        /// \return The number of unique indices.
-        public: std::size_t GetNumUniqueIndices() const
+      /// \brief Get the number of unique indices.
+      /// \return The number of unique indices.
+      public: std::size_t GetNumUniqueIndices() const
+      {
+        return axisIndex.size();
+      }
+
+      /// \brief Get the index of a measurement
+      /// \param[in] _value The position of the measurement.
+      /// \return The index of the measurement if found else return nullopt.
+      public: std::optional<std::size_t> GetIndex(T _value) const
+      {
+        auto res = axisIndex.find(_value);
+        if (res == axisIndex.end())
         {
-          return axisIndex.size();
+          return std::nullopt;
         }
-
-        /// \brief Get the index of a measurement
-        /// \param[in] _value The position of the measurement.
-        /// \return The index of the measurement if found else return nullopt.
-        public: std::optional<std::size_t> GetIndex(T _value) const
+        else
         {
-          auto res = axisIndex.find(_value);
-          if (res == axisIndex.end())
-          {
-            return std::nullopt;
-          }
-          else
-          {
-            return res->second;
-          }
+          return res->second;
         }
+      }
 
-        /// \brief Get interpolators for a measurement.
-        /// \param[in] _value The position of the measurement.
-        /// \param[in] _tol The tolerance for the search. Cannot be zero.
-        /// \return The indices of the measurements that should be used for
-        /// interpolation. If the value is out of range, an empty vector is
-        /// returned. If the value is exact, a vector with a single index is
-        /// returned otherwise return the two indices which should be used for
-        /// interpolation.
-        public: std::vector<InterpolationPoint1D<T>> GetInterpolators(
-          const T &_value,
-          double _tol = 1e-6) const
+      /// \brief Get interpolators for a measurement.
+      /// \param[in] _value The position of the measurement.
+      /// \param[in] _tol The tolerance for the search. Cannot be zero.
+      /// \return The indices of the measurements that should be used for
+      /// interpolation. If the value is out of range, an empty vector is
+      /// returned. If the value is exact, a vector with a single index is
+      /// returned otherwise return the two indices which should be used for
+      /// interpolation.
+      public: std::vector<InterpolationPoint1D<T>> GetInterpolators(
+        const T &_value,
+        double _tol = 1e-6) const
+      {
+        assert(_tol > 0);
+        // Performs a BST to find the first element that is greater than or
+        // equal to the value.
+        auto it = axisIndex.lower_bound(_value);
+        if (it == axisIndex.end())
+        {
+          // Out of range
+          return {};
+        }
+        else if (fabs(it->first - _value) < _tol)
+        {
+          // Exact match
+          return {InterpolationPoint1D<T>{it->first, it->second}};
+        }
+        else if (it == axisIndex.begin())
+        {
+          // Below range
+          return {};
+        }
+        else
         {
-          assert(_tol > 0);
-          // Performs a BST to find the first element that is greater than or
-          // equal to the value.
-          auto it = axisIndex.lower_bound(_value);
-          if (it == axisIndex.end())
-          {
-            // Out of range
-            return {};
-          }
-          else if (fabs(it->first - _value) < _tol)
-          {
-            // Exact match
-            return {InterpolationPoint1D<T>{it->first, it->second}};
-          }
-          else if (it == axisIndex.begin())
-          {
-            // Below range
-            return {};
-          }
-          else
-          {
-            // Interpolate
-            auto itPrev = it;
-            itPrev--;
-            return {InterpolationPoint1D<T>{itPrev->first, itPrev->second},
-              InterpolationPoint1D<T>{it->first, it->second}};
-          }
+          // Interpolate
+          auto itPrev = it;
+          itPrev--;
+          return {InterpolationPoint1D<T>{itPrev->first, itPrev->second},
+            InterpolationPoint1D<T>{it->first, it->second}};
         }
-      };
-    }
-  }
-}
-#endif
+      }
+    };
+  }  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_DETAIL_AXIS_INDEX_LOOKUP_FIELD_HH_
diff --git a/include/gz/math/detail/Box.hh b/include/gz/math/detail/Box.hh
index 9b5950984..57f39574a 100644
--- a/include/gz/math/detail/Box.hh
+++ b/include/gz/math/detail/Box.hh
@@ -25,9 +25,7 @@
 #include <utility>
 #include <vector>
 
-namespace gz
-{
-namespace math
+namespace gz::math
 {
 //////////////////////////////////////////////////
 template<typename T>
@@ -373,7 +371,5 @@ IntersectionPoints<T> Box<T>::Intersections(
 
   return intersections;
 }
-
-}
-}
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_DETAIL_BOX_HH_
diff --git a/include/gz/math/detail/Capsule.hh b/include/gz/math/detail/Capsule.hh
index 8169b2624..1c05ee5d0 100644
--- a/include/gz/math/detail/Capsule.hh
+++ b/include/gz/math/detail/Capsule.hh
@@ -22,11 +22,8 @@
 #include <gz/math/Helpers.hh>
 #include <gz/math/Inertial.hh>
 
-namespace gz
+namespace gz::math
 {
-namespace math
-{
-
 //////////////////////////////////////////////////
 template<typename T>
 Capsule<T>::Capsule(const T _length, const T _radius)
@@ -160,7 +157,5 @@ T Capsule<T>::DensityFromMass(const T _mass) const
 
   return _mass / this->Volume();
 }
-
-}
-}
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_DETAIL_CAPSULE_HH_
diff --git a/include/gz/math/detail/Cone.hh b/include/gz/math/detail/Cone.hh
index ddd7526a4..38944d539 100644
--- a/include/gz/math/detail/Cone.hh
+++ b/include/gz/math/detail/Cone.hh
@@ -21,11 +21,8 @@
 
 #include <optional>
 
-namespace gz
+namespace gz::math
 {
-namespace math
-{
-
 //////////////////////////////////////////////////
 template<typename T>
 Cone<T>::Cone(const T _length, const T _radius,
@@ -167,6 +164,5 @@ T Cone<T>::DensityFromMass(const T _mass) const
   return _mass / this->Volume();
 }
 
-}  // namespace math
-}  // namespace gz
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_DETAIL_CONE_HH_
diff --git a/include/gz/math/detail/Cylinder.hh b/include/gz/math/detail/Cylinder.hh
index aaa6d69c1..35638fab1 100644
--- a/include/gz/math/detail/Cylinder.hh
+++ b/include/gz/math/detail/Cylinder.hh
@@ -19,11 +19,8 @@
 
 #include <optional>
 
-namespace gz
+namespace gz::math
 {
-namespace math
-{
-
 //////////////////////////////////////////////////
 template<typename T>
 Cylinder<T>::Cylinder(const T _length, const T _radius,
@@ -164,7 +161,5 @@ T Cylinder<T>::DensityFromMass(const T _mass) const
 
   return _mass / this->Volume();
 }
-
-}
-}
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_DETAIL_CYLINDER_HH_
diff --git a/include/gz/math/detail/Ellipsoid.hh b/include/gz/math/detail/Ellipsoid.hh
index 5b9db31a2..535d686e9 100644
--- a/include/gz/math/detail/Ellipsoid.hh
+++ b/include/gz/math/detail/Ellipsoid.hh
@@ -22,11 +22,8 @@
 #include <gz/math/Helpers.hh>
 #include <gz/math/Inertial.hh>
 
-namespace gz
+namespace gz::math
 {
-namespace math
-{
-
 //////////////////////////////////////////////////
 template<typename T>
 Ellipsoid<T>::Ellipsoid(const Vector3<T> &_radii) : radii(_radii) {}
@@ -122,7 +119,5 @@ T Ellipsoid<T>::DensityFromMass(const T _mass) const
 
   return _mass / this->Volume();
 }
-
-}
-}
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_DETAIL_ELLIPSOID_HH_
diff --git a/include/gz/math/detail/InterpolationPoint.hh b/include/gz/math/detail/InterpolationPoint.hh
index 48349c3fd..e655c5446 100644
--- a/include/gz/math/detail/InterpolationPoint.hh
+++ b/include/gz/math/detail/InterpolationPoint.hh
@@ -25,237 +25,235 @@
 #include <vector>
 
 #include <cassert>
-namespace gz
+
+namespace gz::math
 {
-  namespace math
+  /// \brief Describes an interpolation point.
+  template<typename T>
+  struct InterpolationPoint3D
   {
-    /// \brief Describes an interpolation point.
-    template<typename T>
-    struct InterpolationPoint3D
-    {
-      /// \brief The position of the point
-      Vector3<T> position;
+    /// \brief The position of the point
+    Vector3<T> position;
 
-      /// \brief The index from which this point was retrieved.
-      /// Can be used by the application calling it to index. The reason
-      /// this is optional is that data may be missing from a sparse grid.
-      std::optional<std::size_t> index;
-    };
+    /// \brief The index from which this point was retrieved.
+    /// Can be used by the application calling it to index. The reason
+    /// this is optional is that data may be missing from a sparse grid.
+    std::optional<std::size_t> index;
+  };
 
-    /// \brief Describes a 4D interpolation point.
-    template<typename T, typename V>
-    struct InterpolationPoint4D
-    {
-      /// \brief The relevant interpolation points in 3D
-      std::vector<InterpolationPoint3D<V>> timeSlice;
+  /// \brief Describes a 4D interpolation point.
+  template<typename T, typename V>
+  struct InterpolationPoint4D
+  {
+    /// \brief The relevant interpolation points in 3D
+    std::vector<InterpolationPoint3D<V>> timeSlice;
 
-      /// \brief The time of this point
-      T time;
-    };
+    /// \brief The time of this point
+    T time;
+  };
 
-    /// \brief Describes an interpolation point in1d.
-    template<typename T>
-    struct InterpolationPoint1D
-    {
-      /// \brief The position of the point
-      T position;
+  /// \brief Describes an interpolation point in1d.
+  template<typename T>
+  struct InterpolationPoint1D
+  {
+    /// \brief The position of the point
+    T position;
 
-      /// \brief The index from which this point was retrieved.
-      /// Can be used by the application calling it to index.
-      std::size_t index;
-    };
+    /// \brief The index from which this point was retrieved.
+    /// Can be used by the application calling it to index.
+    std::size_t index;
+  };
 
-    /// \brief Linear Interpolation of two points along a 1D line.
-    /// \param[in] _a The first point.
-    /// \param[in] _b The second point.
-    /// \param[in] _lst An array of values that are to be used by the
-    /// interpolator. _lst[a.index] and _lst[b.index] are the values
-    /// to be interpolated.
-    /// \param[in] _pos The position to interpolate.
-    /// Warning: This function assumes that the indices of _a and _b correspond
-    /// to values in _lst. It performs no bounds checking whatsoever and if you
-    /// pass it invalid data, it will crash. It also assumes that _a and _b
-    /// are not in the same position.
-    template<typename T, typename V>
-    V LinearInterpolate(
-      const InterpolationPoint1D<T> &_a,
-      const InterpolationPoint1D<T> &_b,
-      const std::vector<V>  &_lst,
-      const T &_pos
-      )
-    {
-      assert(std::abs(_a.position - _b.position) > 0);
-      assert(_a.index < _lst.size());
-      assert(_b.index < _lst.size());
+  /// \brief Linear Interpolation of two points along a 1D line.
+  /// \param[in] _a The first point.
+  /// \param[in] _b The second point.
+  /// \param[in] _lst An array of values that are to be used by the
+  /// interpolator. _lst[a.index] and _lst[b.index] are the values
+  /// to be interpolated.
+  /// \param[in] _pos The position to interpolate.
+  /// Warning: This function assumes that the indices of _a and _b correspond
+  /// to values in _lst. It performs no bounds checking whatsoever and if you
+  /// pass it invalid data, it will crash. It also assumes that _a and _b
+  /// are not in the same position.
+  template<typename T, typename V>
+  V LinearInterpolate(
+    const InterpolationPoint1D<T> &_a,
+    const InterpolationPoint1D<T> &_b,
+    const std::vector<V>  &_lst,
+    const T &_pos
+    )
+  {
+    assert(std::abs(_a.position - _b.position) > 0);
+    assert(_a.index < _lst.size());
+    assert(_b.index < _lst.size());
 
-      auto t =  (_pos - _b.position)/ (_a.position - _b.position);
-      return (1 - t) * _lst[_b.index] + t * _lst[_a.index];
-    }
+    auto t =  (_pos - _b.position)/ (_a.position - _b.position);
+    return (1 - t) * _lst[_b.index] + t * _lst[_a.index];
+  }
 
-    /// \brief Linear Interpolation of two points in 3D space
-    /// \param[in] _a The first point.
-    /// \param[in] _b The second point.
-    /// \param[in] _lst An array of values that are to be used by the
-    /// interpolator. _lst[a.index] and _lst[b.index] are the values
-    /// to be interpolated. If a.index or b.index is std::nullopt then use the
-    /// default value.
-    /// \param[in] _pos The position to interpolate.
-    /// \param[in] _default The default value to use if a.index or b.index is
-    /// std::nullopt.
-    /// Warning: This function assumes that the indices of _a and _b correspond
-    /// to values in _lst. It performs no bounds checking whatsoever and if you
-    /// pass it invalid data, it will crash.
-    template<typename T, typename V>
-    V LinearInterpolate(
-      const InterpolationPoint3D<T> &_a,
-      const InterpolationPoint3D<T> &_b,
-      const std::vector<V>  &_lst,
-      const Vector3<T> &_pos,
-      const V &_default = V(0)
-      )
-    {
-      assert((_a.position - _b.position).Length() > 0);
-      assert((_a.index.has_value()) ? _a.index.value() < _lst.size(): true);
-      assert((_b.index.has_value()) ? _b.index.value() < _lst.size(): true);
+  /// \brief Linear Interpolation of two points in 3D space
+  /// \param[in] _a The first point.
+  /// \param[in] _b The second point.
+  /// \param[in] _lst An array of values that are to be used by the
+  /// interpolator. _lst[a.index] and _lst[b.index] are the values
+  /// to be interpolated. If a.index or b.index is std::nullopt then use the
+  /// default value.
+  /// \param[in] _pos The position to interpolate.
+  /// \param[in] _default The default value to use if a.index or b.index is
+  /// std::nullopt.
+  /// Warning: This function assumes that the indices of _a and _b correspond
+  /// to values in _lst. It performs no bounds checking whatsoever and if you
+  /// pass it invalid data, it will crash.
+  template<typename T, typename V>
+  V LinearInterpolate(
+    const InterpolationPoint3D<T> &_a,
+    const InterpolationPoint3D<T> &_b,
+    const std::vector<V>  &_lst,
+    const Vector3<T> &_pos,
+    const V &_default = V(0)
+    )
+  {
+    assert((_a.position - _b.position).Length() > 0);
+    assert((_a.index.has_value()) ? _a.index.value() < _lst.size(): true);
+    assert((_b.index.has_value()) ? _b.index.value() < _lst.size(): true);
 
-      auto t =
-        (_pos - _b.position).Length() / (_a.position - _b.position).Length();
+    auto t =
+      (_pos - _b.position).Length() / (_a.position - _b.position).Length();
 
-      auto b_val = (_b.index.has_value()) ? _lst[_b.index.value()]: _default;
-      auto a_val = (_a.index.has_value()) ? _lst[_a.index.value()]: _default;
-      return (1 - t) * b_val + t * a_val;
-    }
+    auto b_val = (_b.index.has_value()) ? _lst[_b.index.value()]: _default;
+    auto a_val = (_a.index.has_value()) ? _lst[_a.index.value()]: _default;
+    return (1 - t) * b_val + t * a_val;
+  }
 
-    /// \brief Bilinear interpolation of four points in 3D space. It assumes
-    /// these 4 points form a plane.
-    /// \param[in] _a The list of points to interpolate. The list must have at
-    /// least 4 entries. Furthermore the order of the indices should be such
-    /// that every consecutive pair of indices lie on the same edge. Furthermore
-    /// the 4 points must be coplanar and corners of a rectangular patch.
-    /// \param[in] _start_index The starting index of points to interpolate.
-    /// \param[in] _lst An array of values that are to be used by the
-    /// interpolator. _lst[a.index] and _lst[b.index] are the values
-    /// to be interpolated. If a.index or b.index is std::nullopt then use the
-    /// default value.
-    /// \param[in] _pos The position to interpolate.
-    /// \param[in] _default The default value to use if a.index or b.index is
-    /// std::nullopt.
-    /// Warning: This function assumes that the indices of _a and _b correspond
-    /// to values in _lst. It performs no bounds checking whatsoever and if you
-    /// pass it invalid data, it will crash.
-    template<typename T, typename V>
-    V BiLinearInterpolate(
-      const std::vector<InterpolationPoint3D<T>> &_a,
-      const std::size_t &_start_index,
-      const std::vector<V>  &_lst,
-      const Vector3<T> &_pos,
-      const V &_default = V(0))
-    {
-      /// Assertions
-      #ifndef NDEBUG
-      assert(_a.size() >= _start_index + 4);
-      /// Check if all points co planar.
-      auto planeNormal = (_a[_start_index + 1].position -
-        _a[_start_index].position).Cross(_a[_start_index + 2].position -
-        _a[_start_index].position);
-      auto planeScalar = planeNormal.Dot(_a[_start_index].position);
-      assert(
-        !(std::abs(
-            planeNormal.Dot(_a[_start_index + 3].position) - planeScalar) > 0)
-        );
-      #endif
+  /// \brief Bilinear interpolation of four points in 3D space. It assumes
+  /// these 4 points form a plane.
+  /// \param[in] _a The list of points to interpolate. The list must have at
+  /// least 4 entries. Furthermore the order of the indices should be such
+  /// that every consecutive pair of indices lie on the same edge. Furthermore
+  /// the 4 points must be coplanar and corners of a rectangular patch.
+  /// \param[in] _start_index The starting index of points to interpolate.
+  /// \param[in] _lst An array of values that are to be used by the
+  /// interpolator. _lst[a.index] and _lst[b.index] are the values
+  /// to be interpolated. If a.index or b.index is std::nullopt then use the
+  /// default value.
+  /// \param[in] _pos The position to interpolate.
+  /// \param[in] _default The default value to use if a.index or b.index is
+  /// std::nullopt.
+  /// Warning: This function assumes that the indices of _a and _b correspond
+  /// to values in _lst. It performs no bounds checking whatsoever and if you
+  /// pass it invalid data, it will crash.
+  template<typename T, typename V>
+  V BiLinearInterpolate(
+    const std::vector<InterpolationPoint3D<T>> &_a,
+    const std::size_t &_start_index,
+    const std::vector<V>  &_lst,
+    const Vector3<T> &_pos,
+    const V &_default = V(0))
+  {
+    /// Assertions
+    #ifndef NDEBUG
+    assert(_a.size() >= _start_index + 4);
+    /// Check if all points co planar.
+    auto planeNormal = (_a[_start_index + 1].position -
+      _a[_start_index].position).Cross(_a[_start_index + 2].position -
+      _a[_start_index].position);
+    auto planeScalar = planeNormal.Dot(_a[_start_index].position);
+    assert(
+      !(std::abs(
+          planeNormal.Dot(_a[_start_index + 3].position) - planeScalar) > 0)
+      );
+    #endif
 
-      // Project point onto line
-      std::vector<V> linres;
-      auto n0 = _a[_start_index];
-      auto n1 = _a[_start_index + 1];
-      auto proj1 = n1.position-n0.position;
-      auto unitProj1 = proj1.Normalized();
-      auto pos1 =
-        (_pos - n0.position).Dot(unitProj1) * unitProj1 + n0.position;
-      // Apply normal linear interpolation
-      linres.push_back(LinearInterpolate(n0, n1, _lst, pos1, _default));
+    // Project point onto line
+    std::vector<V> linres;
+    auto n0 = _a[_start_index];
+    auto n1 = _a[_start_index + 1];
+    auto proj1 = n1.position-n0.position;
+    auto unitProj1 = proj1.Normalized();
+    auto pos1 =
+      (_pos - n0.position).Dot(unitProj1) * unitProj1 + n0.position;
+    // Apply normal linear interpolation
+    linres.push_back(LinearInterpolate(n0, n1, _lst, pos1, _default));
 
-      // Project point onto second line
-      auto n2 = _a[_start_index + 2];
-      auto n3 = _a[_start_index + 3];
-      auto pos2 =
-        (_pos - n2.position).Dot(unitProj1) * unitProj1 + n2.position;
-      linres.push_back(LinearInterpolate(n2, n3, _lst, pos2, _default));
+    // Project point onto second line
+    auto n2 = _a[_start_index + 2];
+    auto n3 = _a[_start_index + 3];
+    auto pos2 =
+      (_pos - n2.position).Dot(unitProj1) * unitProj1 + n2.position;
+    linres.push_back(LinearInterpolate(n2, n3, _lst, pos2, _default));
 
-      // Perform final linear interpolation
-      InterpolationPoint3D<T> p1 {
-        pos1,
-        0
-      };
+    // Perform final linear interpolation
+    InterpolationPoint3D<T> p1 {
+      pos1,
+      0
+    };
 
-      InterpolationPoint3D<T> p2 {
-        pos2,
-        1
-      };
-      return LinearInterpolate(p1, p2, linres, _pos, _default);
-    }
+    InterpolationPoint3D<T> p2 {
+      pos2,
+      1
+    };
+    return LinearInterpolate(p1, p2, linres, _pos, _default);
+  }
 
-    /// \brief Project Point onto a plane.
-    /// \param[in] _points a vector of min length _start_index + 3. The points
-    /// at _start_index, _start_index + 1 and  _start_index + 2 are used to
-    /// define the plane.
-    /// \param[in] _start_index defines the slice to use.
-    /// \param[in] _pos The position to project onto the plane
-    template<typename T>
-    gz::math::Vector3<T> ProjectPointToPlane(
-      const std::vector<InterpolationPoint3D<T>> _points,
-      const std::size_t &_start_index,
-      const Vector3<T> &_pos)
-    {
-      auto n =
-        (_points[_start_index + 1].position - _points[_start_index].position)
-        .Cross(
-        _points[_start_index + 2].position - _points[_start_index].position);
-      return
-        _pos - n.Dot(_pos - _points[_start_index].position) * n.Normalized();
-    }
+  /// \brief Project Point onto a plane.
+  /// \param[in] _points a vector of min length _start_index + 3. The points
+  /// at _start_index, _start_index + 1 and  _start_index + 2 are used to
+  /// define the plane.
+  /// \param[in] _start_index defines the slice to use.
+  /// \param[in] _pos The position to project onto the plane
+  template<typename T>
+  gz::math::Vector3<T> ProjectPointToPlane(
+    const std::vector<InterpolationPoint3D<T>> _points,
+    const std::size_t &_start_index,
+    const Vector3<T> &_pos)
+  {
+    auto n =
+      (_points[_start_index + 1].position - _points[_start_index].position)
+      .Cross(
+      _points[_start_index + 2].position - _points[_start_index].position);
+    return
+      _pos - n.Dot(_pos - _points[_start_index].position) * n.Normalized();
+  }
 
-    /// \brief Trilinear interpolation of eight points in 3D space. It assumes
-    /// these eight points form a rectangular prism.
-    /// \param[in] _a The list of points to interpolate. The list must have 8
-    /// points. The first 4 points must form a plane as must the last 4.
-    /// The order of the points within a plane should be such that consecutive
-    /// pairs of indices lie on the same edge.
-    /// \param[in] _lst An array of values that are to be used for interpolation
-    /// \param[in] _pos The position to interpolate.
-    /// \param[in] _default The default value to use if a.index or b.index is
-    /// std::nullopt.
-    template<typename T, typename V>
-    V TrilinearInterpolate(
-      const std::vector<InterpolationPoint3D<T>> &_a,
-      const std::vector<V> &_lst,
-      const Vector3<T> &_pos,
-      const V &_default = V(0))
-    {
-      assert(_a.size() == 8);
+  /// \brief Trilinear interpolation of eight points in 3D space. It assumes
+  /// these eight points form a rectangular prism.
+  /// \param[in] _a The list of points to interpolate. The list must have 8
+  /// points. The first 4 points must form a plane as must the last 4.
+  /// The order of the points within a plane should be such that consecutive
+  /// pairs of indices lie on the same edge.
+  /// \param[in] _lst An array of values that are to be used for interpolation
+  /// \param[in] _pos The position to interpolate.
+  /// \param[in] _default The default value to use if a.index or b.index is
+  /// std::nullopt.
+  template<typename T, typename V>
+  V TrilinearInterpolate(
+    const std::vector<InterpolationPoint3D<T>> &_a,
+    const std::vector<V> &_lst,
+    const Vector3<T> &_pos,
+    const V &_default = V(0))
+  {
+    assert(_a.size() == 8);
 
-      std::vector<V> linres;
-      // First plane
-      auto pos1 = ProjectPointToPlane<T>(_a, 0, _pos);
-      linres.push_back(BiLinearInterpolate(_a, 0, _lst, pos1, _default));
+    std::vector<V> linres;
+    // First plane
+    auto pos1 = ProjectPointToPlane<T>(_a, 0, _pos);
+    linres.push_back(BiLinearInterpolate(_a, 0, _lst, pos1, _default));
 
-      // Second plane
-      auto pos2 = ProjectPointToPlane<T>(_a, 4, _pos);
-      linres.push_back(BiLinearInterpolate(_a, 4, _lst, pos2, _default));
+    // Second plane
+    auto pos2 = ProjectPointToPlane<T>(_a, 4, _pos);
+    linres.push_back(BiLinearInterpolate(_a, 4, _lst, pos2, _default));
 
-      // Perform final linear interpolation
-      InterpolationPoint3D<T> p1 {
-        pos1,
-        0
-      };
+    // Perform final linear interpolation
+    InterpolationPoint3D<T> p1 {
+      pos1,
+      0
+    };
 
-      InterpolationPoint3D<T> p2 {
-        pos2,
-        1
-      };
-      return LinearInterpolate(p1, p2, linres, _pos, _default);
-    }
+    InterpolationPoint3D<T> p2 {
+      pos2,
+      1
+    };
+    return LinearInterpolate(p1, p2, linres, _pos, _default);
   }
-}
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_INTERPOLATION_POINT_HH_
diff --git a/include/gz/math/detail/Sphere.hh b/include/gz/math/detail/Sphere.hh
index 865c96598..f3e65d307 100644
--- a/include/gz/math/detail/Sphere.hh
+++ b/include/gz/math/detail/Sphere.hh
@@ -20,9 +20,7 @@
 #include <optional>
 #include "gz/math/Sphere.hh"
 
-namespace gz
-{
-namespace math
+namespace gz::math
 {
 //////////////////////////////////////////////////
 template<typename T>
@@ -186,6 +184,5 @@ T Sphere<T>::DensityFromMass(const T _mass) const
 
   return _mass / this->Volume();
 }
-}
-}
-#endif
+}  // namespace gz::math
+#endif  // GZ_MATH_DETAIL_SPHERE_HH_
diff --git a/include/gz/math/detail/WellOrderedVector.hh b/include/gz/math/detail/WellOrderedVector.hh
index 61dadbc11..e591e6a2e 100644
--- a/include/gz/math/detail/WellOrderedVector.hh
+++ b/include/gz/math/detail/WellOrderedVector.hh
@@ -18,46 +18,42 @@
 #define GZ_MATH_DETAIL_WELLORDERED_VECTOR_HH_
 #include <gz/math/Vector3.hh>
 
-namespace gz
+namespace gz::math
 {
-  namespace math
+  /// \brief Comparator for well-ordering vectors.
+  template<typename T>
+  struct WellOrderedVectors
   {
-    /// \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
     {
-      /// \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])
       {
-        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[0], _b[0], 1e-3))
+        else if (equal<T>(_a[1], _b[1], 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 _a[2] < _b[2];
+        }
+        else
+        {
+          return false;
         }
-        return false;
       }
-    };
-  }
-}
-
-#endif
+      return false;
+    }
+  };
+}  // namespace gz::math
+#endif  // GZ_MATH_DETAIL_WELLORDERED_VECTOR_HH_
diff --git a/include/gz/math/graph/Edge.hh b/include/gz/math/graph/Edge.hh
index 92f4b937c..ecdf973f6 100644
--- a/include/gz/math/graph/Edge.hh
+++ b/include/gz/math/graph/Edge.hh
@@ -27,9 +27,7 @@
 #include <gz/math/config.hh>
 #include "gz/math/graph/Vertex.hh"
 
-namespace gz
-{
-namespace math
+namespace gz::math
 {
 // Inline bracket to help doxygen filtering.
 inline namespace GZ_MATH_VERSION_NAMESPACE {
@@ -335,8 +333,7 @@ namespace graph
   template<typename E>
   DirectedEdge<E> DirectedEdge<E>::NullEdge(
     VertexId_P(kNullId, kNullId), E(), 1.0, kNullId);
-}
-}
-}
-}
-#endif
+}  // namespace graph
+}  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_GRAPH_EDGE_HH_
diff --git a/include/gz/math/graph/Graph.hh b/include/gz/math/graph/Graph.hh
index 0b48ea2dc..d3b23726e 100644
--- a/include/gz/math/graph/Graph.hh
+++ b/include/gz/math/graph/Graph.hh
@@ -29,9 +29,7 @@
 #include "gz/math/graph/Edge.hh"
 #include "gz/math/graph/Vertex.hh"
 
-namespace gz
-{
-namespace math
+namespace gz::math
 {
 // Inline bracket to help doxygen filtering.
 inline namespace GZ_MATH_VERSION_NAMESPACE {
@@ -815,8 +813,7 @@ namespace graph
   /// \brief A directed graph.
   template<typename V, typename E>
   using DirectedGraph = Graph<V, E, DirectedEdge<E>>;
-}
-}
-}
-}
-#endif
+}  // namespace graph
+}  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math::graph
+#endif  // GZ_MATH_GRAPH_GRAPH_HH_
diff --git a/include/gz/math/graph/GraphAlgorithms.hh b/include/gz/math/graph/GraphAlgorithms.hh
index 1dde695e4..9d3c77fec 100644
--- a/include/gz/math/graph/GraphAlgorithms.hh
+++ b/include/gz/math/graph/GraphAlgorithms.hh
@@ -29,9 +29,7 @@
 #include "gz/math/graph/Graph.hh"
 #include "gz/math/Helpers.hh"
 
-namespace gz
-{
-namespace math
+namespace gz::math
 {
 // Inline bracket to help doxygen filtering.
 inline namespace GZ_MATH_VERSION_NAMESPACE {
@@ -353,8 +351,7 @@ namespace graph
 
     return UndirectedGraph<V, E>(vertices, edges);
   }
-}
-}
-}
-}
-#endif
+}  // namespace graph
+}  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif   // GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_
diff --git a/include/gz/math/graph/Vertex.hh b/include/gz/math/graph/Vertex.hh
index e61fa22df..7d99c373c 100644
--- a/include/gz/math/graph/Vertex.hh
+++ b/include/gz/math/graph/Vertex.hh
@@ -28,9 +28,7 @@
 #include <gz/math/config.hh>
 #include <gz/math/Helpers.hh>
 
-namespace gz
-{
-namespace math
+namespace gz::math
 {
 // Inline bracket to help doxygen filtering.
 inline namespace GZ_MATH_VERSION_NAMESPACE {
@@ -145,8 +143,7 @@ namespace graph
   template<typename V>
   using VertexRef_M =
     std::map<VertexId, std::reference_wrapper<const Vertex<V>>>;
-}
-}
-}
-}
-#endif
+}  // namespace graph
+}  // namespace GZ_MATH_VERSION_NAMESPACE
+}  // namespace gz::math
+#endif  // GZ_MATH_GRAPH_VERTEX_HH_