diff --git a/include/gz/math/Angle.hh b/include/gz/math/Angle.hh index de508778..e6c1691c 100644 --- a/include/gz/math/Angle.hh +++ b/include/gz/math/Angle.hh @@ -120,6 +120,15 @@ namespace gz::math /// \return The normalized value of this Angle. public: Angle Normalized() const; + /// \brief Return the absolute value of this Angle. + /// \return The absolute value of this Angle. + public: Angle Abs() const; + + /// \brief Return the shortest angular distance between this and the + /// other angle. + /// \return The shortest distance. + public: Angle ShortestDistance(const Angle &_other) const; + /// \brief Return the angle's radian value /// \return double containing the angle's radian value public: double operator()() const; @@ -131,6 +140,10 @@ namespace gz::math return value; } + /// \brief Negation operator, result = -this. + /// \return The new angle. + public: Angle operator-() const; + /// \brief Subtraction operator, result = this - _angle. /// \param[in] _angle Angle for subtraction. /// \return The new angle. diff --git a/include/gz/math/CoordinateVector3.hh b/include/gz/math/CoordinateVector3.hh new file mode 100644 index 00000000..e3ade9ca --- /dev/null +++ b/include/gz/math/CoordinateVector3.hh @@ -0,0 +1,257 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_MATH_COORDINATE_VECTOR3_HH_ +#define GZ_MATH_COORDINATE_VECTOR3_HH_ + +#include +#include + +#include +#include +#include +#include +#include + +namespace gz::math +{ + // Inline bracket to help doxygen filtering. + inline namespace GZ_MATH_VERSION_NAMESPACE { + + /// \class CoordinateVector3 CoordinateVector3.hh gz/math/CoordinateVector3.hh + /// \brief The CoordinateVector3 class represents the vector containing 3 + /// coordinates, either metric or spherical. + class GZ_MATH_VISIBLE CoordinateVector3 + { + /// \brief Construct an empty metric vector. + public: CoordinateVector3(); + + public: ~CoordinateVector3(); + + /// \brief Copy constructor + /// \param[in] _other The copied value. + public: CoordinateVector3(const CoordinateVector3 &_other); + + public: CoordinateVector3(CoordinateVector3 &&_other) noexcept; + + /// \brief Constructor for metric values. + /// \param[in] _x value along x + /// \param[in] _y value along y + /// \param[in] _z value along z + /// \return The coordinate vector. + public: static CoordinateVector3 Metric(double _x, double _y, double _z); + + /// \brief Constructor for metric values. + /// \param[in] _v The metric vector. + /// \return The coordinate vector. + public: static CoordinateVector3 Metric(const math::Vector3d &_v); + + /// \brief Constructor for spherical values. + /// \param[in] _lat latitude value + /// \param[in] _lon longitude value + /// \param[in] _z value along z + /// \return The coordinate vector. + public: static CoordinateVector3 Spherical( + const math::Angle &_lat, const math::Angle &_lon, double _z); + + /// \brief Whether this vector is metric. + /// \return Whether this vector is metric. + public: bool IsMetric() const; + + /// \brief Whether this vector is spherical. + /// \return Whether this vector is spherical. + public: bool IsSpherical() const; + + /// \brief Set the metric contents of the vector + /// \param[in] _x value along x + /// \param[in] _y value along y + /// \param[in] _z value aling z + public: void SetMetric(double _x, double _y, double _z); + + /// \brief Set the metric contents of the vector + /// \param[in] _v The metric vector. + public: void SetMetric(const math::Vector3d &_v); + + /// \brief Set the spherical contents of the vector + /// \param[in] _lat latitude value + /// \param[in] _lon longitude value + /// \param[in] _z value along z + public: void SetSpherical( + const math::Angle &_lat, const math::Angle &_lon, double _z); + + /// \brief Return this vector as a metric Vector3d (only valid for metric). + /// \return The metric vector (or nullopt if the vector is not metric). + public: std::optional AsMetricVector() const; + + /// \brief Copy assignment + /// \param[in] _other The copied value. + /// \return Reference to this. + public: CoordinateVector3 &operator=(const CoordinateVector3 &_other); + + /// \brief Move assignment + /// \param[in] _other The copied value. + /// \return Reference to this. + public: CoordinateVector3 &operator=(CoordinateVector3 &&_other) noexcept; + + /// \brief Addition operator + /// \param[in] _v vector to add + /// \return the sum vector + /// \note If one vector is metric and the other is spherical, a NaN + /// vector will be returned and a message logged to cerr. + public: CoordinateVector3 operator+(const CoordinateVector3 &_v) const; + + /// \brief Addition assignment operator + /// \param[in] _v vector to add + /// \return the sum vector + /// \note If one vector is metric and the other is spherical, a NaN + /// vector will be set and a message logged to cerr. + public: const CoordinateVector3 &operator+=(const CoordinateVector3 &_v); + + /// \brief Negation operator + /// \return negative of this vector + public: CoordinateVector3 operator-() const; + + /// \brief Subtraction operators + /// \param[in] _pt a vector to subtract + /// \return a vector after the subtraction + /// \note If one vector is metric and the other is spherical, a NaN + /// vector will be returned and a message logged to cerr. + public: CoordinateVector3 operator-(const CoordinateVector3 &_pt) const; + + /// \brief Subtraction assignment operators + /// \param[in] _pt subtrahend + /// \return a vector after the subtraction + /// \note If one vector is metric and the other is spherical, a NaN + /// vector will be set and a message logged to cerr. + public: const CoordinateVector3 &operator-=(const CoordinateVector3 &_pt); + + /// \brief Equality test with tolerance. + /// \param[in] _v the vector to compare to + /// \param[in] _tol equality tolerance for metric components. + /// \param[in] _ang_tol equality tolerance for spherical components. + /// \return true if the vectors are equal within the tolerance specified by + /// _tol and _ang_tol. + public: bool Equal(const CoordinateVector3 &_v, + const double &_tol, + const math::Angle &_ang_tol) const; + + /// \brief Equal to operator + /// \param[in] _v The vector to compare against + /// \return true if the vectors are equal within a default tolerance (1e-3), + /// false otherwise + public: bool operator==(const CoordinateVector3 &_v) const; + + /// \brief Not equal to operator + /// \param[in] _v The vector to compare against + /// \return false if the vectors are not equal within a default tolerance + /// (1e-3), true otherwise + public: bool operator!=(const CoordinateVector3 &_v) const; + + /// \brief See if all vector components are finite (e.g., not nan) + /// \return true if is finite or false otherwise + public: bool IsFinite() const; + + /// \brief Equality test + /// \remarks This is equivalent to the == operator + /// \param[in] _v the other vector + /// \return true if the 2 vectors have the same values, false otherwise + public: bool Equal(const CoordinateVector3 &_v) const; + + /// \brief Get the x value of a metric vector. + /// \return The x component of the metric vector (or nullopt if spherical). + public: std::optional X() const; + + /// \brief Get the latitude of a spherical vector. + /// \return The latitude of the spherical vector (or nullopt if metric). + public: std::optional Lat() const; + + /// \brief Get the y value of a metric vector. + /// \return The y component of the metric vector (or nullopt if spherical). + public: std::optional Y() const; + + /// \brief Get the longitude of a spherical vector. + /// \return The longitude of the spherical vector (or nullopt if metric). + public: std::optional Lon() const; + + /// \brief Get the z value. + /// \return The z component of the vector (nullopt is never returned). + public: std::optional Z() const; + + /// \brief Set the x value. + /// \param[in] _v Value for the x component. + /// \return True if the vector is metric, false otherwise. + public: bool X(const double &_v); + + /// \brief Set the latitude. + /// \param[in] _v Value for the latitude. + /// \return True if the vector is spherical, false otherwise. + public: bool Lat(const Angle &_v); + + /// \brief Set the y value. + /// \param[in] _v Value for the y component. + /// \return True if the vector is metric, false otherwise. + public: bool Y(const double &_v); + + /// \brief Set the longitude. + /// \param[in] _v Value for the longitude. + /// \return True if the vector is spherical, false otherwise. + public: bool Lon(const Angle &_v); + + /// \brief Set the z value. + /// \param[in] _v Value for the z component. + /// \return Always true. + public: bool Z(const double &_v); + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _pt CoordinateVector3 to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, const gz::math::CoordinateVector3 &_pt) + { + if (_pt.IsMetric()) + { + appendToStream(_out, *_pt.X()); + } + else + { + appendToStream(_out, _pt.Lat()->Degree()); + _out << "°"; + } + _out << " "; + + if (_pt.IsMetric()) + { + appendToStream(_out, *_pt.Y()); + } + else + { + appendToStream(_out, _pt.Lon()->Degree()); + _out << "°"; + } + _out << " "; + + appendToStream(_out, *_pt.Z()); + + return _out; + } + + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; + + } // namespace GZ_MATH_VERSION_NAMESPACE +} // namespace gz::math +#endif // GZ_MATH_COORDINATE_VECTOR3_HH_ diff --git a/include/gz/math/SphericalCoordinates.hh b/include/gz/math/SphericalCoordinates.hh index 8cd36244..ada3bd26 100644 --- a/include/gz/math/SphericalCoordinates.hh +++ b/include/gz/math/SphericalCoordinates.hh @@ -17,14 +17,23 @@ #ifndef GZ_MATH_SPHERICALCOORDINATES_HH_ #define GZ_MATH_SPHERICALCOORDINATES_HH_ +#include #include #include +#include #include #include #include #include +// MSVC doesn't like deprecating enum values with function macros +#if _MSC_VER +#define GZ_LOCAL2_DEPRECATED [[deprecated]] +#else +#define GZ_LOCAL2_DEPRECATED GZ_DEPRECATED(8) +#endif + namespace gz::math { // Inline bracket to help doxygen filtering. @@ -64,12 +73,13 @@ namespace gz::math GLOBAL = 3, /// \brief Heading-adjusted tangent plane (X, Y, Z) - /// This has kept a bug for backwards compatibility, use - /// LOCAL2 for the correct behaviour. LOCAL = 4, /// \brief Heading-adjusted tangent plane (X, Y, Z) - LOCAL2 = 5 + /// \deprecated The computation bugs for LOCAL have been fixed + /// in Gazebo Ionic (8) so the temporary LOCAL2 type + /// is no more needed. + LOCAL2 GZ_LOCAL2_DEPRECATED = 5 }; /// \brief Constructor. @@ -104,33 +114,53 @@ 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 There's a known bug with this computation that can't be + /// fixed until Gazebo 8 to avoid behaviour changes. + /// Call `SphericalFromLocalPosition(CoordinateVector3)` for correct + /// results. /// /// \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( + public: GZ_DEPRECATED(8) gz::math::Vector3d SphericalFromLocalPosition( const gz::math::Vector3d &_xyz) const; + /// \brief Convert a Cartesian position vector to geodetic coordinates. + /// This performs a `PositionTransform` from LOCAL to SPHERICAL. + /// + /// \param[in] _xyz Cartesian position vector in the heading-adjusted + /// world frame. + /// \return Cooordinates: geodetic latitude, longitude, + /// altitude above sea level. + public: std::optional SphericalFromLocalPosition( + const gz::math::CoordinateVector3 &_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, 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 There's a known bug with this computation that can't be + /// fixed until Gazebo 8 to avoid behaviour changes. + /// Call `GlobalFromLocalVelocity(CoordinateVector3)` for correct + /// results. /// /// \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_DEPRECATED(8) gz::math::Vector3d GlobalFromLocalVelocity( 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, 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: std::optional GlobalFromLocalVelocity( + const gz::math::CoordinateVector3 &_xyz) const; + /// \brief Convert a string to a SurfaceType. /// Allowed values: ["EARTH_WGS84"]. /// \param[in] _str String to convert. @@ -245,43 +275,100 @@ namespace gz::math /// \brief Convert a geodetic position vector to Cartesian coordinates. /// This performs a `PositionTransform` from SPHERICAL to LOCAL. + /// \deprecated Use `LocalFromSphericalPosition(CoordinateVector3)` instead. /// \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_DEPRECATED(8) gz::math::Vector3d LocalFromSphericalPosition( const gz::math::Vector3d &_latLonEle) 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, Y: longitude, Z: altitude. + /// \return Cartesian position vector in the heading-adjusted world frame. + public: std::optional LocalFromSphericalPosition( + const gz::math::CoordinateVector3 &_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 `LocalFromSphericalPosition(CoordinateVector3)` instead. /// \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_DEPRECATED(8) gz::math::Vector3d LocalFromGlobalVelocity( const gz::math::Vector3d &_xyz) 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: std::optional LocalFromGlobalVelocity( + const gz::math::CoordinateVector3 &_xyz) const; + /// \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. - /// \param[in] _pos Position vector in frame defined by parameter _in + /// using the reference point. + /// \deprecated To keep compatibility with Gazebo 7 and earlier, this + /// function returns incorrect result for LOCAL->SPHERICAL + /// and LOCAL->GLOBAL cases. Preferably, switch to using + /// `PositionTransform(CoordinateVector3, ..., LOCAL)` which + /// yields correct results. If you need to use this deprecated + /// overload, use coordinate type LOCAL2 to get the correct + /// result. + /// \param[in] _pos Position vector in frame defined by parameter _in. + /// If it is spherical, it has to be in radians. /// \param[in] _in CoordinateType for input /// \param[in] _out CoordinateType for output - /// \return Transformed coordinate using cached origin. - public: gz::math::Vector3d + /// \return Transformed coordinate using cached origin. Spherical + /// coordinates will be in radians. In case of error, `_pos` is + /// returned unchanged. + public: GZ_DEPRECATED(8) gz::math::Vector3d 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 positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame + /// using the cached reference point. + /// \param[in] _pos Position vector in frame defined by parameter _in + /// \param[in] _in CoordinateType for input + /// \param[in] _out CoordinateType for output + /// \return Transformed coordinates (if the transformation succeeded). + public: std::optional + PositionTransform(const gz::math::CoordinateVector3 &_pos, + const CoordinateType &_in, const CoordinateType &_out) const; + + /// \brief Convert between velocity in ECEF/LOCAL/GLOBAL frame using the + /// cached reference point. + /// \note Spherical coordinates are not supported. + /// \deprecated To keep compatibility with Gazebo 7 and earlier, this + /// function returns incorrect result for LOCAL->SPHERICAL + /// and LOCAL->GLOBAL cases. Preferably, switch to using + /// `VelocityTransform(CoordinateVector3, ..., LOCAL)` which + /// yields correct results. If you need to use this deprecated + /// overload, use coordinate type LOCAL2 to get the correct + /// result. /// \param[in] _vel Velocity vector in frame defined by parameter _in /// \param[in] _in CoordinateType for input /// \param[in] _out CoordinateType for output - /// \return Transformed velocity vector - public: gz::math::Vector3d VelocityTransform( + /// \return Transformed velocity vector. In case of error, `_vel` is + /// returned unchanged. + public: GZ_DEPRECATED(8) gz::math::Vector3d VelocityTransform( const gz::math::Vector3d &_vel, const CoordinateType &_in, const CoordinateType &_out) const; + /// \brief Convert between velocity in ECEF/LOCAL/GLOBAL frame. + /// \note Spherical coordinates are not supported. + /// \param[in] _vel Velocity vector in frame defined by parameter _in + /// \param[in] _in CoordinateType for input + /// \param[in] _out CoordinateType for output + /// \return Transformed velocity vector (if the transformation succeeded). + public: std::optional VelocityTransform( + const gz::math::CoordinateVector3 &_vel, + const CoordinateType &_in, const CoordinateType &_out) const; + /// \brief Equality operator, result = this == _sc /// \param[in] _sc Spherical coordinates to check for equality /// \return true if this == _sc diff --git a/src/Angle.cc b/src/Angle.cc index fc566d03..89064f31 100644 --- a/src/Angle.cc +++ b/src/Angle.cc @@ -71,6 +71,26 @@ Angle Angle::Normalized() const return atan2(sin(this->value), cos(this->value)); } +////////////////////////////////////////////////// +Angle Angle::Abs() const +{ + return {std::fabs(this->value)}; +} + +////////////////////////////////////////////////// +Angle Angle::ShortestDistance(const gz::math::Angle& _other) const +{ + const double diff = std::fmod( + _other.value - this->value + GZ_PI, 2 * GZ_PI) - GZ_PI; + return {diff < -GZ_PI ? diff + 2 * GZ_PI : diff}; +} + +////////////////////////////////////////////////// +Angle Angle::operator-() const +{ + return {-this->value}; +} + ////////////////////////////////////////////////// Angle Angle::operator-(const Angle &_angle) const { diff --git a/src/Angle_TEST.cc b/src/Angle_TEST.cc index 7e6089b0..a8dd1b6f 100644 --- a/src/Angle_TEST.cc +++ b/src/Angle_TEST.cc @@ -105,9 +105,32 @@ TEST(AngleTest, Angle) EXPECT_TRUE(angle >= 1.19); EXPECT_TRUE(angle >= 1.2); EXPECT_TRUE(angle >= -1.19); + EXPECT_EQ(-angle, math::Angle(-1.2)); + EXPECT_EQ(-(-angle), angle); EXPECT_TRUE(math::Angle(1.2) >= math::Angle(1.2000000001)); EXPECT_TRUE(math::Angle(1.2000000001) >= math::Angle(1.2)); + + EXPECT_DOUBLE_EQ(math::Angle(1.2).Abs().Radian(), 1.2); + EXPECT_DOUBLE_EQ(math::Angle(-1.2).Abs().Radian(), 1.2); + EXPECT_DOUBLE_EQ(math::Angle(0).Abs().Radian(), 0); + EXPECT_DOUBLE_EQ(math::Angle(-300).Abs().Radian(), 300); + EXPECT_DOUBLE_EQ(math::Angle(300).Abs().Radian(), 300); + + EXPECT_DOUBLE_EQ( + math::Angle(1.0).ShortestDistance(math::Angle(1.0)).Radian(), 0.0); + EXPECT_DOUBLE_EQ( + math::Angle(1.0).ShortestDistance(math::Angle(-1.0)).Radian(), -2.0); + EXPECT_DOUBLE_EQ( + math::Angle(-1.0).ShortestDistance(math::Angle(1.0)).Radian(), 2.0); + EXPECT_DOUBLE_EQ( + math::Angle(-1.0).ShortestDistance(math::Angle(-1.0)).Radian(), 0.0); + EXPECT_DOUBLE_EQ( + math::Angle(-2.0).ShortestDistance(math::Angle(2.0)).Radian(), + -(2 * GZ_PI - 4.0)); + EXPECT_DOUBLE_EQ( + math::Angle(2.0).ShortestDistance(math::Angle(-2.0)).Radian(), + 2 * GZ_PI - 4.0); } ///////////////////////////////////////////////// diff --git a/src/CoordinateVector3.cc b/src/CoordinateVector3.cc new file mode 100644 index 00000000..dfc13f84 --- /dev/null +++ b/src/CoordinateVector3.cc @@ -0,0 +1,511 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License") + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "gz/math/Angle.hh" +#include "gz/math/CoordinateVector3.hh" +#include "gz/math/Vector3.hh" +#include "gz/math/Helpers.hh" +#include "gz/utils/ImplPtr.hh" + +#include +#include +#include +#include + +/// \brief Private data for the CoordinateVector3 class. +class gz::math::CoordinateVector3::Implementation +{ + /// \brief The metric x coordinate or spherical latitude. + public: std::variant x_or_lat; + /// \brief The metric y coordinate or spherical longitude. + public: std::variant y_or_lon; + /// \brief The z coordinate (always metric). + public: double z; + + /// \brief Raw access to the stored metric X value without type checking. + public: double X() const + { + return std::get(this->x_or_lat); + } + + /// \brief Raw access to the stored metric X value without type checking. + public: double& X() + { + return std::get(this->x_or_lat); + } + + /// \brief Raw access to the stored spherical Lat value without type checking. + public: const math::Angle& Lat() const + { + return std::get(this->x_or_lat); + } + + /// \brief Raw access to the stored spherical Lat value without type checking. + public: math::Angle& Lat() + { + return std::get(this->x_or_lat); + } + + /// \brief Raw access to the stored metric Y value without type checking. + public: double Y() const + { + return std::get(this->y_or_lon); + } + + /// \brief Raw access to the stored metric Y value without type checking. + public: double& Y() + { + return std::get(this->y_or_lon); + } + + /// \brief Raw access to the stored spherical Lon value without type checking. + public: const math::Angle& Lon() const + { + return std::get(this->y_or_lon); + } + + /// \brief Raw access to the stored spherical Lon value without type checking. + public: math::Angle& Lon() + { + return std::get(this->y_or_lon); + } + + /// \brief Raw access to the stored Z value without type checking. + public: double Z() const + { + return this->z; + } + + /// \brief Raw access to the stored Z value without type checking. + public: double& Z() + { + return this->z; + } + +}; + +using namespace gz; +using namespace math; + +///////////////////////////////////////////////// +CoordinateVector3::CoordinateVector3() : + dataPtr(gz::utils::MakeUniqueImpl(0.0, 0.0, 0.0)) +{ +} + +///////////////////////////////////////////////// +CoordinateVector3::~CoordinateVector3() = default; + +///////////////////////////////////////////////// +CoordinateVector3::CoordinateVector3(const CoordinateVector3 &_other) : + dataPtr(gz::utils::MakeUniqueImpl()) +{ + *this->dataPtr = *_other.dataPtr; +} + +///////////////////////////////////////////////// +CoordinateVector3::CoordinateVector3(CoordinateVector3 &&_other) noexcept : + dataPtr(std::exchange(_other.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +CoordinateVector3& CoordinateVector3::operator=(const CoordinateVector3 &_other) +{ + // If the object was moved from, dataPtr will be null + if (!this->dataPtr) + this->dataPtr = gz::utils::MakeUniqueImpl(); + + *this->dataPtr = *_other.dataPtr; + return *this; +} + +///////////////////////////////////////////////// +CoordinateVector3& +CoordinateVector3::operator=(CoordinateVector3 &&_other) noexcept +{ + std::swap(this->dataPtr, _other.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +CoordinateVector3 CoordinateVector3::Metric( + const double _x, const double _y, const double _z) +{ + CoordinateVector3 res; + res.SetMetric(_x, _y, _z); + return res; +} + +///////////////////////////////////////////////// +CoordinateVector3 CoordinateVector3::Metric(const Vector3d &_v) +{ + return Metric(_v.X(), _v.Y(), _v.Z()); +} + +///////////////////////////////////////////////// +CoordinateVector3 CoordinateVector3::Spherical( + const Angle &_lat, const Angle &_lon, const double _z) +{ + CoordinateVector3 res; + res.SetSpherical(_lat, _lon, _z); + return res; +} + +///////////////////////////////////////////////// +bool CoordinateVector3::IsMetric() const +{ + return std::holds_alternative(this->dataPtr->x_or_lat); +} + +///////////////////////////////////////////////// +bool CoordinateVector3::IsSpherical() const +{ + return std::holds_alternative(this->dataPtr->x_or_lat); +} + +///////////////////////////////////////////////// +void CoordinateVector3::SetMetric( + const double _x, const double _y, const double _z) +{ + this->dataPtr->x_or_lat = _x; + this->dataPtr->y_or_lon = _y; + this->dataPtr->z = _z; +} + +///////////////////////////////////////////////// +void CoordinateVector3::SetMetric(const Vector3d &_v) +{ + this->SetMetric(_v.X(), _v.Y(), _v.Z()); +} + +///////////////////////////////////////////////// +void CoordinateVector3::SetSpherical( + const Angle &_lat, const Angle &_lon, double _z) +{ + this->dataPtr->x_or_lat = _lat; + this->dataPtr->y_or_lon = _lon; + this->dataPtr->z = _z; +} + +///////////////////////////////////////////////// +std::optional CoordinateVector3::AsMetricVector() const +{ + if (!this->IsMetric()) + return std::nullopt; + + return math::Vector3d{ + this->dataPtr->X(), this->dataPtr->Y(), this->dataPtr->Z()}; +} + +///////////////////////////////////////////////// +CoordinateVector3 CoordinateVector3::operator+( + const CoordinateVector3 &_v) const +{ + if (this->IsMetric() != _v.IsMetric()) + { + if (this->IsMetric()) + { + std::cerr << "Spherical coordinates cannot be added to metric. " + "Returning NaN." << std::endl; + return Metric(NAN_D, NAN_D, NAN_D); + } + else + { + std::cerr << "Metric coordinates cannot be added to spherical. " + "Returning NaN." << std::endl; + return Spherical(NAN_D, NAN_D, NAN_D); + } + } + + if (this->IsMetric()) + { + return CoordinateVector3::Metric( + this->dataPtr->X() + _v.dataPtr->X(), + this->dataPtr->Y() + _v.dataPtr->Y(), + this->dataPtr->Z() + _v.dataPtr->Z()); + } + else + { + return CoordinateVector3::Spherical( + this->dataPtr->Lat() + _v.dataPtr->Lat(), + this->dataPtr->Lon() + _v.dataPtr->Lon(), + this->dataPtr->Z() + _v.dataPtr->Z()); + } +} + +///////////////////////////////////////////////// +const CoordinateVector3& CoordinateVector3::operator+=( + const CoordinateVector3 &_v) +{ + if (this->IsMetric() != _v.IsMetric()) + { + this->dataPtr->Z() = NAN_D; + if (this->IsMetric()) + { + this->dataPtr->X() = this->dataPtr->Y() = NAN_D; + std::cerr << "Spherical coordinates cannot be added to metric. " + "Setting the result to NaN." << std::endl; + } + else + { + this->dataPtr->Lat() = this->dataPtr->Lon() = NAN_D; + std::cerr << "Metric coordinates cannot be added to spherical. " + "Setting the result to NaN." << std::endl; + } + return *this; + } + + if (this->IsMetric()) + { + this->dataPtr->X() += _v.dataPtr->X(); + this->dataPtr->Y() += _v.dataPtr->Y(); + } + else + { + this->dataPtr->Lat() += _v.dataPtr->Lat(); + this->dataPtr->Lon() += _v.dataPtr->Lon(); + } + this->dataPtr->Z() += _v.dataPtr->Z(); + + return *this; +} + +///////////////////////////////////////////////// +CoordinateVector3 CoordinateVector3::operator-() const +{ + if (this->IsMetric()) + { + return Metric( + -this->dataPtr->X(), -this->dataPtr->Y(), -this->dataPtr->Z()); + } + else + { + return Spherical( + -this->dataPtr->Lat(), -this->dataPtr->Lon(), -this->dataPtr->Z()); + } +} + +///////////////////////////////////////////////// +CoordinateVector3 CoordinateVector3::operator-( + const CoordinateVector3 &_pt) const +{ + if (this->IsMetric() != _pt.IsMetric()) + { + if (this->IsMetric()) + { + std::cerr << "Spherical coordinates cannot be subtracted from metric. " + "Returning NaN." << std::endl; + return Metric(NAN_D, NAN_D, NAN_D); + } + else + { + std::cerr << "Metric coordinates cannot be subtracted from spherical. " + "Returning NaN." << std::endl; + return Spherical(NAN_D, NAN_D, NAN_D); + } + } + + if (this->IsMetric()) + { + return Metric( + this->dataPtr->X() - _pt.dataPtr->X(), + this->dataPtr->Y() - _pt.dataPtr->Y(), + this->dataPtr->Z() - _pt.dataPtr->Z()); + } + else + { + return Spherical( + this->dataPtr->Lat() - _pt.dataPtr->Lat(), + this->dataPtr->Lon() - _pt.dataPtr->Lon(), + this->dataPtr->Z() - _pt.dataPtr->Z()); + } +} + +///////////////////////////////////////////////// +const CoordinateVector3& CoordinateVector3::operator-=( + const CoordinateVector3 &_pt) +{ + if (this->IsMetric() != _pt.IsMetric()) + { + this->dataPtr->Z() = NAN_D; + if (this->IsMetric()) + { + this->dataPtr->X() = this->dataPtr->Y() = NAN_D; + std::cerr << "Spherical coordinates cannot be subtracted from metric. " + "Setting the result to NaN." << std::endl; + } + else + { + this->dataPtr->Lat() = this->dataPtr->Lon() = NAN_D; + std::cerr << "Metric coordinates cannot be subtracted from spherical. " + "Setting the result to NaN." << std::endl; + } + return *this; + } + + if (this->IsMetric()) + { + this->dataPtr->X() -= _pt.dataPtr->X(); + this->dataPtr->Y() -= _pt.dataPtr->Y(); + } + else + { + this->dataPtr->Lat() -= _pt.dataPtr->Lat(); + this->dataPtr->Lon() -= _pt.dataPtr->Lon(); + } + this->dataPtr->Z() -= _pt.dataPtr->Z(); + + return *this; +} + +///////////////////////////////////////////////// +bool CoordinateVector3::Equal( + const CoordinateVector3 &_v, const double &_tol, const Angle &_ang_tol) const +{ + if (this->IsMetric() != _v.IsMetric()) + return false; + + if (!equal(this->dataPtr->Z(), _v.dataPtr->Z(), _tol)) + return false; + + if (this->IsMetric()) + { + return equal(this->dataPtr->X(), _v.dataPtr->X(), _tol) && + equal(this->dataPtr->Y(), _v.dataPtr->Y(), _tol); + } + else + { + return + this->dataPtr->Lat().ShortestDistance( + _v.dataPtr->Lat()).Abs() <= _ang_tol && + this->dataPtr->Lon().ShortestDistance( + _v.dataPtr->Lon()).Abs() <= _ang_tol; + } +} + +///////////////////////////////////////////////// +bool CoordinateVector3::operator==(const CoordinateVector3 &_v) const +{ + return this->Equal(_v, 1e-3, 1e-3); +} + +///////////////////////////////////////////////// +bool CoordinateVector3::operator!=(const CoordinateVector3 &_v) const +{ + return !(*this == _v); +} + +///////////////////////////////////////////////// +bool CoordinateVector3::IsFinite() const +{ + if (!std::isfinite(this->dataPtr->Z())) + return false; + if (this->IsMetric()) + return std::isfinite(this->dataPtr->X()) && + std::isfinite(this->dataPtr->Y()); + else + return std::isfinite(this->dataPtr->Lat().Radian()) && + std::isfinite(this->dataPtr->Lon().Radian()); +} + +///////////////////////////////////////////////// +bool CoordinateVector3::Equal(const CoordinateVector3 &_v) const +{ + return *this == _v; +} + +///////////////////////////////////////////////// +std::optional CoordinateVector3::X() const +{ + if (!this->IsMetric()) + return std::nullopt; + return this->dataPtr->X(); +} + +///////////////////////////////////////////////// +std::optional CoordinateVector3::Lat() const +{ + if (!this->IsSpherical()) + return std::nullopt; + return this->dataPtr->Lat(); +} + +///////////////////////////////////////////////// +std::optional CoordinateVector3::Y() const +{ + if (!this->IsMetric()) + return std::nullopt; + return this->dataPtr->Y(); +} + +///////////////////////////////////////////////// +std::optional CoordinateVector3::Lon() const +{ + if (!this->IsSpherical()) + return std::nullopt; + return this->dataPtr->Lon(); +} + +///////////////////////////////////////////////// +std::optional CoordinateVector3::Z() const +{ + return this->dataPtr->Z(); +} + +///////////////////////////////////////////////// +bool CoordinateVector3::X(const double &_v) +{ + if (!this->IsMetric()) + return false; + this->dataPtr->X() = _v; + return true; +} + +///////////////////////////////////////////////// +bool CoordinateVector3::Lat(const Angle &_v) +{ + if (!this->IsSpherical()) + return false; + this->dataPtr->Lat() = _v; + return true; +} + +///////////////////////////////////////////////// +bool CoordinateVector3::Y(const double &_v) +{ + if (!this->IsMetric()) + return false; + this->dataPtr->Y() = _v; + return true; +} + +///////////////////////////////////////////////// +bool CoordinateVector3::Lon(const Angle &_v) +{ + if (!this->IsSpherical()) + return false; + this->dataPtr->Lon() = _v; + return true; +} + +///////////////////////////////////////////////// +bool CoordinateVector3::Z(const double &_v) +{ + this->dataPtr->z = _v; + return true; +} diff --git a/src/CoordinateVector3_TEST.cc b/src/CoordinateVector3_TEST.cc new file mode 100644 index 00000000..97d9205d --- /dev/null +++ b/src/CoordinateVector3_TEST.cc @@ -0,0 +1,440 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include + +#include "gz/math/Angle.hh" +#include "gz/math/CoordinateVector3.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Vector3.hh" + +using namespace gz; + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, Construction) +{ + // Empty constructor + math::CoordinateVector3 vec; + EXPECT_FALSE(vec.IsSpherical()); + ASSERT_TRUE(vec.IsMetric()); + ASSERT_TRUE(vec.X().has_value()); + ASSERT_TRUE(vec.Y().has_value()); + ASSERT_TRUE(vec.Z().has_value()); + EXPECT_FALSE(vec.Lat().has_value()); + EXPECT_FALSE(vec.Lon().has_value()); + EXPECT_EQ(*vec.X(), 0.0); + EXPECT_EQ(*vec.Y(), 0.0); + EXPECT_EQ(*vec.Z(), 0.0); + + // Metric constructor + auto vec_m{math::CoordinateVector3::Metric(1, 2, 3)}; + EXPECT_FALSE(vec_m.IsSpherical()); + ASSERT_TRUE(vec_m.IsMetric()); + ASSERT_TRUE(vec_m.X().has_value()); + ASSERT_TRUE(vec_m.Y().has_value()); + ASSERT_TRUE(vec_m.Z().has_value()); + EXPECT_FALSE(vec_m.Lat().has_value()); + EXPECT_FALSE(vec_m.Lon().has_value()); + EXPECT_EQ(*vec_m.X(), 1.0); + EXPECT_EQ(*vec_m.Y(), 2.0); + EXPECT_EQ(*vec_m.Z(), 3.0); + + // Metric constructor from vector + auto vec_m2{math::CoordinateVector3::Metric(math::Vector3d{1, 2, 3})}; + EXPECT_FALSE(vec_m2.IsSpherical()); + ASSERT_TRUE(vec_m2.IsMetric()); + ASSERT_TRUE(vec_m2.X().has_value()); + ASSERT_TRUE(vec_m2.Y().has_value()); + ASSERT_TRUE(vec_m2.Z().has_value()); + EXPECT_FALSE(vec_m2.Lat().has_value()); + EXPECT_FALSE(vec_m2.Lon().has_value()); + EXPECT_EQ(*vec_m2.X(), 1.0); + EXPECT_EQ(*vec_m2.Y(), 2.0); + EXPECT_EQ(*vec_m2.Z(), 3.0); + + // Spherical constructor + auto vec_s{math::CoordinateVector3::Spherical(1, 2, 3)}; + EXPECT_FALSE(vec_s.IsMetric()); + ASSERT_TRUE(vec_s.IsSpherical()); + ASSERT_TRUE(vec_s.Lat().has_value()); + ASSERT_TRUE(vec_s.Lon().has_value()); + ASSERT_TRUE(vec_s.Z().has_value()); + EXPECT_FALSE(vec_s.X().has_value()); + EXPECT_FALSE(vec_s.Y().has_value()); + EXPECT_EQ(vec_s.Lat()->Radian(), 1.0); + EXPECT_EQ(vec_s.Lon()->Radian(), 2.0); + EXPECT_EQ(*vec_s.Z(), 3.0); + + // Copy constructor and equality + math::CoordinateVector3 vec2(vec_m); + EXPECT_EQ(vec2, vec_m); + + // Copy operator + math::CoordinateVector3 vec3; + vec3 = vec_m; + EXPECT_EQ(vec3, vec_m); + + // Move constructor + math::CoordinateVector3 vec4(std::move(vec_m)); + EXPECT_EQ(vec4, vec2); + vec_m = vec4; + EXPECT_EQ(vec_m, vec2); + + // Move operator + math::CoordinateVector3 vec5; + vec5 = std::move(vec2); + EXPECT_EQ(vec5, vec3); + vec2 = vec5; + EXPECT_EQ(vec2, vec3); + + // Inequality + math::CoordinateVector3 vec6; + EXPECT_NE(vec6, vec3); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, Setters) +{ + auto vec1{math::CoordinateVector3::Metric(0.1, 0.2, 0.4)}; + + vec1.SetMetric(1.1, 2.2, 3.4); + EXPECT_EQ(vec1, math::CoordinateVector3::Metric(1.1, 2.2, 3.4)); + + vec1.SetMetric(math::Vector3d(-1.1, -2.2, -3.4)); + EXPECT_EQ(vec1, math::CoordinateVector3::Metric(-1.1, -2.2, -3.4)); + + vec1.SetSpherical(1.1, 2.2, 3.4); + EXPECT_EQ(vec1, math::CoordinateVector3::Spherical(1.1, 2.2, 3.4)); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, AsMetricVector) +{ + auto vec1{math::CoordinateVector3::Metric(0.1, 0.2, 0.4)}; + ASSERT_TRUE(vec1.AsMetricVector().has_value()); + EXPECT_EQ(*vec1.AsMetricVector(), math::Vector3d(0.1, 0.2, 0.4)); + + auto vec2{math::CoordinateVector3::Spherical(0.1, 0.2, 0.4)}; + ASSERT_FALSE(vec2.AsMetricVector().has_value()); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, AddMetric) +{ + auto vec1{math::CoordinateVector3::Metric(0.1, 0.2, 0.4)}; + auto vec2{math::CoordinateVector3::Metric(1.1, 2.2, 3.4)}; + + math::CoordinateVector3 vec3 = vec1; + vec3 += vec2; + + EXPECT_EQ(vec1 + vec2, math::CoordinateVector3::Metric(1.2, 2.4, 3.8)); + EXPECT_EQ(vec3, math::CoordinateVector3::Metric(1.2, 2.4, 3.8)); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, AddSpherical) +{ + auto vec1{math::CoordinateVector3::Spherical(0.1, 0.2, 0.4)}; + auto vec2{math::CoordinateVector3::Spherical(1.1, 2.2, 3.4)}; + + math::CoordinateVector3 vec3 = vec1; + vec3 += vec2; + + EXPECT_EQ(vec1 + vec2, math::CoordinateVector3::Spherical(1.2, 2.4, 3.8)); + EXPECT_EQ(vec3, math::CoordinateVector3::Spherical(1.2, 2.4, 3.8)); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, AddMismatch) +{ + auto vec1{math::CoordinateVector3::Spherical(0.1, 0.2, 0.4)}; + auto vec2{math::CoordinateVector3::Metric(1.1, 2.2, 3.4)}; + + math::CoordinateVector3 vec3 = vec1; + vec3 += vec2; + math::CoordinateVector3 vec4 = vec2; + vec4 += vec1; + + math::CoordinateVector3 vec12 = vec1 + vec2; + math::CoordinateVector3 vec21 = vec2 + vec1; + + EXPECT_FALSE(vec3.IsMetric()); + EXPECT_FALSE(vec12.IsMetric()); + EXPECT_FALSE(vec4.IsSpherical()); + EXPECT_FALSE(vec21.IsSpherical()); + ASSERT_TRUE(vec3.IsSpherical()); + ASSERT_TRUE(vec12.IsSpherical()); + ASSERT_TRUE(vec21.IsMetric()); + ASSERT_TRUE(vec4.IsMetric()); + + EXPECT_TRUE(std::isnan(vec3.Lat()->Radian())); + EXPECT_TRUE(std::isnan(vec12.Lat()->Radian())); + EXPECT_TRUE(std::isnan(*vec21.X())); + EXPECT_TRUE(std::isnan(*vec4.X())); + + EXPECT_TRUE(std::isnan(vec3.Lon()->Radian())); + EXPECT_TRUE(std::isnan(vec12.Lon()->Radian())); + EXPECT_TRUE(std::isnan(*vec21.Y())); + EXPECT_TRUE(std::isnan(*vec4.Y())); + + EXPECT_TRUE(std::isnan(*vec3.Z())); + EXPECT_TRUE(std::isnan(*vec12.Z())); + EXPECT_TRUE(std::isnan(*vec21.Z())); + EXPECT_TRUE(std::isnan(*vec4.Z())); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, SubtractMetric) +{ + auto vec1{math::CoordinateVector3::Metric(0.1, 0.2, 0.4)}; + auto vec2{math::CoordinateVector3::Metric(1.1, 2.2, 3.4)}; + + math::CoordinateVector3 vec3 = vec1; + vec3 -= vec2; + + EXPECT_EQ(vec1 - vec2, math::CoordinateVector3::Metric(-1.0, -2.0, -3.0)); + EXPECT_EQ(vec3, math::CoordinateVector3::Metric(-1.0, -2.0, -3.0)); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, SubtractSpherical) +{ + auto vec1{math::CoordinateVector3::Spherical(0.1, 0.2, 0.4)}; + auto vec2{math::CoordinateVector3::Spherical(1.1, 2.2, 3.4)}; + + math::CoordinateVector3 vec3 = vec1; + vec3 -= vec2; + + EXPECT_EQ(vec1 - vec2, math::CoordinateVector3::Spherical(-1.0, -2.0, -3.0)); + EXPECT_EQ(vec3, math::CoordinateVector3::Spherical(-1.0, -2.0, -3.0)); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, SubtractMismatch) +{ + auto vec1{math::CoordinateVector3::Spherical(0.1, 0.2, 0.4)}; + auto vec2{math::CoordinateVector3::Metric(1.1, 2.2, 3.4)}; + + math::CoordinateVector3 vec3 = vec1; + vec3 -= vec2; + math::CoordinateVector3 vec4 = vec2; + vec4 -= vec1; + + math::CoordinateVector3 vec12 = vec1 - vec2; + math::CoordinateVector3 vec21 = vec2 - vec1; + + EXPECT_FALSE(vec3.IsMetric()); + EXPECT_FALSE(vec12.IsMetric()); + EXPECT_FALSE(vec4.IsSpherical()); + EXPECT_FALSE(vec21.IsSpherical()); + ASSERT_TRUE(vec3.IsSpherical()); + ASSERT_TRUE(vec12.IsSpherical()); + ASSERT_TRUE(vec21.IsMetric()); + ASSERT_TRUE(vec4.IsMetric()); + + EXPECT_TRUE(std::isnan(vec3.Lat()->Radian())); + EXPECT_TRUE(std::isnan(vec12.Lat()->Radian())); + EXPECT_TRUE(std::isnan(*vec21.X())); + EXPECT_TRUE(std::isnan(*vec4.X())); + + EXPECT_TRUE(std::isnan(vec3.Lon()->Radian())); + EXPECT_TRUE(std::isnan(vec12.Lon()->Radian())); + EXPECT_TRUE(std::isnan(*vec21.Y())); + EXPECT_TRUE(std::isnan(*vec4.Y())); + + EXPECT_TRUE(std::isnan(*vec3.Z())); + EXPECT_TRUE(std::isnan(*vec12.Z())); + EXPECT_TRUE(std::isnan(*vec21.Z())); + EXPECT_TRUE(std::isnan(*vec4.Z())); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, NotEqual) +{ + auto vec1{math::CoordinateVector3::Metric(0.1, 0.2, 0.3)}; + auto vec2{math::CoordinateVector3::Metric(0.2, 0.2, 0.3)}; + auto vec3{math::CoordinateVector3::Metric(0.1, 0.2, 0.3)}; + auto vec4{math::CoordinateVector3::Spherical(0.1, 0.2, 0.3)}; + auto vec5{math::CoordinateVector3::Spherical(0.2, 0.2, 0.3)}; + auto vec6{math::CoordinateVector3::Spherical(0.1, 0.2, 0.3)}; + + EXPECT_TRUE(vec1 != vec2); + EXPECT_FALSE(vec1 != vec3); + + EXPECT_TRUE(vec4 != vec5); + EXPECT_FALSE(vec4 != vec6); + + EXPECT_TRUE(vec1 != vec4); + EXPECT_TRUE(vec1 != vec5); + EXPECT_TRUE(vec1 != vec6); + EXPECT_TRUE(vec2 != vec4); + EXPECT_TRUE(vec2 != vec5); + EXPECT_TRUE(vec2 != vec6); + EXPECT_TRUE(vec3 != vec4); + EXPECT_TRUE(vec3 != vec5); + EXPECT_TRUE(vec3 != vec6); + EXPECT_TRUE(vec4 != vec1); + EXPECT_TRUE(vec4 != vec2); + EXPECT_TRUE(vec4 != vec3); + EXPECT_TRUE(vec5 != vec1); + EXPECT_TRUE(vec5 != vec2); + EXPECT_TRUE(vec5 != vec3); + EXPECT_TRUE(vec6 != vec1); + EXPECT_TRUE(vec6 != vec2); + EXPECT_TRUE(vec6 != vec3); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, EqualToleranceMetric) +{ + const auto zero = math::CoordinateVector3(); + const auto one = math::CoordinateVector3::Metric(1, 1, 1); + EXPECT_FALSE(zero.Equal(one, 1e-6, 1e-6)); + EXPECT_FALSE(zero.Equal(one, 1e-3, 1e-3)); + EXPECT_FALSE(zero.Equal(one, 1e-1, 1e-1)); + EXPECT_TRUE(zero.Equal(one, 1, 1)); + EXPECT_TRUE(zero.Equal(one, 1.1, 1.1)); + EXPECT_TRUE(zero.Equal(one, 1.1, 1e-6)); + EXPECT_FALSE(zero.Equal(one, 1e-6, 1.1)); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, EqualToleranceSpherical) +{ + const auto zero = math::CoordinateVector3::Spherical(0, 0, 0); + const auto one = math::CoordinateVector3::Spherical(1, 1, 1); + EXPECT_FALSE(zero.Equal(one, 1e-6, 1e-6)); + EXPECT_FALSE(zero.Equal(one, 1e-3, 1e-3)); + EXPECT_FALSE(zero.Equal(one, 1e-1, 1e-1)); + EXPECT_TRUE(zero.Equal(one, 1, 1)); + EXPECT_TRUE(zero.Equal(one, 1.1, 1.1)); + EXPECT_FALSE(zero.Equal(one, 1e-6, 1.1)); + EXPECT_FALSE(zero.Equal(one, 1.1, 1e-6)); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, EqualToleranceMismatch) +{ + const auto zero = math::CoordinateVector3::Spherical(0, 0, 0); + const auto one = math::CoordinateVector3::Metric(1, 1, 1); + EXPECT_FALSE(zero.Equal(one, 1e-6, 1e-6)); + EXPECT_FALSE(zero.Equal(one, 1e-3, 1e-3)); + EXPECT_FALSE(zero.Equal(one, 1e-1, 1e-1)); + EXPECT_FALSE(zero.Equal(one, 1, 1)); + EXPECT_FALSE(zero.Equal(one, 1.1, 1.1)); + EXPECT_FALSE(zero.Equal(one, 1e-6, 1.1)); + EXPECT_FALSE(zero.Equal(one, 1.1, 1e-6)); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, Finite) +{ + using namespace gz::math; + + EXPECT_TRUE(math::CoordinateVector3::Metric(0.1, 0.2, 0.3).IsFinite()); + EXPECT_TRUE(math::CoordinateVector3::Spherical(0.1, 0.2, 0.3).IsFinite()); + EXPECT_FALSE( + math::CoordinateVector3::Metric(NAN_D, NAN_D, NAN_D).IsFinite()); + EXPECT_FALSE( + math::CoordinateVector3::Spherical(NAN_D, NAN_D, NAN_D).IsFinite()); + EXPECT_FALSE( + math::CoordinateVector3::Metric(INF_D, INF_D, INF_D).IsFinite()); + EXPECT_FALSE( + math::CoordinateVector3::Spherical(INF_D, INF_D, INF_D).IsFinite()); + EXPECT_FALSE(math::CoordinateVector3::Metric(INF_D, 0, 0).IsFinite()); + EXPECT_FALSE(math::CoordinateVector3::Spherical(INF_D, 0, 0).IsFinite()); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, NaNMetric) +{ + using namespace gz::math; + + auto nanVec{math::CoordinateVector3::Metric(NAN_D, NAN_D, NAN_D)}; + EXPECT_FALSE(nanVec.IsFinite()); + ASSERT_TRUE(nanVec.X().has_value()); + ASSERT_TRUE(nanVec.Y().has_value()); + ASSERT_TRUE(nanVec.Z().has_value()); + EXPECT_FALSE(nanVec.Lat().has_value()); + EXPECT_FALSE(nanVec.Lon().has_value()); + EXPECT_TRUE(math::isnan(*nanVec.X())); + EXPECT_TRUE(math::isnan(*nanVec.Y())); + EXPECT_TRUE(math::isnan(*nanVec.Z())); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, NaNSpherical) +{ + using namespace gz::math; + + auto nanVec{math::CoordinateVector3::Spherical(NAN_D, NAN_D, NAN_D)}; + EXPECT_FALSE(nanVec.IsFinite()); + ASSERT_TRUE(nanVec.Lat().has_value()); + ASSERT_TRUE(nanVec.Lon().has_value()); + ASSERT_TRUE(nanVec.Z().has_value()); + EXPECT_FALSE(nanVec.X().has_value()); + EXPECT_FALSE(nanVec.X().has_value()); + EXPECT_TRUE(math::isnan(nanVec.Lat()->Radian())); + EXPECT_TRUE(math::isnan(nanVec.Lon()->Radian())); + EXPECT_TRUE(math::isnan(*nanVec.Z())); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, OperatorStreamOutMetric) +{ + auto v{math::CoordinateVector3::Metric(0.1234, 1.234, 2.3456)}; + std::ostringstream stream; + stream << v; + EXPECT_EQ(stream.str(), "0.1234 1.234 2.3456"); + + stream.str(""); + stream << std::setprecision(2) << v; + EXPECT_EQ(stream.str(), "0.12 1.2 2.3"); + + stream.str(""); + stream << std::setprecision(3) << v; + EXPECT_EQ(stream.str(), "0.123 1.23 2.35"); + + stream.str(""); + stream << std::setprecision(1) << std::fixed << v; + EXPECT_EQ(stream.str(), "0.1 1.2 2.3"); +} + +///////////////////////////////////////////////// +TEST(CoordinateVector3Test, OperatorStreamOutSpherical) +{ + auto v{math::CoordinateVector3::Spherical(0.1234, 1.234, 2.3456)}; + std::ostringstream stream; + stream << v; + EXPECT_EQ(stream.str(), "7.0703° 70.703° 2.3456"); + + stream.str(""); + stream << std::setprecision(2) << v; + EXPECT_EQ(stream.str(), "7.1° 71° 2.3"); + + stream.str(""); + stream << std::setprecision(3) << v; + EXPECT_EQ(stream.str(), "7.07° 70.7° 2.35"); + + stream.str(""); + stream << std::setprecision(1) << std::fixed << v; + EXPECT_EQ(stream.str(), "7.1° 70.7° 2.3"); +} diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index 38d2989f..f84a8a6b 100644 --- a/src/SphericalCoordinates.cc +++ b/src/SphericalCoordinates.cc @@ -98,7 +98,7 @@ class gz::math::SphericalCoordinates::Implementation public: Matrix3d rotGlobalToECEF; /// \brief Cache the ECEF position of the the origin - public: Vector3d origin; + public: CoordinateVector3 origin; /// \brief Cache cosine head transform public: double cosHea; @@ -377,13 +377,22 @@ void SphericalCoordinates::SetHeadingOffset(const Angle &_angle) Vector3d SphericalCoordinates::SphericalFromLocalPosition( const Vector3d &_xyz) const { - Vector3d result = - this->PositionTransform(_xyz, LOCAL, SPHERICAL); + 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; } +////////////////////////////////////////////////// +std::optional +SphericalCoordinates::SphericalFromLocalPosition( + const math::CoordinateVector3& _xyz) const +{ + return this->PositionTransform(_xyz, LOCAL, SPHERICAL); +} + ////////////////////////////////////////////////// Vector3d SphericalCoordinates::LocalFromSphericalPosition( const Vector3d &_xyz) const @@ -391,12 +400,32 @@ 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 +} + +////////////////////////////////////////////////// +std::optional +SphericalCoordinates::LocalFromSphericalPosition( + const math::CoordinateVector3& _xyz) const +{ + return this->PositionTransform(_xyz, SPHERICAL, LOCAL); } ////////////////////////////////////////////////// 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 +} + +////////////////////////////////////////////////// +std::optional +SphericalCoordinates::GlobalFromLocalVelocity( + const math::CoordinateVector3 &_xyz) const { return this->VelocityTransform(_xyz, LOCAL, GLOBAL); } @@ -404,6 +433,16 @@ Vector3d SphericalCoordinates::GlobalFromLocalVelocity( ////////////////////////////////////////////////// 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 +} + +////////////////////////////////////////////////// +std::optional +SphericalCoordinates::LocalFromGlobalVelocity( + const math::CoordinateVector3 &_xyz) const { return this->VelocityTransform(_xyz, GLOBAL, LOCAL); } @@ -506,215 +545,342 @@ void SphericalCoordinates::UpdateTransformationMatrix() this->dataPtr->sinHea = sin(-this->dataPtr->headingOffset.Radian()); // Cache the ECEF coordinate of the origin - this->dataPtr->origin = Vector3d( - this->dataPtr->latitudeReference.Radian(), - this->dataPtr->longitudeReference.Radian(), + this->dataPtr->origin = CoordinateVector3::Spherical( + this->dataPtr->latitudeReference, + this->dataPtr->longitudeReference, this->dataPtr->elevationReference); this->dataPtr->origin = - this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF); + *this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF); } -///////////////////////////////////////////////// -Vector3d SphericalCoordinates::PositionTransform( - const Vector3d &_pos, - const CoordinateType &_in, const CoordinateType &_out) const +namespace { - 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()); +///////////////////////////////////////////////// +std::optional PositionTransformTmp( + const gz::utils::ImplPtr& dataPtr, + const CoordinateVector3 &_pos, + const SphericalCoordinates::CoordinateType &_in, + const SphericalCoordinates::CoordinateType &_out) +{ + // This unusual concept of passing dataPtr as a free-function argument is just + // a temporary measure for GZ 8. Starting with GZ 9, the code of this function + // should be moved inside PositionTransform(CoordinateVector3). - // Radius of planet curvature (meters) - double curvature = 1.0 - - this->dataPtr->ellE * this->dataPtr->ellE * sinLat * sinLat; - curvature = this->dataPtr->ellA / sqrt(curvature); + if ((_in == SphericalCoordinates::SPHERICAL) != _pos.IsSpherical()) + { + std::cerr << "Invalid input to PositionTransform. " + "The passed coordinate vector has wrong type.\n"; + return std::nullopt; + } + Vector3d tmp; // Convert whatever arrives to a more flexible ECEF coordinate switch (_in) { - // East, North, Up (ENU), note no break at end of case - case LOCAL: + // East, North, Up (ENU) + // When branching code for GZ 9, replace this code block with the block + // from LOCAL2 and remove the LOCAL2 block. + case SphericalCoordinates::LOCAL: { - tmp.X(-_pos.X() * this->dataPtr->cosHea + _pos.Y() * - this->dataPtr->sinHea); - tmp.Y(-_pos.X() * this->dataPtr->sinHea - _pos.Y() * - this->dataPtr->cosHea); - tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp; + // This is incorrect computation + tmp.X(-*_pos.X() * dataPtr->cosHea + *_pos.Y() * dataPtr->sinHea); + tmp.Y(-*_pos.X() * dataPtr->sinHea - *_pos.Y() * dataPtr->cosHea); + tmp.Z(*_pos.Z()); + tmp = *dataPtr->origin.AsMetricVector() + + dataPtr->rotGlobalToECEF * tmp; break; } - case LOCAL2: + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + case SphericalCoordinates::LOCAL2: + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION { - tmp.X(_pos.X() * this->dataPtr->cosHea + _pos.Y() * - this->dataPtr->sinHea); - tmp.Y(-_pos.X() * this->dataPtr->sinHea + _pos.Y() * - this->dataPtr->cosHea); - tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp; + // This is correct computation + tmp.X(*_pos.X() * dataPtr->cosHea + *_pos.Y() * dataPtr->sinHea); + tmp.Y(-*_pos.X() * dataPtr->sinHea + *_pos.Y() * dataPtr->cosHea); + tmp.Z(*_pos.Z()); + tmp = *dataPtr->origin.AsMetricVector() + + dataPtr->rotGlobalToECEF * tmp; break; } - case GLOBAL: + case SphericalCoordinates::GLOBAL: { - tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp; + tmp = *_pos.AsMetricVector(); + tmp = *dataPtr->origin.AsMetricVector() + + dataPtr->rotGlobalToECEF * tmp; break; } - case SPHERICAL: + case SphericalCoordinates::SPHERICAL: { - tmp.X((_pos.Z() + curvature) * cosLat * cosLon); - tmp.Y((_pos.Z() + curvature) * cosLat * sinLon); - tmp.Z(((this->dataPtr->ellB * this->dataPtr->ellB)/ - (this->dataPtr->ellA * this->dataPtr->ellA) * - curvature + _pos.Z()) * sinLat); + // Cache trig results + const auto latRad = _pos.Lat()->Radian(); + const auto lonRad = _pos.Lon()->Radian(); + 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 - dataPtr->ellE * dataPtr->ellE * sinLat * sinLat; + curvature = dataPtr->ellA / sqrt(curvature); + + tmp.X((*_pos.Z() + curvature) * cosLat * cosLon); + tmp.Y((*_pos.Z() + curvature) * cosLat * sinLon); + tmp.Z(((dataPtr->ellB * dataPtr->ellB)/ + (dataPtr->ellA * dataPtr->ellA) * + curvature + *_pos.Z()) * sinLat); break; } - // Do nothing - case ECEF: + // Just copy the vector. + case SphericalCoordinates::ECEF: + tmp = *_pos.AsMetricVector(); break; default: { std::cerr << "Invalid coordinate type[" << _in << "]\n"; - return _pos; + return std::nullopt; } } + CoordinateVector3 res; // Convert ECEF to the requested output coordinate system switch (_out) { - case SPHERICAL: + case SphericalCoordinates::SPHERICAL: { // Convert from ECEF to SPHERICAL double p = sqrt(tmp.X() * tmp.X() + tmp.Y() * tmp.Y()); - double theta = atan((tmp.Z() * this->dataPtr->ellA) / - (p * this->dataPtr->ellB)); + double theta = atan((tmp.Z() * dataPtr->ellA) / + (p * dataPtr->ellB)); // Calculate latitude and longitude double lat = atan( - (tmp.Z() + std::pow(this->dataPtr->ellP, 2) * this->dataPtr->ellB * + (tmp.Z() + std::pow(dataPtr->ellP, 2) * dataPtr->ellB * std::pow(sin(theta), 3)) / - (p - std::pow(this->dataPtr->ellE, 2) * - this->dataPtr->ellA * std::pow(cos(theta), 3))); + (p - std::pow(dataPtr->ellE, 2) * + dataPtr->ellA * std::pow(cos(theta), 3))); double lon = atan2(tmp.Y(), tmp.X()); // Recalculate radius of planet curvature at the current latitude. - double nCurvature = 1.0 - std::pow(this->dataPtr->ellE, 2) * + double nCurvature = 1.0 - std::pow(dataPtr->ellE, 2) * std::pow(sin(lat), 2); - nCurvature = this->dataPtr->ellA / sqrt(nCurvature); - - tmp.X(lat); - tmp.Y(lon); + nCurvature = dataPtr->ellA / sqrt(nCurvature); - // Now calculate Z - tmp.Z(p/cos(lat) - nCurvature); + res = CoordinateVector3::Spherical(lat, lon, p/cos(lat) - nCurvature); break; } // Convert from ECEF TO GLOBAL - case GLOBAL: - tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin); + case SphericalCoordinates::GLOBAL: + tmp = dataPtr->rotECEFToGlobal * + (tmp - *dataPtr->origin.AsMetricVector()); + res.SetMetric(tmp); break; // Convert from ECEF TO LOCAL - case LOCAL: - case LOCAL2: - tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin); - - tmp = Vector3d( - tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea, - tmp.X() * this->dataPtr->sinHea + tmp.Y() * this->dataPtr->cosHea, + case SphericalCoordinates::LOCAL: + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + case SphericalCoordinates::LOCAL2: + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + tmp = dataPtr->rotECEFToGlobal * + (tmp - *dataPtr->origin.AsMetricVector()); + + res.SetMetric( + tmp.X() * dataPtr->cosHea - tmp.Y() * dataPtr->sinHea, + tmp.X() * dataPtr->sinHea + tmp.Y() * dataPtr->cosHea, tmp.Z()); break; // Return ECEF (do nothing) - case ECEF: + case SphericalCoordinates::ECEF: + res.SetMetric(tmp); break; default: std::cerr << "Unknown coordinate type[" << _out << "]\n"; - return _pos; + return std::nullopt; } - return tmp; + return res; } -////////////////////////////////////////////////// -Vector3d SphericalCoordinates::VelocityTransform( - const Vector3d &_vel, +} + +///////////////////////////////////////////////// +Vector3d SphericalCoordinates::PositionTransform( + const Vector3d &_pos, const CoordinateType &_in, const CoordinateType &_out) const { + // This deprecated implementation accepts and returns radians for spherical + // coordinates and has a computation bug when working with LOCAL frames. + + CoordinateVector3 vec = _in == SPHERICAL ? + CoordinateVector3::Spherical(_pos.X(), _pos.Y(), _pos.Z()) : + CoordinateVector3::Metric(_pos.X(), _pos.Y(), _pos.Z()); + + const auto result = PositionTransformTmp(this->dataPtr, vec, _in, _out); + if (!result) + return _pos; + + return result->IsMetric() ? + *result->AsMetricVector() : + Vector3d{result->Lat()->Radian(), result->Lon()->Radian(), *result->Z()}; +} + +///////////////////////////////////////////////// +std::optional SphericalCoordinates::PositionTransform( + const CoordinateVector3 &_pos, + const CoordinateType &_in, const CoordinateType &_out) const +{ + // Temporarily, for Gazebo 8, this function turns all LOCAL frames into + // LOCAL2 to get correct results. Basically, LOCAL and LOCAL2 are equal + // when PositionTransform() is called with a CoordinateVector3 argument, while + // it returns the compatible (but wrong) result when called with Vector3d and + // LOCAL. From Gazebo 9 onwards, LOCAL2 frame will be removed and these + // differences will disappear. + // TODO(peci1): Move PositionTransformTmp code into this function in GZ 9. + + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + const auto in = _in == LOCAL ? LOCAL2 : _in; + const auto out = _out == LOCAL ? LOCAL2 : _out; + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + return PositionTransformTmp(this->dataPtr, _pos, in, out); +} + +namespace +{ + +////////////////////////////////////////////////// +std::optional VelocityTransformTmp( + const gz::utils::ImplPtr& dataPtr, + const CoordinateVector3 &_vel, + const SphericalCoordinates::CoordinateType &_in, + const SphericalCoordinates::CoordinateType &_out) +{ + // This unusual concept of passing dataPtr as a free-function argument is just + // a temporary measure for GZ 8. Starting with GZ 9, the code of this function + // should be moved inside VelocityTransform(CoordinateVector3). + // Sanity check -- velocity should not be expressed in spherical coordinates - if (_in == SPHERICAL || _out == SPHERICAL) + if (_in == SphericalCoordinates::SPHERICAL || + _out == SphericalCoordinates::SPHERICAL || + _vel.IsSpherical()) { - return _vel; + std::cerr << "Velocity cannot be expressed in spherical coordinates.\n"; + return std::nullopt; } // Intermediate data type - Vector3d tmp = _vel; + Vector3d tmp = *_vel.AsMetricVector(); // First, convert to an ECEF vector switch (_in) { // ENU - case LOCAL: - 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; + // When branching code for GZ 9, replace this code block with the block + // from LOCAL2 and remove the LOCAL2 block. + case SphericalCoordinates::LOCAL: + // This is incorrect computation + tmp.X(-*_vel.X() * dataPtr->cosHea + *_vel.Y() * dataPtr->sinHea); + tmp.Y(-*_vel.X() * dataPtr->sinHea - *_vel.Y() * dataPtr->cosHea); + tmp = dataPtr->rotGlobalToECEF * tmp; break; - case LOCAL2: - 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; + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + case SphericalCoordinates::LOCAL2: + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + // This is correct computation + tmp.X(*_vel.X() * dataPtr->cosHea + *_vel.Y() * dataPtr->sinHea); + tmp.Y(-*_vel.X() * dataPtr->sinHea + *_vel.Y() * dataPtr->cosHea); + tmp = dataPtr->rotGlobalToECEF * tmp; break; // spherical - case GLOBAL: - tmp = this->dataPtr->rotGlobalToECEF * tmp; + case SphericalCoordinates::GLOBAL: + tmp = dataPtr->rotGlobalToECEF * tmp; break; // Do nothing - case ECEF: - tmp = _vel; + case SphericalCoordinates::ECEF: break; default: std::cerr << "Unknown coordinate type[" << _in << "]\n"; - return _vel; + return std::nullopt; } + CoordinateVector3 res; + // Then, convert to the request coordinate type switch (_out) { // ECEF, do nothing - case ECEF: + case SphericalCoordinates::ECEF: + res.SetMetric(tmp); break; // Convert from ECEF to global - case GLOBAL: - tmp = this->dataPtr->rotECEFToGlobal * tmp; + case SphericalCoordinates::GLOBAL: + tmp = dataPtr->rotECEFToGlobal * tmp; + res.SetMetric(tmp); break; // Convert from ECEF to local - case LOCAL: - case LOCAL2: - tmp = this->dataPtr->rotECEFToGlobal * tmp; - tmp = Vector3d( - tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea, - tmp.X() * this->dataPtr->sinHea + tmp.Y() * this->dataPtr->cosHea, + case SphericalCoordinates::LOCAL: + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + case SphericalCoordinates::LOCAL2: + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + tmp = dataPtr->rotECEFToGlobal * tmp; + res.SetMetric( + tmp.X() * dataPtr->cosHea - tmp.Y() * dataPtr->sinHea, + tmp.X() * dataPtr->sinHea + tmp.Y() * dataPtr->cosHea, tmp.Z()); break; default: std::cerr << "Unknown coordinate type[" << _out << "]\n"; - return _vel; + return std::nullopt; } - return tmp; + return res; +} + +} + +////////////////////////////////////////////////// +Vector3d SphericalCoordinates::VelocityTransform( + const Vector3d &_vel, + const CoordinateType &_in, const CoordinateType &_out) const +{ + auto vec = CoordinateVector3::Metric(_vel); + + const auto result = VelocityTransformTmp(this->dataPtr, vec, _in, _out); + if (!result || !result->IsMetric()) + return _vel; + + return *result->AsMetricVector(); +} + +////////////////////////////////////////////////// +std::optional SphericalCoordinates::VelocityTransform( + const CoordinateVector3 &_vel, + const CoordinateType &_in, const CoordinateType &_out) const +{ + // Temporarily, for Gazebo 8, this function turns all LOCAL frames into + // LOCAL2 to get correct results. Basically, LOCAL and LOCAL2 are equal + // when VelocityTransform() is called with a CoordinateVector3 argument, while + // it returns the compatible (but wrong) result when called with Vector3d and + // LOCAL. From Gazebo 9 onwards, LOCAL2 frame will be removed and these + // differences will disappear. + // TODO(peci1): Move VelocityTransformTmp code into this function in GZ 9. + + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + const auto in = _in == LOCAL ? LOCAL2 : _in; + const auto out = _out == LOCAL ? LOCAL2 : _out; + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + return VelocityTransformTmp(this->dataPtr, _vel, in, out); } ////////////////////////////////////////////////// diff --git a/src/SphericalCoordinates_TEST.cc b/src/SphericalCoordinates_TEST.cc index 655d1f29..4e6ed798 100644 --- a/src/SphericalCoordinates_TEST.cc +++ b/src/SphericalCoordinates_TEST.cc @@ -218,6 +218,234 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) 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::CoordinateVector3 xyz; + // east, north, up + std::optional enuOpt; + std::optional xyzOpt; + + xyz.SetMetric(1, 0, 0); + enuOpt = sc.GlobalFromLocalVelocity(xyz); + ASSERT_TRUE(enuOpt.has_value()); + ASSERT_TRUE(enuOpt->IsMetric()); + EXPECT_NEAR(*enuOpt->Y()/*North*/, *xyz.X(), 1e-6); + EXPECT_NEAR(*enuOpt->X()/*East*/, -*xyz.Y(), 1e-6); + xyzOpt = sc.LocalFromGlobalVelocity(*enuOpt); + ASSERT_TRUE(xyzOpt.has_value()); + EXPECT_EQ(xyz, *xyzOpt); + + xyz.SetMetric(0, 1, 0); + enuOpt = sc.GlobalFromLocalVelocity(xyz); + ASSERT_TRUE(enuOpt.has_value()); + ASSERT_TRUE(enuOpt->IsMetric()); + EXPECT_NEAR(*enuOpt->Y(), *xyz.X(), 1e-6); + EXPECT_NEAR(*enuOpt->X(), -*xyz.Y(), 1e-6); + xyzOpt = sc.LocalFromGlobalVelocity(*enuOpt); + ASSERT_TRUE(xyzOpt.has_value()); + EXPECT_EQ(xyz, *xyzOpt); + + xyz.SetMetric(1, -1, 0); + enuOpt = sc.GlobalFromLocalVelocity(xyz); + ASSERT_TRUE(enuOpt.has_value()); + ASSERT_TRUE(enuOpt->IsMetric()); + EXPECT_NEAR(*enuOpt->Y(), *xyz.X(), 1e-6); + EXPECT_NEAR(*enuOpt->X(), -*xyz.Y(), 1e-6); + xyzOpt = sc.LocalFromGlobalVelocity(*enuOpt); + ASSERT_TRUE(xyzOpt.has_value()); + EXPECT_EQ(xyz, *xyzOpt); + + xyz.SetMetric(2243.52334, 556.35, 435.6553); + enuOpt = sc.GlobalFromLocalVelocity(xyz); + ASSERT_TRUE(enuOpt.has_value()); + ASSERT_TRUE(enuOpt->IsMetric()); + EXPECT_NEAR(*enuOpt->Y(), *xyz.X(), 1e-6); + EXPECT_NEAR(*enuOpt->X(), -*xyz.Y(), 1e-6); + xyzOpt = sc.LocalFromGlobalVelocity(*enuOpt); + ASSERT_TRUE(xyzOpt.has_value()); + EXPECT_EQ(xyz, *xyzOpt); + } + + // Check SphericalFromLocal + { + // local frame + math::CoordinateVector3 xyz; + // spherical coordinates + std::optional sphOpt; + std::optional xyzOpt; + + // No offset + xyz.SetMetric(0, 0, 0); + sphOpt = sc.SphericalFromLocalPosition(xyz); + ASSERT_TRUE(sphOpt.has_value()); + ASSERT_TRUE(sphOpt->IsSpherical()); + // latitude + EXPECT_NEAR(sphOpt->Lat()->Degree(), lat.Degree(), 1e-6); + // longitude + EXPECT_NEAR(sphOpt->Lon()->Degree(), lon.Degree(), 1e-6); + // elevation + EXPECT_NEAR(*sphOpt->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.SetMetric(2e5, 0, 0); + sphOpt = sc.SphericalFromLocalPosition(xyz); + ASSERT_TRUE(sphOpt.has_value()); + ASSERT_TRUE(sphOpt->IsSpherical()); + // increase in latitude about 1.8 degrees + EXPECT_NEAR(sphOpt->Lat()->Degree(), lat.Degree() + 1.8, 0.008); + // no change in longitude + EXPECT_NEAR(*sphOpt->Z(), 3507.024791, 1e-6); + + auto xyz2 = sc.LocalFromSphericalPosition(*sphOpt); + ASSERT_TRUE(xyz2.has_value()); + 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 + std::optional tmp; + auto osrf_s = math::CoordinateVector3::Spherical( + GZ_DTOR(37.3877349), GZ_DTOR(-122.0651166), 32.0); + auto osrf_e = math::CoordinateVector3::Metric( + -2693701.91434394, -4299942.14687992, 3851691.0393571); + auto goog_s = math::CoordinateVector3::Spherical( + GZ_DTOR(37.4216719), GZ_DTOR(-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) + auto vec = math::CoordinateVector3::Metric(-1510.88, 3766.64, -3.29); + + // Set the ORIGIN to be the Open Source Robotics Foundation + math::SphericalCoordinates sc2(st, *osrf_s.Lat(), + *osrf_s.Lon(), *osrf_s.Z(), math::Angle::Zero); + + // Check that SPHERICAL -> ECEF works + tmp = sc2.PositionTransform(osrf_s, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::ECEF); + + ASSERT_TRUE(tmp.has_value()); + ASSERT_TRUE(tmp->IsMetric()); + 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, + math::SphericalCoordinates::ECEF, + math::SphericalCoordinates::SPHERICAL); + + ASSERT_TRUE(tmp.has_value()); + ASSERT_TRUE(tmp->IsSpherical()); + EXPECT_NEAR(tmp->Lat()->Radian(), osrf_s.Lat()->Radian(), 1e-2); + EXPECT_NEAR(tmp->Lon()->Radian(), osrf_s.Lon()->Radian(), 1e-2); + EXPECT_NEAR(*tmp->Z(), *osrf_s.Z(), 1e-2); + + // Check that SPHERICAL -> LOCAL works + tmp = sc2.LocalFromSphericalPosition(goog_s); + ASSERT_TRUE(tmp.has_value()); + ASSERT_TRUE(tmp->IsMetric()); + 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); + ASSERT_TRUE(tmp.has_value()); + ASSERT_TRUE(tmp->IsSpherical()); + EXPECT_NEAR(tmp->Lat()->Radian(), goog_s.Lat()->Radian(), 8e-2); + EXPECT_NEAR(tmp->Lon()->Radian(), goog_s.Lon()->Radian(), 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::CoordinateVector3 xyz; + // east, north, up + std::optional enuOpt; + std::optional xyzOpt; + + xyz.SetMetric(1, 0, 0); + enuOpt = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(enuOpt.has_value()); + EXPECT_EQ(xyz, *enuOpt); + xyzOpt = sc.LocalFromGlobalVelocity(*enuOpt); + ASSERT_TRUE(xyzOpt.has_value()); + EXPECT_EQ(xyz, *xyzOpt); + + xyz.SetMetric(0, 1, 0); + enuOpt = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(enuOpt.has_value()); + EXPECT_EQ(xyz, enuOpt); + xyzOpt = sc.LocalFromGlobalVelocity(*enuOpt); + ASSERT_TRUE(xyzOpt.has_value()); + EXPECT_EQ(xyz, *xyzOpt); + + xyz.SetMetric(1, -1, 0); + enuOpt = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(enuOpt.has_value()); + EXPECT_EQ(xyz, enuOpt); + xyzOpt = sc.LocalFromGlobalVelocity(*enuOpt); + ASSERT_TRUE(xyzOpt.has_value()); + EXPECT_EQ(xyz, *xyzOpt); + + xyz.SetMetric(2243.52334, 556.35, 435.6553); + enuOpt = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(enuOpt.has_value()); + EXPECT_EQ(xyz, enuOpt); + xyzOpt = sc.LocalFromGlobalVelocity(*enuOpt); + ASSERT_TRUE(xyzOpt.has_value()); + EXPECT_EQ(xyz, *xyzOpt); + } + } +} + +////////////////////////////////////////////////// +// Test coordinate transformations +// TODO(peci1): Remove this test in Gazebo 9 +TEST(SphericalCoordinatesTest, CoordinateTransformsDeprecated) +{ + // 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 @@ -227,6 +455,7 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) // east, north, up math::Vector3d enu; + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION xyz.Set(1, 0, 0); enu = sc.GlobalFromLocalVelocity(xyz); EXPECT_NEAR(enu.Y()/*North*/, xyz.X(), 1e-6); @@ -250,6 +479,7 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } // Check SphericalFromLocal @@ -259,6 +489,7 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) // spherical coordinates math::Vector3d sph; + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION // No offset xyz.Set(0, 0, 0); sph = sc.SphericalFromLocalPosition(xyz); @@ -282,6 +513,7 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) math::Vector3d xyz2 = sc.LocalFromSphericalPosition(sph); EXPECT_EQ(xyz, xyz2); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } // Check position projection @@ -313,6 +545,7 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) math::Angle(osrf_s.Y()), osrf_s.Z(), math::Angle::Zero); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION // Check that SPHERICAL -> ECEF works tmp = sc2.PositionTransform(osrf_s, math::SphericalCoordinates::SPHERICAL, @@ -342,6 +575,7 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) 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); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } } @@ -357,7 +591,9 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) math::Vector3d xyz; // east, north, up math::Vector3d enu; + math::Vector3d wsu; + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION xyz.Set(1, 0, 0); enu = sc.VelocityTransform(xyz, math::SphericalCoordinates::LOCAL2, @@ -385,6 +621,40 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) math::SphericalCoordinates::GLOBAL); EXPECT_EQ(xyz, enu); EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + // This is the incorrect and deprecated behavior of LOCAL frame + 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 } } } @@ -448,18 +718,48 @@ TEST(SphericalCoordinatesTest, BadSetSurface) ////////////////////////////////////////////////// TEST(SphericalCoordinatesTest, Transform) { - math::SphericalCoordinates sc; - math::Vector3d vel(1, 2, -4); + const math::SphericalCoordinates sc; + const auto vel = math::CoordinateVector3::Metric(1, 2, -4); + auto result = sc.VelocityTransform(vel, + math::SphericalCoordinates::ECEF, + math::SphericalCoordinates::ECEF); + ASSERT_TRUE(result.has_value()); + ASSERT_TRUE(result->IsMetric()); + + EXPECT_EQ(*result, vel); + + const auto pos = math::CoordinateVector3::Metric(-1510.88, 2, -4); + result = sc.PositionTransform(pos, + math::SphericalCoordinates::ECEF, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(result.has_value()); + ASSERT_TRUE(result->IsMetric()); + + EXPECT_NEAR(*result->X(), 2, 1e-6); + EXPECT_NEAR(*result->Y(), -4, 1e-6); + EXPECT_NEAR(*result->Z(), -6379647.8799999999, 1e-6); + + std::cout << "NEW POS[" << *result << "]\n"; +} + +////////////////////////////////////////////////// +// TODO(peci1): Remove this test in Gazebo 9 +TEST(SphericalCoordinatesTest, TransformDeprecated) +{ + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + const math::SphericalCoordinates sc; + constexpr math::Vector3d vel(1, 2, -4); math::Vector3d result = sc.VelocityTransform(vel, math::SphericalCoordinates::ECEF, math::SphericalCoordinates::ECEF); EXPECT_EQ(result, vel); - math::Vector3d pos(-1510.88, 2, -4); + constexpr math::Vector3d pos(-1510.88, 2, -4); result = sc.PositionTransform(pos, math::SphericalCoordinates::ECEF, math::SphericalCoordinates::GLOBAL); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION EXPECT_NEAR(result.X(), 2, 1e-6); EXPECT_NEAR(result.Y(), -4, 1e-6); @@ -471,6 +771,46 @@ TEST(SphericalCoordinatesTest, Transform) ////////////////////////////////////////////////// TEST(SphericalCoordinatesTest, BadCoordinateType) { + math::SphericalCoordinates sc; + const auto pos = math::CoordinateVector3::Metric(1, 2, -4); + auto result = sc.PositionTransform(pos, + static_cast(7), + static_cast(6)); + + EXPECT_FALSE(result.has_value()); + + result = sc.PositionTransform(pos, + static_cast(4), + static_cast(6)); + + EXPECT_FALSE(result.has_value()); + + result = sc.VelocityTransform(pos, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::ECEF); + EXPECT_FALSE(result.has_value()); + + result = sc.VelocityTransform(pos, + math::SphericalCoordinates::ECEF, + math::SphericalCoordinates::SPHERICAL); + EXPECT_FALSE(result.has_value()); + + result = sc.VelocityTransform(pos, + static_cast(7), + math::SphericalCoordinates::ECEF); + EXPECT_FALSE(result.has_value()); + + result = sc.VelocityTransform(pos, + math::SphericalCoordinates::ECEF, + static_cast(7)); + EXPECT_FALSE(result.has_value()); +} + +////////////////////////////////////////////////// +// TODO(peci1): Remove this test in Gazebo 9 +TEST(SphericalCoordinatesTest, BadCoordinateTypeDeprecated) +{ + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION math::SphericalCoordinates sc; math::Vector3d pos(1, 2, -4); math::Vector3d result = sc.PositionTransform(pos, @@ -504,6 +844,7 @@ TEST(SphericalCoordinatesTest, BadCoordinateType) math::SphericalCoordinates::ECEF, static_cast(7)); EXPECT_EQ(result, pos); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } ////////////////////////////////////////////////// @@ -563,6 +904,133 @@ TEST(SphericalCoordinatesTest, NoHeading) double elev = 0; math::SphericalCoordinates sc(st, lat, lon, elev, heading); + // Origin matches input + auto latLonAlt = sc.SphericalFromLocalPosition( + math::CoordinateVector3::Metric(0, 0, 0)); + ASSERT_TRUE(latLonAlt.has_value()); + ASSERT_TRUE(latLonAlt->IsSpherical()); + EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt->Lat()->Degree()); + EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt->Lon()->Degree()); + EXPECT_NEAR(elev, *latLonAlt->Z(), 1e-6); + + auto xyzOrigin = sc.LocalFromSphericalPosition(*latLonAlt); + ASSERT_TRUE(xyzOrigin.has_value()); + ASSERT_TRUE(xyzOrigin->IsMetric()); + EXPECT_EQ(math::Vector3d::Zero, xyzOrigin->AsMetricVector()); + + // Check how different lat/lon affect the local position + + // Increase latitude == go North == go +Y + { + auto xyz = sc.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat + GZ_DTOR(1.0), lon, elev)); + ASSERT_TRUE(xyz.has_value()); + ASSERT_TRUE(xyz->IsMetric()); + EXPECT_NEAR(*xyzOrigin->X(), *xyz->X(), 1e-6); + EXPECT_LT(*xyzOrigin->Y(), *xyz->Y()); + } + + // Decrease latitude == go South == go -Y + { + auto xyz = sc.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat - GZ_DTOR(1.0), lon, elev)); + ASSERT_TRUE(xyz.has_value()); + ASSERT_TRUE(xyz->IsMetric()); + 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.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat, lon + GZ_DTOR(1.0), elev)); + ASSERT_TRUE(xyz.has_value()); + ASSERT_TRUE(xyz->IsMetric()); + 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.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat, lon - GZ_DTOR(1.0), elev)); + ASSERT_TRUE(xyz.has_value()); + ASSERT_TRUE(xyz->IsMetric()); + EXPECT_GT(*xyzOrigin->X(), *xyz->X()); + EXPECT_GT(*xyzOrigin->Y(), *xyz->Y()); + } + + // Increase altitude + { + auto xyz = sc.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat, lon, elev + 10.0)); + ASSERT_TRUE(xyz.has_value()); + ASSERT_TRUE(xyz->IsMetric()); + 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.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat, lon, elev - 10.0)); + ASSERT_TRUE(xyz.has_value()); + ASSERT_TRUE(xyz->IsMetric()); + 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 localOpt = sc.LocalFromGlobalVelocity( + math::CoordinateVector3::Metric(global)); + ASSERT_TRUE(localOpt.has_value()); + ASSERT_TRUE(localOpt->IsMetric()); + EXPECT_EQ(global, localOpt->AsMetricVector()); + + auto globalOpt = sc.GlobalFromLocalVelocity(*localOpt); + ASSERT_TRUE(globalOpt.has_value()); + ASSERT_TRUE(globalOpt->IsMetric()); + EXPECT_EQ(*globalOpt, *localOpt); + + // Directly call VelocityTransform + globalOpt = sc.VelocityTransform(*localOpt, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(globalOpt.has_value()); + ASSERT_TRUE(globalOpt->IsMetric()); + EXPECT_EQ(*globalOpt, *localOpt); + } +} + +////////////////////////////////////////////////// +// TODO(peci1): Remove this test in Gazebo 9 +TEST(SphericalCoordinatesTest, NoHeadingDeprecated) +{ + // 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); + + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + // Origin matches input auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); @@ -628,6 +1096,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 +1111,19 @@ 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 +1137,104 @@ TEST(SphericalCoordinatesTest, WithHeading) double elev = 0; math::SphericalCoordinates sc(st, lat, lon, elev, heading); + // Origin matches input + auto latLonAlt = sc.SphericalFromLocalPosition( + math::CoordinateVector3::Metric(0, 0, 0)); + ASSERT_TRUE(latLonAlt.has_value()); + ASSERT_TRUE(latLonAlt->IsSpherical()); + EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt->Lat()->Degree()); + EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt->Lon()->Degree()); + EXPECT_NEAR(elev, *latLonAlt->Z(), 1e-6); + + auto xyzOrigin = sc.LocalFromSphericalPosition(*latLonAlt); + ASSERT_TRUE(xyzOrigin.has_value()); + ASSERT_TRUE(xyzOrigin->IsMetric()); + EXPECT_EQ(math::Vector3d::Zero, xyzOrigin->AsMetricVector()); + + // Check how different lat/lon affect the local position + + // Increase latitude == go North == go +X + { + auto xyz = sc.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat + GZ_DTOR(1.0), lon, elev)); + ASSERT_TRUE(xyz.has_value()); + ASSERT_TRUE(xyz->IsMetric()); + EXPECT_NEAR(*xyzOrigin->Y(), *xyz->Y(), 1e-6); + EXPECT_LT(*xyzOrigin->X(), *xyz->X()); + } + + // Decrease latitude == go South == go -X + { + auto xyz = sc.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat - GZ_DTOR(1.0), lon, elev)); + ASSERT_TRUE(xyz.has_value()); + ASSERT_TRUE(xyz->IsMetric()); + 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.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat, lon + GZ_DTOR(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.LocalFromSphericalPosition(math::CoordinateVector3::Spherical( + lat, lon - GZ_DTOR(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.LocalFromGlobalVelocity( + math::CoordinateVector3::Metric(global)); + ASSERT_TRUE(localRes.has_value()); + ASSERT_TRUE(localRes->IsMetric()); + EXPECT_EQ(local, localRes->AsMetricVector()); + + // Directly call VelocityTransform + auto globalRes = sc.VelocityTransform( + math::CoordinateVector3::Metric(local), + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(globalRes.has_value()); + ASSERT_TRUE(globalRes->IsMetric()); + EXPECT_EQ(global, globalRes->AsMetricVector()); + } +} + +////////////////////////////////////////////////// +// TODO(peci1): Remove this test in Gazebo 9 +TEST(SphericalCoordinatesTest, WithHeadingDeprecated) +{ + // 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); + + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + // Origin matches input auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); @@ -732,12 +1296,14 @@ TEST(SphericalCoordinatesTest, WithHeading) auto localRes = sc.LocalFromGlobalVelocity(global); EXPECT_EQ(local, localRes); - // Directly call fixed version + // Directly call VelocityTransform 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,6 +1314,71 @@ TEST(SphericalCoordinatesTest, Inverse) double elev = 354.1; math::SphericalCoordinates sc(st, lat, lon, elev, heading); + // GLOBAL <-> LOCAL + { + auto in = math::CoordinateVector3::Metric(1, 2, -4); + auto out = sc.VelocityTransform(in, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(out.has_value()); + ASSERT_TRUE(out->IsMetric()); + EXPECT_NE(in, *out); + + auto reverse = sc.VelocityTransform(*out, + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::LOCAL); + ASSERT_TRUE(reverse.has_value()); + ASSERT_TRUE(reverse->IsMetric()); + EXPECT_EQ(in, *reverse); + } + + { + auto in = math::CoordinateVector3::Metric(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::GLOBAL); + ASSERT_TRUE(out.has_value()); + ASSERT_TRUE(out->IsMetric()); + EXPECT_NE(in, *out); + + auto reverse = sc.PositionTransform(*out, + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::LOCAL); + ASSERT_TRUE(reverse.has_value()); + ASSERT_TRUE(reverse->IsMetric()); + EXPECT_EQ(in, *reverse); + } + + // SPHERICAL <-> LOCAL + { + auto in = math::CoordinateVector3::Metric(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL, + math::SphericalCoordinates::SPHERICAL); + ASSERT_TRUE(out.has_value()); + ASSERT_TRUE(out->IsSpherical()); + EXPECT_NE(in, *out); + + auto reverse = sc.PositionTransform(*out, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL); + ASSERT_TRUE(reverse.has_value()); + ASSERT_TRUE(reverse->IsMetric()); + EXPECT_EQ(in, *reverse); + } +} + +////////////////////////////////////////////////// +// TODO(peci1): Remove this test in Gazebo 9 +TEST(SphericalCoordinatesTest, InverseDeprecated) +{ + 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); + + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + // GLOBAL <-> LOCAL2 { math::Vector3d in(1, 2, -4); @@ -785,4 +1416,44 @@ TEST(SphericalCoordinatesTest, Inverse) math::SphericalCoordinates::LOCAL2); EXPECT_EQ(in, reverse); } + + // 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/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 9e8a3446..a3c6fc31 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -7,6 +7,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/AxisAlignedBox.cc src/Capsule.cc src/Color.cc + src/CoordinateVector3.cc src/DiffDriveOdometry.cc src/Ellipsoid.cc src/Filter.cc @@ -114,6 +115,7 @@ if (BUILD_TESTING) Capsule_TEST Color_TEST Cone_TEST + CoordinateVector3_TEST Cylinder_TEST DiffDriveOdometry_TEST Ellipsoid_TEST diff --git a/src/python_pybind11/src/Angle.cc b/src/python_pybind11/src/Angle.cc index 4964af3e..1eccfb69 100644 --- a/src/python_pybind11/src/Angle.cc +++ b/src/python_pybind11/src/Angle.cc @@ -65,10 +65,18 @@ void defineMathAngle(py::module &m, const std::string &typestr) .def("normalized", &Class::Normalized, "Return a normalized vector") + .def("abs", + &Class::Abs, + "Return the absolute value") + .def("shortest_distance", + &Class::ShortestDistance, + "Return the shortest angular distance between this and the " + "other angle.") .def(py::self + py::self) .def(py::self += py::self) .def(py::self * py::self) .def(py::self *= py::self) + .def(-py::self) .def(py::self - py::self) .def(py::self -= py::self) .def(py::self / py::self) diff --git a/src/python_pybind11/src/CoordinateVector3.cc b/src/python_pybind11/src/CoordinateVector3.cc new file mode 100644 index 00000000..90a8c65d --- /dev/null +++ b/src/python_pybind11/src/CoordinateVector3.cc @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include +#include +#include + +#include "CoordinateVector3.hh" + +#include +#include +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace gz +{ +namespace math +{ +namespace python +{ +void defineMathCoordinateVector3(py::module &m, const std::string &typestr) +{ + using Class = gz::math::CoordinateVector3; + auto toString = [](const Class &si) { + std::stringstream stream; + stream << si; + return stream.str(); + }; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init()) + .def("metric", + py::overload_cast(&Class::Metric), + "Constructor for metric values.") + .def("metric", + py::overload_cast(&Class::Metric), + "Constructor for metric values.") + .def("spherical", &Class::Spherical, + "Constructor for spherical values.") + .def("set_metric", + py::overload_cast(&Class::SetMetric), + "Set the metric contents of the vector") + .def("set_metric", + py::overload_cast(&Class::SetMetric), + "Set the metric contents of the vector") + .def("set_spherical", &Class::SetSpherical, + "Set the spherical contents of the vector") + .def("is_metric", &Class::IsMetric, "Whether this vector is metric.") + .def("is_spherical", &Class::IsSpherical, + "Whether this vector is spherical.") + .def("as_metric_vector", &Class::AsMetricVector, + "Return this vector as a metric Vector3d (only valid for metric).") + .def(py::self + py::self) + .def(py::self += py::self) + .def(py::self - py::self) + .def(py::self -= py::self) + .def(py::self != py::self) + .def(py::self == py::self) + .def(-py::self) + .def("equal", + py::overload_cast< + const CoordinateVector3&, + const double&, + const Angle& + >(&Class::Equal, py::const_), "Equality test with tolerance.") + .def("equal", + py::overload_cast(&Class::Equal, py::const_), + "Equal to operator") + .def("is_finite", &Class::IsFinite, + "See if all vector components are finite (e.g., not nan)") + .def("x", py::overload_cast<>(&Class::X, py::const_), + "Get the x value of a metric vector.") + .def("lat", py::overload_cast<>(&Class::Lat, py::const_), + "Get the latitude of a spherical vector.") + .def("y", py::overload_cast<>(&Class::Y, py::const_), + "Get the y value of a metric vector.") + .def("lon", py::overload_cast<>(&Class::Lon, py::const_), + "Get the longitude of a spherical vector.") + .def("z", py::overload_cast<>(&Class::Z, py::const_), + "Get the z value of a metric vector.") + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a) + .def("__str__", toString) + .def("__repr__", toString); +} + +} // namespace python +} // namespace math +} // namespace gz diff --git a/src/python_pybind11/src/CoordinateVector3.hh b/src/python_pybind11/src/CoordinateVector3.hh new file mode 100644 index 00000000..9e9e6b7a --- /dev/null +++ b/src/python_pybind11/src/CoordinateVector3.hh @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_MATH_PYTHON__COORDINATE_VECTOR3_HH_ +#define GZ_MATH_PYTHON__COORDINATE_VECTOR3_HH_ + +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace gz +{ +namespace math +{ +namespace python +{ + +/// Define a pybind11 wrapper for a gz::math::CoordinateVector3 +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineMathCoordinateVector3(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace gz + +#endif // GZ_MATH_PYTHON__COORDINATE_VECTOR3_HH_ diff --git a/src/python_pybind11/src/SphericalCoordinates.cc b/src/python_pybind11/src/SphericalCoordinates.cc index f2d27ed2..697eea84 100644 --- a/src/python_pybind11/src/SphericalCoordinates.cc +++ b/src/python_pybind11/src/SphericalCoordinates.cc @@ -15,6 +15,7 @@ * */ #include +#include #include "SphericalCoordinates.hh" #include @@ -29,6 +30,8 @@ namespace python void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) { using Class = gz::math::SphericalCoordinates; + using CoordinateVector3 = gz::math::CoordinateVector3; + using CoordinateType = gz::math::SphericalCoordinates::CoordinateType; std::string pyclass_name = typestr; py::class_ sphericalCoordinates(m, @@ -47,12 +50,46 @@ 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, + py::overload_cast( + &Class::SphericalFromLocalPosition, py::const_), "Convert a Cartesian position vector to geodetic coordinates.") + .def("spherical_from_local_position", + [](const Class &self, const gz::math::Vector3d &_xyz) + { + PyErr_WarnEx( + PyExc_DeprecationWarning, + "Passing Vector3d to spherical_from_local_position() is deprecated" + " and will be removed. Pass CoordinateVector3 instead and arrange " + "for the different behavior. See Migration.md ", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.SphericalFromLocalPosition(_xyz); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, + "DEPRECATED: Passing Vector3d is deprecated and will be removed. " + "Pass CoordinateVector3 instead and arrange for the different " + "behavior. See Migration.md .") .def("global_from_local_velocity", - &Class::GlobalFromLocalVelocity, + py::overload_cast( + &Class::GlobalFromLocalVelocity, py::const_), "Convert a Cartesian velocity vector in the local frame " " to a global Cartesian frame with components East, North, Up") + .def("global_from_local_velocity", + [](const Class &self, const gz::math::Vector3d &_xyz) + { + PyErr_WarnEx( + PyExc_DeprecationWarning, + "Passing Vector3d to global_from_local_velocity() is deprecated" + " and will be removed. Pass CoordinateVector3 instead and arrange " + "for the different behavior. See Migration.md ", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.GlobalFromLocalVelocity(_xyz); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, + "DEPRECATED: Passing Vector3d is deprecated and will be removed. " + "Pass CoordinateVector3 instead and arrange for the different " + "behavior. See Migration.md .") .def("convert", py::overload_cast(&Class::Convert), "Convert a string to a SurfaceType.") @@ -122,26 +159,103 @@ 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, + py::overload_cast( + &Class::LocalFromSphericalPosition, py::const_), "Convert a geodetic position vector to Cartesian coordinates.") + .def("local_from_spherical_position", + [](const Class &self, const gz::math::Vector3d &_xyz) + { + PyErr_WarnEx( + PyExc_DeprecationWarning, + "Passing Vector3d to local_from_spherical_position() is deprecated" + " and will be removed. Pass CoordinateVector3 instead.", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.LocalFromSphericalPosition(_xyz); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, + "DEPRECATED: Passing Vector3d is deprecated and will be removed. " + "Pass CoordinateVector3 instead.") .def("local_from_global_velocity", - &Class::LocalFromGlobalVelocity, + py::overload_cast( + &Class::LocalFromGlobalVelocity, py::const_), "Convert a Cartesian velocity vector with components East, " "North, Up to a local cartesian frame vector XYZ.") + .def("local_from_global_velocity", + [](const Class &self, const gz::math::Vector3d &_xyz) + { + PyErr_WarnEx( + PyExc_DeprecationWarning, + "Passing Vector3d to local_from_global_velocity() is deprecated" + " and will be removed. Pass CoordinateVector3 instead.", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.LocalFromGlobalVelocity(_xyz); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, + "DEPRECATED: Passing Vector3d is deprecated and will be removed. " + "Pass CoordinateVector3 instead.") .def("update_transformation_matrix", &Class::UpdateTransformationMatrix, "Update coordinate transformation matrix with reference location") .def("position_transform", - &Class::PositionTransform, + py::overload_cast< + const CoordinateVector3&, + const CoordinateType&, + const CoordinateType& + >(&Class::PositionTransform, py::const_), "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame " "Spherical coordinates use radians, while the other frames use " "meters.") + .def("position_transform", + [](const Class &self, + const gz::math::Vector3d &_pos, + const CoordinateType& _in, + const CoordinateType& _out) -> gz::math::Vector3d + { + PyErr_WarnEx( + PyExc_DeprecationWarning, + "Passing Vector3d to position_transform() is deprecated and will " + "be removed. Pass CoordinateVector3 instead and arrange for the " + "different behavior. See Migration.md .", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.PositionTransform(_pos, _in, _out); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, + "DEPRECATED: Passing Vector3d is deprecated and will be removed. " + "Pass CoordinateVector3 instead and arrange for the different " + "behavior. See Migration.md .") .def("velocity_transform", - &Class::VelocityTransform, + py::overload_cast< + const CoordinateVector3&, + const CoordinateType&, + const CoordinateType& + >(&Class::VelocityTransform, py::const_), "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame " "Spherical coordinates use radians, while the other frames use " - "meters."); + "meters.") + .def("velocity_transform", + [](const Class &self, + const gz::math::Vector3d &_vel, + const CoordinateType& _in, + const CoordinateType& _out) -> gz::math::Vector3d + { + PyErr_WarnEx( + PyExc_DeprecationWarning, + "Passing Vector3d to velocity_transform() is deprecated and will " + "be removed. Pass CoordinateVector3 instead and arrange for the " + "different behavior. See Migration.md .", + 1); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + return self.VelocityTransform(_vel, _in, _out); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + }, + "DEPRECATED: Passing Vector3d is deprecated and will be removed. " + "Pass CoordinateVector3 instead and arrange for the different " + "behavior. See Migration.md ."); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION py::enum_(sphericalCoordinates, "CoordinateType") .value("SPHERICAL", Class::CoordinateType::SPHERICAL) .value("ECEF", Class::CoordinateType::ECEF) @@ -149,6 +263,8 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) .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/src/_gz_math_pybind11.cc b/src/python_pybind11/src/_gz_math_pybind11.cc index 8af6c147..51f4f627 100644 --- a/src/python_pybind11/src/_gz_math_pybind11.cc +++ b/src/python_pybind11/src/_gz_math_pybind11.cc @@ -20,6 +20,7 @@ #include "Capsule.hh" #include "Color.hh" #include "Cone.hh" +#include "CoordinateVector3.hh" #include "Cylinder.hh" #include "DiffDriveOdometry.hh" #include "Ellipsoid.hh" @@ -190,4 +191,6 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) m, "OnePoleQuaternion"); gz::math::python::defineMathOnePoleVector3( m, "OnePoleVector3"); + + gz::math::python::defineMathCoordinateVector3(m, "CoordinateVector3"); } diff --git a/src/python_pybind11/test/Angle_TEST.py b/src/python_pybind11/test/Angle_TEST.py index 31864fb6..fd04c31f 100644 --- a/src/python_pybind11/test/Angle_TEST.py +++ b/src/python_pybind11/test/Angle_TEST.py @@ -85,6 +85,30 @@ def test_angle_operations(self): self.assertTrue(angle >= Angle(3)) self.assertTrue(angle <= Angle(3)) + self.assertAlmostEqual(Angle(1.2).abs().radian(), 1.2) + self.assertAlmostEqual(Angle(-1.2).abs().radian(), 1.2) + self.assertAlmostEqual(Angle(0).abs().radian(), 0) + self.assertAlmostEqual(Angle(-300).abs().radian(), 300) + self.assertAlmostEqual(Angle(300).abs().radian(), 300) + + self.assertAlmostEqual(-Angle(1.2).radian(), -1.2) + self.assertAlmostEqual(-Angle(-1.2).radian(), 1.2) + + self.assertAlmostEqual( + Angle(1.0).shortest_distance(Angle(1.0)).radian(), 0.0) + self.assertAlmostEqual( + Angle(1.0).shortest_distance(Angle(-1.0)).radian(), -2.0) + self.assertAlmostEqual( + Angle(-1.0).shortest_distance(Angle(1.0)).radian(), 2.0) + self.assertAlmostEqual( + Angle(-1.0).shortest_distance(Angle(-1.0)).radian(), 0.0) + self.assertAlmostEqual( + Angle(-2.0).shortest_distance(Angle(2.0)).radian(), + -(2 * math.pi - 4.0)) + self.assertAlmostEqual( + Angle(2.0).shortest_distance(Angle(-2.0)).radian(), + 2 * math.pi - 4.0) + if __name__ == '__main__': unittest.main() diff --git a/src/python_pybind11/test/CoordinateVector3_TEST.py b/src/python_pybind11/test/CoordinateVector3_TEST.py new file mode 100644 index 00000000..55574c10 --- /dev/null +++ b/src/python_pybind11/test/CoordinateVector3_TEST.py @@ -0,0 +1,346 @@ +# Copyright (C) 2024 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import copy +import math +import unittest + +from gz.math8 import Angle, CoordinateVector3, Vector3d + + +class TestCoordinateVector3(unittest.TestCase): + + def test_construction(self): + # Empty constructor + vec = CoordinateVector3() + self.assertFalse(vec.is_spherical()) + self.assertTrue(vec.is_metric()) + self.assertIsNotNone(vec.x()) + self.assertIsNotNone(vec.y()) + self.assertIsNotNone(vec.z()) + self.assertIsNone(vec.lat()) + self.assertIsNone(vec.lon()) + self.assertAlmostEqual(0.0, vec.x()) + self.assertAlmostEqual(0.0, vec.y()) + self.assertAlmostEqual(0.0, vec.z()) + + # Metric constructor + vec_m = CoordinateVector3.metric(1, 2, 3) + self.assertFalse(vec_m.is_spherical()) + self.assertTrue(vec_m.is_metric()) + self.assertIsNotNone(vec_m.x()) + self.assertIsNotNone(vec_m.y()) + self.assertIsNotNone(vec_m.z()) + self.assertIsNone(vec_m.lat()) + self.assertIsNone(vec_m.lon()) + self.assertAlmostEqual(1.0, vec_m.x()) + self.assertAlmostEqual(2.0, vec_m.y()) + self.assertAlmostEqual(3.0, vec_m.z()) + + # Metric constructor from vector + vec_m2 = CoordinateVector3.metric(Vector3d(1, 2, 3)) + self.assertFalse(vec_m2.is_spherical()) + self.assertTrue(vec_m2.is_metric()) + self.assertIsNotNone(vec_m2.x()) + self.assertIsNotNone(vec_m2.y()) + self.assertIsNotNone(vec_m2.z()) + self.assertIsNone(vec_m2.lat()) + self.assertIsNone(vec_m2.lon()) + self.assertAlmostEqual(1.0, vec_m2.x()) + self.assertAlmostEqual(2.0, vec_m2.y()) + self.assertAlmostEqual(3.0, vec_m2.z()) + + # Spherical constructor + vec_s = CoordinateVector3.spherical(Angle(1), Angle(2), 3) + self.assertTrue(vec_s.is_spherical()) + self.assertFalse(vec_s.is_metric()) + self.assertIsNone(vec_s.x()) + self.assertIsNone(vec_s.y()) + self.assertIsNotNone(vec_s.z()) + self.assertIsNotNone(vec_s.lat()) + self.assertIsNotNone(vec_s.lon()) + self.assertAlmostEqual(1.0, vec_s.lat().radian()) + self.assertAlmostEqual(2.0, vec_s.lon().radian()) + self.assertAlmostEqual(3.0, vec_s.z()) + + vec2 = CoordinateVector3(vec_m) + self.assertEqual(vec2, vec_m) + + # Copy + vec3 = vec_m + self.assertEqual(vec3, vec_m) + + # Inequality + vec4 = CoordinateVector3() + self.assertNotEqual(vec_m, vec4) + + def test_setters(self): + vec1 = CoordinateVector3.metric(0.1, 0.2, 0.4) + + vec1.set_metric(1.1, 2.2, 3.4) + self.assertTrue(vec1, CoordinateVector3.metric(1.1, 2.2, 3.4)) + + vec1.set_metric(-1.1, -2.2, -3.4) + self.assertTrue(vec1, CoordinateVector3.metric(-1.1, -2.2, -3.4)) + + vec1.set_spherical(Angle(1.1), Angle(2.2), 3.4) + self.assertTrue( + vec1, CoordinateVector3.spherical(Angle(1.1), Angle(2.2), 3.4)) + + def test_as_metric_vector(self): + vec1 = CoordinateVector3.metric(0.1, 0.2, 0.4) + self.assertIsNotNone(vec1.as_metric_vector()) + self.assertEqual(vec1.as_metric_vector(), Vector3d(0.1, 0.2, 0.4)) + + vec2 = CoordinateVector3.spherical(Angle(0.1), Angle(0.2), 0.4) + self.assertIsNone(vec2.as_metric_vector()) + + def test_add_metric(self): + vec1 = CoordinateVector3.metric(0.1, 0.2, 0.4) + vec2 = CoordinateVector3.metric(1.1, 2.2, 3.4) + + vec3 = copy.deepcopy(vec1) + vec3 += vec2 + + self.assertEqual(vec1 + vec2, CoordinateVector3.metric(1.2, 2.4, 3.8)) + self.assertEqual(vec3, CoordinateVector3.metric(1.2, 2.4, 3.8)) + + def test_add_spherical(self): + vec1 = CoordinateVector3.spherical(Angle(0.1), Angle(0.2), 0.4) + vec2 = CoordinateVector3.spherical(Angle(1.1), Angle(2.2), 3.4) + + vec3 = copy.deepcopy(vec1) + vec3 += vec2 + + self.assertEqual( + vec1 + vec2, + CoordinateVector3.spherical(Angle(1.2), Angle(2.4), 3.8)) + self.assertEqual( + vec3, + CoordinateVector3.spherical(Angle(1.2), Angle(2.4), 3.8)) + + def test_add_mismatch(self): + vec1 = CoordinateVector3.spherical(Angle(0.1), Angle(0.2), 0.4) + vec2 = CoordinateVector3.metric(1.1, 2.2, 3.4) + + vec3 = copy.deepcopy(vec1) + vec3 += vec2 + vec4 = copy.deepcopy(vec2) + vec4 += vec1 + + vec12 = vec1 + vec2 + vec21 = vec2 + vec1 + + self.assertFalse(vec3.is_metric()) + self.assertFalse(vec12.is_metric()) + self.assertFalse(vec4.is_spherical()) + self.assertFalse(vec21.is_spherical()) + self.assertTrue(vec3.is_spherical()) + self.assertTrue(vec12.is_spherical()) + self.assertTrue(vec21.is_metric()) + self.assertTrue(vec4.is_metric()) + + self.assertTrue(math.isnan(vec3.lat().radian())) + self.assertTrue(math.isnan(vec12.lat().radian())) + self.assertTrue(math.isnan(vec21.x())) + self.assertTrue(math.isnan(vec4.x())) + + self.assertTrue(math.isnan(vec3.lon().radian())) + self.assertTrue(math.isnan(vec12.lon().radian())) + self.assertTrue(math.isnan(vec21.y())) + self.assertTrue(math.isnan(vec4.y())) + + self.assertTrue(math.isnan(vec3.z())) + self.assertTrue(math.isnan(vec12.z())) + self.assertTrue(math.isnan(vec21.z())) + self.assertTrue(math.isnan(vec4.z())) + + def test_sub_metric(self): + vec1 = CoordinateVector3.metric(0.1, 0.2, 0.4) + vec2 = CoordinateVector3.metric(1.1, 2.2, 3.4) + + vec3 = copy.deepcopy(vec1) + vec3 -= vec2 + + self.assertEqual(vec1 - vec2, CoordinateVector3.metric(-1, -2, -3)) + self.assertEqual(vec3, CoordinateVector3.metric(-1, -2, -3)) + + def test_sub_spherical(self): + vec1 = CoordinateVector3.spherical(Angle(0.1), Angle(0.2), 0.4) + vec2 = CoordinateVector3.spherical(Angle(1.1), Angle(2.2), 3.4) + + vec3 = copy.deepcopy(vec1) + vec3 -= vec2 + + self.assertEqual( + vec1 - vec2, + CoordinateVector3.spherical(Angle(-1), Angle(-2), -3)) + self.assertEqual( + vec3, + CoordinateVector3.spherical(Angle(-1), Angle(-2), -3)) + + def test_sub_mismatch(self): + vec1 = CoordinateVector3.spherical(Angle(0.1), Angle(0.2), 0.4) + vec2 = CoordinateVector3.metric(1.1, 2.2, 3.4) + + vec3 = copy.deepcopy(vec1) + vec3 -= vec2 + vec4 = copy.deepcopy(vec2) + vec4 -= vec1 + + vec12 = vec1 - vec2 + vec21 = vec2 - vec1 + + self.assertFalse(vec3.is_metric()) + self.assertFalse(vec12.is_metric()) + self.assertFalse(vec4.is_spherical()) + self.assertFalse(vec21.is_spherical()) + self.assertTrue(vec3.is_spherical()) + self.assertTrue(vec12.is_spherical()) + self.assertTrue(vec21.is_metric()) + self.assertTrue(vec4.is_metric()) + + self.assertTrue(math.isnan(vec3.lat().radian())) + self.assertTrue(math.isnan(vec12.lat().radian())) + self.assertTrue(math.isnan(vec21.x())) + self.assertTrue(math.isnan(vec4.x())) + + self.assertTrue(math.isnan(vec3.lon().radian())) + self.assertTrue(math.isnan(vec12.lon().radian())) + self.assertTrue(math.isnan(vec21.y())) + self.assertTrue(math.isnan(vec4.y())) + + self.assertTrue(math.isnan(vec3.z())) + self.assertTrue(math.isnan(vec12.z())) + self.assertTrue(math.isnan(vec21.z())) + self.assertTrue(math.isnan(vec4.z())) + + def test_not_equal(self): + vec1 = CoordinateVector3.metric(0.1, 0.2, 0.3) + vec2 = CoordinateVector3.metric(0.2, 0.2, 0.3) + vec3 = CoordinateVector3.metric(0.1, 0.2, 0.3) + vec4 = CoordinateVector3.spherical(Angle(0.1), Angle(0.2), 0.3) + vec5 = CoordinateVector3.spherical(Angle(0.2), Angle(0.2), 0.3) + vec6 = CoordinateVector3.spherical(Angle(0.1), Angle(0.2), 0.3) + + self.assertTrue(vec1 != vec2) + self.assertFalse(vec1 != vec3) + + self.assertTrue(vec4 != vec5) + self.assertFalse(vec4 != vec6) + + self.assertTrue(vec1 != vec4) + self.assertTrue(vec1 != vec5) + self.assertTrue(vec1 != vec6) + self.assertTrue(vec2 != vec4) + self.assertTrue(vec2 != vec5) + self.assertTrue(vec2 != vec6) + self.assertTrue(vec3 != vec4) + self.assertTrue(vec3 != vec5) + self.assertTrue(vec3 != vec6) + self.assertTrue(vec4 != vec1) + self.assertTrue(vec4 != vec2) + self.assertTrue(vec4 != vec3) + self.assertTrue(vec5 != vec1) + self.assertTrue(vec5 != vec2) + self.assertTrue(vec5 != vec3) + self.assertTrue(vec6 != vec1) + self.assertTrue(vec6 != vec2) + self.assertTrue(vec6 != vec3) + + def test_equal_tolerance_metric(self): + zero = CoordinateVector3() + one = CoordinateVector3.metric(1, 1, 1) + self.assertFalse(zero.equal(one, 1e-6, Angle(1e-6))) + self.assertFalse(zero.equal(one, 1e-3, Angle(1e-3))) + self.assertFalse(zero.equal(one, 1e-1, Angle(1e-1))) + self.assertTrue(zero.equal(one, 1, Angle(1))) + self.assertTrue(zero.equal(one, 1.1, Angle(1.1))) + self.assertTrue(zero.equal(one, 1.1, Angle(1e-6))) + self.assertFalse(zero.equal(one, 1e-6, Angle(1.1))) + + def test_equal_tolerance_spherical(self): + zero = CoordinateVector3.spherical(Angle(0), Angle(0), 0) + one = CoordinateVector3.spherical(Angle(1), Angle(1), 1) + self.assertFalse(zero.equal(one, 1e-6, Angle(1e-6))) + self.assertFalse(zero.equal(one, 1e-3, Angle(1e-3))) + self.assertFalse(zero.equal(one, 1e-1, Angle(1e-1))) + self.assertTrue(zero.equal(one, 1, Angle(1))) + self.assertTrue(zero.equal(one, 1.1, Angle(1.1))) + self.assertFalse(zero.equal(one, 1.1, Angle(1e-6))) + self.assertFalse(zero.equal(one, 1e-6, Angle(1.1))) + + def test_equal_tolerance_mismatch(self): + zero = CoordinateVector3.spherical(Angle(0), Angle(0), 0) + one = CoordinateVector3.metric(1, 1, 1) + self.assertFalse(zero.equal(one, 1e-6, Angle(1e-6))) + self.assertFalse(zero.equal(one, 1e-3, Angle(1e-3))) + self.assertFalse(zero.equal(one, 1e-1, Angle(1e-1))) + self.assertFalse(zero.equal(one, 1, Angle(1))) + self.assertFalse(zero.equal(one, 1.1, Angle(1.1))) + self.assertFalse(zero.equal(one, 1.1, Angle(1e-6))) + self.assertFalse(zero.equal(one, 1e-6, Angle(1.1))) + + def test_finite(self): + self.assertTrue(CoordinateVector3.metric(0.1, 0.2, 0.3).is_finite()) + self.assertTrue(CoordinateVector3.spherical( + Angle(0.1), Angle(0.2), 0.3).is_finite()) + self.assertFalse(CoordinateVector3.metric( + math.nan, math.nan, math.nan).is_finite()) + self.assertFalse(CoordinateVector3.spherical( + Angle(math.nan), Angle(math.nan), math.nan).is_finite()) + self.assertFalse(CoordinateVector3.metric( + math.inf, math.inf, math.inf).is_finite()) + self.assertFalse(CoordinateVector3.spherical( + Angle(math.inf), Angle(math.inf), math.inf).is_finite()) + self.assertFalse(CoordinateVector3.metric( + math.inf, 0, 0).is_finite()) + self.assertFalse(CoordinateVector3.spherical( + Angle(math.inf), Angle(0), 0).is_finite()) + + def test_nan_metric(self): + nanVec = CoordinateVector3.metric(math.nan, math.nan, math.nan) + self.assertFalse(nanVec.is_finite()) + self.assertIsNotNone(nanVec.x()) + self.assertIsNotNone(nanVec.y()) + self.assertIsNotNone(nanVec.z()) + self.assertIsNone(nanVec.lat()) + self.assertIsNone(nanVec.lon()) + self.assertTrue(math.isnan(nanVec.x())) + self.assertTrue(math.isnan(nanVec.y())) + self.assertTrue(math.isnan(nanVec.z())) + + def test_nan_spherical(self): + nanVec = CoordinateVector3.spherical( + Angle(math.nan), Angle(math.nan), math.nan) + self.assertFalse(nanVec.is_finite()) + self.assertIsNone(nanVec.x()) + self.assertIsNone(nanVec.y()) + self.assertIsNotNone(nanVec.z()) + self.assertIsNotNone(nanVec.lat()) + self.assertIsNotNone(nanVec.lon()) + self.assertTrue(math.isnan(nanVec.lat().radian())) + self.assertTrue(math.isnan(nanVec.lon().radian())) + self.assertTrue(math.isnan(nanVec.z())) + + def test_str_metric(self): + v = CoordinateVector3.metric(0.1234, 1.234, 2.3456) + self.assertEqual(str(v), "0.1234 1.234 2.3456") + + def test_str_spherical(self): + v = CoordinateVector3.spherical(Angle(0.1234), Angle(1.234), 2.3456) + self.assertEqual(str(v), "7.0703° 70.703° 2.3456") + +if __name__ == '__main__': + unittest.main() diff --git a/src/python_pybind11/test/SphericalCoordinates_TEST.py b/src/python_pybind11/test/SphericalCoordinates_TEST.py index ef333f30..4b459126 100644 --- a/src/python_pybind11/test/SphericalCoordinates_TEST.py +++ b/src/python_pybind11/test/SphericalCoordinates_TEST.py @@ -12,14 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import unittest +import warnings -import gz -from gz.math8 import Angle, SphericalCoordinates, Vector3d -import math +from gz.math8 import Angle, CoordinateVector3, SphericalCoordinates, Vector3d -class TestSphericalCoordinates(unittest.TestCase): +def ignore_deprecation_warnings(test_func): + def do_test(self, *args, **kwargs): + with warnings.catch_warnings(): + warnings.simplefilter("ignore", category=DeprecationWarning) + test_func(self, *args, **kwargs) + return do_test + + +class TestSphericalCoordinates(unittest.TestCase): def test_constructor(self): # Default surface type st = SphericalCoordinates.EARTH_WGS84 @@ -182,6 +190,206 @@ def test_coordinate_transforms(self): elev = 354.1 sc = SphericalCoordinates(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 + xyz = CoordinateVector3() + # east, north, up + enu = CoordinateVector3() + + xyz.set_metric(1, 0, 0) + enu = sc.global_from_local_velocity(xyz) + self.assertIsNotNone(enu) + self.assertTrue(enu.is_metric()) + 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)) + + xyz.set_metric(0, 1, 0) + enu = sc.global_from_local_velocity(xyz) + self.assertIsNotNone(enu) + self.assertTrue(enu.is_metric()) + 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)) + + xyz.set_metric(1, -1, 0) + enu = sc.global_from_local_velocity(xyz) + self.assertIsNotNone(enu) + self.assertTrue(enu.is_metric()) + 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)) + + xyz.set_metric(2243.52334, 556.35, 435.6553) + enu = sc.global_from_local_velocity(xyz) + self.assertIsNotNone(enu) + self.assertTrue(enu.is_metric()) + 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)) + + # Check SphericalFromLocal + # local frame + xyz = CoordinateVector3() + # spherical coordinates + sph = CoordinateVector3() + + # No offset + xyz.set_metric(0, 0, 0) + sph = sc.spherical_from_local_position(xyz) + self.assertIsNotNone(sph) + self.assertTrue(sph.is_spherical()) + # latitude + self.assertAlmostEqual(sph.lat().degree(), lat.degree(), delta=1e-6) + # longitude + self.assertAlmostEqual(sph.lon().degree(), lon.degree(), delta=1e-6) + # elevation + self.assertAlmostEqual(sph.z(), elev, delta=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_metric(2e5, 0, 0) + sph = sc.spherical_from_local_position(xyz) + self.assertIsNotNone(sph) + self.assertTrue(sph.is_spherical()) + # increase in latitude about 1.8 degrees + self.assertAlmostEqual( + sph.lat().degree(), 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) + self.assertIsNotNone(xyz2) + self.assertTrue(xyz2.is_metric()) + self.assertEqual(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 + tmp = CoordinateVector3() + osrf_s = CoordinateVector3.spherical( + Angle(math.radians(37.3877349)), + Angle(math.radians(-122.0651166)), + 32.0) + osrf_e = CoordinateVector3.metric( + -2693701.91434394, -4299942.14687992, 3851691.0393571) + goog_s = CoordinateVector3.spherical( + Angle(math.radians(37.4216719)), + Angle(math.radians(-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) + vec = CoordinateVector3.metric(-1510.88, 3766.64, -3.29) + + # Set the ORIGIN to be the Open Source Robotics Foundation + sc2 = SphericalCoordinates( + st, osrf_s.lat(), osrf_s.lon(), osrf_s.z(), Angle.ZERO) + + # Check that SPHERICAL -> ECEF works + tmp = sc2.position_transform(osrf_s, SphericalCoordinates.SPHERICAL, + SphericalCoordinates.ECEF) + + self.assertIsNotNone(tmp) + self.assertTrue(tmp.is_metric()) + 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) + + self.assertIsNotNone(tmp) + self.assertTrue(tmp.is_spherical()) + self.assertAlmostEqual( + tmp.lat().degree(), osrf_s.lat().degree(), delta=1e-2) + self.assertAlmostEqual( + tmp.lon().degree(), osrf_s.lon().degree(), delta=1e-2) + self.assertAlmostEqual(tmp.z(), osrf_s.z(), delta=1e-2) + + # Check that SPHERICAL -> LOCAL works + tmp = sc2.local_from_spherical_position(goog_s) + self.assertIsNotNone(tmp) + self.assertTrue(tmp.is_metric()) + 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) + self.assertIsNotNone(tmp) + self.assertTrue(tmp.is_spherical()) + self.assertAlmostEqual( + tmp.lat().degree(), goog_s.lat().degree(), delta=8e-2) + self.assertAlmostEqual( + tmp.lon().degree(), goog_s.lon().degree(), delta=8e-2) + self.assertAlmostEqual(tmp.z(), goog_s.z(), delta=1e-2) + + # Give no heading offset to confirm ENU frame + lat = Angle(0.3) + lon = Angle(-1.2) + heading = Angle(0.0) + elev = 354.1 + sc = SphericalCoordinates(st, lat, lon, elev, heading) + + # Check GlobalFromLocal with no heading offset + # local frame + xyz = CoordinateVector3() + # east, north, up + enu = CoordinateVector3() + + xyz.set_metric(1, 0, 0) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) + self.assertIsNotNone(enu) + self.assertTrue(enu.is_metric()) + self.assertEqual(xyz, enu) + self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + + xyz.set_metric(0, 1, 0) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) + self.assertIsNotNone(enu) + self.assertTrue(enu.is_metric()) + self.assertEqual(xyz, enu) + self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + + xyz.set_metric(1, -1, 0) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) + self.assertIsNotNone(enu) + self.assertTrue(enu.is_metric()) + self.assertEqual(xyz, enu) + self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + + xyz.set_metric(2243.52334, 556.35, 435.6553) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) + self.assertIsNotNone(enu) + self.assertTrue(enu.is_metric()) + self.assertEqual(xyz, enu) + self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + + # TODO(peci1): Remove this test in Gazebo 9 + @ignore_deprecation_warnings + def test_coordinate_transforms_deprecated(self): + # Default surface type + st = SphericalCoordinates.EARTH_WGS84 + + # Parameters + lat = Angle(0.3) + lon = Angle(-1.2) + heading = Angle(Angle.HALF_PI) + elev = 354.1 + sc = SphericalCoordinates(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 @@ -330,6 +538,32 @@ def test_coordinate_transforms(self): self.assertEqual(xyz, enu) self.assertEqual(xyz, sc.local_from_global_velocity(enu)) + # This is the incorrect and deprecated behavior of LOCAL frame + + xyz.set(1, 0, 0) + wsu = Vector3d(-xyz.x(), -xyz.y(), xyz.z()) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) + self.assertEqual(wsu, enu) + self.assertEqual(wsu, sc.local_from_global_velocity(enu)) + + xyz.set(0, 1, 0) + wsu = Vector3d(-xyz.x(), -xyz.y(), xyz.z()) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) + self.assertEqual(wsu, enu) + self.assertEqual(wsu, sc.local_from_global_velocity(enu)) + + xyz.set(1, -1, 0) + wsu = Vector3d(-xyz.x(), -xyz.y(), xyz.z()) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) + self.assertEqual(wsu, enu) + self.assertEqual(wsu, sc.local_from_global_velocity(enu)) + + xyz.set(2243.52334, 556.35, 435.6553) + wsu = Vector3d(-xyz.x(), -xyz.y(), xyz.z()) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) + self.assertEqual(wsu, enu) + self.assertEqual(wsu, sc.local_from_global_velocity(enu)) + def test_distance(self): latA = Angle() longA = Angle() @@ -377,6 +611,34 @@ def test_bad_set_surface(self): self.assertEqual(sc.surface(), SphericalCoordinates.SurfaceType(3)) def test_transform(self): + sc = SphericalCoordinates() + vel = CoordinateVector3.metric(1, 2, -4) + result = sc.velocity_transform( + vel, + SphericalCoordinates.ECEF, + SphericalCoordinates.ECEF) + + self.assertIsNotNone(result) + self.assertTrue(result.is_metric()) + self.assertEqual(result, vel) + + pos = CoordinateVector3.metric(-1510.88, 2, -4) + result = sc.position_transform( + pos, + SphericalCoordinates.ECEF, + SphericalCoordinates.GLOBAL) + + self.assertIsNotNone(result) + self.assertTrue(result.is_metric()) + self.assertAlmostEqual(result.x(), 2, delta=1e-6) + self.assertAlmostEqual(result.y(), -4, delta=1e-6) + self.assertAlmostEqual(result.z(), -6379647.8799999999, delta=1e-6) + + print('NEW POS[', result.x(), ' ', result.y(), ' ', result.z(), ']\n') + + # TODO(peci1): Remove this test in Gazebo 9 + @ignore_deprecation_warnings + def test_transform_deprecated(self): sc = SphericalCoordinates() vel = Vector3d(1, 2, -4) result = sc.velocity_transform( @@ -399,6 +661,45 @@ def test_transform(self): print('NEW POS[', result.x(), ' ', result.y(), ' ', result.z(), ']\n') def test_bad_coordinate_type(self): + sc = SphericalCoordinates() + pos = CoordinateVector3.metric(1, 2, -4) + result = sc.position_transform(pos, + SphericalCoordinates.CoordinateType(7), + SphericalCoordinates.CoordinateType(6)) + + self.assertIsNone(result) + + result = sc.position_transform(pos, + SphericalCoordinates.CoordinateType(4), + SphericalCoordinates.CoordinateType(6)) + + self.assertIsNone(result) + + result = sc.velocity_transform( + pos, + SphericalCoordinates.SPHERICAL, + SphericalCoordinates.ECEF) + self.assertIsNone(result) + + result = sc.velocity_transform( + pos, + SphericalCoordinates.ECEF, + SphericalCoordinates.SPHERICAL) + self.assertIsNone(result) + + result = sc.velocity_transform(pos, + SphericalCoordinates.CoordinateType(7), + SphericalCoordinates.ECEF) + self.assertIsNone(result) + + result = sc.velocity_transform(pos, + SphericalCoordinates.ECEF, + SphericalCoordinates.CoordinateType(7)) + self.assertIsNone(result) + + # TODO(peci1): Remove this test in Gazebo 9 + @ignore_deprecation_warnings + def test_bad_coordinate_type_deprecated(self): sc = SphericalCoordinates() pos = Vector3d(1, 2, -4) result = sc.position_transform(pos, @@ -481,6 +782,110 @@ def test_no_heading(self): elev = 0 sc = SphericalCoordinates(st, lat, lon, elev, heading) + # Origin matches input + latLonAlt = sc.spherical_from_local_position( + CoordinateVector3.metric(0, 0, 0)) + self.assertIsNotNone(latLonAlt) + self.assertTrue(latLonAlt.is_spherical()) + self.assertEqual(lat.degree(), latLonAlt.lat().degree()) + self.assertEqual(lon.degree(), latLonAlt.lon().degree()) + self.assertEqual(elev, latLonAlt.z()) + + xyzOrigin = sc.local_from_spherical_position(latLonAlt) + self.assertIsNotNone(xyzOrigin) + self.assertTrue(xyzOrigin.is_metric()) + self.assertEqual(Vector3d.ZERO, xyzOrigin.as_metric_vector()) + + # Check how different lat/lon affect the local position + + # Increase latitude == go North == go +Y + xyz = sc.local_from_spherical_position(CoordinateVector3.spherical( + lat + Angle(math.radians(1.0)), lon, elev)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + 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(CoordinateVector3.spherical( + lat - Angle(math.radians(1.0)), lon, elev)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + 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(CoordinateVector3.spherical( + lat, lon + Angle(math.radians(1.0)), elev)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + 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(CoordinateVector3.spherical( + lat, lon - Angle(math.radians(1.0)), elev)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + self.assertGreater(xyzOrigin.x(), xyz.x()) + self.assertGreater(xyzOrigin.y(), xyz.y()) + + # Increase altitude + xyz = sc.local_from_spherical_position(CoordinateVector3.spherical( + lat, lon, elev + 10.0)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + 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(CoordinateVector3.spherical( + lat, lon, elev - 10.0)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + 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) + + # Check how global and local velocities are connected + + # Velocity in + # +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( + CoordinateVector3.metric(global_var)) + self.assertIsNotNone(local) + self.assertTrue(local.is_metric()) + self.assertEqual(global_var, local.as_metric_vector()) + + global_var = sc.global_from_local_velocity(local) + self.assertIsNotNone(global_var) + self.assertTrue(global_var.is_metric()) + self.assertEqual(global_var, local) + + # Directly call velocity_transform + global_var = sc.velocity_transform( + local, + SphericalCoordinates.LOCAL, + SphericalCoordinates.GLOBAL) + self.assertIsNotNone(global_var) + self.assertTrue(global_var.is_metric()) + self.assertEqual(global_var, local) + + @ignore_deprecation_warnings + def test_no_heading_deprecated(self): + # Default heading + st = SphericalCoordinates.EARTH_WGS84 + lat = Angle(-22.9 * math.pi / 180.0) + lon = Angle(-43.2 * math.pi / 180.0) + heading = Angle(0.0) + elev = 0 + sc = SphericalCoordinates(st, lat, lon, elev, heading) + # Origin matches input latLonAlt = sc.spherical_from_local_position(Vector3d(0, 0, 0)) self.assertEqual(lat.degree(), latLonAlt.x()) @@ -543,17 +948,22 @@ def test_no_heading(self): # 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) + wsu = CoordinateVector3.metric( + -global_var.x(), -global_var.y(), global_var.z()) + self.assertAlmostEqual(wsu.x(), local.x(), delta=1e-6) + self.assertAlmostEqual(wsu.y(), local.y(), delta=1e-6) + self.assertAlmostEqual(wsu.z(), local.z(), delta=1e-6) - # Directly call fixed version + # Directly call velocity_transform global_var = sc.velocity_transform( local, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) - self.assertEqual(global_var, local) + wsu = CoordinateVector3.metric( + -global_var.x(), -global_var.y(), global_var.z()) + self.assertAlmostEqual(wsu.x(), local.x(), delta=1e-6) + self.assertAlmostEqual(wsu.y(), local.y(), delta=1e-6) + self.assertAlmostEqual(wsu.z(), local.z(), delta=1e-6) def test_with_heading(self): # Heading 90 deg: X == North, Y == West , Z == Up @@ -564,6 +974,94 @@ def test_with_heading(self): elev = 0 sc = SphericalCoordinates(st, lat, lon, elev, heading) + # Origin matches input + latLonAlt = sc.spherical_from_local_position( + CoordinateVector3.metric(0, 0, 0)) + self.assertIsNotNone(latLonAlt) + self.assertTrue(latLonAlt.is_spherical()) + self.assertEqual(lat.degree(), latLonAlt.lat().degree()) + self.assertEqual(lon.degree(), latLonAlt.lon().degree()) + self.assertEqual(elev, latLonAlt.z()) + + xyzOrigin = sc.local_from_spherical_position(latLonAlt) + self.assertIsNotNone(xyzOrigin) + self.assertTrue(xyzOrigin.is_metric()) + self.assertEqual(Vector3d.ZERO, xyzOrigin.as_metric_vector()) + + # Check how different lat/lon affect the local position + + # Increase latitude == go North == go +X + xyz = sc.local_from_spherical_position(CoordinateVector3.spherical( + lat + Angle(math.radians(1.0)), lon, elev)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + 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(CoordinateVector3.spherical( + lat - Angle(math.radians(1.0)), lon, elev)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + 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(CoordinateVector3.spherical( + lat, lon + Angle(math.radians(1.0)), elev)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + 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(CoordinateVector3.spherical( + lat, lon - Angle(math.radians(1.0)), elev)) + self.assertIsNotNone(xyz) + self.assertTrue(xyz.is_metric()) + self.assertLess(xyzOrigin.y(), xyz.y()) + self.assertGreater(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 + globalLocal = [ + [Vector3d.UNIT_X, -Vector3d.UNIT_Y], + [-Vector3d.UNIT_X, Vector3d.UNIT_Y], + [Vector3d.UNIT_Y, Vector3d.UNIT_X], + [-Vector3d.UNIT_Y, -Vector3d.UNIT_X]] + for [global_var, local] in globalLocal: + localRes = sc.local_from_global_velocity( + CoordinateVector3.metric(global_var)) + self.assertIsNotNone(localRes) + self.assertTrue(localRes.is_metric()) + self.assertEqual(local, localRes.as_metric_vector()) + + # Directly call velocity_transform + globalRes = sc.velocity_transform( + CoordinateVector3.metric(local), + SphericalCoordinates.LOCAL, + SphericalCoordinates.GLOBAL) + self.assertIsNotNone(globalRes) + self.assertTrue(globalRes.is_metric()) + self.assertEqual(global_var, globalRes.as_metric_vector()) + + # TODO(peci1): Remove this test in Gazebo 9 + @ignore_deprecation_warnings + def test_with_heading_deprecated(self): + # Heading 90 deg: X == North, Y == West , Z == Up + st = SphericalCoordinates.EARTH_WGS84 + lat = Angle(-22.9 * math.pi / 180.0) + lon = Angle(-43.2 * math.pi / 180.0) + heading = Angle(90.0 * math.pi / 180.0) + elev = 0 + sc = SphericalCoordinates(st, lat, lon, elev, heading) + # Origin matches input latLonAlt = sc.spherical_from_local_position(Vector3d(0, 0, 0)) self.assertEqual(lat.degree(), latLonAlt.x()) @@ -619,7 +1117,7 @@ def test_with_heading(self): # Directly call fixed version globalRes = sc.velocity_transform( local, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) self.assertEqual(global_var, globalRes) @@ -631,6 +1129,66 @@ def test_inverse(self): elev = 354.1 sc = SphericalCoordinates(st, lat, lon, elev, heading) + # GLOBAL <-> LOCAL + in_vector = CoordinateVector3.metric(1, 2, -4) + out = sc.velocity_transform( + in_vector, + SphericalCoordinates.LOCAL, + SphericalCoordinates.GLOBAL) + self.assertIsNotNone(out) + self.assertTrue(out.is_metric()) + self.assertNotEqual(in_vector, out) + reverse = sc.velocity_transform( + out, + SphericalCoordinates.GLOBAL, + SphericalCoordinates.LOCAL) + self.assertIsNotNone(reverse) + self.assertTrue(reverse.is_metric()) + self.assertEqual(in_vector, reverse) + + in_vector = CoordinateVector3.metric(1, 2, -4) + out = sc.position_transform( + in_vector, + SphericalCoordinates.LOCAL, + SphericalCoordinates.GLOBAL) + self.assertIsNotNone(out) + self.assertTrue(out.is_metric()) + self.assertNotEqual(in_vector, out) + reverse = sc.position_transform( + out, + SphericalCoordinates.GLOBAL, + SphericalCoordinates.LOCAL) + self.assertIsNotNone(reverse) + self.assertTrue(reverse.is_metric()) + self.assertEqual(in_vector, reverse) + + # SPHERICAL <-> LOCAL + in_vector = CoordinateVector3.metric(1, 2, -4) + out = sc.position_transform( + in_vector, + SphericalCoordinates.LOCAL, + SphericalCoordinates.SPHERICAL) + self.assertIsNotNone(out) + self.assertTrue(out.is_spherical()) + self.assertNotEqual(in_vector, out) + reverse = sc.position_transform( + out, + SphericalCoordinates.SPHERICAL, + SphericalCoordinates.LOCAL) + self.assertIsNotNone(reverse) + self.assertTrue(reverse.is_metric()) + self.assertEqual(in_vector, reverse) + + # TODO(peci1): Remove this test in Gazebo 9 + @ignore_deprecation_warnings + def test_inverse_deprecated(self): + st = SphericalCoordinates.EARTH_WGS84 + lat = Angle(0.3) + lon = Angle(-1.2) + heading = Angle(0.5) + elev = 354.1 + sc = SphericalCoordinates(st, lat, lon, elev, heading) + # GLOBAL <-> LOCAL2 in_vector = Vector3d(1, 2, -4) out = sc.velocity_transform( @@ -669,6 +1227,44 @@ def test_inverse(self): SphericalCoordinates.LOCAL2) self.assertEqual(in_vector, reverse) + # GLOBAL <-> LOCAL + in_vector = Vector3d(1, 2, -4) + out = sc.velocity_transform( + in_vector, + SphericalCoordinates.LOCAL, + SphericalCoordinates.GLOBAL) + self.assertNotEqual(in_vector, out) + reverse = sc.velocity_transform( + out, + SphericalCoordinates.GLOBAL, + SphericalCoordinates.LOCAL) + self.assertNotEqual(in_vector, reverse) + + in_vector = Vector3d(1, 2, -4) + out = sc.position_transform( + in_vector, + SphericalCoordinates.LOCAL, + SphericalCoordinates.GLOBAL) + self.assertNotEqual(in_vector, out) + reverse = sc.position_transform( + out, + SphericalCoordinates.GLOBAL, + SphericalCoordinates.LOCAL) + self.assertNotEqual(in_vector, reverse) + + # SPHERICAL <-> LOCAL + in_vector = Vector3d(1, 2, -4) + out = sc.position_transform( + in_vector, + SphericalCoordinates.LOCAL, + SphericalCoordinates.SPHERICAL) + self.assertNotEqual(in_vector, out) + reverse = sc.position_transform( + out, + SphericalCoordinates.SPHERICAL, + SphericalCoordinates.LOCAL) + self.assertNotEqual(in_vector, reverse) + if __name__ == '__main__': unittest.main() diff --git a/src/ruby/Angle.i b/src/ruby/Angle.i index 5f832e11..7830fb9f 100644 --- a/src/ruby/Angle.i +++ b/src/ruby/Angle.i @@ -40,7 +40,10 @@ namespace gz public: double Degree() const; public: void Normalize(); public: Angle Normalized() const; + public: Angle Abs() const; + public: Angle ShortestDistance(const Angle &_angle) const; public: inline double operator*() const; + public: Angle operator-() const; public: Angle operator-(const Angle &_angle) const; public: Angle operator+(const Angle &_angle) const; public: Angle operator*(const Angle &_angle) const; diff --git a/src/ruby/Angle_TEST.rb b/src/ruby/Angle_TEST.rb index ea047277..79a1706d 100644 --- a/src/ruby/Angle_TEST.rb +++ b/src/ruby/Angle_TEST.rb @@ -111,6 +111,9 @@ def test_construction assert(angle >= 1.2, "Angle should be greater or equal to 1.2") assert(angle >= -1.19, "Angle should be greater or equal to -1.19") + assert(-Gz::Math::Angle::new(1.2) == Gz::Math::Angle::new(-1.2), + "Angle should be greater or equal to -1.19") + assert(Gz::Math::Angle.new(1.2) >= Gz::Math::Angle.new(1.2000000001), "1.2 should be greater than or equal to 1.2000000001") diff --git a/src/ruby/CMakeLists.txt b/src/ruby/CMakeLists.txt index 7c541051..a27f06e5 100644 --- a/src/ruby/CMakeLists.txt +++ b/src/ruby/CMakeLists.txt @@ -15,8 +15,10 @@ if (SWIG_FOUND) set(swig_files Angle + CoordinateVector3 GaussMarkovProcess Rand + SphericalCoordinates Vector2 Vector3 Vector4) diff --git a/src/ruby/CoordinateVector3.i b/src/ruby/CoordinateVector3.i new file mode 100644 index 00000000..bad7a239 --- /dev/null +++ b/src/ruby/CoordinateVector3.i @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifdef SWIGRUBY +%begin %{ +#define HAVE_ISFINITE 1 +%} +#endif + +%module coordinatevector3 +%{ +#include + +#include +#include +#include +%} + +namespace gz +{ + namespace math + { + class CoordinateVector3 + { + public: CoordinateVector3(); + public: CoordinateVector3(const CoordinateVector3 &_other); + public: static CoordinateVector3 Metric(double _x, double _y, double _z); + public: static CoordinateVector3 Metric( + const gz::math::Vector3 &_v); + public: static CoordinateVector3 Spherical(const gz::math::Angle& _lat, + const gz::math::Angle& _lon, double _z); + public: bool IsMetric() const; + public: bool IsSpherical() const; + public: void SetMetric(double _x, double _y, double _z); + public: void SetMetric(const gz::math::Vector3 &_v); + public: void SetSpherical(const gz::math::Angle& _lat, + const gz::math::Angle& _lon, double _z); + public: std::optional AsMetricVector() const; + public: CoordinateVector3 operator+(const CoordinateVector3 &_v) const; + public: CoordinateVector3 operator-() const; + public: CoordinateVector3 operator-(const CoordinateVector3 &_pt) const; + public: bool Equal(const CoordinateVector3 &_v, const double &_tol, + const gz::math::Angle &_ang_tol) const; + public: bool operator==(const CoordinateVector3 &_v) const; + public: bool IsFinite() const; + public: bool Equal(const CoordinateVector3 &_v) const; + public: std::optional X() const; + public: std::optional Lat() const; + public: std::optional Y() const; + public: std::optional Lon() const; + public: std::optional Z() const; + public: void X(const double &_v); + public: void Lat(const gz::math::Angle &_v); + public: void Y(const double &_v); + public: void Lon(const gz::math::Angle &_v); + public: void Z(const double &_v); + }; + } +} diff --git a/src/ruby/CoordinateVector3_TEST.rb b/src/ruby/CoordinateVector3_TEST.rb new file mode 100644 index 00000000..29d09d7a --- /dev/null +++ b/src/ruby/CoordinateVector3_TEST.rb @@ -0,0 +1,52 @@ +# Copyright (C) 2024 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +#!/usr/bin/env ruby + +require 'test/unit/ui/console/testrunner' +require 'test/unit' +require 'math' + +class CoordinateVector3_TEST < Test::Unit::TestCase + def test_construction + v = Gz::Math::CoordinateVector3.new + assert(v.IsMetric(), "CoordinateVector3::IsMetric() should be true") + assert(!v.IsSpherical(), "CoordinateVector3::IsSpherical() should be false") + assert(v.IsFinite(), "CoordinateVector3::IsFinite() should be true") + # TODO(anyone): std::optional<> bindings are not working, + # so this test doesn't test much + # assert(v.X() == 0.0, "x should be 0") + # assert(v.Y() == 0.0, "y should be 0") + # assert(v.Z() == 0.0, "z should be 0") + + v = Gz::Math::CoordinateVector3.Metric(1.0, 2.0, 3.0) + assert(v.IsMetric(), "CoordinateVector3::IsMetric() should be true") + assert(!v.IsSpherical(), "CoordinateVector3::IsSpherical() should be false") + assert(v.IsFinite(), "CoordinateVector3::IsFinite() should be true") + + v = Gz::Math::CoordinateVector3.Metric(Gz::Math::Vector3d.new(1.0, 2.0, 3.0)) + assert(v.IsMetric(), "CoordinateVector3::IsMetric() should be true") + assert(!v.IsSpherical(), "CoordinateVector3::IsSpherical() should be false") + assert(v.IsFinite(), "CoordinateVector3::IsFinite() should be true") + + v = Gz::Math::CoordinateVector3.Spherical( + Gz::Math::Angle.new(1.0), Gz::Math::Angle.new(2.0), 3.0) + assert(!v.IsMetric(), "CoordinateVector3::IsMetric() should be false") + assert(v.IsSpherical(), "CoordinateVector3::IsSpherical() should be true") + assert(v.IsFinite(), "CoordinateVector3::IsFinite() should be true") + end + +end + +exit Test::Unit::UI::Console::TestRunner.run(CoordinateVector3_TEST).passed? ? 0 : -1 diff --git a/src/ruby/SphericalCoordinates.i b/src/ruby/SphericalCoordinates.i index 801536e2..4a60417c 100644 --- a/src/ruby/SphericalCoordinates.i +++ b/src/ruby/SphericalCoordinates.i @@ -15,15 +15,28 @@ * */ +#ifdef SWIGRUBY +%begin %{ +#define HAVE_ISFINITE 1 +%} +#endif + %module sphericalcoordinates %{ #include +#include #include #include #include +#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 @@ -36,7 +49,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 }; public: enum CoordinateType @@ -77,17 +98,25 @@ namespace gz public: gz::math::Vector3 SphericalFromLocalPosition( const gz::math::Vector3 &_xyz) const; + public: std::optional + SphericalFromLocalPosition( + const gz::math::CoordinateVector3 &_xyz) const; + public: gz::math::Vector3 GlobalFromLocalVelocity( const gz::math::Vector3 &_xyz) const; + public: std::optional + GlobalFromLocalVelocity( + const gz::math::CoordinateVector3 &_xyz) const; + public: static SurfaceType Convert(const std::string &_str); public: static std::string Convert(SurfaceType _type); - public: static double Distance(const gz::math::Angle &_latA, - const gz::math::Angle &_lonA, - const gz::math::Angle &_latB, - const gz::math::Angle &_lonB); + public: static double DistanceWGS84(const gz::math::Angle &_latA, + const gz::math::Angle &_lonA, + const gz::math::Angle &_latB, + const gz::math::Angle &_lonB); public: SurfaceType Surface() const; @@ -112,23 +141,37 @@ namespace gz public: gz::math::Vector3 LocalFromSphericalPosition( const gz::math::Vector3 &_latLonEle) const; + public: std::optional + LocalFromSphericalPosition( + const gz::math::CoordinateVector3 &_latLonEle) const; + public: gz::math::Vector3 LocalFromGlobalVelocity( const gz::math::Vector3 &_xyz) const; + public: std::optional + LocalFromGlobalVelocity( + const gz::math::CoordinateVector3 &_xyz) const; + public: void UpdateTransformationMatrix(); public: gz::math::Vector3 PositionTransform(const gz::math::Vector3 &_pos, const CoordinateType &_in, const CoordinateType &_out) const; + public: std::optional PositionTransform( + const gz::math::CoordinateVector3 &_pos, + const CoordinateType &_in, const CoordinateType &_out) const; + /// \return Transformed velocity vector public: gz::math::Vector3 VelocityTransform( const gz::math::Vector3 &_vel, const CoordinateType &_in, const CoordinateType &_out) const; - public: bool operator==(const SphericalCoordinates &_sc) const; + public: std::optional VelocityTransform( + const gz::math::CoordinateVector3 &_vel, + const CoordinateType &_in, const CoordinateType &_out) const; - public: bool operator!=(const SphericalCoordinates &_sc) const; + public: bool operator==(const SphericalCoordinates &_sc) const; }; } } diff --git a/src/ruby/SphericalCoordinates_TEST.rb b/src/ruby/SphericalCoordinates_TEST.rb new file mode 100644 index 00000000..a21af9a5 --- /dev/null +++ b/src/ruby/SphericalCoordinates_TEST.rb @@ -0,0 +1,58 @@ +# Copyright (C) 2024 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +#!/usr/bin/env ruby + +require 'test/unit/ui/console/testrunner' +require 'test/unit' +require 'math' + +class SphericalCoordinates_TEST < Test::Unit::TestCase + def test_construction + v = Gz::Math::SphericalCoordinates.new + assert(v.Surface() == Gz::Math::SphericalCoordinates::EARTH_WGS84, "Wrong Surface()") + assert(v.LatitudeReference() == Gz::Math::Angle.new(), "Wrong LatitudeReference()") + assert(v.LongitudeReference() == Gz::Math::Angle.new(), "Wrong LongitudeReference()") + assert(v.HeadingOffset() == Gz::Math::Angle.new(), "Wrong HeadingOffset()") + assert(v.ElevationReference() == 0.0, "Wrong ElevationReference()") + + v = Gz::Math::SphericalCoordinates.new(Gz::Math::SphericalCoordinates::EARTH_WGS84) + assert(v.Surface() == Gz::Math::SphericalCoordinates::EARTH_WGS84, "Wrong Surface()") + assert(v.LatitudeReference() == Gz::Math::Angle.new(), "Wrong LatitudeReference()") + assert(v.LongitudeReference() == Gz::Math::Angle.new(), "Wrong LongitudeReference()") + assert(v.HeadingOffset() == Gz::Math::Angle.new(), "Wrong HeadingOffset()") + assert(v.ElevationReference() == 0.0, "Wrong ElevationReference()") + + lat = Gz::Math::Angle.new(0.3) + lon = Gz::Math::Angle.new(-1.2) + heading = Gz::Math::Angle.new(0.5) + elev = 354.1 + v = Gz::Math::SphericalCoordinates.new( + Gz::Math::SphericalCoordinates::EARTH_WGS84, lat, lon, elev, heading) + assert(v.Surface() == Gz::Math::SphericalCoordinates::EARTH_WGS84, "Wrong Surface()") + assert(v.LatitudeReference() == lat, "Wrong LatitudeReference()") + assert(v.LongitudeReference() == lon, "Wrong LongitudeReference()") + assert(v.HeadingOffset() == heading, "Wrong HeadingOffset()") + assert(v.ElevationReference() == elev, "Wrong ElevationReference()") + + v2 = Gz::Math::SphericalCoordinates.new(v) + assert(v2 == v, "instances should equal") + + # TODO(anyone): std::optional<> bindings are not working, + # so this test doesn't test much + end + +end + +exit Test::Unit::UI::Console::TestRunner.run(SphericalCoordinates_TEST).passed? ? 0 : -1 diff --git a/src/ruby/ruby.i b/src/ruby/ruby.i index 301e98f7..9a4e30d8 100644 --- a/src/ruby/ruby.i +++ b/src/ruby/ruby.i @@ -1,7 +1,9 @@ %module "gz::math" %include Angle.i +%include CoordinateVector3.i %include GaussMarkovProcess.i %include Rand.i +%include SphericalCoordinates.i %include Vector2.i %include Vector3.i %include Vector4.i