diff --git a/Migration.md b/Migration.md index 8bd35f88..12a36779 100644 --- a/Migration.md +++ b/Migration.md @@ -7,6 +7,12 @@ release will remove the deprecated code. ## Gazebo Math 8.X to 9.X +### Additions + +1. **SphericalCoordinates.hh** + + `SphericalCoordinates::CoordinateType::SPHERICAL_DEG` + + `SphericalCoordinates::CoordinateType::SPHERICAL_RAD` + ### Deprecations 1. **graph/Vertex.hh** @@ -19,6 +25,11 @@ 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** + + `SphericalCoordinates::CoordinateType::SPHERICAL` has been deprecated + because it is unclear what angular units are used. Instead, explicitly + choose either `SPERICAL_DEG` or `SPHERICAL_RAD`. + ## 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..5b938e50 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_SPHERICAL_DEPRECATED [[deprecated]] +#else +#define GZ_SPHERICAL_DEPRECATED GZ_DEPRECATED(8) +#endif + namespace gz::math { // Inline bracket to help doxygen filtering. @@ -54,8 +61,15 @@ namespace gz::math /// \brief Unique identifiers for coordinate types. public: enum CoordinateType { - /// \brief Latitude, Longitude and Altitude by SurfaceType - SPHERICAL = 1, + /// \brief Latitude, Longitude and Altitude by SurfaceType [rad]. + /// \deprecated Use `SPHERICAL_RAD` or `SPHERICAL_DEG` instead. + SPHERICAL GZ_SPHERICAL_DEPRECATED = 1, + + /// \brief Latitude, Longitude and Altitude by SurfaceType [rad]. + SPHERICAL_RAD = 1, + + /// \brief Latitude, Longitude and Altitude by SurfaceType [deg]. + SPHERICAL_DEG = 6, /// \brief Earth centered, earth fixed Cartesian ECEF = 2, @@ -262,8 +276,9 @@ namespace gz::math /// \brief Update coordinate transformation matrix with reference location public: void UpdateTransformationMatrix(); - /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame - /// Spherical coordinates use radians, while the other frames use meters. + /// \brief Convert between positions in SPHERICAL_*/ECEF/LOCAL/GLOBAL frame. + /// Cartesian frames use meters. `SPHERICAL_DEG` frame uses degrees. + /// `SPHERICAL_RAD` frame uses radians. /// \param[in] _pos Position vector in frame defined by parameter _in /// \param[in] _in CoordinateType for input /// \param[in] _out CoordinateType for output @@ -272,8 +287,9 @@ namespace gz::math PositionTransform(const gz::math::Vector3d &_pos, const CoordinateType &_in, const CoordinateType &_out) const; - /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame - /// Spherical coordinates use radians, while the other frames use meters. + /// \brief Convert between velocity in ECEF/LOCAL/GLOBAL frame. + /// Velocity should not be expressed in SPHERICAL_* frames (such values will + /// be passed through). Cartesian frames use meters. /// \param[in] _vel Velocity vector in frame defined by parameter _in /// \param[in] _in CoordinateType for input /// \param[in] _out CoordinateType for output diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index 38d2989f..6e3207ec 100644 --- a/src/SphericalCoordinates.cc +++ b/src/SphericalCoordinates.cc @@ -377,21 +377,14 @@ void SphericalCoordinates::SetHeadingOffset(const Angle &_angle) Vector3d SphericalCoordinates::SphericalFromLocalPosition( const Vector3d &_xyz) const { - Vector3d result = - this->PositionTransform(_xyz, LOCAL, SPHERICAL); - result.X(GZ_RTOD(result.X())); - result.Y(GZ_RTOD(result.Y())); - return result; + return this->PositionTransform(_xyz, LOCAL, SPHERICAL_DEG); } ////////////////////////////////////////////////// Vector3d SphericalCoordinates::LocalFromSphericalPosition( 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); + return this->PositionTransform(_xyz, SPHERICAL_DEG, LOCAL); } ////////////////////////////////////////////////// @@ -511,7 +504,7 @@ void SphericalCoordinates::UpdateTransformationMatrix() this->dataPtr->longitudeReference.Radian(), this->dataPtr->elevationReference); this->dataPtr->origin = - this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF); + this->PositionTransform(this->dataPtr->origin, SPHERICAL_RAD, ECEF); } ///////////////////////////////////////////////// @@ -521,17 +514,6 @@ Vector3d SphericalCoordinates::PositionTransform( { Vector3d tmp = _pos; - // Cache trig results - double cosLat = cos(_pos.X()); - double sinLat = sin(_pos.X()); - double cosLon = cos(_pos.Y()); - double sinLon = sin(_pos.Y()); - - // Radius of planet curvature (meters) - double curvature = 1.0 - - this->dataPtr->ellE * this->dataPtr->ellE * sinLat * sinLat; - curvature = this->dataPtr->ellA / sqrt(curvature); - // Convert whatever arrives to a more flexible ECEF coordinate switch (_in) { @@ -562,8 +544,24 @@ Vector3d SphericalCoordinates::PositionTransform( break; } - case SPHERICAL: + case SPHERICAL_DEG: + case SPHERICAL_RAD: { + // Cache trig results + const double latRad = + (_in == SPHERICAL_DEG) ? GZ_DTOR(_pos.X()) : _pos.X(); + const double lonRad = + (_in == SPHERICAL_DEG) ? GZ_DTOR(_pos.Y()) : _pos.Y(); + double cosLat = cos(latRad); + double sinLat = sin(latRad); + double cosLon = cos(lonRad); + double sinLon = sin(lonRad); + + // Radius of planet curvature (meters) + double curvature = + 1.0 - this->dataPtr->ellE * this->dataPtr->ellE * sinLat * sinLat; + curvature = this->dataPtr->ellA / sqrt(curvature); + tmp.X((_pos.Z() + curvature) * cosLat * cosLon); tmp.Y((_pos.Z() + curvature) * cosLat * sinLon); tmp.Z(((this->dataPtr->ellB * this->dataPtr->ellB)/ @@ -585,7 +583,8 @@ Vector3d SphericalCoordinates::PositionTransform( // Convert ECEF to the requested output coordinate system switch (_out) { - case SPHERICAL: + case SPHERICAL_DEG: + case SPHERICAL_RAD: { // Convert from ECEF to SPHERICAL double p = sqrt(tmp.X() * tmp.X() + tmp.Y() * tmp.Y()); @@ -606,8 +605,8 @@ Vector3d SphericalCoordinates::PositionTransform( std::pow(sin(lat), 2); nCurvature = this->dataPtr->ellA / sqrt(nCurvature); - tmp.X(lat); - tmp.Y(lon); + tmp.X(_out == SPHERICAL_DEG ? GZ_RTOD(lat) : lat); + tmp.Y(_out == SPHERICAL_DEG ? GZ_RTOD(lon) : lon); // Now calculate Z tmp.Z(p/cos(lat) - nCurvature); @@ -648,7 +647,8 @@ Vector3d SphericalCoordinates::VelocityTransform( const CoordinateType &_in, const CoordinateType &_out) const { // Sanity check -- velocity should not be expressed in spherical coordinates - if (_in == SPHERICAL || _out == SPHERICAL) + if (_in == SPHERICAL_RAD || _in == SPHERICAL_DEG || + _out == SPHERICAL_RAD || _out == SPHERICAL_DEG) { return _vel; } diff --git a/src/SphericalCoordinates_TEST.cc b/src/SphericalCoordinates_TEST.cc index 655d1f29..0f86d9fe 100644 --- a/src/SphericalCoordinates_TEST.cc +++ b/src/SphericalCoordinates_TEST.cc @@ -291,7 +291,9 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) // > latitude longitude altitude // > X Y Z math::Vector3d tmp; - math::Vector3d osrf_s(37.3877349, -122.0651166, 32.0); + math::Vector3d osrf_s_deg(37.3877349, -122.0651166, 32.0); + math::Vector3d osrf_s_rad( + GZ_DTOR(osrf_s_deg.X()), GZ_DTOR(osrf_s_deg.Y()), osrf_s_deg.Z()); math::Vector3d osrf_e( -2693701.91434394, -4299942.14687992, 3851691.0393571); math::Vector3d goog_s(37.4216719, -122.0821853, 30.0); @@ -304,40 +306,72 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) // > -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::SphericalCoordinates sc2( + st, osrf_s_rad.X(), osrf_s_rad.Y(), osrf_s_rad.Z(), math::Angle::Zero); - // Check that SPHERICAL -> ECEF works - tmp = sc2.PositionTransform(osrf_s, - math::SphericalCoordinates::SPHERICAL, + // Check that SPHERICAL_DEG -> ECEF works + tmp = sc2.PositionTransform(osrf_s_deg, + math::SphericalCoordinates::SPHERICAL_DEG, + math::SphericalCoordinates::ECEF); + + EXPECT_NEAR(tmp.X(), osrf_e.X(), 8e-2); + EXPECT_NEAR(tmp.Y(), osrf_e.Y(), 8e-2); + EXPECT_NEAR(tmp.Z(), osrf_e.Z(), 1e-2); + + // Check that SPHERICAL_RAD -> ECEF works + tmp = sc2.PositionTransform(osrf_s_rad, + math::SphericalCoordinates::SPHERICAL_RAD, math::SphericalCoordinates::ECEF); EXPECT_NEAR(tmp.X(), osrf_e.X(), 8e-2); EXPECT_NEAR(tmp.Y(), osrf_e.Y(), 8e-2); EXPECT_NEAR(tmp.Z(), osrf_e.Z(), 1e-2); - // Check that ECEF -> SPHERICAL works - tmp = sc2.PositionTransform(tmp, + // Check that ECEF -> SPHERICAL_DEG works + tmp = sc2.PositionTransform(osrf_e, + math::SphericalCoordinates::ECEF, + math::SphericalCoordinates::SPHERICAL_DEG); + + EXPECT_NEAR(tmp.X(), osrf_s_deg.X(), 1e-2); + EXPECT_NEAR(tmp.Y(), osrf_s_deg.Y(), 1e-2); + EXPECT_NEAR(tmp.Z(), osrf_s_deg.Z(), 1e-2); + + // Check that ECEF -> SPHERICAL_RAD works + tmp = sc2.PositionTransform(osrf_e, math::SphericalCoordinates::ECEF, - math::SphericalCoordinates::SPHERICAL); + math::SphericalCoordinates::SPHERICAL_RAD); + + EXPECT_NEAR(tmp.X(), osrf_s_rad.X(), 1e-2); + EXPECT_NEAR(tmp.Y(), osrf_s_rad.Y(), 1e-2); + EXPECT_NEAR(tmp.Z(), osrf_s_rad.Z(), 1e-2); + + // Check that SPHERICAL_DEG -> SPHERICAL_RAD works + tmp = sc2.PositionTransform(osrf_s_deg, + math::SphericalCoordinates::SPHERICAL_DEG, + math::SphericalCoordinates::SPHERICAL_RAD); - EXPECT_NEAR(tmp.X(), osrf_s.X(), 1e-2); - EXPECT_NEAR(tmp.Y(), osrf_s.Y(), 1e-2); - EXPECT_NEAR(tmp.Z(), osrf_s.Z(), 1e-2); + EXPECT_NEAR(tmp.X(), osrf_s_rad.X(), 1e-2); + EXPECT_NEAR(tmp.Y(), osrf_s_rad.Y(), 1e-2); + EXPECT_NEAR(tmp.Z(), osrf_s_rad.Z(), 1e-2); - // Check that SPHERICAL -> LOCAL works + // Check that SPHERICAL_RAD -> SPHERICAL_DEG works + tmp = sc2.PositionTransform(osrf_s_rad, + math::SphericalCoordinates::SPHERICAL_RAD, + math::SphericalCoordinates::SPHERICAL_DEG); + + EXPECT_NEAR(tmp.X(), osrf_s_deg.X(), 1e-2); + EXPECT_NEAR(tmp.Y(), osrf_s_deg.Y(), 1e-2); + EXPECT_NEAR(tmp.Z(), osrf_s_deg.Z(), 1e-2); + + // Check that SPHERICAL_DEG -> 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 + // Check that SPHERICAL_DEG -> LOCAL -> SPHERICAL_DEG works tmp = sc2.SphericalFromLocalPosition(tmp); EXPECT_NEAR(tmp.X(), goog_s.X(), 8e-2); EXPECT_NEAR(tmp.Y(), goog_s.Y(), 8e-2); @@ -474,35 +508,45 @@ TEST(SphericalCoordinatesTest, BadCoordinateType) math::SphericalCoordinates sc; math::Vector3d pos(1, 2, -4); math::Vector3d result = sc.PositionTransform(pos, - static_cast(7), - static_cast(6)); + static_cast(17), + static_cast(16)); EXPECT_EQ(result, pos); result = sc.PositionTransform(pos, static_cast(4), - static_cast(6)); + static_cast(16)); + + EXPECT_EQ(result, pos); + result = sc.VelocityTransform(pos, + math::SphericalCoordinates::SPHERICAL_DEG, + math::SphericalCoordinates::ECEF); + EXPECT_EQ(result, pos); + + result = sc.VelocityTransform(pos, + math::SphericalCoordinates::ECEF, + math::SphericalCoordinates::SPHERICAL_DEG); EXPECT_EQ(result, pos); result = sc.VelocityTransform(pos, - math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::SPHERICAL_RAD, math::SphericalCoordinates::ECEF); EXPECT_EQ(result, pos); result = sc.VelocityTransform(pos, math::SphericalCoordinates::ECEF, - math::SphericalCoordinates::SPHERICAL); + math::SphericalCoordinates::SPHERICAL_RAD); EXPECT_EQ(result, pos); result = sc.VelocityTransform(pos, - static_cast(7), + static_cast(17), math::SphericalCoordinates::ECEF); EXPECT_EQ(result, pos); result = sc.VelocityTransform(pos, math::SphericalCoordinates::ECEF, - static_cast(7)); + static_cast(17)); EXPECT_EQ(result, pos); } @@ -773,15 +817,28 @@ TEST(SphericalCoordinatesTest, Inverse) EXPECT_EQ(in, reverse); } - // SPHERICAL <-> LOCAL2 + // SPHERICAL_DEG <-> LOCAL2 + { + math::Vector3d in(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::SPHERICAL_DEG); + EXPECT_NE(in, out); + auto reverse = sc.PositionTransform(out, + math::SphericalCoordinates::SPHERICAL_DEG, + math::SphericalCoordinates::LOCAL2); + EXPECT_EQ(in, reverse); + } + + // SPHERICAL_RAD <-> LOCAL2 { math::Vector3d in(1, 2, -4); auto out = sc.PositionTransform(in, math::SphericalCoordinates::LOCAL2, - math::SphericalCoordinates::SPHERICAL); + math::SphericalCoordinates::SPHERICAL_RAD); EXPECT_NE(in, out); auto reverse = sc.PositionTransform(out, - math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::SPHERICAL_RAD, math::SphericalCoordinates::LOCAL2); EXPECT_EQ(in, reverse); } diff --git a/src/python_pybind11/src/SphericalCoordinates.cc b/src/python_pybind11/src/SphericalCoordinates.cc index f2d27ed2..e2a4dcd8 100644 --- a/src/python_pybind11/src/SphericalCoordinates.cc +++ b/src/python_pybind11/src/SphericalCoordinates.cc @@ -133,22 +133,26 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) "Update coordinate transformation matrix with reference location") .def("position_transform", &Class::PositionTransform, - "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame " - "Spherical coordinates use radians, while the other frames use " - "meters.") + "Convert between positions in SPHERICAL_*/ECEF/LOCAL/GLOBAL frame. " + "Cartesian frames use meters. SPHERICAL_DEG frame uses degrees. " + "SPHERICAL_RAD frame uses radians.") .def("velocity_transform", &Class::VelocityTransform, - "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame " - "Spherical coordinates use radians, while the other frames use " - "meters."); + "Convert between velocity in ECEF/LOCAL/GLOBAL frame ." + "Velocity should not be expressed in SPHERICAL_* frames (such values " + "will be passed through). All Cartesian frames use meters."); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION py::enum_(sphericalCoordinates, "CoordinateType") + .value("SPHERICAL_RAD", Class::CoordinateType::SPHERICAL_RAD) + .value("SPHERICAL_DEG", Class::CoordinateType::SPHERICAL_DEG) .value("SPHERICAL", Class::CoordinateType::SPHERICAL) .value("ECEF", Class::CoordinateType::ECEF) .value("GLOBAL", Class::CoordinateType::GLOBAL) .value("LOCAL", Class::CoordinateType::LOCAL) .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..b32959c2 100644 --- a/src/python_pybind11/test/SphericalCoordinates_TEST.py +++ b/src/python_pybind11/test/SphericalCoordinates_TEST.py @@ -250,7 +250,11 @@ def test_coordinate_transforms(self): # > latitude longitude altitude # > X Y Z tmp = Vector3d() - osrf_s = Vector3d(37.3877349, -122.0651166, 32.0) + osrf_s_deg = Vector3d(37.3877349, -122.0651166, 32.0) + osrf_s_rad = Vector3d( + math.radians(osrf_s_deg.x()), + math.radians(osrf_s_deg.y()), + osrf_s_deg.z()) osrf_e = Vector3d(-2693701.91434394, -4299942.14687992, 3851691.0393571) goog_s = Vector3d(37.4216719, -122.0821853, 30.0) @@ -262,36 +266,72 @@ def test_coordinate_transforms(self): # > -1510.88 3766.64 (EAST,NORTH) vec = Vector3d(-1510.88, 3766.64, -3.29) - # Convert degrees to radians - osrf_s.x(osrf_s.x() * 0.0174532925) - osrf_s.y(osrf_s.y() * 0.0174532925) - # Set the ORIGIN to be the Open Source Robotics Foundation - sc2 = SphericalCoordinates(st, Angle(osrf_s.x()), - Angle(osrf_s.y()), osrf_s.z(), Angle.ZERO) + sc2 = SphericalCoordinates(st, Angle(osrf_s_rad.x()), + Angle(osrf_s_rad.y()), osrf_s_rad.z(), + Angle.ZERO) - # Check that SPHERICAL -> ECEF works - tmp = sc2.position_transform(osrf_s, SphericalCoordinates.SPHERICAL, + # Check that SPHERICAL_DEG -> ECEF works + tmp = sc2.position_transform(osrf_s_deg, + SphericalCoordinates.SPHERICAL_DEG, SphericalCoordinates.ECEF) self.assertAlmostEqual(tmp.x(), osrf_e.x(), delta=8e-2) self.assertAlmostEqual(tmp.y(), osrf_e.y(), delta=8e-2) self.assertAlmostEqual(tmp.z(), osrf_e.z(), delta=1e-2) - # Check that ECEF -> SPHERICAL works - tmp = sc2.position_transform(tmp, SphericalCoordinates.ECEF, SphericalCoordinates.SPHERICAL) + # Check that SPHERICAL_RAD -> ECEF works + tmp = sc2.position_transform(osrf_s_rad, + SphericalCoordinates.SPHERICAL_RAD, + SphericalCoordinates.ECEF) + + self.assertAlmostEqual(tmp.x(), osrf_e.x(), delta=8e-2) + self.assertAlmostEqual(tmp.y(), osrf_e.y(), delta=8e-2) + self.assertAlmostEqual(tmp.z(), osrf_e.z(), delta=1e-2) + + # Check that ECEF -> SPHERICAL_DEG works + tmp = sc2.position_transform(osrf_e, + SphericalCoordinates.ECEF, + SphericalCoordinates.SPHERICAL_DEG) + + self.assertAlmostEqual(tmp.x(), osrf_s_deg.x(), delta=1e-2) + self.assertAlmostEqual(tmp.y(), osrf_s_deg.y(), delta=1e-2) + self.assertAlmostEqual(tmp.z(), osrf_s_deg.z(), delta=1e-2) + + # Check that ECEF -> SPHERICAL_RAD works + tmp = sc2.position_transform(osrf_e, + SphericalCoordinates.ECEF, + SphericalCoordinates.SPHERICAL_RAD) - self.assertAlmostEqual(tmp.x(), osrf_s.x(), delta=1e-2) - self.assertAlmostEqual(tmp.y(), osrf_s.y(), delta=1e-2) - self.assertAlmostEqual(tmp.z(), osrf_s.z(), delta=1e-2) + self.assertAlmostEqual(tmp.x(), osrf_s_rad.x(), delta=1e-2) + self.assertAlmostEqual(tmp.y(), osrf_s_rad.y(), delta=1e-2) + self.assertAlmostEqual(tmp.z(), osrf_s_rad.z(), delta=1e-2) - # Check that SPHERICAL -> LOCAL works + # Check that SPHERICAL_DEG -> SPHERICAL_RAD works + tmp = sc2.position_transform(osrf_s_deg, + SphericalCoordinates.SPHERICAL_DEG, + SphericalCoordinates.SPHERICAL_RAD) + + self.assertAlmostEqual(tmp.x(), osrf_s_rad.x(), delta=1e-2) + self.assertAlmostEqual(tmp.y(), osrf_s_rad.y(), delta=1e-2) + self.assertAlmostEqual(tmp.z(), osrf_s_rad.z(), delta=1e-2) + + # Check that SPHERICAL_RAD -> SPHERICAL_DEG works + tmp = sc2.position_transform(osrf_s_rad, + SphericalCoordinates.SPHERICAL_RAD, + SphericalCoordinates.SPHERICAL_DEG) + + self.assertAlmostEqual(tmp.x(), osrf_s_deg.x(), delta=1e-2) + self.assertAlmostEqual(tmp.y(), osrf_s_deg.y(), delta=1e-2) + self.assertAlmostEqual(tmp.z(), osrf_s_deg.z(), delta=1e-2) + + # Check that SPHERICAL_DEG -> LOCAL works tmp = sc2.local_from_spherical_position(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 + # Check that SPHERICAL_DEG -> LOCAL -> SPHERICAL_DEG works tmp = sc2.spherical_from_local_position(tmp) self.assertAlmostEqual(tmp.x(), goog_s.x(), delta=8e-2) self.assertAlmostEqual(tmp.y(), goog_s.y(), delta=8e-2) @@ -402,37 +442,49 @@ def test_bad_coordinate_type(self): sc = SphericalCoordinates() pos = Vector3d(1, 2, -4) result = sc.position_transform(pos, - SphericalCoordinates.CoordinateType(7), - SphericalCoordinates.CoordinateType(6)) + SphericalCoordinates.CoordinateType(17), + SphericalCoordinates.CoordinateType(16)) self.assertEqual(result, pos) result = sc.position_transform(pos, SphericalCoordinates.CoordinateType(4), - SphericalCoordinates.CoordinateType(6)) + SphericalCoordinates.CoordinateType(16)) self.assertEqual(result, pos) result = sc.velocity_transform( pos, - SphericalCoordinates.SPHERICAL, + SphericalCoordinates.SPHERICAL_DEG, SphericalCoordinates.ECEF) self.assertEqual(result, pos) result = sc.velocity_transform( pos, SphericalCoordinates.ECEF, - SphericalCoordinates.SPHERICAL) + SphericalCoordinates.SPHERICAL_DEG) + self.assertEqual(result, pos) + + result = sc.velocity_transform( + pos, + SphericalCoordinates.SPHERICAL_RAD, + SphericalCoordinates.ECEF) + self.assertEqual(result, pos) + + result = sc.velocity_transform( + pos, + SphericalCoordinates.ECEF, + SphericalCoordinates.SPHERICAL_RAD) self.assertEqual(result, pos) result = sc.velocity_transform(pos, - SphericalCoordinates.CoordinateType(7), + SphericalCoordinates.CoordinateType(17), SphericalCoordinates.ECEF) self.assertEqual(result, pos) result = sc.velocity_transform(pos, SphericalCoordinates.ECEF, - SphericalCoordinates.CoordinateType(7)) + SphericalCoordinates.CoordinateType(17)) self.assertEqual(result, pos) def test_equality_ops(self): @@ -656,16 +708,29 @@ def test_inverse(self): SphericalCoordinates.LOCAL2) self.assertEqual(in_vector, reverse) - # SPHERICAL <-> LOCAL2 + # SPHERICAL_DEG <-> LOCAL2 + in_vector = Vector3d(1, 2, -4) + out = sc.position_transform( + in_vector, + SphericalCoordinates.LOCAL2, + SphericalCoordinates.SPHERICAL_DEG) + self.assertNotEqual(in_vector, out) + reverse = sc.position_transform( + out, + SphericalCoordinates.SPHERICAL_DEG, + SphericalCoordinates.LOCAL2) + self.assertEqual(in_vector, reverse) + + # SPHERICAL_RAD <-> LOCAL2 in_vector = Vector3d(1, 2, -4) out = sc.position_transform( in_vector, SphericalCoordinates.LOCAL2, - SphericalCoordinates.SPHERICAL) + SphericalCoordinates.SPHERICAL_RAD) self.assertNotEqual(in_vector, out) reverse = sc.position_transform( out, - SphericalCoordinates.SPHERICAL, + SphericalCoordinates.SPHERICAL_RAD, SphericalCoordinates.LOCAL2) self.assertEqual(in_vector, reverse) diff --git a/src/ruby/SphericalCoordinates.i b/src/ruby/SphericalCoordinates.i index 801536e2..e447014e 100644 --- a/src/ruby/SphericalCoordinates.i +++ b/src/ruby/SphericalCoordinates.i @@ -41,9 +41,15 @@ namespace gz public: enum CoordinateType { - /// \brief Latitude, Longitude and Altitude by SurfaceType + /// \brief Latitude, Longitude and Altitude by SurfaceType [rad] SPHERICAL = 1, + /// \brief Latitude, Longitude and Altitude by SurfaceType [rad] + SPHERICAL_RAD = 1, + + /// \brief Latitude, Longitude and Altitude by SurfaceType [deg] + SPHERICAL_DEG = 6, + /// \brief Earth centered, earth fixed Cartesian ECEF = 2,