From 0a4f2751fe9eadad895544fa96fb2a1d7657c9ff Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Mon, 8 Jan 2024 15:02:24 +0100
Subject: [PATCH] Generic Angles

---
 Angle.cpp     | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++-
 Angle.h       | 30 ++++++++++++++++++--
 Spherical.cpp | 18 ++++++++++--
 Spherical.h   | 17 +++++++++--
 Vector2.cpp   | 32 ++++++++-------------
 5 files changed, 147 insertions(+), 28 deletions(-)

diff --git a/Angle.cpp b/Angle.cpp
index 1092594..431ef1d 100644
--- a/Angle.cpp
+++ b/Angle.cpp
@@ -3,9 +3,10 @@
 // file, You can obtain one at https ://mozilla.org/MPL/2.0/.
 
 #include "Angle.h"
-#include <math.h>
 #include "FloatSingle.h"
+#include <math.h>
 
+/*
 const float Angle::Rad2Deg = 57.29578F;
 const float Angle::Deg2Rad = 0.0174532924F;
 
@@ -68,4 +69,79 @@ float Angle::CosineRuleAngle(float a, float b, float c) {
 float Angle::SineRuleAngle(float a, float beta, float b) {
   float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b);
   return alpha;
+}
+*/
+//----------------------
+
+template <> Angle2<float> Angle2<float>::Rad2Deg = 57.29578F;
+template <> Angle2<float> Angle2<float>::Deg2Rad = 0.0174532924F;
+
+template <> Angle2<float> Angle2<float>::Normalize(Angle2<float> angle) {
+  float angleValue = angle;
+  if (!isfinite(angleValue))
+    return angleValue;
+
+  while (angleValue <= -180)
+    angleValue += 360;
+  while (angleValue > 180)
+    angleValue -= 360;
+  return angleValue;
+}
+
+template <typename T>
+Angle2<T> Angle2<T>::Clamp(Angle2<T> angle, Angle2<T> min, Angle2<T> max) {
+  float normalizedAngle = Normalize(angle);
+  float r = Float::Clamp(normalizedAngle, min, max);
+  return r;
+}
+
+template <typename T>
+Angle2<T> Angle2<T>::Difference(Angle2<T> a, Angle2<T> b) {
+  float r = Normalize(b - a);
+  return r;
+}
+
+template <typename T>
+Angle2<T> Angle2<T>::MoveTowards(Angle2<T> fromAngle, Angle2<T> toAngle,
+                                 Angle2<T> maxAngle) {
+  float d = toAngle - fromAngle;
+  float sign = signbit(d) ? -1 : 1;
+  d = sign * Float::Clamp(fabs(d), 0, maxAngle);
+  return fromAngle + d;
+}
+
+template <>
+Angle2<float> Angle2<float>::CosineRuleSide(float a, float b,
+                                            Angle2<float> gamma) {
+  float a2 = a * a;
+  float b2 = b * b;
+  float d = a2 + b2 - 2 * a * b * cos(gamma * Angle2<float>::Deg2Rad);
+  // Catch edge cases where float inacuracies lead tot nans
+  if (d < 0)
+    return 0;
+
+  float c = sqrtf(d);
+  return c;
+}
+
+template <>
+Angle2<float> Angle2<float>::CosineRuleAngle(float a, float b, float c) {
+  float a2 = a * a;
+  float b2 = b * b;
+  float c2 = c * c;
+  float d = (a2 + b2 - c2) / (2 * a * b);
+  // Catch edge cases where float inacuracies lead tot nans
+  if (d >= 1)
+    return 0;
+  if (d <= -1)
+    return 180;
+
+  float gamma = acos(d) * Angle::Rad2Deg;
+  return gamma;
+}
+
+template <typename T>
+Angle2<T> Angle2<T>::SineRuleAngle(float a, Angle2<T> beta, float b) {
+  float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b);
+  return alpha;
 }
\ No newline at end of file
diff --git a/Angle.h b/Angle.h
index b46cfac..6027eef 100644
--- a/Angle.h
+++ b/Angle.h
@@ -5,8 +5,34 @@
 #ifndef ANGLE_H
 #define ANGLE_H
 
+template <typename T> class Angle2 {
+public:
+  Angle2(){};
+  Angle2(T v) : value(v) {}
+  operator T() { return value; }
+
+  static Angle2<T> Rad2Deg;
+  static Angle2<T> Deg2Rad;
+
+  static Angle2<T> Normalize(Angle2<T> angle);
+  static Angle2<T> Clamp(Angle2<T> angle, Angle2<T> min, Angle2<T> max);
+  static Angle2<T> Difference(Angle2<T> a, Angle2<T> b);
+  static Angle2<T> MoveTowards(Angle2<T> fromAngle, Angle2<T> toAngle,
+                               Angle2<T> maxAngle);
+
+  static Angle2<T> CosineRuleSide(float a, float b, Angle2<T> gamma);
+  static Angle2<T> CosineRuleAngle(float a, float b, float c);
+
+  static Angle2<T> SineRuleAngle(float a, Angle2<T> beta, float c);
+
+private:
+  T value;
+};
+
+using Angle = Angle2<float>;
+/*
 class Angle {
- public:
+public:
   const static float Rad2Deg;
   const static float Deg2Rad;
 
@@ -20,5 +46,5 @@ class Angle {
 
   static float SineRuleAngle(float a, float beta, float c);
 };
-
+*/
 #endif
\ No newline at end of file
diff --git a/Spherical.cpp b/Spherical.cpp
index 5888443..8ff3ff2 100644
--- a/Spherical.cpp
+++ b/Spherical.cpp
@@ -1,11 +1,15 @@
 #include "Spherical.h"
 
