Add Capsule class with inertia calculation method

CMakeLists.txt

@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(ignition-math6 VERSION 6.6.0)
+project(ignition-math6 VERSION 6.7.0)
 
 #============================================================================
 # Find ignition-cmake
@@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.0.0 REQUIRED)
 # Configure the project
 #============================================================================
 set (c++standard 17)
-ign_configure_project()
+ign_configure_project(VERSION_SUFFIX pre1)
 
 #============================================================================
 # Set project-specific options

include/ignition/math/Capsule.hh

@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_MATH_CAPSULE_HH_
+#define IGNITION_MATH_CAPSULE_HH_
+
+#include <optional>
+#include "ignition/math/MassMatrix3.hh"
+#include "ignition/math/Material.hh"
+
+namespace ignition
+{
+  namespace math
+  {
+    // Foward declarations
+    class CapsulePrivate;
+
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
+    //
+    /// \class Capsule Capsule.hh ignition/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;
+    }
+  }
+}
+#include "ignition/math/detail/Capsule.hh"
+
+#endif

include/ignition/math/Cylinder.hh

@@ -32,7 +32,7 @@ namespace ignition
     inline namespace IGNITION_MATH_VERSION_NAMESPACE {
     //
     /// \class Cylinder Cylinder.hh ignition/math/Cylinder.hh
-    /// \brief A represntation of a cylinder.
+    /// \brief A representation of a cylinder.
     ///
     /// The cylinder class supports defining a cylinder with a radius,
     /// length, rotational offset, and material properties. Radius and

include/ignition/math/MassMatrix3.hh

@@ -1069,7 +1069,7 @@ namespace ignition
                             const Quaternion<T> &_rot = Quaternion<T>::Identity)
       {
         // Check that _mass and _size are strictly positive
-        // and that quatenion is valid
+        // and that quaternion is valid
         if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
         {
           return false;
@@ -1087,7 +1087,7 @@ namespace ignition
                             const Quaternion<T> &_rot = Quaternion<T>::Identity)
       {
         // Check that _mass and _size are strictly positive
-        // and that quatenion is valid
+        // and that quaternion is valid
         if (this->Mass() <= 0 || _size.Min() <= 0 ||
             _rot == Quaternion<T>::Zero)
         {
@@ -1120,7 +1120,7 @@ namespace ignition
                             const Quaternion<T> &_rot = Quaternion<T>::Identity)
       {
         // Check that density, _radius and _length are strictly positive
-        // and that quatenion is valid
+        // and that quaternion is valid
         if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
             _rot == Quaternion<T>::Zero)
         {
@@ -1144,7 +1144,7 @@ namespace ignition
                             const Quaternion<T> &_rot = Quaternion<T>::Identity)
       {
         // Check that _mass, _radius and _length are strictly positive
-        // and that quatenion is valid
+        // and that quaternion is valid
         if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
             _rot == Quaternion<T>::Zero)
         {
@@ -1165,7 +1165,7 @@ namespace ignition
                                     const Quaternion<T> &_rot)
       {
         // Check that _mass and _size are strictly positive
-        // and that quatenion is valid
+        // and that quaternion is valid
         if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
             _rot == Quaternion<T>::Zero)
         {

include/ignition/math/detail/Capsule.hh

@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_MATH_DETAIL_CAPSULE_HH_
+#define IGNITION_MATH_DETAIL_CAPSULE_HH_
+
+#include <limits>
+#include <optional>
+#include <ignition/math/Helpers.hh>
+#include <ignition/math/Inertial.hh>
+
+namespace ignition
+{
+namespace math
+{
+
+//////////////////////////////////////////////////
+template<typename T>
+Capsule<T>::Capsule(const T _length, const T _radius)
+{
+  this->length = _length;
+  this->radius = _radius;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+Capsule<T>::Capsule(const T _length, const T _radius, const Material &_mat)
+{
+  this->length = _length;
+  this->radius = _radius;
+  this->material = _mat;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+T Capsule<T>::Radius() const
+{
+  return this->radius;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+void Capsule<T>::SetRadius(const T _radius)
+{
+  this->radius = _radius;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+T Capsule<T>::Length() const
+{
+  return this->length;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+void Capsule<T>::SetLength(const T _length)
+{
+  this->length = _length;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+const Material &Capsule<T>::Mat() const
+{
+  return this->material;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+void Capsule<T>::SetMat(const Material &_mat)
+{
+  this->material = _mat;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+bool Capsule<T>::operator==(const Capsule &_capsule) const
+{
+  return equal(this->radius, _capsule.Radius()) &&
+    equal(this->length, _capsule.Length()) &&
+    this->material == _capsule.Mat();
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+std::optional< MassMatrix3<T> > Capsule<T>::MassMatrix() const
+{
+  // mass and moment of inertia of cylinder about centroid
+  MassMatrix3<T> cylinder;
+  cylinder.SetFromCylinderZ(this->material, this->length, this->radius);
+
+  // mass and moment of inertia of hemisphere about centroid
+  const T r2 = this->radius * this->radius;
+  const T hemisphereMass = this->material.Density() *
+      2. / 3. * IGN_PI * r2 * this->radius;
+  // efunda.com/math/solids/solids_display.cfm?SolidName=Hemisphere
+  const T ixx = 83. / 320. * hemisphereMass * r2;
+  const T izz = 2. / 5.  * hemisphereMass * r2;
+  MassMatrix3<T> hemisphere(hemisphereMass, Vector3<T>(ixx, ixx, izz),
+                            Vector3<T>::Zero);;
+
+  // Distance from centroid of cylinder to centroid of hemisphere,
+  // since centroid of hemisphere is 3/8 radius from its flat base
+  const T dz = this->length / 2. + this->radius * 3. / 8.;
+  Inertial<T> upperCap(hemisphere, Pose3<T>(0, 0, dz, 0, 0, 0));
+  Inertial<T> lowerCap(hemisphere, Pose3<T>(0, 0, -dz, 0, 0, 0));
+
+  // Use Inertial class to add MassMatrix3 objects at different poses
+  Inertial<T> capsule =
+      Inertial<T>(cylinder, Pose3<T>::Zero) + upperCap + lowerCap;
+
+  if (!capsule.MassMatrix().IsValid())
+  {
+    return std::nullopt;
+  }
+
+  return std::make_optional(capsule.MassMatrix());
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+T Capsule<T>::Volume() const
+{
+  return IGN_PI * std::pow(this->radius, 2) *
+         (this->length + 4. / 3. * this->radius);
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+bool Capsule<T>::SetDensityFromMass(const T _mass)
+{
+  T newDensity = this->DensityFromMass(_mass);
+  if (isnan(newDensity))
+    return false;
+
+  this->material.SetDensity(newDensity);
+  return true;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+T Capsule<T>::DensityFromMass(const T _mass) const
+{
+  if (this->radius <= 0 || this->length <=0 || _mass <= 0)
+    return std::numeric_limits<T>::quiet_NaN();
+
+  return _mass / this->Volume();
+}
+
+}
+}
+#endif