Improved unit tests

Polar.cpp

@@ -5,56 +5,54 @@
 #include "Spherical.h"
 
 Polar::Polar() {
-  angle = 0.0F;
+  this->distance = 0.0f;
-  distance = 0.0F;
+  this->angle = 0.0f;
 }
 
 Polar::Polar(float distance, Angle angle) {
   // distance should always be 0 or greater
-  if (distance < 0) {
+  if (distance < 0.0f) {
     this->distance = -distance;
     this->angle = Angle::Normalize(angle - 180);
   } else {
     this->distance = distance;
-    this->angle = Angle::Normalize(angle);
+    if (this->distance == 0.0f)
+      // angle is always 0 if distance is 0
+      this->angle = 0.0f;
+    else
+      this->angle = Angle::Normalize(angle);
   }
 }
 
 Polar::Polar(Vector2 v) {
-  angle = Vector2::SignedAngle(
+  this->distance = v.magnitude();
-      Vector2::forward,
+  this->angle = Vector2::SignedAngle(Vector2::forward, v);
-      v); // atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg;
-  distance = v.magnitude();
 }
 
-Polar::Polar(Spherical s) {
+Polar::Polar(Spherical v) {
-  angle = s.horizontalAngle;
+  this->distance = v.distance * cosf(v.verticalAngle * Angle::Deg2Rad);
-  distance = s.distance * cosf(s.verticalAngle * Angle::Deg2Rad);
+  this->angle = v.horizontalAngle;
 }
 
-const Polar Polar::zero = Polar(0, 0);
+const Polar Polar::zero = Polar(0.0f, 0.0f);
 
-float Polar::Distance(Polar &v1, Polar &v2) {
+bool Polar::operator==(const Polar &v) {
-  float d = Angle::CosineRuleSide(v1.distance, v2.distance,
+  return (this->distance == v.distance && this->angle == v.angle);
-                                  (float)v2.angle - (float)v1.angle);
-  return d;
 }
 
 Polar Polar::operator+(Polar &v2) {
   if (v2.distance == 0)
-    return Polar(this->distance,
+    return Polar(this->distance, this->angle);
-                 this->angle); // Polar(this->angle, this->distance);
+  if (this->distance == 0.0f)
-  if (this->distance == 0)
     return v2;
 
   float deltaAngle = Angle::Normalize(v2.angle - (float)this->angle);
-  float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle;
+  float rotation =
+      deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;
 
-  if (rotation == 180 && v2.distance > 0) {
+  if (rotation == 180.0f && v2.distance > 0.0f) {
     //  angle is too small, take this angle and add the distances
-    return Polar(
+    return Polar(this->distance + v2.distance, this->angle);
-        this->distance + v2.distance,
-        this->angle); // Polar(this->angle, this->distance + v2.distance);
   }
 
   float newDistance =
@@ -63,24 +61,22 @@ Polar Polar::operator+(Polar &v2) {
   float angle =
       Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
 
-  float newAngle =
+  float newAngle = deltaAngle < 0.0f ? (float)this->angle - angle
-      deltaAngle < 0 ? (float)this->angle - angle : (float)this->angle + angle;
+                                     : (float)this->angle + angle;
   newAngle = Angle::Normalize(newAngle);
-  Polar vector = Polar(newDistance, newAngle); // Polar(newAngle, newDistance);
+  Polar vector = Polar(newDistance, newAngle);
   return vector;
 }
 
 Polar Polar::operator-() {
-  Polar vector = Polar(this->distance,
+  Polar v = Polar(this->distance, this->angle + 180);
-                       (float)this->angle -
+  return v;
-                           180); // Polar(this->angle - 180, this->distance);
-  return vector;
 }
 
 Polar Polar::operator-(Polar &v2) {
   // Polar vector = *this + Polar(v2.distance, (float)v2.angle - 180);
   //(Polar(v2.angle - 180, v2.distance));
-  Polar vector = -v2;
+  v2 = -v2;
   return *this + v2;
 }
 
@@ -94,6 +90,12 @@ Polar Polar::operator/(const float &f) {
                this->angle); // Polar(this->angle, this->distance / f);
 }
 
+float Polar::Distance(Polar &v1, Polar &v2) {
+  float d = Angle::CosineRuleSide(v1.distance, v2.distance,
+                                  (float)v2.angle - (float)v1.angle);
+  return d;
+}
+
 Polar Polar::Rotate(Polar v, Angle angle) {
   v.angle = Angle::Normalize(v.angle + angle);
   return v;

Polar.h

@@ -19,57 +19,48 @@ struct Spherical;
 /// reference direction and a distance.
 struct Polar {
 public:
-  /// <summary>
-  /// The angle in degrees, clockwise rotation
-  /// </summary>
-  /// The angle is normalized to -180 .. 180
-  Angle angle;
   /// <summary>
   /// The distance in meters
   /// </summary>
   /// The distance should never be negative
+
+  /// @brief The distance in meters
+  /// @remark The distance shall never be negative
   float distance;
+  /// @brief The angle in degrees clockwise rotation
+  /// @remark The angle shall be between -180 .. 180
+  Angle angle;
 
-  /// <summary>
+  /// @brief A new vector with polar coordinates with zero degrees and distance
-  /// Create a new polar vector with zero degrees and distance
-  /// </summary>
   Polar();
-  /// <summary>
+  /// @brief A new vector with polar coordinates
-  /// Create a new polar vector
+  /// @param distance The distance in meters
-  /// </summary>
+  /// @param angle The angle in degrees, clockwise rotation
-  /// <param name="angle">The angle in degrees, clockwise rotation</param>
+  /// @note The distance is automatically converted to a positive value.
-  /// <param name="distance">The distance in meters</param>
+  /// @note The angle is automatically normalized to -180 .. 180
-  // Polar(float angle, float distance);
   Polar(float distance, Angle angle);
-
+  /// @brief Convert a vector from 2D carthesian coordinates to polar
-  /// <summary>
+  /// coordinates
-  /// Convert a Vector2 to a Polar coordinate
+  /// @param v The vector to convert
-  /// </summary>
-  /// <param name="v">The 2D carthesian vector</param>
   Polar(Vector2 v);
 
-  /// <summary>
+  /// @brief Convert a vector from spherical coordinates to polar coordinates
-  /// Convert a Spherical coordinate to a Polar coordinate
+  /// @param s The vector to convert
-  /// </summary>
+  /// @note The resulting vector will be projected on the horizontal plane
-  /// <param name="s">The spherical coordinate</param>
   Polar(Spherical s);
 
-  /// <summary>
+  /// @brief A polar vector with zero degrees and distance
-  /// A polar vector with zero degrees and distance
-  /// </summary>
   const static Polar zero;
 
-  /// <summary>
+  bool operator==(const Polar &v);
-  /// Negate the polar vector.
+
-  /// </summary>
+  /// @brief Negate the vector
+  /// @return The negated vector
   /// This will rotate the vector by 180 degrees. Distance will stay the same.
-  /// <returns>The negated vector</returns>
   Polar operator-();
-  /// <summary>
+  /// @brief Subtract a polar vector from this vector
-  /// Substract a polar vector from this coordinate
+  /// @param v The vector to subtract
-  /// </summary>
+  /// @return The result of the subtraction
-  /// <param name="v">The vector to subtract from this vector</param>
-  /// <returns>The result of the subtraction</returns>
   Polar operator-(Polar &v);
 
   /// <summary>

Spherical.cpp

@@ -5,18 +5,16 @@
 
 #include <math.h>
 
-// using Angle = float;
-
 Spherical::Spherical() {
-  this->horizontalAngle = 0;
+  this->distance = 0.0f;
-  this->verticalAngle = 0;
+  this->horizontalAngle = 0.0f;
-  this->distance = 0;
+  this->verticalAngle = 0.0f;
 }
 
 Spherical::Spherical(Polar polar) {
-  this->horizontalAngle = polar.angle;
-  this->verticalAngle = 0.0F;
   this->distance = polar.distance;
+  this->horizontalAngle = polar.angle;
+  this->verticalAngle = 0.0f;
 }
 
 Spherical::Spherical(float distance, Angle horizontalAngle,
@@ -33,17 +31,24 @@ Spherical::Spherical(float distance, Angle horizontalAngle,
 }
 
 Spherical::Spherical(Vector3 v) {
-  distance = v.magnitude();
+  this->distance = v.magnitude();
   if (distance == 0.0f) {
-    verticalAngle = 0;
+    this->verticalAngle = 0.0f;
-    horizontalAngle = 0;
+    this->horizontalAngle = 0.0f;
   } else {
-    verticalAngle = (90 - acosf(v.y / distance) * Angle::Rad2Deg);
+    this->verticalAngle =
-    horizontalAngle = atan2f(v.x, v.z) * Angle::Rad2Deg;
+        (90.0f - acosf(v.y / this->distance) * Angle::Rad2Deg);
+    this->horizontalAngle = atan2f(v.x, v.z) * Angle::Rad2Deg;
   }
 }
 
-const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);
+const Spherical Spherical::zero = Spherical(0.0f, 0.0f, 0.0f);
+
+Spherical Spherical::operator-() {
+  Spherical v = Spherical(this->distance, this->horizontalAngle + 180.0f,
+                          this->verticalAngle + 180.0f);
+  return v;
+}
 
 // float Spherical::GetSwing() {
 //   // Not sure if this is correct
@@ -51,19 +56,7 @@ const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);
 //                verticalAngle * verticalAngle);
 // }
 
-// Polar Spherical::ProjectOnHorizontalPlane() {
+// float Spherical::Distance(const Spherical &s1, const Spherical &s2) {
-//   return Polar(horizontalAngle, distance);
+//   float d = 0;
-// }
+//   return d;
-
-// Vector3 Spherical::ToVector3() {
-//   float verticalRad = (90 - verticalAngle) * Angle::Deg2Rad;
-//   float horizontalRad = horizontalAngle * Angle::Deg2Rad;
-//   float cosVertical = cosf(verticalRad);
-//   float sinVertical = sinf(verticalRad);
-//   float cosHorizontal = cosf(horizontalRad);
-//   float sinHorizontal = sinf(horizontalRad);
-//   Vector3 v = Vector3(this->distance * sinVertical * sinHorizontal,
-//                       this->distance * cosVertical,
-//                       this->distance * sinVertical * cosHorizontal);
-//   return v;
 // }

Spherical.h

@@ -1,4 +1,3 @@
-/// @copyright
 /// This Source Code Form is subject to the terms of the Mozilla Public
 /// License, v. 2.0.If a copy of the MPL was not distributed with this
 /// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
@@ -21,8 +20,9 @@ struct Vector3;
 /// as a forward direction.
 struct Spherical {
 public:
+  /// @brief The distance in meters
+  /// @remark The distance should never be negative
   float distance;
-
   /// @brief The angle in the horizontal plane in degrees, clockwise rotation
   /// @details The angle is automatically normalized to -180 .. 180
   Angle horizontalAngle;
@@ -33,18 +33,15 @@ public:
   /// @brief Create a new spherical vector with zero degrees and distance
   Spherical();
   /// @brief Create a new spherical vector
-  /// @param polarAngle The angle in the horizontal plane in degrees,
-  /// clockwise rotation
-  /// @param elevationAngle The angle in the vertical plan in degrees,
-  /// zero is forward, positive is upward
   /// @param distance The distance in meters
-  //   Spherical(float polarAngle, float elevationAngle, float distance);
+  /// @param horizontalAngle The angle in the horizontal plane in degrees,
+  /// clockwise rotation
+  /// @param verticalAngle The angle in the vertical plan in degrees,
+  /// zero is forward, positive is upward
   Spherical(float distance, Angle horizontalAngle, Angle verticalAngle);
-
   /// @brief Convert polar coordinates to spherical coordinates
   /// @param polar The polar coordinate
   Spherical(Polar polar);
-
   /// @brief Convert 3D carthesian coordinates to spherical coordinates
   /// @param v Vector in 3D carthesian coordinates;
   Spherical(Vector3 v);
@@ -52,11 +49,19 @@ public:
   /// @brief A spherical vector with zero degree angles and distance
   const static Spherical zero;
 
-  // float GetSwing();
+  /// @brief Negate the vector
+  /// @return The negated vector
+  /// This will rotate the vector by 180 degrees horizontally and
+  /// vertically. Distance will stay the same.
+  Spherical operator-();
 
-  // Polar ProjectOnHorizontalPlane();
+  /// <summary>
-
+  /// The distance between two vectors
-  // Vector3 ToVector3();
+  /// </summary>
+  /// <param name="v1">The first vector</param>
+  /// <param name="v2">The second vector</param>
+  /// <returns>The distance between the two vectors</returns>
+  // static float Distance(const Spherical &s1, const Spherical &s2);
 };
 
 } // namespace Passer

test/Polar_test.cc

@@ -68,4 +68,28 @@ TEST(Polar, FromSpherical) {
   EXPECT_FLOAT_EQ(p.angle, 0.0F) << "p.angle FromSpherical(0 0 90)";
 }
 
+TEST(Polar, Negate) {
+  Polar v = Polar(2, 45);
+  Polar r = Polar::zero;
+
+  r = -v;
+  EXPECT_FLOAT_EQ(r.distance, 2);
+  EXPECT_FLOAT_EQ(r.angle, -135);
+  EXPECT_TRUE(r == Polar(2, -135)) << "Negate(2 45)";
+
+  v = Polar(2, -45);
+  r = -v;
+  EXPECT_TRUE(r == Polar(2, 135)) << "Negate(2 -45)";
+
+  v = Polar(2, 0);
+  r = -v;
+  EXPECT_TRUE(r == Polar(2, 180)) << "Negate(2 0)";
+
+  v = Polar(0, 0);
+  r = -v;
+  EXPECT_FLOAT_EQ(r.distance, 0.0f);
+  EXPECT_FLOAT_EQ(r.angle, 0.0f);
+  EXPECT_TRUE(r == Polar(0, 0)) << "Negate(0 0)";
+}
+
 #endif