+#include "Angle.h"
+
 #include <math.h>
 
+// using Angle = float;
+
 Spherical::Spherical() {
   this->horizontalAngle = 0;
   this->verticalAngle = 0;
-  this->distance = 0;
+  this->magnitude = 0;
 }
 
 // Spherical::Spherical(float polarAngle, float elevationAngle, float distance)
@@ -16,10 +20,18 @@ Spherical::Spherical() {
 // }
 
 Spherical::Spherical(float distance, Angle horizontalAngle, Angle verticalAngle)
-    : distance(distance), horizontalAngle(horizontalAngle),
+    : magnitude(distance), horizontalAngle(horizontalAngle),
       verticalAngle(verticalAngle) {}
 
-const Spherical Spherical::zero = Spherical(0, 0, 0);
+Spherical::Spherical(Vector3 v) {
+  float signZ = (v.z >= 0) - (v.z < 0);
+  horizontalAngle =
+      atan2(v.y, signZ * sqrt(v.z * v.z + v.x * v.x)) * Angle::Rad2Deg;
+  verticalAngle = -atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg;
+  magnitude = v.magnitude();
+}
+
+const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);
 
 float Spherical::GetSwing() {
   // Not sure if this is correct
diff --git a/Spherical.h b/Spherical.h
index 65b2d3b..2b70462 100644
--- a/Spherical.h
+++ b/Spherical.h
@@ -6,7 +6,11 @@
 #ifndef SPHERICAL_H
 #define SPHERICAL_H
 
-typedef float Angle;
+#include "Angle.h"
+
+class Vector3;
+
+namespace Passer {
 
 /// @brief A spherical vector
 /// @details
@@ -16,7 +20,7 @@ typedef float Angle;
 /// as a forward direction.
 struct Spherical {
 public:
-  float distance;
+  float magnitude;
 
   /// @brief The angle in the horizontal plane in degrees, clockwise rotation
   /// @details The angle is automatically normalized to -180 .. 180
@@ -37,10 +41,19 @@ public:
 
   Spherical(float distance, Angle horizontalAngle, Angle verticalAngle);
 
+  /// @brief Convert 3D carthesian coordinates to spherical coordinates
+  /// @param v Vector in 3D carthesian coordinates;
+  Spherical(Vector3 v);
+
   /// @brief A spherical vector with zero degree angles and distance
   const static Spherical zero;
 
   float GetSwing();
 };
 
+} // namespace Passer
+using namespace Passer;
+
+#include "Vector3.h"
+
 #endif
\ No newline at end of file
diff --git a/Vector2.cpp b/Vector2.cpp
index abf1b1b..22c039d 100644
--- a/Vector2.cpp
+++ b/Vector2.cpp
@@ -38,19 +38,13 @@ const Vector2 Vector2::down = Vector2(0, -1);
 const Vector2 Vector2::forward = Vector2(0, 1);
 const Vector2 Vector2::back = Vector2(0, -1);
 
-float Vector2::Magnitude(const Vector2& a) {
+float Vector2::Magnitude(const Vector2 &a) {
   return sqrtf(a.x * a.x + a.y * a.y);
 }
-float Vector2::magnitude() const {
-  return (float)sqrtf(x * x + y * y);
-}
+float Vector2::magnitude() const { return (float)sqrtf(x * x + y * y); }
 
-float Vector2::SqrMagnitude(const Vector2& a) {
-  return a.x * a.x + a.y * a.y;
-}
-float Vector2::sqrMagnitude() const {
-  return (x * x + y * y);
-}
+float Vector2::SqrMagnitude(const Vector2 &a) { return a.x * a.x + a.y * a.y; }
+float Vector2::sqrMagnitude() const { return (x * x + y * y); }
 
 Vector2 Vector2::Normalize(Vector2 v) {
   float num = Vector2::Magnitude(v);
@@ -69,19 +63,17 @@ Vector2 Vector2::normalized() const {
   return result;
 }
 
-Vector2 Vector2::operator-(const Vector2& v2) const {
+Vector2 Vector2::operator-(const Vector2 &v2) const {
   return Vector2(this->x - v2.x, this->y - v2.y);
 }
 
-Vector2 Vector2::operator-() {
-  return Vector2(-this->x, -this->y);
-}
+Vector2 Vector2::operator-() { return Vector2(-this->x, -this->y); }
 
-Vector2 Vector2::operator+(const Vector2& v2) const {
+Vector2 Vector2::operator+(const Vector2 &v2) const {
   return Vector2(this->x + v2.x, this->y + v2.y);
 }
 
-Vector2 Vector2::Scale(const Vector2& p1, const Vector2& p2) {
+Vector2 Vector2::Scale(const Vector2 &p1, const Vector2 &p2) {
   return Vector2(p1.x * p2.x, p1.y * p2.y);
 }
 
@@ -89,19 +81,19 @@ Vector2 Vector2::operator*(float f) const {
   return Vector2(this->x * f, this->y * f);
 }
 
-Vector2 Vector2::operator/(const float& d) {
+Vector2 Vector2::operator/(const float &d) {
   return Vector2(this->x / d, this->y / d);
 }
 
-float Vector2::Dot(const Vector2& v1, const Vector2& v2) {
+float Vector2::Dot(const Vector2 &v1, const Vector2 &v2) {
   return v1.x * v2.x + v1.y * v2.y;
 }
 
-bool Vector2::operator==(const Vector2& v) {
+bool Vector2::operator==(const Vector2 &v) {
   return (this->x == v.x && this->y == v.y);
 }
 
-float Vector2::Distance(const Vector2& p1, const Vector2& p2) {
+float Vector2::Distance(const Vector2 &p1, const Vector2 &p2) {
   return Magnitude(p1 - p2);
 }