Add Feature to Get/Set Gravity vector in a World

Signed-off-by: Steven Peters <scpeters@openrobotics.org>

include/ignition/physics/World.hh

@@ -21,6 +21,7 @@
 #include <string>
 
 #include <ignition/physics/FeatureList.hh>
+#include <ignition/physics/FrameSemantics.hh>
 
 namespace ignition
 {
@@ -61,6 +62,81 @@ namespace ignition
       };
     };
 
+    /////////////////////////////////////////////////
+    using GravityRequiredFeatures = FeatureList<FrameSemantics>;
+
+    /////////////////////////////////////////////////
+    /// \brief Get and set the World's gravity vector in a specified frame.
+    class IGNITION_PHYSICS_VISIBLE Gravity
+        : public virtual
+          FeatureWithRequirements<GravityRequiredFeatures>
+    {
+      /// \brief The World API for getting and setting the gravity vector.
+      public: template <typename PolicyT, typename FeaturesT>
+      class World : public virtual Feature::World<PolicyT, FeaturesT>
+      {
+        public: using LinearVectorType =
+            typename FromPolicy<PolicyT>::template Use<LinearVector>;
+
+        public: using RelativeGravityType = RelativeQuantity<
+            LinearVectorType, PolicyT::Dim,
+            detail::VectorSpace<typename PolicyT::Scalar, PolicyT::Dim>>;
+
+        /// \brief Set the World gravity vector.
+        /// \param[in] _gravity The desired gravity as a Relative Gravity
+        /// (a quantity that contains information about the coordinates
+        /// in which it is expressed).
+        public: void SetGravity(const RelativeGravityType &_gravity);
+
+        /// \brief Set the World gravity vector. Optionally, you may specify
+        /// the frame whose coordinates are used to express the gravity vector.
+        /// The World frame is used as a default if no frame is specified.
+        /// \param[in] _gravity Gravity vector.
+        /// \param[in] _inCoordinatesOf Frame whose coordinates are used
+        /// to express _gravity.
+        public: void SetGravity(
+            const LinearVectorType &_gravity,
+            const FrameID &_forceInCoordinatesOf = FrameID::World());
+
+        /// \brief Get the World gravity vector as a Relative Gravity
+        /// (a quantity that contains information about the coordinates
+        /// in which it is expressed).
+        /// \return Relative Gravity vector.
+        public: RelativeGravityType GetGravity() const;
+
+        /// \brief Get the World gravity vector. Optionally, you may specify
+        /// the frame whose coordinates are used to express the gravity vector.
+        /// The World frame is used as a default if no frame is specified.
+        /// \param[in] _inCoordinatesOf Frame whose coordinates are used
+        /// to express _gravity.
+        /// \return Gravity vector in corrdinates of _inCoordinatesOf.
+        public: LinearVectorType GetGravity(
+            const FrameID &_forceInCoordinatesOf = FrameID::World()) const;
+      };
+
+      /// \private The implementation API for the gravity.
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: using LinearVectorType =
+            typename FromPolicy<PolicyT>::template Use<LinearVector>;
+
+        /// \brief Implementation API for setting the gravity vector, which is
+        /// expressed in the World frame..
+        /// \param[in] _id Identity of the world.
+        /// \param[in] _gravity Name of gravity.
+        public: virtual void SetWorldGravity(
+            const Identity &_id, const LinearVectorType &_gravity) = 0;
+
+        /// \brief Implementation API for getting the gravity expressed in the
+        /// world frame.
+        /// \param[in] _id Identity of the world.
+        /// \return Name of gravity.
+        public: virtual LinearVectorType GetWorldGravity(
+            const Identity &_id) const = 0;
+      };
+    };
+
     /////////////////////////////////////////////////
     class IGNITION_PHYSICS_VISIBLE Solver : public virtual Feature
     {

include/ignition/physics/detail/World.hh

@@ -44,6 +44,72 @@ const std::string &CollisionDetector::World<PolicyT, FeaturesT>::
       ->GetWorldCollisionDetector(this->identity);
 }
 
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+void Gravity::World<PolicyT, FeaturesT>::SetGravity(
+    const RelativeGravityType &_gravity)
+{
+  // Resolve to world coordinates
+  auto &impl = *this->template Interface<FrameSemantics>();
+  auto gravityInWorld =
+      detail::Resolve(impl, _gravity, FrameID::World(), FrameID::World());
+
+  this->template Interface<Gravity>()
+      ->SetWorldGravity(this->identity, gravityInWorld);
+}
+
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+void Gravity::World<PolicyT, FeaturesT>::SetGravity(
+    const LinearVectorType &_gravity,
+    const FrameID &_inCoordinatesOf)
+{
+  // Call SetWorldGravity directly if using world coordinates
+  if (_inCoordinatesOf == FrameID::World())
+  {
+    this->template Interface<Gravity>()
+        ->SetWorldGravity(this->identity, _gravity);
+  }
+  // Otherwise make a RelativeGravity object and call the other API
+  else
+  {
+    RelativeGravityType gravityInRef(_inCoordinatesOf, _gravity);
+    this->SetGravity(gravityInRef);
+  }
+}
+
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+auto Gravity::World<PolicyT, FeaturesT>::GetGravity() const
+    -> RelativeGravityType
+{
+  return RelativeGravityType(
+      FrameID::World(),
+      this->template Interface<Gravity>()
+          ->GetWorldGravity(this->identity));
+}
+
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+auto Gravity::World<PolicyT, FeaturesT>::GetGravity(
+    const FrameID &_inCoordinatesOf) const
+    -> LinearVectorType
+{
+  // Return quickly if using world coordinates
+  auto gravityInWorld = this->template Interface<Gravity>()
+                            ->GetWorldGravity(this->identity);
+  if (_inCoordinatesOf == FrameID::World())
+  {
+    return gravityInWorld;
+  }
+
+  // Otherwise resolve to proper frame
+  auto &impl = *this->template Interface<FrameSemantics>();
+  RelativeGravityType gravityInRef(FrameID::World(), gravityInWorld);
+  return detail::Resolve(impl, gravityInRef, FrameID::World(),
+                         _inCoordinatesOf);
+}
+
 /////////////////////////////////////////////////
 template <typename PolicyT, typename FeaturesT>
 void Solver::World<PolicyT, FeaturesT>::SetSolver(