Extend ulitity functions

Polar.cpp

@@ -8,7 +8,6 @@ Polar::Polar() {
   this->distance = 0.0f;
   this->angle = 0.0f;
 }
-
 Polar::Polar(float distance, Angle angle) {
   // distance should always be 0 or greater
   if (distance < 0.0f) {
@@ -23,20 +22,22 @@ Polar::Polar(float distance, Angle angle) {
       this->angle = Angle::Normalize(angle);
   }
 }
-
 Polar::Polar(Vector2 v) {
   this->distance = v.magnitude();
   this->angle = Vector2::SignedAngle(Vector2::forward, v);
 }
-
 Polar::Polar(Spherical v) {
   this->distance = v.distance * cosf(v.verticalAngle * Angle::Deg2Rad);
   this->angle = v.horizontalAngle;
 }
 
 const Polar Polar::zero = Polar(0.0f, 0.0f);
+const Polar Polar::forward = Polar(1.0f, 0.0f);
+const Polar Polar::back = Polar(1.0, 180.0f);
+const Polar Polar::right = Polar(1.0, 90.0f);
+const Polar Polar::left = Polar(1.0, -90.0f);
 
-bool Polar::operator==(const Polar &v) {
+bool Polar::operator==(const Polar &v) const {
   return (this->distance == v.distance && this->angle == v.angle);
 }
 
@@ -49,26 +50,38 @@ Polar Polar::normalized() const {
   return r;
 }
 
-Polar Polar::operator+(Polar &v2) {
+Polar Polar::operator-() const {
-  if (v2.distance == 0)
+  Polar v = Polar(this->distance, this->angle + 180);
+  return v;
+}
+
+Polar Polar::operator-(const Polar &v) const {
+  Polar r = -v;
+  return *this + r;
+}
+Polar Polar::operator-=(const Polar &v) {
+  *this = *this - v;
+  return *this;
+}
+Polar Polar::operator+(const Polar &v) const {
+  if (v.distance == 0)
     return Polar(this->distance, this->angle);
   if (this->distance == 0.0f)
-    return v2;
+    return v;
 
-  float deltaAngle = Angle::Normalize(v2.angle - (float)this->angle);
+  float deltaAngle = Angle::Normalize(v.angle - (float)this->angle);
   float rotation =
       deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;
 
-  if (rotation == 180.0f && v2.distance > 0.0f) {
+  if (rotation == 180.0f && v.distance > 0.0f) {
     //  angle is too small, take this angle and add the distances
-    return Polar(this->distance + v2.distance, this->angle);
+    return Polar(this->distance + v.distance, this->angle);
   }
 
   float newDistance =
-      Angle::CosineRuleSide(v2.distance, this->distance, rotation);
+      Angle::CosineRuleSide(v.distance, this->distance, rotation);
 
-  float angle =
+  float angle = Angle::CosineRuleAngle(newDistance, this->distance, v.distance);
-      Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
 
   float newAngle = deltaAngle < 0.0f ? (float)this->angle - angle
                                      : (float)this->angle + angle;
@@ -76,27 +89,30 @@ Polar Polar::operator+(Polar &v2) {
   Polar vector = Polar(newDistance, newAngle);
   return vector;
 }
-
+Polar Polar::operator+=(const Polar &v) {
-Polar Polar::operator-() {
+  *this = *this + v;
-  Polar v = Polar(this->distance, this->angle + 180);
+  return *this;
-  return v;
 }
 
-Polar Polar::operator-(Polar &v2) {
+Polar Passer::operator*(const Polar &v, float f) {
-  // Polar vector = *this + Polar(v2.distance, (float)v2.angle - 180);
+  return Polar(v.distance * f, v.angle);
-  //(Polar(v2.angle - 180, v2.distance));
-  v2 = -v2;
-  return *this + v2;
 }
-
+Polar Passer::operator*(float f, const Polar &v) {
-Polar Polar::operator*(float f) const {
+  return Polar(v.distance * f, v.angle);
-  return Polar(this->distance * f,
-               this->angle); // Polar(this->angle, this->distance * f);
 }
-
+Polar Polar::operator*=(float f) {
-Polar Polar::operator/(const float &f) {
+  this->distance *= f;
-  return Polar(this->distance / f,
+  return *this;
-               this->angle); // Polar(this->angle, this->distance / f);
+}
+Polar Passer::operator/(const Polar &v, float f) {
+  return Polar(v.distance / f, v.angle);
+}
+Polar Passer::operator/(float f, const Polar &v) {
+  return Polar(v.distance / f, v.angle);
+}
+Polar Polar::operator/=(float f) {
+  this->distance /= f;
+  return *this;
 }
 
 float Polar::Distance(const Polar &v1, const Polar &v2) {

Polar.h

@@ -13,8 +13,8 @@ struct Vector2;
 struct Spherical;
 
 /// @brief A polar vector
-/// This will use the polar coordinate system consisting of a angle from a
+/// @details This will use the polar coordinate system consisting of a angle
-/// reference direction and a distance.
+/// from a reference direction and a distance.
 struct Polar {
 public:
   /// @brief The distance in meters
@@ -36,7 +36,6 @@ public:
   /// coordinates
   /// @param v The vector to convert
   Polar(Vector2 v);
-
   /// @brief Convert a vector from spherical coordinates to polar coordinates
   /// @param s The vector to convert
   /// @note The resulting vector will be projected on the horizontal plane
@@ -44,13 +43,21 @@ public:
 
   /// @brief A polar vector with zero degrees and distance
   const static Polar zero;
+  /// @brief A normalized forward-oriented vector
+  const static Polar forward;
+  /// @brief A normalized back-oriented vector
+  const static Polar back;
+  /// @brief A normalized right-oriented vector
+  const static Polar right;
+  /// @brief A normalized left-oriented vector
+  const static Polar left;
 
   /// @brief Equality test to another vector
   /// @param v The vector to check against
   /// @return true: if it is identical to the given vector
   /// @note This uses float comparison to check equality which may have strange
   /// effects. Equality on floats should be avoided.
-  bool operator==(const Polar &v);
+  bool operator==(const Polar &v) const;
 
   /// @brief The vector length
   /// @param v The vector for which you need the length
@@ -71,27 +78,35 @@ public:
   /// @brief Negate the vector
   /// @return The negated vector
   /// This will rotate the vector by 180 degrees. Distance will stay the same.
-  Polar operator-();
+  Polar operator-() const;
+
   /// @brief Subtract a polar vector from this vector
   /// @param v The vector to subtract
   /// @return The result of the subtraction
-  Polar operator-(Polar &v);
+  Polar operator-(const Polar &v) const;
+  Polar operator-=(const Polar &v);
   /// @brief Add a polar vector to this vector
   /// @param v The vector to add
   /// @return The result of the addition
-  Polar operator+(Polar &v);
+  Polar operator+(const Polar &v) const;
+  Polar operator+=(const Polar &v);
+
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  Polar operator*(float f) const;
+  friend Polar operator*(const Polar &v, float f);
+  friend Polar operator*(float f, const Polar &v);
+  Polar operator*=(float f);
   /// @brief Scale the vector uniformly down
   /// @param f The scaling factor
   /// @return The scaled factor
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  Polar operator/(const float &f);
+  friend Polar operator/(const Polar &v, float f);
+  friend Polar operator/(float f, const Polar &v);
+  Polar operator/=(float f);
 
   /// @brief The distance between two vectors
   /// @param v1 The first vector

Spherical.cpp

@@ -43,13 +43,55 @@ Spherical::Spherical(Vector3 v) {
 }
 
 const Spherical Spherical::zero = Spherical(0.0f, 0.0f, 0.0f);
+const Spherical Spherical::forward = Spherical(1.0f, 0.0f, 0.0f);
+const Spherical Spherical::back = Spherical(1.0f, 180.0f, 0.0f);
+const Spherical Spherical::right = Spherical(1.0f, 90.0f, 0.0f);
+const Spherical Spherical::left = Spherical(1.0f, -90.0f, 0.0f);
+const Spherical Spherical::up = Spherical(1.0f, 0.0f, 90.0f);
+const Spherical Spherical::down = Spherical(1.0f, 0.0f, -90.0f);
 
-Spherical Spherical::operator-() {
+bool Spherical::operator==(const Spherical &v) const {
+  return (this->distance == v.distance &&
+          this->horizontalAngle == v.horizontalAngle &&
+          this->verticalAngle == v.verticalAngle);
+}
+
+Spherical Spherical::Normalize(const Spherical &v) {
+  Spherical r = Spherical(1, v.horizontalAngle, v.verticalAngle);
+  return r;
+}
+Spherical Spherical::normalized() const {
+  Spherical r = Spherical(1, this->horizontalAngle, this->verticalAngle);
+  return r;
+}
+
+Spherical Spherical::operator-() const {
   Spherical v = Spherical(this->distance, this->horizontalAngle + 180.0f,
                           this->verticalAngle + 180.0f);
   return v;
 }
 
+Spherical Passer::operator*(const Spherical &v, float f) {
+  return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
+}
+Spherical Passer::operator*(float f, const Spherical &v) {
+  return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
+}
+Spherical Spherical::operator*=(float f) {
+  this->distance *= f;
+  return *this;
+}
+Spherical Passer::operator/(const Spherical &v, float f) {
+  return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
+}
+Spherical Passer::operator/(float f, const Spherical &v) {
+  return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
+}
+Spherical Spherical::operator/=(float f) {
+  this->distance /= f;
+  return *this;
+}
+
 // float Spherical::GetSwing() {
 //   // Not sure if this is correct
 //   return sqrtf(horizontalAngle * horizontalAngle +
@@ -59,4 +101,19 @@ Spherical Spherical::operator-() {
 // float Spherical::Distance(const Spherical &s1, const Spherical &s2) {
 //   float d = 0;
 //   return d;
-// }
+// }
+
+Spherical Spherical::Rotate(const Spherical &v, Angle horizontalAngle,
+                            Angle verticalAngle) {
+  Spherical r = Spherical(v.distance, v.horizontalAngle + horizontalAngle,
+                          v.verticalAngle + verticalAngle);
+  return r;
+}
+Spherical Spherical::RotateHorizontal(const Spherical &v, Angle a) {
+  Spherical r = Spherical(v.distance, v.horizontalAngle + a, v.verticalAngle);
+  return r;
+}
+Spherical Spherical::RotateVertical(const Spherical &v, Angle a) {
+  Spherical r = Spherical(v.distance, v.horizontalAngle, v.verticalAngle + a);
+  return r;
+}

Spherical.h

@@ -1,6 +1,6 @@
-/// This Source Code Form is subject to the terms of the Mozilla Public
+// 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
+// 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/.
+// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
 
 #ifndef SPHERICAL_H
 #define SPHERICAL_H
@@ -13,8 +13,7 @@ namespace Passer {
 struct Vector3;
 
 /// @brief A spherical vector
-/// @details
+/// @details This is a vector in 3D space using a spherical coordinate system.
-/// This is a vector in 3D space using a spherical coordinate system.
 /// It consists of a distance and the polar and elevation angles from a
 /// reference direction. The reference direction is typically thought of
 /// as a forward direction.
@@ -48,12 +47,64 @@ public:
 
   /// @brief A spherical vector with zero degree angles and distance
   const static Spherical zero;
+  /// @brief A normalized forward-oriented vector
+  const static Spherical forward;
+  /// @brief A normalized back-oriented vector
+  const static Spherical back;
+  /// @brief A normalized right-oriented vector
+  const static Spherical right;
+  /// @brief A normalized left-oriented vector
+  const static Spherical left;
+  /// @brief A normalized up-oriented vector
+  const static Spherical up;
+  /// @brief A normalized down-oriented vector
+  const static Spherical down;
+
+  /// @brief Equality test to another vector
+  /// @param v The vector to check against
+  /// @return true: if it is identical to the given vector
+  /// @note This uses float comparison to check equality which may have strange
+  /// effects. Equality on floats should be avoided.
+  bool operator==(const Spherical &v) const;
+
+  /// @brief The vector length
+  /// @param v The vector for which you need the length
+  /// @return The vector length;
+  inline static float Magnitude(const Spherical &v) { return v.distance; }
+  /// @brief The vector length
+  /// @return The vector length
+  inline float magnitude() const { return this->distance; }
+
+  /// @brief Convert the vector to a length of 1
+  /// @param v The vector to convert
+  /// @return The vector normalized to a length of 1
+  static Spherical Normalize(const Spherical &v);
+  /// @brief Convert the vector to a length of a
+  /// @return The vector normalized to a length of 1
+  Spherical normalized() const;
 
   /// @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-();
+  Spherical operator-() const;
+
+  /// @brief Scale the vector uniformly up
+  /// @param f The scaling factor
+  /// @return The scaled vector
+  /// @remark This operation will scale the distance of the vector. The angle
+  /// will be unaffected.
+  friend Spherical operator*(const Spherical &v, float f);
+  friend Spherical operator*(float f, const Spherical &v);
+  Spherical operator*=(float f);
+  /// @brief Scale the vector uniformly down
+  /// @param f The scaling factor
+  /// @return The scaled factor
+  /// @remark This operation will scale the distance of the vector. The angle
+  /// will be unaffected.
+  friend Spherical operator/(const Spherical &v, float f);
+  friend Spherical operator/(float f, const Spherical &v);
+  Spherical operator/=(float f);
 
   /// <summary>
   /// The distance between two vectors
@@ -62,6 +113,11 @@ public:
   /// <param name="v2">The second vector</param>
   /// <returns>The distance between the two vectors</returns>
   // static float Distance(const Spherical &s1, const Spherical &s2);
+
+  static Spherical Rotate(const Spherical &v, Angle horizontalAngle,
+                          Angle verticalAngle);
+  static Spherical RotateHorizontal(const Spherical &v, Angle angle);
+  static Spherical RotateVertical(const Spherical &v, Angle angle);
 };
 
 } // namespace Passer

Vector2.cpp

@@ -99,24 +99,24 @@ Vector2 Vector2::operator+=(const Vector2 &v) {
 Vector2 Vector2::Scale(const Vector2 &v1, const Vector2 &v2) {
   return Vector2(v1.x * v2.x, v1.y * v2.y);
 }
-Vector2 Passer::operator*(const Vector2 &v, const float f) {
+Vector2 Passer::operator*(const Vector2 &v, float f) {
   return Vector2(v.x * f, v.y * f);
 }
-Vector2 Passer::operator*(const float f, const Vector2 &v) {
+Vector2 Passer::operator*(float f, const Vector2 &v) {
   return Vector2(v.x * f, v.y * f);
 }
-Vector2 Vector2::operator*=(const float f) {
+Vector2 Vector2::operator*=(float f) {
   this->x *= f;
   this->y *= f;
   return *this;
 }
-Vector2 Passer::operator/(const Vector2 &v, const float f) {
+Vector2 Passer::operator/(const Vector2 &v, float f) {
   return Vector2(v.x / f, v.y / f);
 }
-Vector2 Passer::operator/(const float f, const Vector2 &v) {
+Vector2 Passer::operator/(float f, const Vector2 &v) {
   return Vector2(v.x / f, v.y / f);
 }
-Vector2 Vector2::operator/=(const float f) {
+Vector2 Vector2::operator/=(float f) {
   this->x /= f;
   this->y /= f;
   return *this;

Vector2.h

@@ -59,16 +59,14 @@ public:
   const static Vector2 zero;
   /// @brief A vector with one for all axis
   const static Vector2 one;
+  /// @brief A normalized forward-oriented vector
+  const static Vector2 forward;
+  /// @brief A normalized back-oriented vector
+  const static Vector2 back;
   /// @brief A normalized right-oriented vector
   const static Vector2 right;
   /// @brief A normalized left-oriented vector
   const static Vector2 left;
-  /// @brief A normalized forward-oriented vector
-  /// @note This is equal to Vector2::up
-  const static Vector2 forward;
-  /// @brief A normalized back-oriented vector
-  /// @note This is equal to Vector2::down
-  const static Vector2 back;
   /// @brief A normalized up-oriented vector
   /// @note This is a convenience function which is equal to Vector2::forward
   const static Vector2 up;
@@ -139,16 +137,16 @@ public:
   /// @return The scaled vector
   /// @remark Each component of the vector will be multipled with the same
   /// factor f.
-  friend Vector2 operator*(const Vector2 &v, const float f);
+  friend Vector2 operator*(const Vector2 &v, float f);
-  friend Vector2 operator*(const float f, const Vector2 &v);
+  friend Vector2 operator*(float f, const Vector2 &v);
-  Vector2 operator*=(const float f);
+  Vector2 operator*=(float f);
   /// @brief Scale the vector uniformly down
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each componet of the vector will be divided by the same factor.
-  friend Vector2 operator/(const Vector2 &v, const float f);
+  friend Vector2 operator/(const Vector2 &v, float f);
-  friend Vector2 operator/(const float f, const Vector2 &v);
+  friend Vector2 operator/(float f, const Vector2 &v);
-  Vector2 operator/=(const float f);
+  Vector2 operator/=(float f);
 
   /// @brief The dot product of two vectors
   /// @param v1 The first vector

Vector3.cpp

@@ -118,25 +118,25 @@ Vector3 Vector3::operator+=(const Vector3 &v) {
 Vector3 Vector3::Scale(const Vector3 &v1, const Vector3 &v2) {
   return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
 }
-Vector3 Passer::operator*(const Vector3 &v, const float f) {
+Vector3 Passer::operator*(const Vector3 &v, float f) {
   return Vector3(v.x * f, v.y * f, v.z * f);
 }
-Vector3 Passer::operator*(const float f, const Vector3 &v) {
+Vector3 Passer::operator*(float f, const Vector3 &v) {
   return Vector3(v.x * f, v.y * f, v.z * f);
 }
-Vector3 Vector3::operator*=(const float f) {
+Vector3 Vector3::operator*=(float f) {
   this->x *= f;
   this->y *= f;
   this->z *= f;
   return *this;
 }
-Vector3 Passer::operator/(const Vector3 &v, const float f) {
+Vector3 Passer::operator/(const Vector3 &v, float f) {
   return Vector3(v.x / f, v.y / f, v.z / f);
 }
-Vector3 Passer::operator/(const float f, const Vector3 &v) {
+Vector3 Passer::operator/(float f, const Vector3 &v) {
   return Vector3(v.x / f, v.y / f, v.z / f);
 }
-Vector3 Vector3::operator/=(const float f) {
+Vector3 Vector3::operator/=(float f) {
   this->x /= f;
   this->y /= f;
   this->z /= f;

Vector3.h

@@ -64,6 +64,10 @@ public:
   const static Vector3 zero;
   /// @brief A vector with one for all axis
   const static Vector3 one;
+  /// @brief A normalized forward-oriented vector
+  const static Vector3 forward;
+  /// @brief A normalized back-oriented vector
+  const static Vector3 back;
   /// @brief A normalized right-oriented vector
   const static Vector3 right;
   /// @brief A normalized left-oriented vector
@@ -72,10 +76,6 @@ public:
   const static Vector3 up;
   /// @brief A normalized down-oriented vector
   const static Vector3 down;
-  /// @brief A normalized forward-oriented vector
-  const static Vector3 forward;
-  /// @brief A normalized back-oriented vector
-  const static Vector3 back;
 
   // Access functions which are intended to replace the use of XYZ
   inline float Forward() const { return z; };
@@ -145,16 +145,16 @@ public:
   /// @return The scaled vector
   /// @remark Each component of the vector will be multipled with the same
   /// factor f.
-  friend Vector3 operator*(const Vector3 &v, const float f);
+  friend Vector3 operator*(const Vector3 &v, float f);
-  friend Vector3 operator*(const float f, const Vector3 &v);
+  friend Vector3 operator*(float f, const Vector3 &v);
-  Vector3 operator*=(const float f);
+  Vector3 operator*=(float f);
   /// @brief Scale the vector uniformly down
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each componet of the vector will be divided by the same factor.
-  friend Vector3 operator/(const Vector3 &v, const float f);
+  friend Vector3 operator/(const Vector3 &v, float f);
-  friend Vector3 operator/(const float f, const Vector3 &v);
+  friend Vector3 operator/(float f, const Vector3 &v);
-  Vector3 operator/=(const float f);
+  Vector3 operator/=(float f);
 
   /// @brief The distance between two vectors
   /// @param v1 The first vector

test/Polar_test.cc

@@ -116,6 +116,10 @@ TEST(Polar, Addition) {
   v2 = Polar::zero;
   r = v1 + v2;
   EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
+
+  r = v1;
+  r += v2;
+  EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
 }
 
 TEST(Polar, Scale_Multiply) {