Fixed issues from review, added more tests.

Signed-off-by: Martin Pecka <[email protected]>
gazebosim · Aug 19, 2024 · 56b9fcb · 56b9fcb
1 parent 7212ead
commit 56b9fcb
Show file tree

Hide file tree

Showing 11 changed files with 1,721 additions and 60 deletions.
diff --git a/include/gz/math/CoordinateVector3.hh b/include/gz/math/CoordinateVector3.hh
@@ -110,14 +110,14 @@ namespace gz::math
     /// \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.
+    ///       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.
+    ///       vector will be set and a message logged to cerr.
     public: const CoordinateVector3 &operator+=(const CoordinateVector3 &_v);
 
     /// \brief Negation operator
@@ -128,14 +128,14 @@ namespace gz::math
     /// \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.
+    ///       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.
+    ///       vector will be set and a message logged to cerr.
     public: const CoordinateVector3 &operator-=(const CoordinateVector3 &_pt);
 
     /// \brief Equality test with tolerance.

diff --git a/src/CoordinateVector3.cc b/src/CoordinateVector3.cc
@@ -22,6 +22,7 @@
 #include "gz/utils/ImplPtr.hh"
 
 #include <cmath>
+#include <iostream>
 #include <optional>
 #include <variant>
 
@@ -111,12 +112,9 @@ CoordinateVector3::~CoordinateVector3() = default;
 
 /////////////////////////////////////////////////
 CoordinateVector3::CoordinateVector3(const CoordinateVector3 &_other) :
-  dataPtr(_other.IsMetric() ?
-  gz::utils::MakeUniqueImpl<Implementation>(
-    _other.dataPtr->X(), _other.dataPtr->Y(), _other.dataPtr->Z()) :
-  gz::utils::MakeUniqueImpl<Implementation>(
-    _other.dataPtr->Lat(), _other.dataPtr->Lon(), _other.dataPtr->Z()))
+  dataPtr(gz::utils::MakeUniqueImpl<Implementation>())
 {
+  *this->dataPtr = *_other.dataPtr;
 }
 
 /////////////////////////////////////////////////
@@ -132,16 +130,7 @@ CoordinateVector3& CoordinateVector3::operator=(const CoordinateVector3 &_other)
   if (!this->dataPtr)
     this->dataPtr = gz::utils::MakeUniqueImpl<Implementation>();
 
-  if (_other.IsMetric())
-  {
-    this->SetMetric(
-      _other.dataPtr->X(), _other.dataPtr->Y(), _other.dataPtr->Z());
-  }
-  else
-  {
-    this->SetSpherical(
-      _other.dataPtr->Lat(), _other.dataPtr->Lon(), _other.dataPtr->Z());
-  }
+  *this->dataPtr = *_other.dataPtr;
   return *this;
 }
 
@@ -229,8 +218,18 @@ CoordinateVector3 CoordinateVector3::operator+(
 {
   if (this->IsMetric() != _v.IsMetric())
   {
-    return this->IsMetric() ? Metric(NAN_D, NAN_D, NAN_D) :
-      Spherical(NAN_D, NAN_D, NAN_D);
+    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())
@@ -257,9 +256,17 @@ const CoordinateVector3& CoordinateVector3::operator+=(
   {
     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;
   }
 
@@ -299,8 +306,18 @@ CoordinateVector3 CoordinateVector3::operator-(
 {
   if (this->IsMetric() != _pt.IsMetric())
   {
-    return this->IsMetric() ? Metric(NAN_D, NAN_D, NAN_D) :
-           Spherical(NAN_D, NAN_D, NAN_D);
+    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())
@@ -327,9 +344,17 @@ const CoordinateVector3& CoordinateVector3::operator-=(
   {
     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;
   }
 

diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc
@@ -553,6 +553,9 @@ void SphericalCoordinates::UpdateTransformationMatrix()
     *this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF);
 }
 
+namespace
+{
+
 /////////////////////////////////////////////////
 std::optional<CoordinateVector3> PositionTransformTmp(
     const gz::utils::ImplPtr<SphericalCoordinates::Implementation>& dataPtr,
@@ -707,6 +710,8 @@ std::optional<CoordinateVector3> PositionTransformTmp(
   return res;
 }
 
+}
+
 /////////////////////////////////////////////////
 Vector3d SphericalCoordinates::PositionTransform(
     const Vector3d &_pos,
@@ -748,6 +753,9 @@ std::optional<CoordinateVector3> SphericalCoordinates::PositionTransform(
   return PositionTransformTmp(this->dataPtr, _pos, in, out);
 }
 
+namespace
+{
+
 //////////////////////////////////////////////////
 std::optional<CoordinateVector3> VelocityTransformTmp(
     const gz::utils::ImplPtr<SphericalCoordinates::Implementation>& dataPtr,
@@ -839,6 +847,8 @@ std::optional<CoordinateVector3> VelocityTransformTmp(
   return res;
 }
 
+}
+
 //////////////////////////////////////////////////
 Vector3d SphericalCoordinates::VelocityTransform(
     const Vector3d &_vel,