Added SphericalOf

Angle.cpp

@@ -98,7 +98,7 @@ float AngleOf<float>::ToFloat() const {
 // AngleOf<float> AngleOf<float>::Deg2Rad = (pi * 2) / 360.0f;
 
 template <>
-bool Passer::LinearAlgebra::AngleOf<float>::operator==(AngleOf<float> a) {
+bool AngleOf<float>::operator==(AngleOf<float> a) {
   return this->ToFloat() == a.ToFloat();
 }
 
@@ -122,6 +122,12 @@ AngleOf<float> AngleOf<float>::operator+(const AngleOf<float>& a) const {
   return angle;
 }
 
+template <typename T>
+AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& a) {
+  this->value += a.value;
+  return *this;
+}
+
 template <>
 AngleOf<float> AngleOf<float>::Normalize(AngleOf<float> angle) {
   float angleValue = angle.ToFloat();
@@ -200,4 +206,6 @@ AngleOf<float> AngleOf<float>::SineRuleAngle(float a,
   float deg2rad = Passer::LinearAlgebra::Deg2Rad;
   float alpha = asinf(a * sinf(beta.ToFloat() * deg2rad) / b);
   return alpha;
-}
+}
+template class AngleOf<float>;
+template class AngleOf<signed short>;

Angle.h

@@ -35,6 +35,7 @@ class AngleOf {
   AngleOf<T> operator-() const;
   AngleOf<T> operator-(const AngleOf<T>& a) const;
   AngleOf<T> operator+(const AngleOf<T>& a) const;
+  AngleOf<T> operator+=(const AngleOf<T>& a);
 
   friend AngleOf<T> operator*(const AngleOf<T>& a, float f) {
     return AngleOf((float)a.ToFloat() * f);

Spherical.cpp

@@ -5,6 +5,189 @@
 
 #include <math.h>
 
+template <typename T>
+SphericalOf<T>::SphericalOf() {
+  this->distance = 0.0f;
+  this->horizontal = AngleOf<T>(0);
+  this->vertical = AngleOf<T>(0);
+}
+
+// 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,
+                            AngleOf<T> vertical) {
+  this->distance = distance;
+  this->horizontal = horizontal;
+  this->vertical = vertical;
+}
+
+// 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>::FromVector3(Vector3 v) {
+  float distance = v.magnitude();
+  if (distance == 0.0f) {
+    return SphericalOf(distance, 0, 0);
+  } else {
+    float verticalAngle =
+        (90.0f - acosf(v.Up() / distance) * Passer::LinearAlgebra::Rad2Deg);
+    float horizontalAngle =
+        atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg;
+    return SphericalOf(distance, horizontalAngle, verticalAngle);
+  }
+}
+
+template <typename T>
+Vector3 SphericalOf<T>::ToVector3() const {
+  float verticalRad =
+      (90.0f - this->vertical.ToFloat()) * Passer::LinearAlgebra::Deg2Rad;
+  float horizontalRad =
+      this->horizontal.ToFloat() * Passer::LinearAlgebra::Deg2Rad;
+  float cosVertical = cosf(verticalRad);
+  float sinVertical = sinf(verticalRad);
+  float cosHorizontal = cosf(horizontalRad);
+  float sinHorizontal = sinf(horizontalRad);
+
+  float x = this->distance * sinVertical * sinHorizontal;
+  float y = this->distance * cosVertical;
+  float z = this->distance * sinVertical * cosHorizontal;
+
+  Vector3 v = Vector3(x, y, z);
+  return Vector3();
+}
+
+template <typename T>
+const SphericalOf<T> SphericalOf<T>::zero =
+    SphericalOf<T>(0.0f, AngleOf<T>(0), AngleOf<T>(0));
+template <typename T>
+const SphericalOf<T> SphericalOf<T>::forward = SphericalOf<T>(1.0f, 0.0f, 0.0f);
+template <typename T>
+const SphericalOf<T> SphericalOf<T>::back = SphericalOf<T>(1.0f, 180.0f, 0.0f);
+template <typename T>
+const SphericalOf<T> SphericalOf<T>::right = SphericalOf<T>(1.0f, 90.0f, 0.0f);
+template <typename T>
+const SphericalOf<T> SphericalOf<T>::left = SphericalOf<T>(1.0f, -90.0f, 0.0f);
+template <typename T>
+const SphericalOf<T> SphericalOf<T>::up = SphericalOf<T>(1.0f, 0.0f, 90.0f);
+template <typename T>
+const SphericalOf<T> SphericalOf<T>::down = SphericalOf<T>(1.0f, 0.0f, -90.0f);
+
+template <>
+const SphericalOf<signed short> SphericalOf<signed short>::zero =
+    SphericalOf(0.0f, AngleOf<signed short>(0), AngleOf<signed short>(0));
+
+template <typename T>
+SphericalOf<T> SphericalOf<T>::operator-() const {
+  SphericalOf<T> v =
+      SphericalOf<T>(this->distance, this->horizontal.ToFloat() + 180.0f,
+                     this->vertical.ToFloat() + 180.0f);
+  return v;
+}
+
+template <typename T>
+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();
+  Vector3 v = v1 - v2;
+  SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
+  return r;
+}
+template <typename T>
+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 {
+  // let's do it the easy way...
+  Vector3 v1 = this->ToVector3();
+  Vector3 v2 = s2.ToVector3();
+  Vector3 v = v1 + v2;
+  SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
+  return r;
+  /*
+  // This is the hard way...
+  if (v2.distance <= 0)
+    return Spherical(this->distance, this->horizontalAngle,
+                     this->verticalAngle);
+  if (this->distance <= 0)
+    return v2;
+
+  float deltaHorizontalAngle =
+      (float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle);
+  float horizontalRotation = deltaHorizontalAngle < 0
+                                 ? 180 + deltaHorizontalAngle
+                                 : 180 - deltaHorizontalAngle;
+  float deltaVerticalAngle =
+      Angle::Normalize(v2.verticalAngle - this->verticalAngle);
+  float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle
+                                                  : 180 - deltaVerticalAngle;
+
+  if (horizontalRotation == 180 && verticalRotation == 180)
+    // angle is too small, take this angle and add the distances
+    return Spherical(this->distance + v2.distance, this->horizontalAngle,
+                     this->verticalAngle);
+
+  Angle rotation = AngleBetween(*this, v2);
+  float newDistance =
+      Angle::CosineRuleSide(v2.distance, this->distance, rotation);
+  float angle =
+      Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
+
+  // Now we have to project the angle to the horizontal and vertical planes...
+  // The axis for the angle is the cross product of the two spherical vectors
+  // (which function we do not have either...)
+  float horizontalAngle = 0;
+  float verticalAngle = 0;
+
+  float newHorizontalAngle =
+      deltaHorizontalAngle < 0
+          ? Angle::Normalize(this->horizontalAngle - horizontalAngle)
+          : Angle::Normalize(this->horizontalAngle + horizontalAngle);
+  float newVerticalAngle =
+      deltaVerticalAngle < 0
+          ? Angle::Normalize(this->verticalAngle - verticalAngle)
+          : Angle::Normalize(this->verticalAngle + verticalAngle);
+
+  Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
+  */
+}
+template <typename T>
+SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
+  *this = *this + v;
+  return *this;
+}
+
+template class SphericalOf<float>;
+template class SphericalOf<signed short>;
+
+//---------------------------------------
+
 Spherical::Spherical() {
   this->distance = 0.0f;
   this->horizontalAngle = 0.0f;

Spherical.h

@@ -13,6 +13,61 @@ namespace LinearAlgebra {
 
 struct Vector3;
 
+template <typename T>
+class SphericalOf {
+ 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
+  AngleOf<T> horizontal;
+  /// @brief The angle in the vertical plane in degrees. Positive is upward.
+  /// @details The angle is automatically normalized to -180 .. 180
+  AngleOf<T> vertical;
+
+  SphericalOf<T>();
+  SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
+
+  static SphericalOf<T> FromVector3(Vector3 v);
+  Vector3 ToVector3() const;
+
+  /// @brief A spherical vector with zero degree angles and distance
+  const static SphericalOf<T> zero;
+  /// @brief A normalized forward-oriented vector
+  const static SphericalOf<T> forward;
+  /// @brief A normalized back-oriented vector
+  const static SphericalOf<T> back;
+  /// @brief A normalized right-oriented vector
+  const static SphericalOf<T> right;
+  /// @brief A normalized left-oriented vector
+  const static SphericalOf<T> left;
+  /// @brief A normalized up-oriented vector
+  const static SphericalOf<T> up;
+  /// @brief A normalized down-oriented vector
+  const static SphericalOf<T> down;
+
+  /// @brief Negate the vector
+  /// @return The negated vector
+  /// This will rotate the vector by 180 degrees horizontally and
+  /// vertically. Distance will stay the same.
+  SphericalOf<T> operator-() const;
+
+  /// @brief Subtract a spherical vector from this vector
+  /// @param v The vector to subtract
+  /// @return The result of the subtraction
+  SphericalOf<T> operator-(const SphericalOf<T>& v) const;
+  SphericalOf<T> operator-=(const SphericalOf<T>& v);
+  /// @brief Add a spherical vector to this vector
+  /// @param v The vector to add
+  /// @return The result of the addition
+  SphericalOf<T> operator+(const SphericalOf<T>& v) const;
+  SphericalOf<T> operator+=(const SphericalOf<T>& v);
+};
+
+using SphericalSingle = SphericalOf<float>;
+using Spherical16 = SphericalOf<signed short>;
+
 /// @brief A spherical vector
 /// @details 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

Spherical16.cpp

@@ -1,3 +1,4 @@
+/*
 #include "Spherical16.h"
 
 #include "Quaternion.h"
@@ -44,6 +45,24 @@ Spherical16::Spherical16(Vector3 v) {
   }
 }
 
+Vector3 Passer::LinearAlgebra::Spherical16::ToVector3() {
+  float verticalRad =
+      (90.0f - this->verticalAngle.ToFloat()) * Passer::LinearAlgebra::Deg2Rad;
+  float horizontalRad =
+      this->horizontalAngle.ToFloat() * Passer::LinearAlgebra::Deg2Rad;
+  float cosVertical = cosf(verticalRad);
+  float sinVertical = sinf(verticalRad);
+  float cosHorizontal = cosf(horizontalRad);
+  float sinHorizontal = sinf(horizontalRad);
+
+  float x = this->distance * sinVertical * sinHorizontal;
+  float y = this->distance * cosVertical;
+  float z = this->distance * sinVertical * cosHorizontal;
+
+  Vector3 v = Vector3(x, y, z);
+  return Vector3();
+}
+
 const Spherical16 Spherical16::zero = Spherical16(0.0f, 0.0f, 0.0f);
 const Spherical16 Spherical16::forward = Spherical16(1.0f, 0.0f, 0.0f);
 const Spherical16 Spherical16::back = Spherical16(1.0f, 180.0f, 0.0f);
@@ -103,7 +122,7 @@ Spherical16 Spherical16::operator+(const Spherical16& s2) const {
   Vector3 v = v1 + v2;
   Spherical16 r = Spherical16(v);
   return r;
-  /*
+
   // This is the hard way...
   if (v2.distance <= 0)
     return Spherical(this->distance, this->horizontalAngle,
@@ -148,7 +167,7 @@ Spherical16 Spherical16::operator+(const Spherical16& s2) const {
           : Angle::Normalize(this->verticalAngle + verticalAngle);
 
   Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
-  */
+
 }
 Spherical16 Spherical16::operator+=(const Spherical16& v) {
   *this = *this + v;
@@ -234,4 +253,5 @@ Spherical16 Spherical16::RotateVertical(const Spherical16& v, Angle a) {
   Spherical16 r = Spherical16(v.distance, v.horizontalAngle,
                               v.verticalAngle.ToFloat() + a.ToFloat());
   return r;
-}
+}
+*/

Spherical16.h

@@ -1,3 +1,4 @@
+/*
 // 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/.
@@ -48,6 +49,8 @@ struct Spherical16 {
   /// @param v Vector in 3D carthesian coordinates;
   Spherical16(Vector3 v);
 
+  Vector3 ToVector3();
+
   /// @brief A spherical vector with zero degree angles and distance
   const static Spherical16 zero;
   /// @brief A normalized forward-oriented vector
@@ -155,4 +158,5 @@ using namespace Passer::LinearAlgebra;
 
 #include "Vector3.h"
 
-#endif
+#endif
+*/