Compute distance between points on a general celestial body (#434)

* Added distance calculation method considering the radius, deprecated old distance method, updated python bindings

Signed-off-by: Aditya <aditya050995@gmail.com>

Co-authored-by: Louise Poubel <louise@openrobotics.org>
Co-authored-by: methylDragon <methylDragon@gmail.com>
Co-authored-by: Michael Carroll <michael@openrobotics.org>

Author: 4 people

Migration.md

@@ -24,6 +24,10 @@ release will remove the deprecated code.
     + All mutator functions that lacked a `Set` prefix have been deprecated
     and replaced by version with a `Set` prefix.
 
+1. **SphericalCoordinates.hh**
+    + ***Deprecation:*** public: static double Distance(const Angle&, const Angle&, const Angle&, const Angle&)
+    + ***Replacement:*** public: static double DistanceWGS84(const Angle&, const Angle&, const Angle&, const Angle&)
+
 1. **Matrix3.hh**
     + ***Deprecation:*** public: void Axes(const Vector3<T> &, const Vector3<T> &, const Vector3<T> &)
     + ***Replacement:*** public: void SetAxes(const Vector3<T> &, const Vector3<T> &, const Vector3<T> &)

include/gz/math/SphericalCoordinates.hh

@@ -41,7 +41,15 @@ namespace gz
               {
                 /// \brief Model of reference ellipsoid for earth, based on
                 /// WGS 84 standard. see wikipedia: World_Geodetic_System
-                EARTH_WGS84 = 1
+                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
@@ -73,6 +81,16 @@ namespace gz
       /// \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.
@@ -85,7 +103,6 @@ namespace gz
                                    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.
       ///
@@ -136,15 +153,66 @@ namespace gz
       /// \param[in] _latB Latitude of point B.
       /// \param[in] _lonB Longitude of point B.
       /// \return Distance in meters.
-      public: static double Distance(const gz::math::Angle &_latA,
+      /// \deprecated Use DistanceWGS84 instead.
+      public: GZ_DEPRECATED(7) static double Distance(
+                                     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 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();
+
+      /// \brief Get the major axis of the surface.
+      /// \return Equatorial axis of the surface in use.
+      public: double SurfaceAxisEquatorial();
+
+      /// \brief Get the minor axis of the surface.
+      /// \return Polar axis of the surface in use.
+      public: double SurfaceAxisPolar();
+
+      /// \brief Get the flattening of the surface.
+      /// \return Flattening parameter of the surface in use.
+      public: double SurfaceFlattening();
+
       /// \brief Get reference geodetic latitude.
       /// \return Reference geodetic latitude.
       public: gz::math::Angle LatitudeReference() const;
@@ -167,6 +235,16 @@ namespace gz
       /// \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);

src/SphericalCoordinates.cc

@@ -38,12 +38,31 @@ const double g_EarthWGS84Flattening = 1.0/298.257223563;
 // Radius of the Earth (meters).
 const double g_EarthRadius = 6371000.0;
 
+// Radius of the Moon (meters).
+// Source: https://lunar.gsfc.nasa.gov/library/451-SCI-000958.pdf
+const double g_MoonRadius = 1737400.0;
+
+// a: Equatorial radius of the Moon.
+// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
+const double g_MoonAxisEquatorial = 1738100.0;
+
+// b: Polar radius of the Moon.
+// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
+const double g_MoonAxisPolar = 1736000.0;
+
+// if: Unitless flattening parameter for the Moon.
+// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
+const double g_MoonFlattening = 0.0012;
+
 // Private data for the SphericalCoordinates class.
 class gz::math::SphericalCoordinates::Implementation
 {
   /// \brief Type of surface being used.
   public: SphericalCoordinates::SurfaceType surfaceType;
 
+  /// \brief Radius of the given SurfaceType.
+  public: double surfaceRadius = 0;
+
   /// \brief Latitude of reference point.
   public: gz::math::Angle latitudeReference;
 
@@ -94,6 +113,10 @@ SphericalCoordinates::SurfaceType SphericalCoordinates::Convert(
 {
   if ("EARTH_WGS84" == _str)
     return EARTH_WGS84;
+  else if ("MOON_SCS" == _str)
+    return MOON_SCS;
+  else if ("CUSTOM_SURFACE" == _str)
+    return CUSTOM_SURFACE;
 
   std::cerr << "SurfaceType string not recognized, "
     << "EARTH_WGS84 returned by default" << std::endl;
@@ -106,6 +129,10 @@ std::string SphericalCoordinates::Convert(
 {
   if (_type == EARTH_WGS84)
     return "EARTH_WGS84";
+  else if (_type == MOON_SCS)
+    return "MOON_SCS";
+  else if (_type == CUSTOM_SURFACE)
+    return "CUSTOM_SURFACE";
 
   std::cerr << "SurfaceType not recognized, "
     << "EARTH_WGS84 returned by default" << std::endl;
@@ -128,6 +155,20 @@ SphericalCoordinates::SphericalCoordinates(const SurfaceType _type)
   this->SetElevationReference(0.0);
 }
 
+//////////////////////////////////////////////////
+SphericalCoordinates::SphericalCoordinates(
+    const SurfaceType _type,
+    const double _axisEquatorial,
+    const double _axisPolar)
+  : SphericalCoordinates()
+{
+  // Set properties
+  this->SetSurface(_type, _axisEquatorial,
+      _axisPolar);
+
+  this->SetElevationReference(0.0);
+}
+
 //////////////////////////////////////////////////
 SphericalCoordinates::SphericalCoordinates(const SurfaceType _type,
     const gz::math::Angle &_latitude,
@@ -208,6 +249,42 @@ void SphericalCoordinates::SetSurface(const SurfaceType &_type)
           std::pow(this->dataPtr->ellA, 2) / std::pow(this->dataPtr->ellB, 2) -
           1.0);
 
+      // Set the radius of the surface.
+      this->dataPtr->surfaceRadius = g_EarthRadius;
+
+      break;
+      }
+    case MOON_SCS:
+      {
+      // Set the semi-major axis
+      this->dataPtr->ellA = g_MoonAxisEquatorial;
+
+      // Set the semi-minor axis
+      this->dataPtr->ellB = g_MoonAxisPolar;
+
+      // Set the flattening parameter
+      this->dataPtr->ellF = g_MoonFlattening;
+
+      // Set the first eccentricity ellipse parameter
+      // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
+      this->dataPtr->ellE = sqrt(1.0 -
+          std::pow(this->dataPtr->ellB, 2) / std::pow(this->dataPtr->ellA, 2));
+
+      // Set the second eccentricity ellipse parameter
+      // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
+      this->dataPtr->ellP = sqrt(
+          std::pow(this->dataPtr->ellA, 2) / std::pow(this->dataPtr->ellB, 2) -
+          1.0);
+
+      // Set the radius of the surface.
+      this->dataPtr->surfaceRadius = g_MoonRadius;
+
+      break;
+      }
+    case CUSTOM_SURFACE:
+      {
+      std::cerr << "For custom surfaces, use SetSurface(type, radius,"
+        "axisEquatorial, axisPolar)" << std::endl;
       break;
       }
     default:
@@ -219,6 +296,53 @@ void SphericalCoordinates::SetSurface(const SurfaceType &_type)
   }
 }
 
+//////////////////////////////////////////////////
+void SphericalCoordinates::SetSurface(
+    const SurfaceType &_type,
+    const double _axisEquatorial,
+    const double _axisPolar)
+{
+  if ((_type != EARTH_WGS84) &&
+      (_type != MOON_SCS) &&
+      (_type != CUSTOM_SURFACE))
+  {
+    std::cerr << "Unknown surface type["
+      << _type << "]\n";
+    return;
+  }
+
+  this->dataPtr->surfaceType = _type;
+
+  if ((_axisEquatorial > 0)
+      && (_axisPolar > 0)
+      && (_axisPolar <= _axisEquatorial))
+  {
+    this->dataPtr->ellA = _axisEquatorial;
+    this->dataPtr->ellB = _axisPolar;
+    this->dataPtr->ellF =
+      (_axisEquatorial - _axisPolar) / _axisEquatorial;
+    // Arithmetic mean radius
+    this->dataPtr->surfaceRadius =
+      (2 * _axisEquatorial + _axisPolar) / 3.0;
+  }
+  else
+  {
+    std::cerr << "Invalid parameters found, defaulting to "
+      "Earth's parameters" << std::endl;
+
+    this->dataPtr->ellA = g_EarthWGS84AxisEquatorial;
+    this->dataPtr->ellB = g_EarthWGS84AxisPolar;
+    this->dataPtr->ellF = g_EarthWGS84Flattening;
+    this->dataPtr->surfaceRadius = g_EarthRadius;
+  }
+
+  this->dataPtr->ellE = sqrt(1.0 -
+      std::pow(this->dataPtr->ellB, 2) / std::pow(this->dataPtr->ellA, 2));
+  this->dataPtr->ellP = sqrt(
+      std::pow(this->dataPtr->ellA, 2) / std::pow(this->dataPtr->ellB, 2) -
+      1.0);
+}
+
 //////////////////////////////////////////////////
 void SphericalCoordinates::SetLatitudeReference(
     const gz::math::Angle &_angle)
@@ -286,7 +410,7 @@ gz::math::Vector3d SphericalCoordinates::LocalFromGlobalVelocity(
 
 //////////////////////////////////////////////////
 /// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula).
-double SphericalCoordinates::Distance(const gz::math::Angle &_latA,
+double SphericalCoordinates::DistanceWGS84(const gz::math::Angle &_latA,
                                       const gz::math::Angle &_lonA,
                                       const gz::math::Angle &_latB,
                                       const gz::math::Angle &_lonB)
@@ -303,6 +427,62 @@ double SphericalCoordinates::Distance(const gz::math::Angle &_latA,
   return d;
 }
 
+//////////////////////////////////////////////////
+/// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula).
+double SphericalCoordinates::Distance(const gz::math::Angle &_latA,
+                                      const gz::math::Angle &_lonA,
+                                      const gz::math::Angle &_latB,
+                                      const gz::math::Angle &_lonB)
+{
+  return gz::math::SphericalCoordinates::DistanceWGS84(
+      _latA, _lonA, _latB, _lonB);
+}
+
+//////////////////////////////////////////////////
+/// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula).
+/// This takes into account the surface type.
+double SphericalCoordinates::DistanceBetweenPoints(
+    const gz::math::Angle &_latA,
+    const gz::math::Angle &_lonA,
+    const gz::math::Angle &_latB,
+    const gz::math::Angle &_lonB)
+{
+  gz::math::Angle dLat = _latB - _latA;
+  gz::math::Angle dLon = _lonB - _lonA;
+
+  double a = sin(dLat.Radian() / 2) * sin(dLat.Radian() / 2) +
+             sin(dLon.Radian() / 2) * sin(dLon.Radian() / 2) *
+             cos(_latA.Radian()) * cos(_latB.Radian());
+
+  double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+  double d = this->dataPtr->surfaceRadius * c;
+  return d;
+}
+
+//////////////////////////////////////////////////
+double SphericalCoordinates::SurfaceRadius()
+{
+  return this->dataPtr->surfaceRadius;
+}
+
+//////////////////////////////////////////////////
+double SphericalCoordinates::SurfaceAxisEquatorial()
+{
+  return this->dataPtr->ellA;
+}
+
+//////////////////////////////////////////////////
+double SphericalCoordinates::SurfaceAxisPolar()
+{
+  return this->dataPtr->ellB;
+}
+
+//////////////////////////////////////////////////
+double SphericalCoordinates::SurfaceFlattening()
+{
+  return this->dataPtr->ellF;
+}
+
 //////////////////////////////////////////////////
 void SphericalCoordinates::UpdateTransformationMatrix()
 {