From 11259a92a6a802b090679bfb951d09acaa01910e Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Fri, 20 Sep 2024 12:29:47 +0200
Subject: [PATCH] Extend functionality

---
 Direction.cpp  |   6 +++
 Direction.h    |   1 +
 Spherical.cpp  |  29 +++++++++----
 Spherical.h    |   3 ++
 SwingTwist.cpp | 109 +++++++++++++++++++++++++++++++++++++++++++++++++
 SwingTwist.h   |  69 +++++++++++++++++++++++++++++++
 6 files changed, 210 insertions(+), 7 deletions(-)
 create mode 100644 SwingTwist.cpp
 create mode 100644 SwingTwist.h

diff --git a/Direction.cpp b/Direction.cpp
index dac8e50..ae5b503 100644
--- a/Direction.cpp
+++ b/Direction.cpp
@@ -41,6 +41,12 @@ const DirectionOf<T> DirectionOf<T>::left = DirectionOf<T>(-90.0f, 0.0f);
 template <typename T>
 const DirectionOf<T> DirectionOf<T>::right = DirectionOf<T>(90.0f, 0.0f);
 
+template <typename T>
+DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
+  DirectionOf<T> r = DirectionOf<T>(-this->horizontal, -this->vertical);
+  return r;
+}
+
 template <typename T>
 DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator+(
     const DirectionOf<T>& v) const {
diff --git a/Direction.h b/Direction.h
index 348f4d7..96d5bac 100644
--- a/Direction.h
+++ b/Direction.h
@@ -29,6 +29,7 @@ class DirectionOf {
   const static DirectionOf left;
   const static DirectionOf right;
 
+  DirectionOf<T> operator-() const;
   DirectionOf<T> operator+(const DirectionOf<T>& v) const;
   DirectionOf<T> operator+=(const DirectionOf<T>& v);
 
diff --git a/Spherical.cpp b/Spherical.cpp
index 9552373..a6c74cc 100644
--- a/Spherical.cpp
+++ b/Spherical.cpp
@@ -70,10 +70,9 @@ SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
 
 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 verticalRad = (pi / 2) - this->vertical.InRadians();
+  float horizontalRad = this->horizontal.InRadians();
+
   float cosVertical = cosf(verticalRad);
   float sinVertical = sinf(verticalRad);
   float cosHorizontal = cosf(horizontalRad);
@@ -217,8 +216,12 @@ const float epsilon = 1E-05f;
 template <typename T>
 float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
                                       const SphericalOf<T>& v2) {
-  SphericalOf<T> difference = v1 - v2;
-  return difference.distance;
+  // SphericalOf<T> difference = v1 - v2;
+  // return difference.distance;
+  Vector3 vec1 = v1.ToVector3();
+  Vector3 vec2 = v2.ToVector3();
+  float distance = Vector3::Distance(vec1, vec2);
+  return distance;
 }
 
 template <typename T>
@@ -238,7 +241,19 @@ AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
   // float cdot = Float::Clamp(fraction, -1.0, 1.0);
   // float r = ((float)acos(cdot)) * Rad2Deg;
   Angle r = Vector3::Angle(v1_3, v2_3);
-  return AngleOf<T>(r.ToFloat());
+  return AngleOf<T>::Degrees(r.InDegrees());
+}
+
+template <typename T>
+AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
+    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();
+  Angle r = Vector3::SignedAngle(v1_vector, v2_vector, axis_vector);
+  return AngleOf<T>::Degrees(r.InDegrees());
 }
 
 template <typename T>
diff --git a/Spherical.h b/Spherical.h
index 159520a..89480e2 100644
--- a/Spherical.h
+++ b/Spherical.h
@@ -109,6 +109,9 @@ class SphericalOf {
                                const SphericalOf<T>& v2);
   static AngleOf<T> AngleBetween(const SphericalOf<T>& v1,
                                  const SphericalOf<T>& v2);
