From 28ee2254b89167641322ab0efb753fe2670efa8a Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Wed, 18 Dec 2024 16:54:33 +0100
Subject: [PATCH] Normalize Spherical at creation

---
 Spherical.cpp | 95 ++++++++++++++++++---------------------------------
 1 file changed, 34 insertions(+), 61 deletions(-)

diff --git a/Spherical.cpp b/Spherical.cpp
index a88f704..55836a9 100644
--- a/Spherical.cpp
+++ b/Spherical.cpp
@@ -5,55 +5,34 @@
 
 #include <math.h>
 
-template <typename T>
-SphericalOf<T>::SphericalOf() {
+template <typename T> SphericalOf<T>::SphericalOf() {
   this->distance = 0.0f;
   this->direction = DirectionOf<T>();
-  // this->horizontal = AngleOf<T>();
-  // this->vertical = AngleOf<T>();
 }
 
-// template <>
-// SphericalOf<signed short>::SphericalOf() {
-//   this->distance = 0.0f;
-//   this->horizontal = AngleOf<signed short>(0);
-//   this->vertical = AngleOf<signed short>(0);
-// }
-
 template <typename T>
-SphericalOf<T>::SphericalOf(float distance,
-                            AngleOf<T> horizontal,
+SphericalOf<T>::SphericalOf(float distance, AngleOf<T> horizontal,
                             AngleOf<T> vertical) {
-  this->distance = distance;
-  this->direction = DirectionOf<T>(horizontal, vertical);
-  // this->horizontal = horizontal;
-  // this->vertical = vertical;
+  if (distance < 0) {
+    this->distance = distance;
+    this->direction = DirectionOf<T>(horizontal, vertical);
+  } else {
+    this->distance = -distance;
+    this->direction = -DirectionOf<T>(horizontal, vertical);
+  }
 }
 
 template <typename T>
 SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
-  this->distance = distance;
-  this->direction = direction;
+  if (distance < 0) {
+    this->distance = -distance;
+    this->direction = -direction;
+  } else {
+    this->distance = distance;
+    this->direction = direction;
+  }
 }
 
-// template <>
-// SphericalOf<float>::SphericalOf(float distance,
-//                                 AngleOf<float> horizontal,
-//                                 AngleOf<float> vertical) {
-//   this->distance = distance;
-//   this->horizontal = horizontal;
-//   this->vertical = vertical;
-// }
-
-// template <>
-// SphericalOf<signed short>::SphericalOf(float distance,
-//                                        AngleOf<signed short> horizontal,
-//                                        AngleOf<signed short> vertical) {
-//   this->distance = distance;
-//   this->horizontal = horizontal;
-//   this->vertical = vertical;
-// }
-
 template <typename T>
 SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
   AngleOf<T> horizontal = polar.angle;
@@ -62,8 +41,7 @@ SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
   return r;
 }
 
-template <typename T>
-SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
+template <typename T> SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
   float distance = v.magnitude();
   if (distance == 0.0f) {
     return SphericalOf(distance, AngleOf<T>(), AngleOf<T>());
@@ -76,8 +54,7 @@ SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
   }
 }
 
-template <typename T>
-Vector3 SphericalOf<T>::ToVector3() const {
+template <typename T> Vector3 SphericalOf<T>::ToVector3() const {
   float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
   float horizontalRad = this->direction.horizontal.InRadians();
 
@@ -122,8 +99,7 @@ SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
   return SphericalOf<T>();
 }
 
-template <typename T>
-SphericalOf<T> SphericalOf<T>::operator-() const {
+template <typename T> SphericalOf<T> SphericalOf<T>::operator-() const {
   SphericalOf<T> v = SphericalOf<T>(
       this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
       this->direction.vertical + AngleOf<T>::Degrees(180));
@@ -131,7 +107,7 @@ SphericalOf<T> SphericalOf<T>::operator-() const {
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
+SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T> &s2) const {
   // let's do it the easy way...
   Vector3 v1 = this->ToVector3();
   Vector3 v2 = s2.ToVector3();
@@ -140,13 +116,13 @@ SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T>& v) {
+SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T> &v) {
   *this = *this - v;
   return *this;
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
+SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T> &s2) const {
   // let's do it the easy way...
   Vector3 v1 = this->ToVector3();
   Vector3 v2 = s2.ToVector3();
@@ -201,19 +177,17 @@ SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
   */
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
+SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T> &v) {
   *this = *this + v;
   return *this;
 }
 
-template <typename T>
-SphericalOf<T> SphericalOf<T>::operator*=(float f) {
+template <typename T> SphericalOf<T> SphericalOf<T>::operator*=(float f) {
   this->distance *= f;
   return *this;
 }
 
-template <typename T>
-SphericalOf<T> SphericalOf<T>::operator/=(float f) {
+template <typename T> SphericalOf<T> SphericalOf<T>::operator/=(float f) {
   this->distance /= f;
   return *this;
 }
@@ -224,8 +198,8 @@ SphericalOf<T> SphericalOf<T>::operator/=(float f) {
 const float epsilon = 1E-05f;
 
 template <typename T>
-float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
-                                      const SphericalOf<T>& v2) {
+float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
+                                      const SphericalOf<T> &v2) {
   // SphericalOf<T> difference = v1 - v2;
   // return difference.distance;
   Vector3 vec1 = v1.ToVector3();
@@ -235,8 +209,8 @@ float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
 }
 
 template <typename T>
-AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
-                                        const SphericalOf& v2) {
+AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
+                                        const SphericalOf &v2) {
   // float denominator = v1.distance * v2.distance;
   // if (denominator < epsilon)
   //   return 0.0f;
@@ -256,9 +230,8 @@ AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
 
 template <typename T>
 AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
-    const SphericalOf<T>& v1,
-    const SphericalOf<T>& v2,
-    const SphericalOf<T>& axis) {
+    const SphericalOf<T> &v1, const SphericalOf<T> &v2,
+    const SphericalOf<T> &axis) {
   Vector3 v1_vector = v1.ToVector3();
   Vector3 v2_vector = v2.ToVector3();
   Vector3 axis_vector = axis.ToVector3();
@@ -267,7 +240,7 @@ AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
+SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T> &v,
                                       AngleOf<T> horizontalAngle,
                                       AngleOf<T> verticalAngle) {
   SphericalOf<T> r =
@@ -276,14 +249,14 @@ SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
+SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T> &v,
                                                 AngleOf<T> a) {
   SphericalOf<T> r =
       SphericalOf(v.distance, v.direction.horizontal + a, v.direction.vertical);
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
+SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T> &v,
                                               AngleOf<T> a) {
   SphericalOf<T> r =
       SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a);