From b60b248f7529a56244681bce0b204443abceacb2 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Fri, 21 Mar 2025 09:52:07 +0100
Subject: [PATCH] Initial filling

---
 .vscode/settings.json | 11 +++++++++
 Angle.py              | 13 +++++++++++
 Direction.py          | 52 +++++++++++++++++++++++++++++++++++++++++++
 Quaternion.py         | 44 ++++++++++++++++++++++++++++++++++++
 Spherical.py          | 41 ++++++++++++++++++++++++++++++++++
 SwingTwist.py         | 28 +++++++++++++++++++++++
 test/Angle_test.py    | 14 ++++++++++++
 7 files changed, 203 insertions(+)
 create mode 100644 .vscode/settings.json
 create mode 100644 Angle.py
 create mode 100644 Direction.py
 create mode 100644 Quaternion.py
 create mode 100644 Spherical.py
 create mode 100644 SwingTwist.py
 create mode 100644 test/Angle_test.py

diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..5001e91
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,11 @@
+{
+    "python.testing.unittestArgs": [
+        "-v",
+        "-s",
+        "./test",
+        "-p",
+        "*_test.py"
+    ],
+    "python.testing.pytestEnabled": false,
+    "python.testing.unittestEnabled": true
+}
\ No newline at end of file
diff --git a/Angle.py b/Angle.py
new file mode 100644
index 0000000..f4741c9
--- /dev/null
+++ b/Angle.py
@@ -0,0 +1,13 @@
+import math
+
+class Angle:
+    Rad2Deg = 360 / (math.pi * 2)
+    Deg2Rad = (math.pi * 2) / 360
+
+    @staticmethod
+    def Normalize(angle):
+        while angle < -180:
+            angle += 360
+        while angle >= 180:
+            angle -= 360
+        return angle
\ No newline at end of file
diff --git a/Direction.py b/Direction.py
new file mode 100644
index 0000000..7be3a97
--- /dev/null
+++ b/Direction.py
@@ -0,0 +1,52 @@
+import math
+from LinearAlgebra.Angle import Angle
+
+class Direction:
+    def __init__(self, horizontal=0, vertical=0):
+        self.horizontal: float = Angle.Normalize(horizontal)
+        self.vertical: float = Angle.Normalize(vertical)
+        self.Normalize()
+    
+    @staticmethod
+    def Degrees(horizontal: float, vertical: float):
+        direction = Direction (horizontal, vertical)
+        return direction
+
+    def __add__(self, other):
+        return Direction(self.horizontal + other.x, self.vertical + other.y)
+
+    def __neg__(self):
+        return Direction(self.horizontal + 180, -self.vertical)
+    
+    def __sub__(self, other):
+        return Direction(self.horizontal - other.x, self.vertical - other.y)
+
+    def __mul__(self, scalar):
+        return Direction(self.horizontal * scalar, self.vertical * scalar)
+
+    def __truediv__(self, scalar):
+        if scalar != 0:
+            return Direction(self.horizontal / scalar, self.vertical / scalar)
+        else:
+            raise ValueError("Cannot divide by zero")
+
+    def Magnitude(self):
+        return math.sqrt(self.horizontal**2 + self.vertical**2)
+
+    def Normalize(self):
+        if self.vertical > 90 or self.vertical < -90:
+            self.horizontal += 180
+            self.verical = 180 - self.verical
+
+    def Dot(self, other):
+        return self.horizontal * other.x + self.vertical * other.y
+
+    def __repr__(self):
+        return f"Direction(x={self.horizontal}, y={self.vertical})"
+    
+Direction.forward = Direction(0, 0)
+Direction.backward = Direction(-180, 0)
+Direction.up = Direction(0, 90)
+Direction.down = Direction(0, -90)
+Direction.left = Direction(-90, 0)
+Direction.right = Direction(90, 0)
\ No newline at end of file
diff --git a/Quaternion.py b/Quaternion.py
new file mode 100644
index 0000000..0340227
--- /dev/null
+++ b/Quaternion.py
@@ -0,0 +1,44 @@
+import math
+
+Deg2Rad = (math.pi * 2) / 360
+
+class Quaternion:
+
+    def __init__(self):
+        self.x = 0
+        self.y = 0
+        self.z = 0
+        self.w = 1
+    
+    @staticmethod
+    def Euler(x, y, z):
+        yaw = x * Deg2Rad
+        pitch = y * Deg2Rad
+        roll = z * Deg2Rad
+
+        roll_over_2 = roll * 0.5
+        sin_roll_over_2 = math.sin(roll_over_2)
+        cos_roll_over_2 = math.cos(roll_over_2)
+
+        pitch_over_2 = pitch * 0.5
+        sin_pitch_over_2 = math.sin(pitch_over_2)
+        cos_pitch_over_2 = math.cos(pitch_over_2)
+
+        yaw_over_2 = yaw * 0.5
+        sin_yaw_over_2 = math.sin(yaw_over_2)
+        cos_yaw_over_2 = math.cos(yaw_over_2)
+
+        result = Quaternion()
+        result.w = (cos_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 +
+                    sin_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2)
+        result.x = (sin_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 +
+                    cos_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2)
+        result.y = (cos_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2 -
+                    sin_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2)
+        result.z = (cos_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2 -
+                    sin_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2)
+
+        return result
+
+
+Quaternion.identity = Quaternion()
\ No newline at end of file
diff --git a/Spherical.py b/Spherical.py
new file mode 100644
index 0000000..7b39253
--- /dev/null
+++ b/Spherical.py
@@ -0,0 +1,41 @@
+import math
+from LinearAlgebra.Direction import Direction
+
+class Spherical:
+    def __init__(self, distance, direction):
+        if distance < 0:
+            self.distance = -distance
+            self.direction = -direction
+        else:
+            self.distance: float = distance
+            self.direction: Direction = direction
+
+    # def __init__(self, distance, horizontal, vertical):
+    #     self.distance = distance
+    #     self.direction = Direction(horizontal, vertical)
+
+    def to_cartesian(self):
+        x = self.distance * math.sin(self.direction.horizontal) * math.cos(self.direction.vertical)
+        y = self.distance * math.sin(self.direction.horizontal) * math.sin(self.direction.vertical)
+        z = self.distance * math.cos(self.direction.horizontal)
+        return x, y, z
+
+    def from_cartesian(self, x, y, z):
+        self.distance = math.sqrt(x**2 + y**2 + z**2)
+        self.direction.horizontal = math.acos(z / self.distance)
+        self.direction.vertical = math.atan2(y, x)
+
+    def __add__(self, other):
+        x1, y1, z1 = self.to_cartesian()
+        x2, y2, z2 = other.to_cartesian()
+        return Spherical.from_cartesian(x1 + x2, y1 + y2, z1 + z2)
+
+    def __sub__(self, other):
+        x1, y1, z1 = self.to_cartesian()
+        x2, y2, z2 = other.to_cartesian()
+        return Spherical.from_cartesian(x1 - x2, y1 - y2, z1 - z2)
+
+    def __repr__(self):
+        return f"Spherical(r={self.distance}, horizontal={self.direction.horizontal}, phi={self.direction.vertical})"
+
+Spherical.zero = Spherical(0, Direction.forward)
diff --git a/SwingTwist.py b/SwingTwist.py
new file mode 100644
index 0000000..19a8992
--- /dev/null
+++ b/SwingTwist.py
@@ -0,0 +1,28 @@
+from LinearAlgebra.Direction import Direction
+from LinearAlgebra.Quaternion import Quaternion
+
+class SwingTwist:
+    """A rotation using swing and twist angle components"""
+    def __init__(self, swing: Direction, twist: float):
+        if swing.vertical > 90 or swing.vertical < -90:
+            swing.horizontal += 180
+            swing.vertical = 180 - swing.vertical
+            twist += 180        
+
+        ## Swing component of the rotation
+        self.swing = swing
+        ## The twist component of the rotation
+        self.twist = twist
+    
+    @staticmethod
+    def Degrees(horizontal: float, vertical: float, twist: float):
+        direction = Direction(horizontal, vertical)
+        swing_twist = SwingTwist(direction, twist)
+        return swing_twist
+
+    def ToQuaternion(self) -> Quaternion:
+        """Convert the SwingTwist rotation to a Quaternion"""
+        q = Quaternion.Euler(-self.swing.vertical,
+                             self.swing.horizontal,
+                             self.twist)
+        return q
\ No newline at end of file
diff --git a/test/Angle_test.py b/test/Angle_test.py
new file mode 100644
index 0000000..e74ab71
--- /dev/null
+++ b/test/Angle_test.py
@@ -0,0 +1,14 @@
+import sys
+from pathlib import Path
+# Add the project root to sys.path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+                
+import unittest
+
+class AngleTest(unittest.TestCase):
+    def test_one(self):
+        pass
+
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file