diff --git a/Migration.md b/Migration.md index 8bd35f88..5a2f0810 100644 --- a/Migration.md +++ b/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 diff --git a/include/gz/math/SphericalCoordinates.hh b/include/gz/math/SphericalCoordinates.hh index 8cd36244..308ad89e 100644 --- a/include/gz/math/SphericalCoordinates.hh +++ b/include/gz/math/SphericalCoordinates.hh @@ -25,6 +25,13 @@ #include #include +// 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,12 +272,32 @@ 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, @@ -256,7 +305,7 @@ namespace gz::math /// 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 diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index 38d2989f..321fe3e2 100644 --- a/src/SphericalCoordinates.cc +++ b/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, diff --git a/src/SphericalCoordinates_TEST.cc b/src/SphericalCoordinates_TEST.cc index 655d1f29..e70a3f68 100644 --- a/src/SphericalCoordinates_TEST.cc +++ b/src/SphericalCoordinates_TEST.cc @@ -228,28 +228,28 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) math::Vector3d enu; xyz.Set(1, 0, 0); - enu = sc.GlobalFromLocalVelocity(xyz); + enu = sc.GlobalVelocityFromLocal(xyz); EXPECT_NEAR(enu.Y()/*North*/, xyz.X(), 1e-6); EXPECT_NEAR(enu.X()/*East*/, -xyz.Y(), 1e-6); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + EXPECT_EQ(xyz, sc.LocalVelocityFromGlobal(enu)); xyz.Set(0, 1, 0); - enu = sc.GlobalFromLocalVelocity(xyz); + enu = sc.GlobalVelocityFromLocal(xyz); EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + EXPECT_EQ(xyz, sc.LocalVelocityFromGlobal(enu)); xyz.Set(1, -1, 0); - enu = sc.GlobalFromLocalVelocity(xyz); + enu = sc.GlobalVelocityFromLocal(xyz); EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + EXPECT_EQ(xyz, sc.LocalVelocityFromGlobal(enu)); xyz.Set(2243.52334, 556.35, 435.6553); - enu = sc.GlobalFromLocalVelocity(xyz); + enu = sc.GlobalVelocityFromLocal(xyz); EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + EXPECT_EQ(xyz, sc.LocalVelocityFromGlobal(enu)); } // Check SphericalFromLocal @@ -261,7 +261,7 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) // No offset xyz.Set(0, 0, 0); - sph = sc.SphericalFromLocalPosition(xyz); + sph = sc.SphericalPositionFromLocal(xyz); // latitude EXPECT_NEAR(sph.X(), lat.Degree(), 1e-6); // longitude @@ -274,13 +274,13 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) // a plane (not along the curvature of Earth). This will result in // a large height offset. xyz.Set(2e5, 0, 0); - sph = sc.SphericalFromLocalPosition(xyz); + sph = sc.SphericalPositionFromLocal(xyz); // increase in latitude about 1.8 degrees EXPECT_NEAR(sph.X(), lat.Degree() + 1.8, 0.008); // no change in longitude EXPECT_NEAR(sph.Z(), 3507.024791, 1e-6); - math::Vector3d xyz2 = sc.LocalFromSphericalPosition(sph); + math::Vector3d xyz2 = sc.LocalPositionFromSpherical(sph); EXPECT_EQ(xyz, xyz2); } @@ -331,14 +331,14 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) EXPECT_NEAR(tmp.Y(), osrf_s.Y(), 1e-2); EXPECT_NEAR(tmp.Z(), osrf_s.Z(), 1e-2); - // Check that SPHERICAL -> LOCAL works - tmp = sc2.LocalFromSphericalPosition(goog_s); + // Check that SPHERICAL -> LOCAL_TANGENT works + tmp = sc2.LocalPositionFromSpherical(goog_s); EXPECT_NEAR(tmp.X(), vec.X(), 8e-2); EXPECT_NEAR(tmp.Y(), vec.Y(), 8e-2); EXPECT_NEAR(tmp.Z(), vec.Z(), 1e-2); - // Check that SPHERICAL -> LOCAL -> SPHERICAL works - tmp = sc2.SphericalFromLocalPosition(tmp); + // Check that SPHERICAL -> LOCAL_TANGENT -> SPHERICAL works + tmp = sc2.SphericalPositionFromLocal(tmp); EXPECT_NEAR(tmp.X(), goog_s.X(), 8e-2); EXPECT_NEAR(tmp.Y(), goog_s.Y(), 8e-2); EXPECT_NEAR(tmp.Z(), goog_s.Z(), 1e-2); @@ -360,33 +360,209 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) xyz.Set(1, 0, 0); enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL_TANGENT, math::SphericalCoordinates::GLOBAL); EXPECT_EQ(xyz, enu); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + EXPECT_EQ(xyz, sc.LocalVelocityFromGlobal(enu)); xyz.Set(0, 1, 0); enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL_TANGENT, math::SphericalCoordinates::GLOBAL); EXPECT_EQ(xyz, enu); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + EXPECT_EQ(xyz, sc.LocalVelocityFromGlobal(enu)); xyz.Set(1, -1, 0); enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL_TANGENT, math::SphericalCoordinates::GLOBAL); EXPECT_EQ(xyz, enu); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + EXPECT_EQ(xyz, sc.LocalVelocityFromGlobal(enu)); xyz.Set(2243.52334, 556.35, 435.6553); enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL_TANGENT, math::SphericalCoordinates::GLOBAL); EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalVelocityFromGlobal(enu)); + } + } +} + +////////////////////////////////////////////////// +// Test coordinate transformations +TEST(SphericalCoordinatesTest, CoordinateTransformsDeprecated) +{ + // This test should be removed in Gazebo 9 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + // Default surface type + math::SphericalCoordinates::SurfaceType st = + math::SphericalCoordinates::EARTH_WGS84; + + { + // Parameters + math::Angle lat(0.3), lon(-1.2), + heading(math::Angle::HalfPi); + double elev = 354.1; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // Check GlobalFromLocal with heading offset of 90 degrees + // Heading 0: X == East, Y == North, Z == Up + // Heading 90: X == North, Y == West , Z == Up + { + // local frame + math::Vector3d xyz; + // east, north, up + math::Vector3d enu; + + xyz.Set(1, 0, 0); + enu = sc.GlobalFromLocalVelocity(xyz); + EXPECT_NEAR(enu.Y()/*North*/, xyz.X(), 1e-6); + EXPECT_NEAR(enu.X()/*East*/, -xyz.Y(), 1e-6); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(0, 1, 0); + enu = sc.GlobalVelocityFromLocal(xyz); + EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); + EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(1, -1, 0); + enu = sc.GlobalFromLocalVelocity(xyz); + EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); + EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(2243.52334, 556.35, 435.6553); + enu = sc.GlobalFromLocalVelocity(xyz); + EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); + EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); } + + // Check SphericalFromLocal + { + // local frame + math::Vector3d xyz; + // spherical coordinates + math::Vector3d sph; + + // No offset + xyz.Set(0, 0, 0); + sph = sc.SphericalFromLocalPosition(xyz); + // latitude + EXPECT_NEAR(sph.X(), lat.Degree(), 1e-6); + // longitude + EXPECT_NEAR(sph.Y(), lon.Degree(), 1e-6); + // elevation + EXPECT_NEAR(sph.Z(), elev, 1e-6); + + // 200 km offset in x (pi/2 heading offset means North). We use + // SphericalFromLocal, which means that xyz is a linear movement on + // a plane (not along the curvature of Earth). This will result in + // a large height offset. + xyz.Set(2e5, 0, 0); + sph = sc.SphericalFromLocalPosition(xyz); + // increase in latitude about 1.8 degrees + EXPECT_NEAR(sph.X(), lat.Degree() + 1.8, 0.008); + // no change in longitude + EXPECT_NEAR(sph.Z(), 3507.024791, 1e-6); + + math::Vector3d xyz2 = sc.LocalFromSphericalPosition(sph); + EXPECT_EQ(xyz, xyz2); + } + + // Check position projection + { + // WGS84 coordinate obtained from online mapping software + // > gdaltransform -s_srs WGS84 -t_srs EPSG:4978 + // > latitude longitude altitude + // > X Y Z + math::Vector3d tmp; + math::Vector3d osrf_s(37.3877349, -122.0651166, 32.0); + math::Vector3d osrf_e( + -2693701.91434394, -4299942.14687992, 3851691.0393571); + math::Vector3d goog_s(37.4216719, -122.0821853, 30.0); + + // Local tangent plane coordinates (ENU = GLOBAL) coordinates of + // Google when OSRF is taken as the origin: + // > proj +ellps=WGS84 +proj=tmerc + // +lat_0=37.3877349 +lon_0=-122.0651166 +k=1 +x_0=0 +y_0=0 + // > -122.0821853 37.4216719 (LON,LAT) + // > -1510.88 3766.64 (EAST,NORTH) + math::Vector3d vec(-1510.88, 3766.64, -3.29); + + // Convert degrees to radians + osrf_s.X() *= 0.0174532925; + osrf_s.Y() *= 0.0174532925; + + // Set the ORIGIN to be the Open Source Robotics Foundation + math::SphericalCoordinates sc2(st, math::Angle(osrf_s.X()), + math::Angle(osrf_s.Y()), osrf_s.Z(), + math::Angle::Zero); + + // Check that SPHERICAL -> LOCAL works + tmp = sc2.LocalFromSphericalPosition(goog_s); + EXPECT_NEAR(tmp.X(), vec.X(), 8e-2); + EXPECT_NEAR(tmp.Y(), vec.Y(), 8e-2); + EXPECT_NEAR(tmp.Z(), vec.Z(), 1e-2); + + // Check that SPHERICAL -> LOCAL -> SPHERICAL works + tmp = sc2.SphericalFromLocalPosition(tmp); + EXPECT_NEAR(tmp.X(), goog_s.X(), 8e-2); + EXPECT_NEAR(tmp.Y(), goog_s.Y(), 8e-2); + EXPECT_NEAR(tmp.Z(), goog_s.Z(), 1e-2); + } + } + + // Give no heading offset to confirm ENU frame + { + math::Angle lat(0.3), lon(-1.2), heading(0.0); + double elev = 354.1; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // Check GlobalFromLocal with no heading offset + { + // local frame + math::Vector3d xyz; + // east, north, up + math::Vector3d enu; + math::Vector3d wsu; + + xyz.Set(1, 0, 0); + wsu = math::Vector3d(-xyz.X(), -xyz.Y(), xyz.Z()); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(wsu, enu); + EXPECT_EQ(wsu, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(0, 1, 0); + wsu = math::Vector3d(-xyz.X(), -xyz.Y(), xyz.Z()); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(wsu, enu); + EXPECT_EQ(wsu, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(1, -1, 0); + wsu = math::Vector3d(-xyz.X(), -xyz.Y(), xyz.Z()); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(wsu, enu); + EXPECT_EQ(wsu, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(2243.52334, 556.35, 435.6553); + wsu = math::Vector3d(-xyz.X(), -xyz.Y(), xyz.Z()); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(wsu, enu); + EXPECT_EQ(wsu, sc.LocalFromGlobalVelocity(enu)); + } } + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } ////////////////////////////////////////////////// @@ -563,6 +739,109 @@ TEST(SphericalCoordinatesTest, NoHeading) double elev = 0; math::SphericalCoordinates sc(st, lat, lon, elev, heading); + // Origin matches input + auto latLonAlt = sc.SphericalPositionFromLocal({0, 0, 0}); + EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); + EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt.Y()); + EXPECT_NEAR(elev, latLonAlt.Z(), 1e-6); + + auto xyzOrigin = sc.LocalPositionFromSpherical(latLonAlt); + EXPECT_EQ(math::Vector3d::Zero, xyzOrigin); + + // Check how different lat/lon affect the local position + + // Increase latitude == go North == go +Y + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree() + 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_LT(xyzOrigin.Y(), xyz.Y()); + } + + // Decrease latitude == go South == go -Y + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree() - 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Increase longitude == go East == go +X + // Also move a bit -Y because this is the Southern Hemisphere + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree(), lon.Degree() + 1.0, elev}); + EXPECT_LT(xyzOrigin.X(), xyz.X()); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Decrease longitude == go West == go -X + // Also move a bit -Y because this is the Southern Hemisphere + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree(), lon.Degree() - 1.0, elev}); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Increase altitude + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree(), lon.Degree(), elev + 10.0}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_NEAR(xyzOrigin.Z() + 10.0, xyz.Z(), 1e-6); + } + + // Decrease altitude + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree(), lon.Degree(), elev - 10.0}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_NEAR(xyzOrigin.Z() - 10.0, xyz.Z(), 1e-6); + } + + // Check how global and local velocities are connected + + // Velocity in + // +X (East), +Y (North), -X (West), -Y (South), +Z (up), -Z (down) + for (auto global : { + math::Vector3d::UnitX, + math::Vector3d::UnitY, + math::Vector3d::UnitZ, + -math::Vector3d::UnitX, + -math::Vector3d::UnitY, + -math::Vector3d::UnitZ}) + { + auto local = sc.LocalVelocityFromGlobal(global); + EXPECT_EQ(global, local); + + global = sc.GlobalVelocityFromLocal(local); + EXPECT_EQ(global, local); + + // Directly call VelocityTransform + global = sc.VelocityTransform(local, + math::SphericalCoordinates::LOCAL_TANGENT, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(global, local); + } +} + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, NoHeadingDeprecated) +{ + // This test should be removed in Gazebo 9 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + + // Default heading + auto st = math::SphericalCoordinates::EARTH_WGS84; + math::Angle lat(GZ_DTOR(-22.9)); + math::Angle lon(GZ_DTOR(-43.2)); + math::Angle heading(0.0); + double elev = 0; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + // Origin matches input auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); @@ -628,6 +907,8 @@ TEST(SphericalCoordinatesTest, NoHeading) // Check how global and local velocities are connected + math::Vector3d wsu; + // Velocity in // +X (East), +Y (North), -X (West), -Y (South), +Z (up), -Z (down) for (auto global : { @@ -641,23 +922,18 @@ TEST(SphericalCoordinatesTest, NoHeading) auto local = sc.LocalFromGlobalVelocity(global); EXPECT_EQ(global, local); - // This function is broken for horizontal velocities global = sc.GlobalFromLocalVelocity(local); - if (abs(global.Z()) < 0.1) - { - EXPECT_NE(global, local); - } - else - { - EXPECT_EQ(global, local); - } + wsu = {-global.X(), -global.Y(), global.Z()}; + EXPECT_EQ(wsu, local); - // Directly call fixed version + // Directly call VelocityTransform global = sc.VelocityTransform(local, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL, math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(global, local); + wsu = {-global.X(), -global.Y(), global.Z()}; + EXPECT_EQ(wsu, local); } + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } ////////////////////////////////////////////////// @@ -671,6 +947,89 @@ TEST(SphericalCoordinatesTest, WithHeading) double elev = 0; math::SphericalCoordinates sc(st, lat, lon, elev, heading); + // Origin matches input + auto latLonAlt = sc.SphericalPositionFromLocal({0, 0, 0}); + EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); + EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt.Y()); + EXPECT_NEAR(elev, latLonAlt.Z(), 1e-6); + + auto xyzOrigin = sc.LocalPositionFromSpherical(latLonAlt); + EXPECT_EQ(math::Vector3d::Zero, xyzOrigin); + + // Check how different lat/lon affect the local position + + // Increase latitude == go North == go +X + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree() + 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_LT(xyzOrigin.X(), xyz.X()); + } + + // Decrease latitude == go South == go -X + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree() - 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Increase longitude == go East == go -Y (and a bit -X) + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree(), lon.Degree() + 1.0, elev}); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Decrease longitude == go West == go +Y (and a bit -X) + { + auto xyz = sc.LocalPositionFromSpherical( + {lat.Degree(), lon.Degree() - 1.0, elev}); + EXPECT_LT(xyzOrigin.Y(), xyz.Y()); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Check how global and local velocities are connected + + // Global | Local + // ---------- | ------ + // +X (East) | -Y + // -X (West) | +Y + // +Y (North) | +X + // -Y (South) | -X + std::vector> globalLocal = + {{math::Vector3d::UnitX, -math::Vector3d::UnitY}, + {-math::Vector3d::UnitX, math::Vector3d::UnitY}, + {math::Vector3d::UnitY, math::Vector3d::UnitX}, + {-math::Vector3d::UnitY, -math::Vector3d::UnitX}}; + for (auto [global, local] : globalLocal) + { + auto localRes = sc.LocalVelocityFromGlobal(global); + EXPECT_EQ(local, localRes); + + // Directly call fixed version + auto globalRes = sc.VelocityTransform(local, + math::SphericalCoordinates::LOCAL_TANGENT, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(global, globalRes); + } +} + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, WithHeadingDeprecated) +{ + // This test should be removed in Gazebo 9 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + + // Heading 90 deg: X == North, Y == West , Z == Up + auto st = math::SphericalCoordinates::EARTH_WGS84; + math::Angle lat(GZ_DTOR(-22.9)); + math::Angle lon(GZ_DTOR(-43.2)); + math::Angle heading(GZ_DTOR(90.0)); + double elev = 0; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + // Origin matches input auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); @@ -734,10 +1093,12 @@ TEST(SphericalCoordinatesTest, WithHeading) // Directly call fixed version auto globalRes = sc.VelocityTransform(local, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL, math::SphericalCoordinates::GLOBAL); EXPECT_EQ(global, globalRes); } + + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } ////////////////////////////////////////////////// @@ -748,41 +1109,92 @@ TEST(SphericalCoordinatesTest, Inverse) double elev = 354.1; math::SphericalCoordinates sc(st, lat, lon, elev, heading); - // GLOBAL <-> LOCAL2 + // GLOBAL <-> LOCAL_TANGENT { math::Vector3d in(1, 2, -4); auto out = sc.VelocityTransform(in, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL_TANGENT, math::SphericalCoordinates::GLOBAL); EXPECT_NE(in, out); auto reverse = sc.VelocityTransform(out, math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::LOCAL2); + math::SphericalCoordinates::LOCAL_TANGENT); EXPECT_EQ(in, reverse); } { math::Vector3d in(1, 2, -4); auto out = sc.PositionTransform(in, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL_TANGENT, math::SphericalCoordinates::GLOBAL); EXPECT_NE(in, out); auto reverse = sc.PositionTransform(out, math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::LOCAL2); + math::SphericalCoordinates::LOCAL_TANGENT); EXPECT_EQ(in, reverse); } - // SPHERICAL <-> LOCAL2 + // SPHERICAL <-> LOCAL_TANGENT { math::Vector3d in(1, 2, -4); auto out = sc.PositionTransform(in, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL_TANGENT, math::SphericalCoordinates::SPHERICAL); EXPECT_NE(in, out); auto reverse = sc.PositionTransform(out, math::SphericalCoordinates::SPHERICAL, - math::SphericalCoordinates::LOCAL2); + math::SphericalCoordinates::LOCAL_TANGENT); EXPECT_EQ(in, reverse); } } + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, InverseDeprecated) +{ + // This test should be removed in Gazebo 9 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + + auto st = math::SphericalCoordinates::EARTH_WGS84; + math::Angle lat(0.3), lon(-1.2), heading(0.5); + double elev = 354.1; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // GLOBAL <-> LOCAL + { + math::Vector3d in(1, 2, -4); + auto out = sc.VelocityTransform(in, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + EXPECT_NE(in, out); + auto reverse = sc.VelocityTransform(out, + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::LOCAL); + EXPECT_NE(in, reverse); + } + + { + math::Vector3d in(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + EXPECT_NE(in, out); + auto reverse = sc.PositionTransform(out, + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::LOCAL); + EXPECT_NE(in, reverse); + } + + // SPHERICAL <-> LOCAL + { + math::Vector3d in(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::SPHERICAL); + EXPECT_NE(in, out); + auto reverse = sc.PositionTransform(out, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL); + EXPECT_NE(in, reverse); + } + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION +} diff --git a/src/python_pybind11/src/SphericalCoordinates.cc b/src/python_pybind11/src/SphericalCoordinates.cc index f2d27ed2..6e43400b 100644 --- a/src/python_pybind11/src/SphericalCoordinates.cc +++ b/src/python_pybind11/src/SphericalCoordinates.cc @@ -47,10 +47,34 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) .def(py::self != py::self) .def(py::self == py::self) .def("spherical_from_local_position", - &Class::SphericalFromLocalPosition, + [](const Class &self, const gz::math::Vector3d &_xyz) + { + PyErr_WarnEx(PyExc_DeprecationWarning, + "spherical_from_local_position() is deprecated, " + "use spherical_position_from_local() instead.", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.SphericalFromLocalPosition(_xyz); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, "Convert a Cartesian position vector to geodetic coordinates.") .def("global_from_local_velocity", - &Class::GlobalFromLocalVelocity, + [](const Class &self, const gz::math::Vector3d &_xyz) + { + PyErr_WarnEx(PyExc_DeprecationWarning, + "global_from_local_velocity() is deprecated, " + "use global_velocity_from_local() instead.", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.GlobalFromLocalVelocity(_xyz); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, + "Convert a Cartesian velocity vector in the local frame " + " to a global Cartesian frame with components West, South, Up") + .def("spherical_position_from_local", &Class::SphericalPositionFromLocal, + "Convert a Cartesian position vector to geodetic coordinates.") + .def("global_velocity_from_local", + &Class::GlobalVelocityFromLocal, "Convert a Cartesian velocity vector in the local frame " " to a global Cartesian frame with components East, North, Up") .def("convert", @@ -122,10 +146,35 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) &Class::SetHeadingOffset, "Set heading angle offset for the frame.") .def("local_from_spherical_position", - &Class::LocalFromSphericalPosition, + [](const Class &self, const gz::math::Vector3d&_xyz) + { + PyErr_WarnEx(PyExc_DeprecationWarning, + "local_from_spherical_position() is deprecated, " + "use local_position_from_spherical() instead.", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.LocalFromSphericalPosition(_xyz); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, "Convert a geodetic position vector to Cartesian coordinates.") .def("local_from_global_velocity", - &Class::LocalFromGlobalVelocity, + [](const Class &self, const gz::math::Vector3d&_xyz) + { + PyErr_WarnEx(PyExc_DeprecationWarning, + "local_from_global_velocity() is deprecated, " + "use local_velocity_from_global() instead.", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.LocalFromGlobalVelocity(_xyz); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, + "Convert a Cartesian velocity vector with components East, " + "North, Up to a local cartesian frame vector XYZ.") + .def("local_position_from_spherical", + &Class::LocalPositionFromSpherical, + "Convert a geodetic position vector to Cartesian coordinates.") + .def("local_velocity_from_global", + &Class::LocalVelocityFromGlobal, "Convert a Cartesian velocity vector with components East, " "North, Up to a local cartesian frame vector XYZ.") .def("update_transformation_matrix", @@ -142,13 +191,16 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) "Spherical coordinates use radians, while the other frames use " "meters."); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION py::enum_(sphericalCoordinates, "CoordinateType") .value("SPHERICAL", Class::CoordinateType::SPHERICAL) .value("ECEF", Class::CoordinateType::ECEF) .value("GLOBAL", Class::CoordinateType::GLOBAL) .value("LOCAL", Class::CoordinateType::LOCAL) + .value("LOCAL_TANGENT", Class::CoordinateType::LOCAL_TANGENT) .value("LOCAL2", Class::CoordinateType::LOCAL2) .export_values(); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION py::enum_(sphericalCoordinates, "SurfaceType") .value("EARTH_WGS84", Class::SurfaceType::EARTH_WGS84) .value("MOON_SCS", Class::SurfaceType::MOON_SCS) diff --git a/src/python_pybind11/test/SphericalCoordinates_TEST.py b/src/python_pybind11/test/SphericalCoordinates_TEST.py index ef333f30..2a1565c8 100644 --- a/src/python_pybind11/test/SphericalCoordinates_TEST.py +++ b/src/python_pybind11/test/SphericalCoordinates_TEST.py @@ -191,28 +191,28 @@ def test_coordinate_transforms(self): enu = Vector3d() xyz.set(1, 0, 0) - enu = sc.global_from_local_velocity(xyz) + enu = sc.global_velocity_from_local(xyz) self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + self.assertEqual(xyz, sc.local_velocity_from_global(enu)) xyz.set(0, 1, 0) - enu = sc.global_from_local_velocity(xyz) + enu = sc.global_velocity_from_local(xyz) self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + self.assertEqual(xyz, sc.local_velocity_from_global(enu)) xyz.set(1, -1, 0) - enu = sc.global_from_local_velocity(xyz) + enu = sc.global_velocity_from_local(xyz) self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + self.assertEqual(xyz, sc.local_velocity_from_global(enu)) xyz.set(2243.52334, 556.35, 435.6553) - enu = sc.global_from_local_velocity(xyz) + enu = sc.global_velocity_from_local(xyz) self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + self.assertEqual(xyz, sc.local_velocity_from_global(enu)) # Check SphericalFromLocal # local frame @@ -222,7 +222,7 @@ def test_coordinate_transforms(self): # No offset xyz.set(0, 0, 0) - sph = sc.spherical_from_local_position(xyz) + sph = sc.spherical_position_from_local(xyz) # latitude self.assertAlmostEqual(sph.x(), lat.degree(), delta=1e-6) # longitude @@ -235,13 +235,13 @@ def test_coordinate_transforms(self): # a plane (not along the curvature of Earth). This will result in # a large height offset. xyz.set(2e5, 0, 0) - sph = sc.spherical_from_local_position(xyz) + sph = sc.spherical_position_from_local(xyz) # increase in latitude about 1.8 degrees self.assertAlmostEqual(sph.x(), lat.degree() + 1.8, delta=0.008) # no change in longitude self.assertAlmostEqual(sph.z(), 3507.024791, delta=1e-6) - xyz2 = sc.local_from_spherical_position(sph) + xyz2 = sc.local_position_from_spherical(sph) self.assertEqual(xyz, xyz2) # Check position projection @@ -286,13 +286,13 @@ def test_coordinate_transforms(self): self.assertAlmostEqual(tmp.z(), osrf_s.z(), delta=1e-2) # Check that SPHERICAL -> LOCAL works - tmp = sc2.local_from_spherical_position(goog_s) + tmp = sc2.local_position_from_spherical(goog_s) self.assertAlmostEqual(tmp.x(), vec.x(), delta=8e-2) self.assertAlmostEqual(tmp.y(), vec.y(), delta=8e-2) self.assertAlmostEqual(tmp.z(), vec.z(), delta=1e-2) # Check that SPHERICAL -> LOCAL -> SPHERICAL works - tmp = sc2.spherical_from_local_position(tmp) + tmp = sc2.spherical_position_from_local(tmp) self.assertAlmostEqual(tmp.x(), goog_s.x(), delta=8e-2) self.assertAlmostEqual(tmp.y(), goog_s.y(), delta=8e-2) self.assertAlmostEqual(tmp.z(), goog_s.z(), delta=1e-2) @@ -311,24 +311,24 @@ def test_coordinate_transforms(self): enu = Vector3d() xyz.set(1, 0, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.GLOBAL) self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + self.assertEqual(xyz, sc.local_velocity_from_global(enu)) xyz.set(0, 1, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.GLOBAL) self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + self.assertEqual(xyz, sc.local_velocity_from_global(enu)) xyz.set(1, -1, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.GLOBAL) self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + self.assertEqual(xyz, sc.local_velocity_from_global(enu)) xyz.set(2243.52334, 556.35, 435.6553) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.GLOBAL) self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + self.assertEqual(xyz, sc.local_velocity_from_global(enu)) def test_distance(self): latA = Angle() @@ -482,51 +482,51 @@ def test_no_heading(self): sc = SphericalCoordinates(st, lat, lon, elev, heading) # Origin matches input - latLonAlt = sc.spherical_from_local_position(Vector3d(0, 0, 0)) + latLonAlt = sc.spherical_position_from_local(Vector3d(0, 0, 0)) self.assertEqual(lat.degree(), latLonAlt.x()) self.assertEqual(lon.degree(), latLonAlt.y()) self.assertEqual(elev, latLonAlt.z()) - xyzOrigin = sc.local_from_spherical_position(latLonAlt) + xyzOrigin = sc.local_position_from_spherical(latLonAlt) self.assertEqual(Vector3d.ZERO, xyzOrigin) # Check how different lat/lon affect the local position # Increase latitude == go North == go +Y - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree() + 1.0, lon.degree(), elev)) self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) self.assertLess(xyzOrigin.y(), xyz.y()) # Decrease latitude == go South == go -Y - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree() - 1.0, lon.degree(), elev)) self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) self.assertGreater(xyzOrigin.y(), xyz.y()) # Increase longitude == go East == go +X # Also move a bit -Y because this is the Southern Hemisphere - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree(), lon.degree() + 1.0, elev)) self.assertLess(xyzOrigin.x(), xyz.x()) self.assertGreater(xyzOrigin.y(), xyz.y()) # Decrease longitude == go West == go -X # Also move a bit -Y because this is the Southern Hemisphere - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree(), lon.degree() - 1.0, elev)) self.assertGreater(xyzOrigin.x(), xyz.x()) self.assertGreater(xyzOrigin.y(), xyz.y()) # Increase altitude - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree(), lon.degree(), elev + 10.0)) self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) self.assertAlmostEqual(xyzOrigin.z() + 10.0, xyz.z(), delta=1e-6) # Decrease altitude - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree(), lon.degree(), elev - 10.0)) self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) @@ -538,20 +538,16 @@ def test_no_heading(self): # +X (East), +Y (North), -X (West), -Y (South), +Z (up), -Z (down) for global_var in [Vector3d.UNIT_X, Vector3d.UNIT_Y, Vector3d.UNIT_Z, -Vector3d.UNIT_X, -Vector3d.UNIT_Y, -Vector3d.UNIT_Z]: - local = sc.local_from_global_velocity(global_var) + local = sc.local_velocity_from_global(global_var) self.assertEqual(global_var, local) - # This function is broken for horizontal velocities - global_var = sc.global_from_local_velocity(local) - if abs(global_var.z()) < 0.1: - self.assertNotEqual(global_var, local) - else: - self.assertEqual(global_var, local) + global_var = sc.global_velocity_from_local(local) + self.assertEqual(global_var, local) - # Directly call fixed version + # Directly call velocity_transform global_var = sc.velocity_transform( local, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.GLOBAL) self.assertEqual(global_var, local) @@ -565,36 +561,36 @@ def test_with_heading(self): sc = SphericalCoordinates(st, lat, lon, elev, heading) # Origin matches input - latLonAlt = sc.spherical_from_local_position(Vector3d(0, 0, 0)) + latLonAlt = sc.spherical_position_from_local(Vector3d(0, 0, 0)) self.assertEqual(lat.degree(), latLonAlt.x()) self.assertEqual(lon.degree(), latLonAlt.y()) self.assertEqual(elev, latLonAlt.z()) - xyzOrigin = sc.local_from_spherical_position(latLonAlt) + xyzOrigin = sc.local_position_from_spherical(latLonAlt) self.assertEqual(Vector3d.ZERO, xyzOrigin) # Check how different lat/lon affect the local position # Increase latitude == go North == go +X - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree() + 1.0, lon.degree(), elev)) self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) self.assertLess(xyzOrigin.x(), xyz.x()) # Decrease latitude == go South == go -X - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree() - 1.0, lon.degree(), elev)) self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) self.assertGreater(xyzOrigin.x(), xyz.x()) # Increase longitude == go East == go -Y (and a bit -X) - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree(), lon.degree() + 1.0, elev)) self.assertGreater(xyzOrigin.y(), xyz.y()) self.assertGreater(xyzOrigin.x(), xyz.x()) # Decrease longitude == go West == go +Y (and a bit -X) - xyz = sc.local_from_spherical_position( + xyz = sc.local_position_from_spherical( Vector3d(lat.degree(), lon.degree() - 1.0, elev)) self.assertLess(xyzOrigin.y(), xyz.y()) self.assertGreater(xyzOrigin.x(), xyz.x()) @@ -613,13 +609,13 @@ def test_with_heading(self): [Vector3d.UNIT_Y, Vector3d.UNIT_X], [-Vector3d.UNIT_Y, -Vector3d.UNIT_X]] for [global_var, local] in globalLocal: - localRes = sc.local_from_global_velocity(global_var) + localRes = sc.local_velocity_from_global(global_var) self.assertEqual(local, localRes) # Directly call fixed version globalRes = sc.velocity_transform( local, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.GLOBAL) self.assertEqual(global_var, globalRes) @@ -631,42 +627,42 @@ def test_inverse(self): elev = 354.1 sc = SphericalCoordinates(st, lat, lon, elev, heading) - # GLOBAL <-> LOCAL2 + # GLOBAL <-> LOCAL_TANGENT in_vector = Vector3d(1, 2, -4) out = sc.velocity_transform( in_vector, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.GLOBAL) self.assertNotEqual(in_vector, out) reverse = sc.velocity_transform( out, SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL2) + SphericalCoordinates.LOCAL_TANGENT) self.assertEqual(in_vector, reverse) in_vector = Vector3d(1, 2, -4) out = sc.position_transform( in_vector, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.GLOBAL) self.assertNotEqual(in_vector, out) reverse = sc.position_transform( out, SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL2) + SphericalCoordinates.LOCAL_TANGENT) self.assertEqual(in_vector, reverse) - # SPHERICAL <-> LOCAL2 + # SPHERICAL <-> LOCAL_TANGENT in_vector = Vector3d(1, 2, -4) out = sc.position_transform( in_vector, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL_TANGENT, SphericalCoordinates.SPHERICAL) self.assertNotEqual(in_vector, out) reverse = sc.position_transform( out, SphericalCoordinates.SPHERICAL, - SphericalCoordinates.LOCAL2) + SphericalCoordinates.LOCAL_TANGENT) self.assertEqual(in_vector, reverse) diff --git a/src/ruby/SphericalCoordinates.i b/src/ruby/SphericalCoordinates.i index 801536e2..af626e5b 100644 --- a/src/ruby/SphericalCoordinates.i +++ b/src/ruby/SphericalCoordinates.i @@ -24,6 +24,11 @@ #include #include #include + +GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION +// GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION is not present; +// that's not a problem in SWIG generated files + %} namespace gz @@ -52,11 +57,14 @@ namespace gz /// \brief Heading-adjusted tangent plane (X, Y, Z) /// This has kept a bug for backwards compatibility, use - /// LOCAL2 for the correct behaviour. + /// LOCAL_TANGENT for the correct behaviour. LOCAL = 4, + /// \brief Equivalent to LOCAL_TANGENT. + LOCAL2 = 5, + /// \brief Heading-adjusted tangent plane (X, Y, Z) - LOCAL2 = 5 + LOCAL_TANGENT = 5 }; public: SphericalCoordinates(); @@ -80,6 +88,12 @@ namespace gz public: gz::math::Vector3 GlobalFromLocalVelocity( const gz::math::Vector3 &_xyz) const; + public: gz::math::Vector3 SphericalPositionFromLocal( + const gz::math::Vector3 &_xyz) const; + + public: gz::math::Vector3 GlobalVelocityFromLocal( + const gz::math::Vector3 &_xyz) const; + public: static SurfaceType Convert(const std::string &_str); public: static std::string Convert(SurfaceType _type); @@ -115,6 +129,12 @@ namespace gz public: gz::math::Vector3 LocalFromGlobalVelocity( const gz::math::Vector3 &_xyz) const; + public: gz::math::Vector3 LocalPositionFromSpherical( + const gz::math::Vector3 &_latLonEle) const; + + public: gz::math::Vector3 LocalVelocityFromGlobal( + const gz::math::Vector3 &_xyz) const; + public: void UpdateTransformationMatrix(); public: gz::math::Vector3