Removed the workaround LOCAL2 coordinate system from SphericalCoordinates.

Migration.md

@@ -7,6 +7,19 @@ release will remove the deprecated code.
 
 ## Gazebo Math 8.X to 9.X
 
+### Additions
+
+1. **SphericalCoordinates.hh**
+    + `SphericalCoordinates::SphericalPositionFromLocal()`.
+    + `SphericalCoordinates::LocalPositionFromSpherical()`.
+    + `SphericalCoordinates::GlobalVelocityFromLocal()`.
+    + `SphericalCoordinates::LocalVelocityFromGlobal()`.
+    + Enum value `SphericalCoordinates::CoordinateType::LOCAL_TANGENT`
+      which substitutes the wrong `LOCAL` coordinate and improves the
+      name of `LOCAL2` (it is its functional equivalent).
+    + These functions use the newly introduced `LOCAL_TANGENT`
+      coordinate type instead of the wrong `LOCAL` frame.
+
 ### Deprecations
 
 1. **graph/Vertex.hh**
@@ -19,6 +32,27 @@ release will remove the deprecated code.
       `Vertex::NullEdge()` instead.
       E.g.: https://github.com/gazebosim/gz-math/pull/606/files#diff-0c0220a7e72be70337975433eeddc3f5e072ade5cd80dfb1ac03da233c39c983L222-R222
 
+1. **SphericalCoordinates.hh**
+    + Deprecated `LOCAL` and `LOCAL2` values of enum
+      `SphericalCoordinates::CoordinateType`. Use `LOCAL_TANGENT`
+      instead. When replacing `LOCAL`, you have to rotate the output by
+      180 degrees in heading.
+    + Deprecated `SphericalCoordinates::SphericalFromLocalPosition` which
+      gives incorrect results.
+      Use `SphericalCoordinates::SphericalPositionFromLocal` instead.
+    + Deprecated `SphericalCoordinates::GlobalFromLocalVelocity` which
+      gives incorrect results (WSU frame instead of ENU).
+      Use `SphericalCoordinates::GlobalVelocityFromLocal` instead and
+      negate the `x` component of the result.
+    + Deprecated `SphericalCoordinates::LocalFromSphericalPosition` to
+      unify naming.
+      Use `SphericalCoordinates::LocalPositionFromSpherical` instead (it
+      yields identical results).
+    + Deprecated `SphericalCoordinates::LocalFromGlobalVelocity` to
+      unify naming.
+      Use `SphericalCoordinates::LocalVelocityFromGlobal` instead (it
+      yields identical results).
+
 ## Gazebo Math 7.X to 8.X
 
 ### Breaking Changes

include/gz/math/SphericalCoordinates.hh

@@ -25,6 +25,13 @@
 #include <gz/math/config.hh>
 #include <gz/utils/ImplPtr.hh>
 
+// MSVC doesn't like deprecating enum values with function macros
+#if _MSC_VER
+#define GZ_LOCAL_DEPRECATED
+#else
+#define GZ_LOCAL_DEPRECATED GZ_DEPRECATED(8)
+#endif
+
 namespace gz::math
 {
   // Inline bracket to help doxygen filtering.
@@ -65,11 +72,14 @@ namespace gz::math
 
               /// \brief Heading-adjusted tangent plane (X, Y, Z)
               /// This has kept a bug for backwards compatibility, use
-              /// LOCAL2 for the correct behaviour.
-              LOCAL = 4,
+              /// LOCAL_TANGENT for the correct behaviour.
+              LOCAL GZ_LOCAL_DEPRECATED = 4,
+
+              /// \brief Equivalent to LOCAL_TANGENT.
+              LOCAL2 GZ_LOCAL_DEPRECATED = 5,
 
               /// \brief Heading-adjusted tangent plane (X, Y, Z)
-              LOCAL2 = 5
+              LOCAL_TANGENT = 5
             };
 
     /// \brief Constructor.
@@ -104,31 +114,50 @@ namespace gz::math
     /// \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.