+  static AngleOf<T> SignedAngleBetween(const SphericalOf<T>& v1,
+                                       const SphericalOf<T>& v2,
+                                       const SphericalOf<T>& axis);
 
   static SphericalOf<T> Rotate(const SphericalOf& v,
                                AngleOf<T> horizontalAngle,
diff --git a/SwingTwist.cpp b/SwingTwist.cpp
new file mode 100644
index 0000000..7899631
--- /dev/null
+++ b/SwingTwist.cpp
@@ -0,0 +1,109 @@
+// 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/.
+
+#include "SwingTwist.h"
+
+template <typename T>
+SwingTwistOf<T>::SwingTwistOf() {
+  // this->horizontal = AngleOf<T>(0);
+  // this->vertical = AngleOf<T>(0);
+  this->swing = DirectionOf<T>(0, 0);
+  this->twist = AngleOf<T>(0);
+}
+
+template <typename T>
+Passer::LinearAlgebra::SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing,
+                                                     AngleOf<T> twist) {
+  this->swing = swing;
+  this->twist = twist;
+}
+
+template <typename T>
+SwingTwistOf<T>::SwingTwistOf(AngleOf<T> horizontal,
+                              AngleOf<T> vertical,
+                              AngleOf<T> twist) {
+  // this->horizontal = horizontal;
+  // this->vertical = vertical;
+  this->swing = DirectionOf<T>(horizontal, vertical);
+  this->twist = twist;
+}
+
+template <typename T>
+SwingTwistOf<T> SwingTwistOf<T>::Degrees(float horizontal,
+                                         float vertical,
+                                         float twist) {
+  DirectionOf<T> swing = DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
+                                        AngleOf<T>::Degrees(vertical));
+  AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
+  SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
+  return orientation;
+}
+
+template <typename T>
+Quaternion SwingTwistOf<T>::ToQuaternion() {
+  Quaternion q = Quaternion::Euler(this->swing.vertical.ToFloat(),
+                                   this->swing.horizontal.ToFloat(),
+                                   this->twist.ToFloat());
+  return q;
+}
+
+template <typename T>
+SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
+    Quaternion q) {
+  Vector3 angles = Quaternion::ToAngles(q);
+  SwingTwistOf<T> r =
+      SwingTwistOf<T>(angles.Up(), angles.Right(), angles.Forward());
+
+  return r;
+}
+
+template <typename T>
+const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();
+
+template <typename T>
+SphericalOf<T> SwingTwistOf<T>::operator*(const SphericalOf<T>& vector) const {
+  SphericalOf<T> v = SphericalOf<T>(vector.distance,
+                                    vector.horizontal + this->swing.horizontal,
+                                    vector.vertical + this->swing.vertical);
+  return v;
+}
+
+template <typename T>
+SwingTwistOf<T> SwingTwistOf<T>::operator*(
+    const SwingTwistOf<T>& rotation) const {
+  SwingTwistOf<T> r =
+      SwingTwistOf(this->swing.horizontal + rotation.swing.horizontal,
+                   this->swing.vertical + rotation.swing.vertical,
+                   this->twist + rotation.twist);
+  return r;
+}
+
+template <typename T>
+SwingTwistOf<T> SwingTwistOf<T>::operator*=(const SwingTwistOf<T>& rotation) {
+  // this->swing.horizontal += rotation.swing.horizontal;
+  // this->swing.vertical += rotation.swing.vertical;
+  this->swing += rotation.swing;
+  this->twist += rotation.twist;
+  return *this;
+}
+
+template <typename T>
+SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::Inverse(
+    SwingTwistOf<T> rotation) {
+  SwingTwistOf<T> r = SwingTwistOf<T>(-rotation.swing, -rotation.twist);
+  return r;
+}
+
+template <typename T>
+SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::AngleAxis(
+    float angle,
+    const SphericalOf<T>& axis) {
+  Vector3 axis_vector = axis.ToVector3();
+  Quaternion q = Quaternion::AngleAxis(angle, axis_vector);
+  SwingTwistOf<T> r = SwingTwistOf<T>::FromQuaternion(q);
+  return r;
+}
+
+template class SwingTwistOf<float>;
+template class SwingTwistOf<signed short>;
\ No newline at end of file
diff --git a/SwingTwist.h b/SwingTwist.h
new file mode 100644
index 0000000..6258ced
--- /dev/null
+++ b/SwingTwist.h
@@ -0,0 +1,69 @@
+// 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/.
+
+#ifndef SWINGTWIST_H
+#define SWINGTWIST_H
+
+#include "RoboidControlCore/LinearAlgebra/Angle.h"
+#include "RoboidControlCore/LinearAlgebra/Direction.h"
+#include "RoboidControlCore/LinearAlgebra/Quaternion.h"
+#include "RoboidControlCore/LinearAlgebra/Spherical.h"
+// #include "RoboidControlCore/LinearAlgebra/Spherical16.h"
+
+namespace Passer {
+namespace LinearAlgebra {
+
+template <typename T>
+class SwingTwistOf {
+ public:
+  DirectionOf<T> swing;
+  AngleOf<T> twist;
+
+  SwingTwistOf<T>();
+  SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
+  SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
+
+  static SwingTwistOf<T> Degrees(float horizontal,
+                                 float vertical = 0,
+                                 float twist = 0);
+  Quaternion ToQuaternion();
+  static SwingTwistOf<T> FromQuaternion(Quaternion q);
+
+  const static SwingTwistOf<T> identity;
+
+  /// <summary>
+  /// Rotate a vector using this rotation
+  /// </summary>
+  /// <param name="vector">The vector to rotate</param>
+  /// <returns>The rotated vector</returns>
+  SphericalOf<T> operator*(const SphericalOf<T>& vector) const;
+  /// <summary>
+  /// Multiply this rotation with another rotation
+  /// </summary>
+  /// <param name="rotation">The swing/twist rotation to multiply with</param>
+  /// <returns>The resulting swing/twist rotation</returns>
+  /// The result will be this rotation rotated according to
+  /// the give rotation.
+  SwingTwistOf<T> operator*(const SwingTwistOf<T>& rotation) const;
+  SwingTwistOf<T> operator*=(const SwingTwistOf<T>& rotation);
+
+  static SwingTwistOf<T> Inverse(SwingTwistOf<T> rotation);
+
+  /// <summary>
+  /// Convert an angle/axis representation to a swingt
+  /// </summary>
+  /// <param name="angle">The angle</param>
+  /// <param name="axis">The axis</param>
+  /// <returns>The resulting quaternion</returns>
+  static SwingTwistOf<T> AngleAxis(float angle, const SphericalOf<T>& axis);
+};
+
+using SwingTwistSingle = SwingTwistOf<float>;
+using SwingTwist16 = SwingTwistOf<signed short>;
+
+}  // namespace LinearAlgebra
+}  // namespace Passer
+using namespace Passer::LinearAlgebra;
+
+#endif