+    /// \deprecated This function returns incorrect results.
+    ///             Call `SphericalPositionFromLocal` for correct results.
+    ///             Please note the results of this function are in a wrong
+    ///             frame (West-Sout-Up instead of East-North-Up).
     ///
     /// \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(
+    /// \return Cooordinates: geodetic latitude (deg West),
+    ///         longitude (deg South), altitude above sea level (m).
+    public: GZ_DEPRECATED(8) 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.
+    ///        to a global Cartesian frame with components West, South, 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.
+    /// \deprecated This function returns incorrect results.
+    ///             Call `GlobalVelocityFromLocal` for correct results.
+    ///
+    /// \param[in] _xyz Cartesian velocity vector in the heading-adjusted
+    /// world frame.
+    /// \return Rotated vector with components (x,y,z): (West, South, Up).
+    public: GZ_DEPRECATED(8) gz::math::Vector3d GlobalFromLocalVelocity(
+                const gz::math::Vector3d &_xyz) const;
+
+    /// \brief Convert a Cartesian position vector to geodetic coordinates.
+    /// This performs a `PositionTransform` from LOCAL_TANGENT to SPHERICAL.
+    ///
+    /// \param[in] _xyz Cartesian position vector in the heading-adjusted
+    /// world frame.
+    /// \return Cooordinates: geodetic latitude (deg East),
+    ///         longitude (deg North), altitude above sea level (m).
+    public: gz::math::Vector3d SphericalPositionFromLocal(
+                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_TANGENT, GLOBAL)`
     ///
     /// \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(
+    public: gz::math::Vector3d GlobalVelocityFromLocal(
                 const gz::math::Vector3d &_xyz) const;
 
     /// \brief Convert a string to a SurfaceType.
@@ -243,20 +272,40 @@ namespace gz::math
     /// \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.
+    /// \deprecated Use `LocalPositionFromSpherical` instead (it gives identical
+    ///             results).
+    /// \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_DEPRECATED(8) 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)`
+    /// \deprecated Use `LocalVelocityFromGlobal` instead (it gives identical
+    ///             results).
+    /// \param[in] _xyz Vector with components (x,y,z): (East, North, Up).
+    /// \return Cartesian vector in the world frame.
+    public: GZ_DEPRECATED(8) gz::math::Vector3d LocalFromGlobalVelocity(
+                const gz::math::Vector3d &_xyz) const;
+
     /// \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(
+    public: gz::math::Vector3d LocalPositionFromSpherical(
                 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(
+    public: gz::math::Vector3d LocalVelocityFromGlobal(
                 const gz::math::Vector3d &_xyz) const;
 
     /// \brief Update coordinate transformation matrix with reference location

src/SphericalCoordinates.cc

@@ -377,8 +377,10 @@ void SphericalCoordinates::SetHeadingOffset(const Angle &_angle)
 Vector3d SphericalCoordinates::SphericalFromLocalPosition(
     const Vector3d &_xyz) const
 {
+  GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
   Vector3d result =
     this->PositionTransform(_xyz, LOCAL, SPHERICAL);
+  GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
   result.X(GZ_RTOD(result.X()));
   result.Y(GZ_RTOD(result.Y()));
   return result;
@@ -391,21 +393,62 @@ Vector3d SphericalCoordinates::LocalFromSphericalPosition(
   Vector3d result = _xyz;
   result.X(GZ_DTOR(result.X()));
   result.Y(GZ_DTOR(result.Y()));
+  GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
   return this->PositionTransform(result, SPHERICAL, LOCAL);
+  GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
 }
 
 //////////////////////////////////////////////////
 Vector3d SphericalCoordinates::GlobalFromLocalVelocity(
     const Vector3d &_xyz) const
 {
+  GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
   return this->VelocityTransform(_xyz, LOCAL, GLOBAL);
+  GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
 }
 
 //////////////////////////////////////////////////
 Vector3d SphericalCoordinates::LocalFromGlobalVelocity(
     const Vector3d &_xyz) const
 {
+  GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
   return this->VelocityTransform(_xyz, GLOBAL, LOCAL);
+  GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
+}
+
+//////////////////////////////////////////////////
+Vector3d SphericalCoordinates::SphericalPositionFromLocal(
+    const Vector3d &_xyz) const
+{
+  Vector3d result =
+    this->PositionTransform(_xyz, LOCAL_TANGENT, SPHERICAL);
+  result.X(GZ_RTOD(result.X()));
+  result.Y(GZ_RTOD(result.Y()));
+  return result;
+}
+
+//////////////////////////////////////////////////
+Vector3d SphericalCoordinates::LocalPositionFromSpherical(
+    const Vector3d &_xyz) const
+{
+  Vector3d result = _xyz;
+  result.X(GZ_DTOR(result.X()));
+  result.Y(GZ_DTOR(result.Y()));
+  return this->PositionTransform(result, SPHERICAL, LOCAL_TANGENT);
+}
+
+//////////////////////////////////////////////////
+Vector3d SphericalCoordinates::GlobalVelocityFromLocal(
+    const Vector3d &_xyz) const
+{
+  return this->VelocityTransform(_xyz, LOCAL_TANGENT, GLOBAL);
+}
+
+//////////////////////////////////////////////////
+Vector3d SphericalCoordinates::LocalVelocityFromGlobal(
+    const Vector3d &_xyz) const
+{
+  return this->VelocityTransform(_xyz, GLOBAL, LOCAL_TANGENT);
 }
 
 //////////////////////////////////////////////////
@@ -535,8 +578,10 @@ Vector3d SphericalCoordinates::PositionTransform(
   // Convert whatever arrives to a more flexible ECEF coordinate
   switch (_in)
   {
-    // East, North, Up (ENU), note no break at end of case
+    // West, South, Up (WSU)
+    GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
     case LOCAL:
+    GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
       {
         tmp.X(-_pos.X() * this->dataPtr->cosHea + _pos.Y() *
             this->dataPtr->sinHea);
@@ -546,7 +591,8 @@ Vector3d SphericalCoordinates::PositionTransform(
         break;
       }
 
-    case LOCAL2:
+    // East, North, Up (ENU)
+    case LOCAL_TANGENT:
       {
         tmp.X(_pos.X() * this->dataPtr->cosHea + _pos.Y() *
             this->dataPtr->sinHea);
@@ -620,8 +666,10 @@ Vector3d SphericalCoordinates::PositionTransform(
       break;
 
     // Convert from ECEF TO LOCAL
+    GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
     case LOCAL:
-    case LOCAL2:
+    GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
+    case LOCAL_TANGENT:
       tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin);
 
       tmp = Vector3d(
@@ -659,15 +707,18 @@ Vector3d SphericalCoordinates::VelocityTransform(
   // First, convert to an ECEF vector
   switch (_in)
   {
-    // ENU
+    // WSU
+    GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
     case LOCAL:
+    GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
       tmp.X(-_vel.X() * this->dataPtr->cosHea + _vel.Y() *
             this->dataPtr->sinHea);
       tmp.Y(-_vel.X() * this->dataPtr->sinHea - _vel.Y() *
             this->dataPtr->cosHea);
       tmp = this->dataPtr->rotGlobalToECEF * tmp;
       break;
-    case LOCAL2:
+    // ENU
+    case LOCAL_TANGENT:
       tmp.X(_vel.X() * this->dataPtr->cosHea + _vel.Y() *
             this->dataPtr->sinHea);
       tmp.Y(-_vel.X() * this->dataPtr->sinHea + _vel.Y() *
@@ -700,8 +751,10 @@ Vector3d SphericalCoordinates::VelocityTransform(
       break;
 
     // Convert from ECEF to local
+    GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
     case LOCAL:
-    case LOCAL2:
+    GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
+    case LOCAL_TANGENT:
       tmp = this->dataPtr->rotECEFToGlobal * tmp;
       tmp = Vector3d(
           tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea,