diff --git a/Angle.py b/Angle.py
index f4741c9..c7a70b7 100644
--- a/Angle.py
+++ b/Angle.py
@@ -1,13 +1,272 @@
-import math
+# 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/.
 
+import math
+from Float import *
+
+# This is in fact AngleSingle
 class Angle:
+    # The angle is internally limited to (-180..180] degrees or (-PI...PI]
+    # radians. When an angle exceeds this range, it is normalized to a value
+    # within the range.
     Rad2Deg = 360 / (math.pi * 2)
     Deg2Rad = (math.pi * 2) / 360
 
+    def __init__(self, degrees = 0):
+        self.value: float = degrees
+        Angle.Normalize(self)
+
+    @staticmethod
+    def Degrees(degrees):
+        angle = Angle(degrees)
+        return angle
+    
+    @staticmethod
+    def Radians(radians):
+        angle = Angle(radians * Angle.Rad2Deg)
+        return angle;
+    
+    def InDegrees(self):
+        return self.value;
+    def InRadians(self) -> float:
+        return self.value * Angle.Deg2Rad;
+
+    def __eq__(self, angle):
+        """! Tests whether this angle is equal to the given angle
+        @param angle The angle to compare to
+        @return True when the angles are equal, False otherwise
+        @note The equality is determine within the limits of precision of the raw
+        type T
+        """
+        return self.value == angle.value
+    def __gt__(self, angle):
+        """! Tests if this angle is greater than the given angle
+        @param angle The given angle
+        @return True when this angle is greater than the given angle, False
+        otherwise
+        """
+        return self.value > angle.value
+    def __gte__(self, angle):
+        """! Tests if this angle is greater than or equal to the given angle
+        @param angle The given angle
+        @return True when this angle is greater than or equal to the given angle.
+        False otherwise.
+        """
+        return self.value >= angle.value
+    def __lt__(self, angle):
+        """! Tests if this angle is less than the given angle
+        @param angle The given angle
+        @return True when this angle is less than the given angle, False
+        otherwise
+        """
+        return self.value < angle.value
+    def __lte__(self, angle):
+        """! Tests if this angle is less than or equal to the given angle
+        @param angle The given angle
+        @return True when this angle is less than or equal to the given angle.
+        False otherwise.
+        """
+        return self.value <= angle.value
+    
+    def Sign(self):
+        """! Returns the sign of the angle
+        @param angle The angle
+        @return -1 when the angle is negative, 1 when it is positive and 0
+        otherwise.
+        """
+        if self.value < 0:
+            return -1
+        if self.value > 0:
+            return 1
+        return 0
+    def Abs(self):
+        """! Returns the magnitude of the angle
+        @param angle The angle
+        @return The positive magitude of the angle.
+        Negative values are negated to get a positive result
+        """
+        if self.value < 0:
+            return -self
+        return self
+    
+    def __neg__(self):
+        """! Negate the angle
+        @return The negated angle
+        """
+        return Angle(-self.value)
+    def Inverse(self):
+        """! Invert the angle: rotate by 180 degrees
+        """
+        return self + Angle.Degrees(180)
+
+    def __sub__(self, other):
+        """! Substract another angle from this angle
+        @param angle The angle to subtract from this angle
+        @return The result of the subtraction
+        """
+        return Angle(self.value - other.value)
+    def __add__(self, other):
+        """! Add another angle from this angle
+        @param angle The angle to add to this angle
+        @return The result of the addition
+        """
+        return Angle(self.value + other.value)
+    def __mul__(self, factor):
+        """! Multiplies the angle by a factor
+        @param angle The angle to multiply
+        @param factor The factor by which the angle is multiplied
+        @return The multiplied angle
+        """
+        return Angle(self.value * factor)
+    def __truediv__(self, factor):
+        """! Divides the angle by a factor
+        @param angle The angle to devide
+        @param factor The factor by which the angle is devided
+        @return The devided angle
+        """
+        return Angle(self.value / factor)
+
     @staticmethod
     def Normalize(angle):
-        while angle < -180:
-            angle += 360
-        while angle >= 180:
-            angle -= 360
-        return angle
\ No newline at end of file
+        """! Normalizes the angle to (-180..180] or (-PI..PI]
+        @note Should not be needed but available in case it is.
+        """
+        while angle.value < -180:
+            angle.value += 360
+        while angle.value >= 180:
+            angle.value -= 360
+        return angle
+    
+    @staticmethod
+    def Clamp(angle, min, max):
+        """! Clamps the angle value between the two given angles
+        @param angle The angle to clamp
+        @param min The minimum angle
+        @param max The maximum angle
+        @return The clamped value
+        @remark When the min value is greater than the max value, angle is
+        returned unclamped.
+        """
+        degrees = Float.Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees());
+        return Angle.Degrees(degrees)
+
+    @staticmethod
+    def MoveTowards(from_angle, to_angle, max_angle):
+        """! Rotates an angle towards another angle with a max distance
+        @param from_angle The angle to start from
+        @param to_angle The angle to rotate towards
+        @param max_angle The maximum angle to rotate
+        @return The rotated angle
+        """
+        max_degrees = max(0, max_angle) # filter out negative distances
+        delta_angle = Angle.Abs(to_angle - from_angle)
+        delta_degrees = delta_angle.InDegrees()
+        delta_degrees = Float.Clamp(delta_degrees, 0, max_degrees)
+        if delta_degrees < 0:
+            delta_degrees = -delta_degrees
+        return from_angle + Angle.Degrees(delta_degrees)
+
+    @staticmethod
+    def Cos(angle):
+        """! Calculates the cosine of an angle
+        @param angle The given angle
+        @return The cosine of the angle
+        """
+        return math.cos(angle.InRadians())
+    @staticmethod
+    def Sin(angle):
+        """! Calculates the sine of an angle
+        @param angle The given angle
+        @return The sine of the angle
+        """
+        return math.sin(angle.InRadians())
+    @staticmethod
+    def Tan(angle):
+        """! Calculates the tangent of an angle
+        @param angle The given angle
+        @return The tangent of the angle
+        """
+        return math.tan(angle.InRadians())
+    
+    @staticmethod
+    def Acos(f):
+        """! Calculates the arc cosine angle
+        @param f The value
+        @return The arc cosine for the given value
+        """
+        return Angle.Radians(math.acos(f))
+    @staticmethod
+    def Asin(f):
+        """! Calculates the arc sine angle
+        @param f The value
+        @return The arc sine for the given value
+        """
+        return Angle.Radians(math.asin(f))
+    @staticmethod
+    def Atan(f):
+        """! Calculates the arc tangent angle
+        @param f The value
+        @return The arc tangent for the given value
+        """
+        return Angle.Radians(math.atan(f))
+    def Atan2(y, x):
+        """! Calculates the tangent for the given values
+        @param y The vertical value
+        @param x The horizontal value
+        @return The tanget for the given values
+        Uses the y and x signs to compute the quadrant
+        """
+        return Angle.Radians(math.atan2(y, x))
+    
+    @staticmethod
+    def CosineRuleSide(a, b, gamma):
+        """! Computes the length of a side of a triangle using the rule of cosines
+        @param a The length of side A
+        @param b The length of side B
+        @param gamma The angle of the corner opposing side C
+        @return The length of side C
+        """
+        a2: float = a * a
+        b2: float = b * b
+        d: float = a2 + b2 - 2 * a * b * Angle.Cos(gamma)
+        # Catch edge cases where float inaccuracies lead tot NaNs
+        if d < 0:
+            return 0
+
+        c: float = math.sqrt(d)
+        return c
+    @staticmethod
+    def CosineRuleAngle(a, b, c):
+        """! Computes the angle of a corner of a triangle using the rule of cosines
+        @param a The length of side A
+        @param b The length of side B
+        @param c The length of side C
+        @return The angle of the corner opposing side C
+        """
+        a2: float = a * a
+        b2: float = b * b
+        c2: float = c * c
+        d: float = (a2 + b2 - c2) / (2 * a * b);
+        # Catch edge cases where float inaccuracies lead tot NaNs
+        if d >= 1:
+            return Angle()
+        if d <= -1:
+            return Angle.Degrees(180)
+        gamma: Angle = Angle.Acos(d)
+        return gamma;
+
+    @staticmethod
+    def SineRuleAngle(a, beta, c):
+        """! Computes the angle of a triangle corner using the rule of sines
+        @param a The length of side A
+        @param beta the angle of the corner opposing side B
+        @param c The length of side C
+        @return The angle of the corner opposing side A
+        """
+        alpha:Angle = Angle.Asin(a * Angle.Sin(beta) / c);
+        return alpha;
+
+
+Angle.zero = Angle(0)
+## An zero value angle
diff --git a/Direction.py b/Direction.py
index 7be3a97..abb2117 100644
--- a/Direction.py
+++ b/Direction.py
@@ -1,52 +1,109 @@
 import math
-from LinearAlgebra.Angle import Angle
+from Angle import Angle
+from Vector import Vector3
 
 class Direction:
-    def __init__(self, horizontal=0, vertical=0):
-        self.horizontal: float = Angle.Normalize(horizontal)
-        self.vertical: float = Angle.Normalize(vertical)
+    """! A direction using angles
+
+    * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive)
+    degrees which is a rotation in the horizontal plane
+    * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees
+    which is the rotation in the up/down direction applied after the horizontal
+    rotation has been applied.
+    The angles are automatically normalized to stay within the abovenmentioned
+    ranges.
+    """
+    def __init__(self, horizontal=Angle(), vertical=Angle()):
+        """! Create a new direction
+        @param horizontal The horizontal angle
+        @param vertical The vertical angle.
+        """
+        ## horizontal angle, range in degrees = (-180..180]
+        self.horizontal: Angle = horizontal
+        ## vertical angle, range in degrees = (-90..90]
+        self.vertical: Angle = vertical
+
         self.Normalize()
     
     @staticmethod
     def Degrees(horizontal: float, vertical: float):
-        direction = Direction (horizontal, vertical)
+        """! Create a direction using angle values in degrees
+        @param horizontal The horizontal angle in degrees
+        @param vertical The vertical angle in degrees
+        @return The direction
+        """
+        direction = Direction(Angle.Degrees(horizontal), Angle.Degrees(vertical))
+        return direction
+    @staticmethod
+    def Radians(horizontal: float, vertical: float):
+        """! Create a direction using angle values in radians
+        @param horizontal The horizontal angle in radians
+        @param vertical The vertical angle in radians
+        @return The direction
+        """
+        direction = Direction(Angle.Radians(horizontal), Angle.Radians(vertical))
         return direction
 
-    def __add__(self, other):
-        return Direction(self.horizontal + other.x, self.vertical + other.y)
+    def FromVector3(v: Vector3):
+        d = Direction(
+            horizontal = Angle.Atan2(v.right, v.forward),
+            vertical = Angle.Degrees(-90) - Angle.Acos(v.up)
+        )
+        return d;
+    def ToVector3(self) -> Vector3:
+        """! Convert the direction to a Vector3 coordinate
+        @return The vector coordinate
+        """
+        verticalRad = (math.pi / 2) - self.vertical.InRadians()
+        horizontalRad = self.horizontal.InRadians()
+
+        cosVertical = math.cos(verticalRad)
+        sinVertical = math.sin(verticalRad)
+        cosHorizontal = math.cos(horizontalRad)
+        sinHorizontal = math.sin(horizontalRad)
+
+        right = sinVertical * sinHorizontal
+        up = cosVertical
+        forward = sinVertical * cosHorizontal
+        return Vector3(right, up, forward)
+        
+    def __eq__(self, direction):
+        """! Test whether this direction is equal to another direction
+        @param direction The direction to compare to
+        @return True when the direction angles are equal, false otherwise.
+        """
+        return (self.horizontal == direction.horizontal and
+                self.vertical == direction.vertical)
 
     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)
+        """! Negate/reverse the direction
+        @return The reversed direction.
+        """
+        h: Angle = self.horizontal + Angle.Degrees(-180)
+        v: Angle = -self.vertical
+        return Direction(h, v)
+    def Inverse(self):
+        """! This is a synonym for negation
+        """
+        return -self
 
     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
+        """! Normalize this vector to the specified ranges
+        @note Should not be needed but available in case it is.
+        """
+        v = self.vertical.InDegrees()
+        deg180 = Angle.Degrees(-180)
+        if v > 90 or v < -90:
+            self.horizontal += deg180
+            self.vertical = deg180 - self.vertical
 
     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
+
+Direction.zero = Direction.Degrees(0, 0)
+Direction.forward = Direction.Degrees(0, 0)
+Direction.backward = Direction.Degrees(-180, 0)
+Direction.up = Direction.Degrees(0, 90)
+Direction.down = Direction.Degrees(0, -90)
+Direction.left = Direction.Degrees(-90, 0)
+Direction.right = Direction.Degrees(90, 0)
\ No newline at end of file
diff --git a/Float.py b/Float.py
new file mode 100644
index 0000000..780f656
--- /dev/null
+++ b/Float.py
@@ -0,0 +1,13 @@
+# 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/.
+
+class Float:
+    def Clamp(f, min, max):
+        if max < min:
+            return f
+        if f < min:
+            return min
+        if f > max:
+            return max
+        return f
diff --git a/Quaternion.py b/Quaternion.py
index 0340227..43a7550 100644
--- a/Quaternion.py
+++ b/Quaternion.py
@@ -1,30 +1,36 @@
 import math
 
+from Vector import Vector3
+from Angle import Angle
+from Direction import Direction
+
 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
+    def __init__(self, x: float = 0, y: float = 0, z: float = 0, w: float = 1):
+        self.x = x
+        self.y = y
+        self.z = z
+        self.w = w
 
-        roll_over_2 = roll * 0.5
+        magnitude: float = self.Magnitude()
+        self.x /= magnitude
+        self.y /= magnitude
+        self.z /= magnitude
+        self.w /= magnitude
+
+    @staticmethod
+    def FromAngles(yaw: Angle, pitch: Angle, roll: Angle):
+        roll_over_2 = roll.InRadians() * 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
+        pitch_over_2 = pitch.InRadians() * 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
+        yaw_over_2 = yaw.InRadians() * 0.5
         sin_yaw_over_2 = math.sin(yaw_over_2)
         cos_yaw_over_2 = math.cos(yaw_over_2)
 
@@ -37,8 +43,154 @@ class Quaternion:
                     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
 
+    def ToAngles(self):
+        test: float = self.x * self.y + self.z * self.w;
+        if test > 0.499: # singularity at north pole
+            return (
+                Angle.zero,
+                Angle.Radians(2 * math.atan2(self.x, self.w)),
+                Angle.Degrees(90)
+            )
+        elif test < -0.499: # singularity at south pole
+            return (
+                Angle.zero,
+                Angle.Radians(-2 * math.atan2(self.x, self.w)),
+                Angle.Degrees(-90)
+            )
+        else:
+            sqx: float = self.x * self.x
+            sqy: float = self.y * self.y
+            sqz: float = self.z * self.z
+
+            yaw = Angle.Radians(math.atan2(2 * self.y * self.w - 2 * self.x * self.z, 1 - 2 * sqy - 2 * sqz))
+            pitch = Angle.Radians(math.atan2(2 * self.x * self.w - 2 * self.y * self.z, 1 - 2 * sqx - 2 * sqz))
+            roll = Angle.Radians(math.asin(2 * test))
+            return (yaw, pitch, roll)
+        
+    def Degrees(yaw: float, pitch: float, roll: float):
+        return Quaternion.FromAngles(
+            Angle.Degrees(yaw),
+            Angle.Degrees(pitch),
+            Angle.Degrees(roll)
+        )
+
+    def Radians(yaw: float, pitch: float, roll: float):
+        return Quaternion.FromAngles(
+            Angle.Radians(yaw),
+            Angle.Radians(pitch),
+            Angle.Radians(roll)
+        )
+
+    def FromAngleAxis(angle: Angle, axis: Direction):
+        if axis.SqrMagnitude() == 0:
+            return Quaternion.identity
+
+        result: Quaternion = Quaternion.identity
+        radians = angle.InRadians()
+        radians *= 0.5
+
+        axis2: Vector3 = axis * math.sin(radians);
+        q = Quaternion(
+            axis2.right,
+            axis2.up,
+            axis2.forward,
+            math.cos(radians)
+        )
+        return q
+
+    def ToAngleAxis(self) -> tuple[Angle, Direction]:
+        angle: Angle = Angle.Radians(2 * math.acos(self.w))
+        den: float = math.sqrt(1 - self.w * self.w)
+        if den > 0.0001:
+            axis = Direction.FromVector3(self.Axis() / den)
+        else:
+            # This occurs when the angle is zero.
+            # Not a problem: just set an arbitrary normalized axis.
+            axis = Direction.right
+        return (angle, axis)
+
+    def __eq__(self, other) -> bool:
+        return (
+            self.x == other.x and 
+            self.y == other.y and
+            self.z == other.z and
+            self.w == other.w
+        )
+
+    def SqrMagnitude(self) -> float:
+        return self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w
+
+    def Magnitude(self) -> float:
+        """! The vector length
+        @return The vector length
+        """
+        return math.sqrt(self.SqrMagnitude())    
+
+    def __mul__(self, other):
+        if isinstance(other, Quaternion):
+            return self.MultQuaternion(other)
+        elif isinstance(other, Vector3):
+            return self.MultVector(other)
+
+    def MultQuaternion(self, q):
+        return Quaternion(
+            self.x * q.w + self.y * q.z - self.z * q.y + self.w * q.x,
+            -self.x * q.z + self.y * q.w + self.z * q.x + self.w * q.y,
+            self.x * q.y - self.y * q.x + self.z * q.w + self.w * q.z,
+            -self.x * q.x - self.y * q.y - self.z * q.z + self.w * q.w
+        )
+
+    def MultVector(self, v: Vector3):
+        num = self.x * 2
+        num2 = self.y * 2
+        num3 = self.z * 2
+        num4 = self.x * num
+        num5 = self.y * num2
+        num6 = self.z * num3
+        num7 = self.x * num2
+        num8 = self.x * num3
+        num9 = self.y * num3
+        num10 = self.w * num
+        num11 = self.w * num2
+        num12 = self.w * num3
+
+        px = v.right
+        py = v.up
+        pz = v.forward
+        rx = (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz
+        ry = (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz
+        rz = (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz
+        result = Vector3(rx, ry, rz)
+        return result
+
+    def Dot(q1, q2) -> float:
+        return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w
+
+    def Angle(q1, q2):
+        f: float = Quaternion.Dot(q1, q2)
+        angle = Angle.Radians(math.acos(min(math.fabs(f), 1)) * 2)
+        return angle
+        # return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
+
+    def AngleAround(self, axis: Direction):
+        secondaryRotation: Quaternion = self.RotationAround(axis);
+        (rotationAngle, rotationAxis) = secondaryRotation.ToAngleAxis();
+
+        # Do the axis point in opposite directions?
+        if Vector3.Dot(axis.ToVector3(), rotationAxis.ToVector3()) < 0:
+            return -rotationAngle;
+
+        return rotationAngle;
+
+    def RotationAround(self, axis: Direction):
+        ra = Vector3(self.x, self.y, self.z)  # rotation axis
+        p: Vector3 = Vector3.Project(ra, axis.ToVector3())  # return projection ra on to axis  (parallel component)
+        twist: Quaternion = Quaternion(p.right, p.up, p.forward, self.w)
+        return twist;
+
+    def Axis(self) -> Vector3:
+        return Vector3(self.x, self.y, self.z)
 
 Quaternion.identity = Quaternion()
\ No newline at end of file
diff --git a/Spherical.py b/Spherical.py
index 7b39253..3b70699 100644
--- a/Spherical.py
+++ b/Spherical.py
@@ -1,41 +1,431 @@
 import math
-from LinearAlgebra.Direction import Direction
+from Direction import *
+from Vector import *
 
-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
+class Polar:
+    """! A polar 2D vector
+    """
 
-    # def __init__(self, distance, horizontal, vertical):
-    #     self.distance = distance
-    #     self.direction = Direction(horizontal, vertical)
+    def __init__(self, distance: float, angle: Angle):
+        """! Create a new polar vector
+        @param distance The length of the vector
+        @param angle The direction of the angle
+        """
+        self.distance: float = distance
+        ## The length of the vector
+        self.direction: Angle = angle
+        ## The direction of the vector
 
-    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
+        ## Normalizing such that distance >= 0
+        if self.distance < 0:
+            self.distance = -self.distance
+            self.direction = self.direction.Inverse()
+        elif self.distance == 0:
+            self.direction = Angle.zero
 
-    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 Degrees(distance: float, degrees: float):
+        """! Create a polar vector without using the Angle type. All given
+        angles are in degrees
+        @param distance The distance in meters
+        @param horizontal The angle in degrees
+        @return The polar vector
+        """
+        angle: Angle = Angle.Degrees(degrees)
+        r: Polar = Polar(distance, angle)
+        return r
 
-    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 Radians(distance: float, radians: float):
+        """! Create polar vector without using the Angle type. All given
+        angles are in radians
+        @param distance The distance in meters
+        @param horizontal The horizontal angle in radians
+        @param vertical The vertical angle in radians
+        @return The polar vector
+        """
+        angle: Angle = Angle.Radians(radians)
+        r: Polar = Polar(distance, angle)
+        return r
+
+    @staticmethod
+    def FromVector2(v: Vector2):
+        """! Create a polar coordinate from a Vector2 coordinate
+        @param v The vector coordinate
+        @return The polar coordinate
+        """
+        distance = v.Magnitude()
+        if distance == 0:
+            return Polar(0, Angle.zero)
+        
+        angle = Angle.Radians(math.atan2(v.right, v.up))
+        return Polar(distance, angle)
+
+    def ToVector2(self) -> Vector2:
+        """! Convert the polar coordinate to a Vector2 coordinate
+        @return The vector coordinate
+        """
+        horizontalRad = self.direction.InRadians()
+
+        cosHorizontal = math.cos(horizontalRad)
+        sinHorizontal = math.sin(horizontalRad)
+
+        right = self.distance * sinHorizontal
+        up = self.distance * cosHorizontal
+        return Vector2(right, up)
+
+    def __eq__(self, other) -> bool:
+        """! Check if this vector is equal to the given vector
+        @param v The vector to check against
+        @return true if it is identical to the given vector
+        @note This uses float comparison to check equality which may have strange
+        effects. Equality on floats should be avoided.
+        """
+        return (
+            self.distance == other.distance and
+            self.direction == other.direction
+        )    
+        
+    def Magnitude(self) -> float:
+        return math.fabs(self.distance)
+
+    def Normalized(self):
+        if self.distance == 0:
+            return Polar(0, self.direction)
+        
+        return Polar(1, self.direction)
+
+    def __neg__(self):
+        """! Negate the vector
+        @return The negated vector
+        This will negate the direction. Distance will stay the same.
+        """
+        return Polar(self.distance, self.direction.Inverse())
 
     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)
+        """! Subtract a polar vector from this vector
+        @param other The vector to subtract
+        @return The result of the subtraction
+        """
+        # v1 = self.ToVector2()
+        # v2 = other.ToVector2()
+        # r = v1 - v2
+        # return Polar.FromVector2(r)
+        r = self + (-other)
+        return r
+
+    def __add__(self, other):
+        """! Add a polar vector to this vector
+        @param other The vector to add
+        @return The result of the addition
+        """
+        # v1 = self.ToVector2()
+        # v2 = other.ToVector2()
+        # r = v1 - v2
+        # return Polar.FromVector2(r)
+
+        if other.distance == 0:
+            return Polar(self.distance, self.direction)
+        if self.distance == 0:
+            return other
+
+        deltaAngle: float = (other.direction - self.direction).InDegrees();
+        if deltaAngle < 0:
+            rotation = 180 + deltaAngle
+        else:
+            rotation = 180 - deltaAngle
+
+        if rotation == 180 and other.distance > 0:
+            # angle is too small, take this angle and add the distances
+            return Polar(self.distance + other.distance, self.direction)
+
+        newDistance: float = Angle.CosineRuleSide(other.distance, self.distance, Angle.Degrees(rotation))
+
+        angle: float = Angle.CosineRuleAngle(newDistance, self.distance, other.distance).InDegrees()
+        if deltaAngle < 0:
+            new_angle: float = self.direction.InDegrees() - angle
+        else:
+            new_angle: float = self.direction.InDegrees() + angle
+        new_angle_a: Angle = Angle.Degrees(new_angle)
+        vector = Polar(newDistance, new_angle_a)
+        return vector
+
+    def __mul__(self, factor):
+        """! Scale the vector uniformly up
+        @param factor The scaling factor
+        @return The scaled vector
+        @remark This operation will scale the distance of the vector. The angle
+        will be unaffected.
+        """
+        return Polar(
+            self.distance * factor,
+            self.direction
+        )
+
+    def __truediv__(self, factor):
+        """! Scale the vector uniformly down
+        @param factor The scaling factor
+        @return The scaled vector
+        @remark This operation will scale the distance of the vector. The angle
+        will be unaffected.
+        """
+        return Polar(
+            self.distance / factor,
+            self.direction
+        )
+
+    @staticmethod
+    def Distance(v1, v2) -> float:
+        """! Calculate the distance between two spherical coordinates
+        @param v1 The first coordinate
+        @param v2 The second coordinate
+        @return The distance between the coordinates in meters
+        """
+        v1 = v1.ToVector2()
+        v2 = v2.ToVector2()
+        distance: float = Vector2.Distance(v1, v2)
+        return distance
+
+    @staticmethod
+    def Angle(v1, v2) -> Angle:
+        """! Calculate the unsigned angle between two spherical vectors
+        @param v1 The first vector
+        @param v2 The second vector
+        @return The unsigned angle between the vectors [0..179.9999999..., -180]
+        @remark the strange range is caused by the 2s complement signed values
+        which has range [minvalue..maxvalue). This is a hardware limitation,
+        not something we can change.
+        """
+        angle: Angle = Angle.Abs(v1.direction - v2.direction)
+        return angle
+    
+    @staticmethod
+    def SignedAngle(v1, v2) -> Angle:
+        """! Calculate the unsigned angle between two spherical vectors
+        @param v1 The first vector
+        @param v2 The second vector
+        @return The unsigned angle between the vectors [0..179.9999999..., -180]
+        @remark the strange range is caused by the 2s complement signed values
+        which has range [minvalue..maxvalue). This is a hardware limitation,
+        not something we can change.
+        """
+        angle: Angle = v2.direction - v1.direction
+        return angle
+
+    def Lerp(v1, v2, f: float):
+        """! Lerp (linear interpolation) between two vectors
+        @param v1 The starting vector
+        @param v2 The ending vector
+        @param f The interpolation distance
+        @return The lerped vector
+        @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
+        1 matches vector *v2*. Value -1 is vector *v1* minus the difference
+        between *v1* and *v2* etc.
+        """
+        return v1 + (v2 - v1) * f
+        # return v1 * (1 - f) + v2 * f
+
+Polar.zero = Polar(0, Angle.zero)
+
+class Spherical(Polar):
+    """! A spherical 3D vector
+    """
+
+    def __init__(self, distance: float, direction: Direction):
+        """! Create a new spherical vector
+        @param distance The length of the vector
+        @param direction The direction of the vector
+        """
+        self.distance: float = distance
+        ## The length of the vector
+        self.direction: Direction = direction
+        ## The direction of the vector
+        
+        ## Normalizing such that distance >= 0
+        if self.distance < 0:
+            self.distance = -self.distance
+            self.direction = -self.direction
+        elif self.distance == 0:
+            self.direction = Direction.zero
+
+    def Degrees(distance: float, horizontal: float, vertical: float):
+        """! Create sperical vector without using the Direction type. All given
+        angles are in degrees
+        @param distance The distance in meters
+        @param horizontal The horizontal angle in degrees
+        @param vertical The vertical angle in degrees
+        @return The spherical vector
+        """
+        direction: Direction = Direction.Degrees(horizontal, vertical)
+        r: Spherical = Spherical(distance, direction)
+        return r
+
+    def Radians(distance: float, horizontal: float, vertical: float):
+        """! Create sperical vector without using the Direction type. All given
+        angles are in radians
+        @param distance The distance in meters
+        @param horizontal The horizontal angle in radians
+        @param vertical The vertical angle in radians
+        @return The spherical vector
+        """
+        direction: Direction = Direction.Radians(horizontal, vertical)
+        r: Spherical = Spherical(distance, direction)
+        return r
+
+    @staticmethod 
+    def FromVector3(v: Vector3):
+        """! Create a Spherical coordinate from a Vector3 coordinate
+        @param v The vector coordinate
+        @return The spherical coordinate
+        """
+        distance = v.Magnitude()
+        if distance == 0:
+            return Spherical(0, Angle(), Angle())
+        
+        verticalAngle = Angle.Radians((math.pi / 2 - math.acos(v.up / distance)))
+        horizontalAngle = Angle.Radians(math.atan2(v.right, v.forward))
+        return Spherical(distance, Direction(horizontalAngle, verticalAngle))
+    
+    def ToVector3(self) -> Vector3:
+        """! Convert the spherical coordinate to a Vector3 coordinate
+        @return The vector coordinate
+        """
+        verticalRad = (math.pi / 2) - self.direction.vertical.InRadians()
+        horizontalRad = self.direction.horizontal.InRadians()
+
+        cosVertical = math.cos(verticalRad)
+        sinVertical = math.sin(verticalRad)
+        cosHorizontal = math.cos(horizontalRad)
+        sinHorizontal = math.sin(horizontalRad)
+
+        right = self.distance * sinVertical * sinHorizontal
+        up = self.distance * cosVertical
+        forward = self.distance * sinVertical * cosHorizontal
+        return Vector3(right, up, forward)
+
+    def __eq__(self, other) -> bool:
+        """! Check if this vector is equal to the given vector
+        @param v The vector to check against
+        @return true if it is identical to the given vector
+        @note This uses float comparison to check equality which may have strange
+        effects. Equality on floats should be avoided.
+        """
+        return (
+            self.distance == other.distance and
+            self.direction == other.direction
+        )    
+
+    def Normalized(self) -> float:
+        if self.distance == 0:
+            return Spherical(0, self.direction)
+        
+        return Spherical(1, self.direction)        
+
+    def __neg__(self):
+        """! Negate the vector
+        @return The negated vector
+        This will negate the direction. Distance will stay the same.
+        """
+        return Spherical(self.distance, self.direction.Inverse())
+    
+    def __sub__(self, other):
+        """! Subtract a spherical vector from this vector
+        @param other The vector to subtract
+        @return The result of the subtraction
+        """
+        v1 = self.ToVector3()
+        v2 = other.ToVector3()
+        r = v1 - v2
+        return Spherical.FromVector3(r)
+
+    def __add__(self, other):
+        """! Add a spherical vector to this vector
+        @param other The vector to add
+        @return The result of the addition
+        """
+        v1 = self.ToVector3()
+        v2 = other.ToVector3()
+        r = v1 + v2
+        return Spherical.FromVector3(r)
+    
+    def __mul__(self, factor):
+        """! Scale the vector uniformly up
+        @param factor The scaling factor
+        @return The scaled vector
+        @remark This operation will scale the distance of the vector. The angle
+        will be unaffected.
+        """
+        return Spherical(
+            self.distance * factor,
+            self.direction
+        )
+
+    def __truediv__(self, factor):
+        """! Scale the vector uniformly down
+        @param factor The scaling factor
+        @return The scaled vector
+        @remark This operation will scale the distance of the vector. The angle
+        will be unaffected.
+        """
+        return Spherical(
+            self.distance / factor,
+            self.direction
+        )
+
+    @staticmethod
+    def Distance(v1, v2) -> float:
+        """! Calculate the distance between two spherical coordinates
+        @param s1 The first coordinate
+        @param s2 The second coordinate
+        @return The distance between the coordinates in meters
+        """
+        v1: Vector3 = v1.ToVector3()
+        v2: Vector3 = v2.ToVector3()
+        distance: float = Vector3.Distance(v1, v2)
+        return distance
+
+    @staticmethod
+    def Angle(s1, s2) -> Angle:
+        """! Calculate the unsigned angle between two spherical vectors
+        @param s1 The first vector
+        @param s2 The second vector
+        @return The unsigned angle between the vectors [0..179.9999999..., -180]
+        @remark the strange range is caused by the 2s complement signed values
+        which has range [minvalue..maxvalue). This is a hardware limitation,
+        not something we can change.
+        """
+        v1: Vector3 = s1.ToVector3()
+        v2: Vector3 = s2.ToVector3()
+        angle: Angle = Vector3.Angle(v1, v2)
+        return angle
+    
+    def SignedAngle(s1, s2, axis) -> Angle:
+        """! Calculate the signed angle between two spherical vectors
+        @param s1 The first vector
+        @param s2 The second vector
+        @param axis The axis around which the angle is calculated
+        @return The signed angle between the vectors
+        """
+        v1 = s1.ToVector3()
+        v2 = s2.ToVector3()
+        v_axis = axis.ToVector3()
+        angle = Vector3.SignedAngle(v1, v2, v_axis)
+        return angle
+    
+    @staticmethod
+    def Rotate(s, horizontal: Angle, vertical: Angle):
+        """! Rotate a spherical vector
+        @param s The vector to rotate
+        @param horizontal The horizontal rotation angle in local space
+        @param vertical The vertical rotation angle in local space
+        @return The rotated vector
+        """
+        direction: Direction = Direction(
+            s.direction.horizontal + horizontal,
+            s.direction.vertical + vertical
+        )
+        r = Spherical(s.distance, direction)
+        return r
 
     def __repr__(self):
         return f"Spherical(r={self.distance}, horizontal={self.direction.horizontal}, phi={self.direction.vertical})"
 
-Spherical.zero = Spherical(0, Direction.forward)
+Spherical.zero = Spherical(0, Direction.zero)
diff --git a/SwingTwist.py b/SwingTwist.py
index 19a8992..86699f5 100644
--- a/SwingTwist.py
+++ b/SwingTwist.py
@@ -1,14 +1,9 @@
-from LinearAlgebra.Direction import Direction
-from LinearAlgebra.Quaternion import Quaternion
+from Direction import *
+from Quaternion import *
 
 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        
-
+    def __init__(self, swing: Direction, twist: Angle):
         ## Swing component of the rotation
         self.swing = swing
         ## The twist component of the rotation
@@ -16,13 +11,79 @@ class SwingTwist:
     
     @staticmethod
     def Degrees(horizontal: float, vertical: float, twist: float):
-        direction = Direction(horizontal, vertical)
-        swing_twist = SwingTwist(direction, twist)
+        horizontal_angle = Angle.Degrees(horizontal)
+        vertical_angle = Angle.Degrees(vertical)
+        twist_angle = Angle.Degrees(twist)
+
+        deg90 = Angle.Degrees(90)
+        deg180 = Angle.Degrees(180)
+        if vertical_angle > deg90 or vertical_angle < -deg90:
+            horizontal_angle += deg180
+            vertical_angle = deg180 - vertical_angle
+            twist_angle += deg180
+
+        direction = Direction(horizontal_angle, vertical_angle)
+        swing_twist = SwingTwist(direction, twist_angle)
+        return swing_twist
+    @staticmethod
+    def Radians(horizontal: float, vertical: float, twist: float):
+        horizontal_angle = Angle.Radians(horizontal)
+        vertical_angle = Angle.Radians(vertical)
+        twist_angle = Angle.Radians(twist)
+
+        deg90 = Angle.Radians(math.pi / 2)
+        deg180 = Angle.Radians(math.pi)
+        if vertical_angle > deg90 or vertical_angle < -deg90:
+            horizontal_angle += deg180
+            vertical_angle = deg180 - vertical_angle
+            twist_angle += deg180
+
+        direction = Direction(horizontal_angle, vertical_angle)
+        swing_twist = SwingTwist(direction, twist_angle)
         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
+        q = Quaternion.FromAngles(
+            -self.swing.vertical,
+            self.swing.horizontal,
+            self.twist
+        )
+        return q
+    @staticmethod
+    def FromQuaternion(q: Quaternion):
+        angles = Quaternion.ToAngles(q)
+        # direction = Direction(angles[0], angles[1])
+        # r: SwingTwist = SwingTwist(direction, angles[2])
+        r = SwingTwist.Degrees(
+            angles[0].InDegrees(),
+            angles[1].InDegrees(),
+            angles[2].InDegrees()
+        )
+        return r
+ 
+    def FromAngleAxis(angle: Angle, axis: Direction):
+        vectorAxis: Vector3 = axis.ToVector3();
+        q: Quaternion = Quaternion.FromAngleAxis(angle, vectorAxis);
+        return SwingTwist.FromQuaternion(q)
+
+
+    def __eq__(self, other) -> bool:
+        """! Check if this orientation is equal to the given orientation
+        @param other The orientation to check against
+        @return true if it is identical to the given orientation
+        @note This uses float comparison to check equality which may have strange
+        effects. Equality on floats should be avoided.
+        """
+        return (
+            self.swing == other.swing and
+            self.twist == other.twist
+        )    
+    
+    @staticmethod
+    def Angle(r1, r2) -> Angle:
+        q1: Quaternion = r1.ToQuaternion()
+        q2: Quaternion = r2.ToQuaternion()
+        angle: float = Quaternion.Angle(q1, q2)
+        return angle
+            
\ No newline at end of file
diff --git a/Vector.py b/Vector.py
new file mode 100644
index 0000000..10bcfd4
--- /dev/null
+++ b/Vector.py
@@ -0,0 +1,416 @@
+import math
+
+from Angle import *
+
+epsilon = 1E-05
+
+class Vector2:
+    def __init__(self, right: float = 0, up: float = 0):
+        """! A new 2-dimensional vector
+        @param right The distance in the right direction in meters
+        @param up The distance in the upward direction in meters
+        """
+        ## The right axis of the vector
+        self.right: float = right
+        ## The upward axis of the vector
+        self.up: float = up
+
+    def __eq__(self, other) -> bool:
+        """! Check if this vector is equal to the given vector
+        @param v The vector to check against
+        @return true if it is identical to the given vector
+        @note This uses float comparison to check equality which may have strange
+        effects. Equality on floats should be avoided.
+        """
+        return (
+            self.right == other.right and
+            self.up == other.up
+        )    
+
+    def SqrMagnitude(self) -> float:
+        """! The squared vector length
+        @return The squared vector length
+        @remark The squared length is computationally simpler than the real
+        length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
+        calculation of the squared root of C.
+        """
+        return self.right ** 2 + self.up ** 2
+
+    def Magnitude(self) -> float:
+        """! The vector length
+        @return The vector length
+        """
+        return math.sqrt(self.SqrMagnitude())
+
+    def Normalized(self):
+        """! Convert the vector to a length of 1
+        @return The vector normalized to a length of 1
+        """
+        length: float = self.Magnitude();
+        result = Vector2.zero
+        if length > epsilon:
+            result = self / length;
+        return result
+
+    def __neg__(self):
+        """! Negate te vector such that it points in the opposite direction
+        @return The negated vector
+        """
+        return Vector2(-self.right, -self.up)
+    
+    def __sub__(self, other):
+        """! Subtract a vector from this vector
+        @param other The vector to subtract from this vector
+        @return The result of this subtraction
+        """
+        return Vector2(
+            self.right - other.right,
+            self.up - other.up
+        )
+
+    def __add__(self, other):
+        """! Add a vector to this vector
+        @param other The vector to add to this vector
+        @return The result of the addition
+        """
+        return Vector2(
+            self.right + other.right,
+            self.up + other.up
+        )
+    
+    def Scale(self, scaling):
+        """! Scale the vector using another vector
+        @param scaling A vector with the scaling factors
+        @return The scaled vector
+        @remark Each component of the vector will be multiplied with the
+        matching component from the scaling vector.
+        """
+        return Vector2(
+            self.right * scaling.right,
+            self.up * scaling.up
+        )
+    
+    def __mul__(self, factor):
+        """! Scale the vector uniformly up
+        @param factor The scaling factor
+        @return The scaled vector
+        @remark Each component of the vector will be multiplied by the same factor.
+        """
+        return Vector2(
+            self.right * factor,
+            self.up * factor
+        )
+    
+    def __truediv__(self, factor):
+        """! Scale the vector uniformly down
+        @param f The scaling factor
+        @return The scaled vector
+        @remark Each component of the vector will be divided by the same factor.
+        """
+        return Vector2(
+            self.right / factor,
+            self.up / factor
+        )
+
+    @staticmethod
+    def Distance(v1, v2) -> float:
+        """! The distance between two vectors
+        @param v1 The first vector
+        @param v2 The second vector
+        @return The distance between the two vectors
+        """
+        return (v1 - v2).Magnitude()
+    
+    @staticmethod
+    def Dot(v1, v2) -> float:
+        """! The dot product of two vectors
+        @param v1 The first vector
+        @param v2 The second vector
+        @return The dot product of the two vectors
+        """
+        return v1.right * v2.right + v1.up * v2.up
+    
+    @staticmethod
+    def Angle(v1, v2) -> Angle:
+        """! The angle between two vectors
+        @param v1 The first vector
+        @param v2 The second vector
+        @return The angle between the two vectors
+        @remark This reterns an unsigned angle which is the shortest distance
+        between the two vectors. Use Vector3::SignedAngle if a signed angle is
+        needed.
+        """
+        denominator: float = math.sqrt(v1.SqrMagnitude() * v2.SqrMagnitude())
+        if denominator < epsilon:
+            return Angle.zero
+
+        dot: float = Vector2.Dot(v1, v2)
+        fraction: float = dot / denominator
+        # if math.nan(fraction):
+        #     return Angle.Degrees(fraction) # short cut to returning NaN universally
+
+        cdot: float = Float.Clamp(fraction, -1.0, 1.0)
+        r: float = math.acos(cdot)
+        return Angle.Radians(r);
+
+    @staticmethod
+    def SignedAngle(v1, v2) -> Angle:
+        """! The signed angle between two vectors
+        @param v1 The starting vector
+        @param v2 The ending vector
+        @param axis The axis to rotate around
+        @return The signed angle between the two vectors
+        """
+        sqr_mag_from: float = v1.SqrMagnitude()
+        sqr_mag_to: float = v2.SqrMagnitude()
+
+        if sqr_mag_from == 0 or sqr_mag_to == 0:
+            return Angle.zero
+        # if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
+        #     return nanf("");
+
+        angle_from = math.atan2(v1.up, v1.right)
+        angle_to = math.atan2(v2.up, v2.right)
+        return Angle.Radians(-(angle_to - angle_from))
+    
+    @staticmethod
+    def Lerp(v1, v2, f: float):
+        """! Lerp (linear interpolation) between two vectors
+        @param v1 The starting vector
+        @param v2 The ending vector
+        @param f The interpolation distance
+        @return The lerped vector
+        @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
+        1 matches vector *v2*. Value -1 is vector *v1* minus the difference
+        between *v1* and *v2* etc.
+        """
+        return v1 + (v2 - v1) * f
+
+
+## A vector with zero for all axis
+Vector2.zero = Vector2(0, 0)
+## A vector with one for all axis
+Vector2.one = Vector2(1, 1)
+## A normalized right-oriented vector
+Vector2.right = Vector2(1, 0)
+## A normalized left-oriented vector
+Vector2.left = Vector2(-1, 0)
+## A normalized up-oriented vector
+Vector2.up = Vector2(0, 1)
+## A normalized down-oriented vector
+Vector2.down = Vector2(0, -1)
+
+class Vector3(Vector2):
+    def __init__(self, right: float = 0, up: float = 0, forward: float = 0):
+        """! A new 3-dimensional vector
+        @param right The distance in the right direction in meters
+        @param up The distance in the upward direction in meters
+        @param forward The distance in the forward direction in meters
+        """
+        ## The right axis of the vector
+        self.right: float = right
+        ## The upward axis of the vector
+        self.up: float = up
+        ## The forward axis of the vector
+        self.forward: float = forward
+    
+    def __eq__(self, other) -> bool:
+        """! Check if this vector is equal to the given vector
+        @param v The vector to check against
+        @return true if it is identical to the given vector
+        @note This uses float comparison to check equality which may have strange
+        effects. Equality on floats should be avoided.
+        """
+        return (
+            self.right == other.right and
+            self.up == other.up and
+            self.forward == other.forward
+        )    
+
+    def SqrMagnitude(self) -> float:
+        """! The squared vector length
+        @return The squared vector length
+        @remark The squared length is computationally simpler than the real
+        length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
+        calculation of the squared root of C.
+        """
+        return self.right ** 2 + self.up ** 2 + self.forward ** 2
+    
+    def Normalized(self):
+        """! Convert the vector to a length of 1
+        @return The vector normalized to a length of 1
+        """
+        length: float = self.Magnitude();
+        result = Vector3()
+        if length > epsilon:
+            result = self / length;
+        return result
+
+    def __neg__(self):
+        """! Negate te vector such that it points in the opposite direction
+        @return The negated vector
+        """
+        return Vector3(-self.right, -self.up, -self.forward)
+    
+    def __sub__(self, other):
+        """! Subtract a vector from this vector
+        @param other The vector to subtract from this vector
+        @return The result of this subtraction
+        """
+        return Vector3(
+            self.right - other.right,
+            self.up - other.up,
+            self.forward - other.forward
+        )
+
+    def __add__(self, other):
+        """! Add a vector to this vector
+        @param other The vector to add to this vector
+        @return The result of the addition
+        """
+        return Vector3(
+            self.right + other.right,
+            self.up + other.up,
+            self.forward + other.forward
+        )
+    
+    def Scale(self, scaling):
+        """! Scale the vector using another vector
+        @param scaling A vector with the scaling factors
+        @return The scaled vector
+        @remark Each component of the vector will be multiplied with the
+        matching component from the scaling vector.
+        """
+        return Vector3(
+            self.right * scaling.right,
+            self.up * scaling.up,
+            self.forward * scaling.forward
+        )
+    
+    def __mul__(self, factor):
+        """! Scale the vector uniformly up
+        @param factor The scaling factor
+        @return The scaled vector
+        @remark Each component of the vector will be multiplied by the same factor.
+        """
+        return Vector3(
+            self.right * factor,
+            self.up * factor,
+            self.forward * factor
+        )
+    
+    def __truediv__(self, factor):
+        """! Scale the vector uniformly down
+        @param f The scaling factor
+        @return The scaled vector
+        @remark Each component of the vector will be divided by the same factor.
+        """
+        return Vector3(
+            self.right / factor,
+            self.up / factor,
+            self.forward / factor
+        )
+    
+    @staticmethod
+    def Dot(v1, v2) -> float:
+        """! The dot product of two vectors
+        @param v1 The first vector
+        @param v2 The second vector
+        @return The dot product of the two vectors
+        """
+        return v1.right * v2.right + v1.up * v2.up + v1.forward * v2.forward
+    
+    @staticmethod
+    def Cross(v1, v2):
+        """! The cross product of two vectors
+        @param v1 The first vector
+        @param v2 The second vector
+        @return The cross product of the two vectors
+        """
+        return Vector3(
+            v1.up * v2.forward - v1.forward * v2.up,
+            v1.forward * v2.right - v1.right * v2.forward,
+            v1.right * v2.up - v1.up * v2.right
+        )
+
+    def Project(self, other):
+        """! Project the vector on another vector
+        @param other The normal vecto to project on
+        @return The projected vector
+        """
+        sqrMagnitude = other.SqrMagnitude()
+        if sqrMagnitude < epsilon:
+            return Vector3.zero
+        else:
+            dot = Vector3.Dot(self, other)
+            return other * dot / sqrMagnitude;
+
+    def ProjectOnPlane(self, normal):
+        """! Project the vector on a plane defined by a normal orthogonal to the
+        plane.
+        @param normal The normal of the plane to project on
+        @return Teh projected vector
+        """
+        return self - self.Project(normal)
+    
+    @staticmethod
+    def Angle(v1, v2) -> Angle:
+        """! The angle between two vectors
+        @param v1 The first vector
+        @param v2 The second vector
+        @return The angle between the two vectors
+        @remark This reterns an unsigned angle which is the shortest distance
+        between the two vectors. Use Vector3::SignedAngle if a signed angle is
+        needed.
+        """
+        denominator: float = math.sqrt(v1.SqrMagnitude() * v2.SqrMagnitude())
+        if denominator < epsilon:
+            return Angle.zero
+
+        dot: float = Vector3.Dot(v1, v2)
+        fraction: float = dot / denominator
+        if math.isnan(fraction):
+            return Angle.Degrees(fraction) # short cut to returning NaN universally
+
+        cdot: float = Float.Clamp(fraction, -1.0, 1.0)
+        r: float = math.acos(cdot)
+        return Angle.Radians(r);
+
+    @staticmethod
+    def SignedAngle(v1, v2, axis) -> Angle:
+        """! The signed angle between two vectors
+        @param v1 The starting vector
+        @param v2 The ending vector
+        @param axis The axis to rotate around
+        @return The signed angle between the two vectors
+        """
+        # angle in [0,180]
+        angle: Angle = Vector3.Angle(v1, v2)
+
+        cross: Vector3 = Vector3.Cross(v1, v2)
+        b: float = Vector3.Dot(axis, cross)
+        sign:int = 0
+        if b < 0:
+            sign = -1
+        elif b > 0:
+            sign = 1
+
+        # angle in [-179,180]
+        return angle * sign
+    
+## A vector with zero for all axis
+Vector3.zero = Vector3(0, 0, 0)
+## A vector with one for all axis
+Vector3.one = Vector3(1, 1, 1)
+## A normalized forward-oriented vector
+Vector3.forward = Vector3(0, 0, 1)
+## A normalized back-oriented vector
+Vector3.back = Vector3(0, 0, -1)
+## A normalized right-oriented vector
+Vector3.right = Vector3(1, 0, 0)
+## A normalized left-oriented vector
+Vector3.left = Vector3(-1, 0, 0)
+## A normalized up-oriented vector
+Vector3.up = Vector3(0, 1, 0)
+## A normalized down-oriented vector
+Vector3.down = Vector3(0, -1, 0)
diff --git a/test/Angle_test.py b/test/Angle_test.py
index e74ab71..84a8111 100644
--- a/test/Angle_test.py
+++ b/test/Angle_test.py
@@ -1,14 +1,144 @@
+import unittest
 import sys
 from pathlib import Path
 # Add the project root to sys.path
 sys.path.append(str(Path(__file__).resolve().parent.parent))
                 
-import unittest
+from Angle import *
 
 class AngleTest(unittest.TestCase):
-    def test_one(self):
-        pass
+    def test_Construct(self):
+        degrees: float = 0
+        a: Angle = Angle.Degrees(degrees)
+        assert(a.InDegrees() == degrees)
 
+        degrees = -180
+        a = Angle.Degrees(degrees)
+        assert(a.InDegrees() == degrees)
+
+        degrees = 270
+        a = Angle.Degrees(degrees)
+        assert(a.InDegrees() == -90.0)
+
+    def test_Negate(self):
+        angle = 0
+        a:Angle  = Angle.Degrees(angle)
+        a = -a
+        assert(a.InDegrees() == angle)
+
+        angle = 90
+        a = Angle.Degrees(angle)
+        a = -a
+        assert(a.InDegrees() == -angle)
+
+    def test_Add(self):
+        a: Angle = Angle.Degrees(-45)
+        b: Angle = Angle.Degrees(45)
+        r: Angle = a + b
+        assert(r.InDegrees() == 0)
+
+    def test_Subtract(self):
+        a: Angle = Angle.Degrees(0)
+        b: Angle = Angle.Degrees(45)
+        r: Angle = a - b
+        assert(r.InDegrees() == -45)
+
+    def test_Compare(self):
+        a: Angle = Angle.Degrees(45)
+        r: bool = False
+
+        r = a > Angle.Degrees(0)
+        assert(r == True)
+
+        r = a > Angle.Degrees(90)
+        assert(r == False)
+
+        r = a > Angle.Degrees(-90)
+        assert(r == True)
+
+    def test_Normalize(self):
+        r = Angle()
+
+        r = Angle.Normalize(Angle.Degrees(90))
+        assert(r.InDegrees() == 90)
+
+        r = Angle.Normalize(Angle.Degrees(-90))
+        assert(r.InDegrees() == -90)
+
+        r = Angle.Normalize(Angle.Degrees(270))
+        assert(r.InDegrees() == -90)
+
+        r = Angle.Normalize(Angle.Degrees(270 + 360))
+        assert(r.InDegrees() == -90)
+
+        r = Angle.Normalize(Angle.Degrees(-270));
+        assert(r.InDegrees() == 90)
+
+        r = Angle.Normalize(Angle.Degrees(-270 - 360));
+        assert(r.InDegrees() == 90)
+
+        r = Angle.Normalize(Angle.Degrees(0));
+        assert(r.InDegrees() == 0)
+
+    def test_Clamp(self):
+        r = Angle()
+
+        r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(2))
+        assert(r.InDegrees() == 1)
+
+        r = Angle.Clamp(Angle.Degrees(-1), Angle.Degrees(0), Angle.Degrees(2))
+        assert(r.InDegrees() == 0)
+
+        r = Angle.Clamp(Angle.Degrees(3), Angle.Degrees(0), Angle.Degrees(2))
+        assert(r.InDegrees() == 2)
+
+        r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(0))
+        assert(r.InDegrees() == 0)
+
+        r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(0), Angle.Degrees(0))
+        assert(r.InDegrees() == 0)
+
+        r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(1), Angle.Degrees(-1))
+        assert(r.InDegrees() == 0)
+
+    def test_MoveTowards(self):
+        r = Angle();
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 30)
+        assert(r.InDegrees() == 30)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 90)
+        assert(r.InDegrees() == 90)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 180)
+        assert(r.InDegrees() == 90)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 270)
+        assert(r.InDegrees() == 90)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), -30)
+        assert(r.InDegrees() == 0)
+        
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -30)
+        assert(r.InDegrees() == 0)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -90)
+        assert(r.InDegrees() == 0)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -180)
+        assert(r.InDegrees() == 0)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -270)
+        assert(r.InDegrees() == 0)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 0)
+        assert(r.InDegrees() == 0)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 0)
+        assert(r.InDegrees() == 0)
+
+        r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 30)
+        assert(r.InDegrees() == 0)
 
 if __name__ == '__main__':
     unittest.main()
\ No newline at end of file
diff --git a/test/Direction_test.py b/test/Direction_test.py
new file mode 100644
index 0000000..69bd944
--- /dev/null
+++ b/test/Direction_test.py
@@ -0,0 +1,46 @@
+import unittest
+import sys
+from pathlib import Path
+# Add the project root to sys.path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+                
+from Direction import *
+
+class DirectionTest(unittest.TestCase):
+    def test_Compare(self):
+        d = Direction.Degrees(45, 135)
+        r = Direction(Angle.Degrees(45), Angle.Degrees(135))
+        assert(d == r)
+
+        r = Direction(Angle.Degrees(45 + 360), Angle.Degrees(135 - 360))
+        assert (d == r)
+
+    def test_Inverse(self):
+        d = Direction.Degrees(45, 135)
+        r = Direction.Degrees(-135, -135)
+        assert(-d == r)
+
+        d = Direction.Degrees(-45, -135)
+        r = Direction.Degrees(135, 135)
+        assert(-d == r)
+
+        d = Direction.Degrees(0, 0)
+        r = Direction.Degrees(180, 0)
+        assert(-d == r)
+
+        d = Direction.Degrees(0, 45)
+        r = Direction.Degrees(180, -45)
+        assert(-d == r)
+
+    def test_Equality(self):
+        d = Direction.Degrees(135, 45)
+        r = Direction.Degrees(135, 45)
+        assert(d == r)
+        r = Direction.Degrees(135 + 360, 45)
+        assert(d == r)
+        r = Direction.Degrees(135 - 360, 45)
+        assert(d == r)
+
+        d = Direction.Degrees(0, 45 + 180);
+        r = Direction.Degrees(180, -45)
+        assert(d == r)
diff --git a/test/Float_test.py b/test/Float_test.py
new file mode 100644
index 0000000..8ddb89b
--- /dev/null
+++ b/test/Float_test.py
@@ -0,0 +1,27 @@
+import unittest
+import sys
+from pathlib import Path
+# Add the project root to sys.path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+                
+from Float import *
+
+class FloatTest(unittest.TestCase):
+    def test_Clamp(self):
+        r = Float.Clamp(1, 0, 2)
+        assert(r == 1)
+
+        r = Float.Clamp(-1, 0, 2)
+        assert(r == 0)
+
+        r = Float.Clamp(3, 0, 2)
+        assert(r == 2)
+
+        r = Float.Clamp(1, 0, 0)
+        assert(r == 0)
+
+        r = Float.Clamp(0, 0, 0)
+        assert(r == 0)
+
+        r = Float.Clamp(0, 1, -1)
+        assert(r == 0)
diff --git a/test/Quaternion_test.py b/test/Quaternion_test.py
new file mode 100644
index 0000000..4a15845
--- /dev/null
+++ b/test/Quaternion_test.py
@@ -0,0 +1,106 @@
+import unittest
+import sys
+from pathlib import Path
+# Add the project root to sys.path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+                
+from Quaternion import *
+
+class QuaternionTest(unittest.TestCase):
+    def test_Equality(self):
+        q1 = Quaternion.identity
+        q2 = Quaternion(1, 0, 0, 0)
+
+        assert(q1 != q2)
+
+        q2 = Quaternion(0, 0, 0, 1)
+        assert(q1 == q2)
+
+    def test_FromAngles(self):
+        q = Quaternion.FromAngles(Angle.zero, Angle.zero, Angle.zero)
+        assert(q == Quaternion.identity)
+
+        q = Quaternion.FromAngles(Angle.Degrees(90), Angle.Degrees(90), Angle.Degrees(-90))
+        sqrt2_2 = math.sqrt(2) / 2
+        assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
+        
+    def test_ToAngles(self):
+        q1 = Quaternion.identity
+        angles = Quaternion.ToAngles(q1)
+        assert(angles == (Angle.zero, Angle.zero, Angle.zero))
+
+        q1 = Quaternion(1, 0, 0, 0)
+        angles = Quaternion.ToAngles(q1)
+        assert(angles == (Angle.zero, Angle.Degrees(180), Angle.zero))
+
+    def test_Degrees(self):
+        q = Quaternion.Degrees(0, 0, 0)
+        assert(q == Quaternion.identity)
+
+        q = Quaternion.Degrees(90, 90, -90)
+        sqrt2_2 = math.sqrt(2) / 2
+        assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
+
+    def test_Radians(self):
+        q = Quaternion.Radians(0, 0, 0)
+        assert(q == Quaternion.identity)
+
+        q = Quaternion.Radians(math.pi / 2, math.pi / 2, -math.pi / 2)
+        sqrt2_2 = math.sqrt(2) / 2
+        assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
+
+    def test_Multiply(self):
+        q1 = Quaternion.identity
+        q2 = Quaternion(1, 0, 0, 0)
+
+        r = q1 * q2;
+        assert(r == Quaternion(1, 0, 0, 0))
+   
+    def test_MultiplyVector(self):
+        v1 = Vector3.up
+
+        q1 = Quaternion.identity
+        v = q1 * v1
+        assert(v == Vector3(0, 1, 0))
+
+        q1 = Quaternion(1, 0, 0, 0)
+        v = q1 * v1
+        assert(v == Vector3(0, -1, 0))
+
+    def test_Angle(self):
+        q1 = Quaternion.identity
+        q2 = Quaternion.identity
+        r = Quaternion.Angle(q1, q2)
+        assert(r == Angle.zero)
+
+    def test_AngleAround(self):
+        axis = Direction.up
+        q1 = Quaternion.identity
+
+        angle = q1.AngleAround(axis)
+        assert(angle == Angle.Degrees(0))
+
+        sqrt2_2 = math.sqrt(2) / 2
+        q1 = Quaternion(0, sqrt2_2, -sqrt2_2, 0)
+        angle = q1.AngleAround(axis)
+        assert(angle == Angle.Degrees(180))
+
+        axis = Direction.zero
+        angle = q1.AngleAround(axis)
+        assert(math.isnan(angle.InDegrees()))
+
+    def test_RotationAround(self):
+        axis = Direction.up
+        q1 = Quaternion.identity
+
+        q = q1.RotationAround(axis)
+        assert(q == Quaternion.identity)
+
+        sqrt2_2 = math.sqrt(2) / 2
+        q1 = Quaternion(0, sqrt2_2, -sqrt2_2, 0)
+        q = q1.RotationAround(axis)
+        assert(q == Quaternion(0, 1, 0, 0))
+
+        axis = Direction.zero
+        q = q1.RotationAround(axis);
+        assert(math.isnan(q.x) and math.isnan(q.y) and math.isnan(q.z) and math.isnan(q.w))
diff --git a/test/Spherical_test.py b/test/Spherical_test.py
new file mode 100644
index 0000000..4c935d9
--- /dev/null
+++ b/test/Spherical_test.py
@@ -0,0 +1,442 @@
+import unittest
+import sys
+from pathlib import Path
+# Add the project root to sys.path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+                
+from Spherical import *
+
+class PolarTest(unittest.TestCase):
+    def test_FromVector2(self):
+        v: Vector2 = Vector2(0, 1)
+        p: Polar = Polar.FromVector2(v)
+
+        assert(p.distance == 1)
+        assert(p.direction.InDegrees() == 0)
+
+        v = Vector2(1, 0)
+        p = Polar.FromVector2(v)
+
+        assert(p.distance, 1)
+        assert(p.direction.InDegrees(), 90)
+
+        v = Vector2(-1, 1)
+        p = Polar.FromVector2(v)
+
+        assert(p.distance == math.sqrt(2))
+        assert(p.direction.InDegrees() == -45)
+
+    def test_Equality(self):
+        v1: Polar = Polar.Degrees(4, 5)
+
+        v2: Polar = Polar.Degrees(1, 2)
+        assert(v1 != v2)
+
+        v2 = Polar.Degrees(4, 5)
+        assert(v1 == v2)
+
+    def test_Magnitude(self):
+        v: Polar = Polar.Degrees(2, 30)
+        r: float = 0
+
+        r = v.Magnitude()
+        assert(r == 2)
+
+        v = Polar.Degrees(-2, -30)
+        r = v.Magnitude()
+        assert(r == 2)
+
+        v = Polar.Degrees(0, 0)
+        r = v.Magnitude()
+        assert(r == 0)
+
+    def test_Normalize(self):
+        v1: Polar = Polar.Degrees(2, 90)
+        r: Polar = Polar.zero
+
+        r = v1.Normalized()
+        assert(r == Polar.Degrees(1, 90))
+
+        v1 = Polar.Degrees(2, -90)
+        r = v1.Normalized()
+        assert(r == Polar.Degrees(1, -90))
+
+        v1 = Polar.Degrees(0, 0)
+        r = v1.Normalized()
+        assert(r == Polar.Degrees(0, 0))
+
+    def test_Negate(self):
+        v1: Polar = Polar.Degrees(2, 45)
+        r: Polar = Polar.zero
+
+        r = -v1
+        assert(r == Polar.Degrees(2, -135))
+
+        v1 = Polar.Degrees(2, -45)
+        r = -v1
+        assert(r == Polar.Degrees(2, 135))
+
+        v1 = Polar.Degrees(2, 0)
+        r = -v1
+        assert(r == Polar.Degrees(2, 180))
+
+        v1 = Polar.Degrees(0, 0)
+        r = -v1
+        assert(r == Polar.Degrees(0, 0))
+
+    def test_Subtract(self):
+        r: Polar = Polar.zero
+        v1: Polar = Polar.Degrees(4, 45)
+
+        v2: Polar = Polar.zero
+        r = v1 - v2
+        assert(r == v1)
+
+        r = v1
+        r -= v2
+        assert(r == v1)
+
+        v2: Polar = Polar.Degrees(1, 45)
+        r = v1 - v2
+        assert(r == Polar.Degrees(3, 45))
+
+        v2: Polar = Polar.Degrees(1, -135)
+        r = v1 - v2
+        assert(r == Polar.Degrees(5, 45))
+
+    def test_Addition(self):
+        r = Polar.zero
+
+        v1 = Polar.Degrees(4, 45)
+        v2 = Polar.zero
+        r = v1 + v2
+        assert(r == v1)
+
+        r = v1
+        r += v2
+        assert(r == v1)
+
+        v2 = Polar.Degrees(1, 45)
+        r = v1 + v2
+        assert(r == Polar.Degrees(5, 45))
+
+        v2 = Polar.Degrees(1, -135)
+        r = v1 + v2
+        assert(r == Polar.Degrees(3, 45))
+
+    def test_Multiply(self):
+        r: Polar = Polar.zero
+
+        v1: Polar = Polar.Degrees(4, 45)
+        r = v1 * 2
+        assert(r == Polar.Degrees(8, 45))
+
+        r = v1 * -2
+        assert(r == Polar.Degrees(8, -135))
+
+        r = v1 * 0
+        assert(r == Polar.Degrees(0, 0))
+
+    def test_Divide(self):
+        r: Polar.zero
+
+        v1 = Polar.Degrees(4, 45)
+        r = v1 / 2
+        assert(r == Polar.Degrees(2, 45))
+
+        r = v1 / -2
+        assert(r == Polar.Degrees(2, -135))
+
+    def test_Distance(self):
+        r: float = 0
+
+        v1 = Polar.Degrees(4, 45)
+        v2 = Polar.Degrees(1, -135)
+        r = Polar.Distance(v1, v2)
+        assert(r == 5)
+
+        v2 = Polar.Degrees(-1, -135)
+        r = Polar.Distance(v1, v2)
+        assert(r == 3)
+
+        v2 = Polar.Degrees(0, 0)
+        r = Polar.Distance(v1, v2)
+        assert(r == 4)
+
+    def test_Angle(self):
+        r = Angle.zero
+
+        v1 = Polar.Degrees(4, 45)
+        v2 = Polar.Degrees(1, -45)
+
+        r = Polar.Angle(v1, v2)
+        assert(r.InDegrees() == 90)
+
+        v2 = Polar.Degrees(1, 135)
+        r = Polar.Angle(v1, v2)
+        assert(r.InDegrees() == 90)
+
+        v2 = Polar.Degrees(1, 45)
+        r = Polar.Angle(v1, v2)
+        assert(r.InDegrees() == 0)
+
+    def test_SignedAngle(self):
+        r = Angle.zero
+
+        v1 = Polar.Degrees(4, 45)
+        v2 = Polar.Degrees(1, -45)
+
+        r = Polar.SignedAngle(v1, v2)
+        assert(r.InDegrees() == -90)
+
+        v2 = Polar.Degrees(1, 135)
+        r = Polar.SignedAngle(v1, v2)
+        assert(r.InDegrees() == 90)
+
+        v2 = Polar.Degrees(1, 45)
+        r = Polar.SignedAngle(v1, v2)
+        assert(r.InDegrees() == 0)
+
+    def test_Lerp(self):
+        r = Polar.zero
+
+        v1 = Polar.Degrees(5, 45)
+        v2 = Polar.Degrees(1, -45)
+
+        r = Polar.Lerp(v1, v2, 0)
+        assert(r == v1)
+  
+        r = Polar.Lerp(v1, v2, 1)
+        assert(r == v2)
+
+        r = Polar.Lerp(v1, v2, 0.5)
+        assert(r == Polar.Degrees(3, 0))
+
+        r = Polar.Lerp(v1, v2, -1)
+        assert(r == Polar.Degrees(9, 135))
+
+        r = Polar.Lerp(v1, v2, 2)
+        assert(r == Polar.Degrees(-3, -135))
+
+class SphericalTest(unittest.TestCase):
+    def test_FromVector3(self):
+        v: Vector3 = Vector3(0, 0, 1)
+        p: Spherical = Spherical.FromVector3(v)
+
+        assert(p.distance == 1)
+        assert(p.direction.horizontal.InDegrees() == 0)
+        assert(p.direction.vertical.InDegrees() == 0)
+
+        v = Vector3(1, 0, 0)
+        p = Spherical.FromVector3(v)
+
+        assert(p.distance, 1)
+        assert(p.direction.horizontal.InDegrees(), 90)
+        assert(p.direction.vertical.InDegrees(), 0)
+
+        v = Vector3(0, 1, 0)
+        p = Spherical.FromVector3(v)
+
+        assert(p.distance, 1)
+        assert(p.direction.horizontal.InDegrees(), 0)
+        assert(p.direction.vertical.InDegrees(), 90)
+
+        v = Vector3(-1, 0, 1)
+        p = Spherical.FromVector3(v)
+
+        assert(p.distance == math.sqrt(2))
+        assert(p.direction.horizontal.InDegrees() == -45)
+        assert(p.direction.vertical.InDegrees() == 0)
+
+    def test_Equality(self):
+        v1: Spherical = Spherical.Degrees(4, 5, 6)
+
+        v2: Spherical = Spherical.Degrees(1, 2, 3)
+        assert(v1 != v2)
+
+        v2 = Spherical.Degrees(4, 5, 6)
+        assert(v1 == v2)
+
+    def test_Magnitude(self):
+        v: Spherical = Spherical.Degrees(2, 30, 0)
+        r: float = 0
+
+        r = v.Magnitude()
+        assert(r == 2)
+
+        v = Spherical.Degrees(-2, -30, 0)
+        r = v.Magnitude()
+        assert(r == 2)
+
+        v = Spherical.Degrees(0, 0, 0)
+        r = v.Magnitude()
+        assert(r == 0)
+
+    def test_Normalize(self):
+        v1: Spherical = Spherical.Degrees(2, 90, 0)
+        r: Spherical = Spherical.zero
+
+        r = v1.Normalized()
+        assert(r == Spherical.Degrees(1, 90, 0))
+
+        v1 = Spherical.Degrees(2, -90, 0)
+        r = v1.Normalized()
+        assert(r == Spherical.Degrees(1, -90, 0))
+
+        v1 = Spherical.Degrees(0, 0, 0)
+        r = v1.Normalized()
+        assert(r == Spherical.Degrees(0, 0, 0))
+
+    def test_Negate(self):
+        v1: Spherical = Spherical.Degrees(2, 45, 0)
+        r: Spherical = Spherical.zero
+
+        r = -v1
+        assert(r == Spherical.Degrees(2, -135, 0))
+
+        v1 = Spherical.Degrees(2, -45, 0)
+        r = -v1
+        assert(r == Spherical.Degrees(2, 135, 0))
+
+        v1 = Spherical.Degrees(0, 0, 0)
+        r = -v1
+        assert(r == Spherical.Degrees(0, 180, 0))
+
+    def test_Subtract(self):
+        r: Spherical = Spherical.zero
+
+        v1: Spherical = Spherical.Degrees(4, 45, 0)
+        v2: Spherical = Spherical.zero
+        r = v1 - v2
+        assert(r == v1)
+
+        r = v1
+        r -= v2
+        assert(r == v1)
+
+        v2 = Spherical.Degrees(1, 45, 0)
+        r = v1 - v2
+        assert(r == Spherical(3, 45, 0))
+
+        v2 = Spherical.Degrees(1, -135, 0)
+        r = v1 - v2
+        assert(r == Spherical.Degrees(5, 45, 0))
+
+    def test_Addition(self):
+        v1 = Spherical(1, Direction.Degrees(45, 0))
+        v2 = Spherical.zero
+        r = Spherical.zero
+
+        r = v1 + v2
+        assert(r.distance == v1.distance)
+
+        r = v1
+        r += v2
+        assert(r.distance == v1.distance)
+
+        v2 = Spherical(1, Direction.Degrees(-45, 0))
+        r = v1 + v2
+        assert(r.distance == math.sqrt(2))
+        assert(r.direction.horizontal.InDegrees() == 0)
+        assert(r.direction.vertical.InDegrees() == 0)
+
+        v2 = Spherical(1, Direction.Degrees(0, 90))
+        r = v1 + v2
+        assert(r.distance == math.sqrt(2))
+        assert(r.direction.horizontal.InDegrees() == 45)
+        assert(r.direction.vertical.InDegrees() == 45)
+
+    def test_Multiply(self):
+        r = Spherical.zero
+
+        v1 = Spherical.Degrees(4, 45, 0)
+        r = v1 * 3
+        assert(r == Spherical.Degrees(12, 45, 0))
+
+        r = v1 * -3
+        assert(r == Spherical.Degrees(12, -135, 0))
+
+        r = v1 * 0
+        assert(r == Spherical.Degrees(0, 0, 0))
+
+    def test_Divide(self):
+        r: Spherical.zero
+
+        v1 = Spherical.Degrees(4, 45, 0)
+        r = v1 / 2
+        assert(r == Spherical.Degrees(2, 45, 0))
+
+        r = v1 / -2
+        assert(r == Spherical.Degrees(2, -135, 0))
+
+    def test_Distance(self):
+        r: float = 0
+
+        v1 = Spherical.Degrees(4, 45, 0)
+        v2 = Spherical.Degrees(1, -135, 0)
+        r = Spherical.Distance(v1, v2)
+        assert(r == 5)
+
+        v2 = Spherical.Degrees(-1, -135, 0)
+        r = Spherical.Distance(v1, v2)
+        assert(r == 3)
+
+        v2 = Spherical.Degrees(0, 0, 0)
+        r = Spherical.Distance(v1, v2)
+        assert(r == 4)
+
+    def test_Angle(self):
+        r = Angle.zero
+
+        v1 = Spherical.Degrees(4, 45, 0)
+        v2 = Spherical.Degrees(1, -45, 0)
+
+        r = Spherical.Angle(v1, v2)
+        assert(r.InDegrees() == 90)
+
+        v2 = Spherical.Degrees(1, 135, 0)
+        r = Spherical.Angle(v1, v2)
+        assert(r.InDegrees() == 90)
+
+        v2 = Spherical.Degrees(1, 45, 0)
+        r = Spherical.Angle(v1, v2)
+        assert(r.InDegrees() == 0)
+
+    def test_SignedAngle(self):
+        r = Angle.zero
+
+        v1 = Spherical.Degrees(4, 45, 0)
+        v2 = Spherical.Degrees(1, -45, 0)
+
+        r = Spherical.SignedAngle(v1, v2, Direction.up)
+        assert(r.InDegrees() == -90)
+
+        v2 = Spherical.Degrees(1, 135, 0)
+        r = Spherical.SignedAngle(v1, v2, Direction.up)
+        assert(r.InDegrees() == 90)
+
+        v2 = Spherical.Degrees(1, 45, 0)
+        r = Spherical.SignedAngle(v1, v2, Direction.up)
+        assert(r.InDegrees() == 0)
+
+    def test_Lerp(self):
+        r = Spherical.zero
+
+        v1 = Spherical.Degrees(5, 45, 0)
+        v2 = Spherical.Degrees(1, -45, 0)
+
+        r = Spherical.Lerp(v1, v2, 0)
+        assert(r == v1)
+  
+        r = Spherical.Lerp(v1, v2, 1)
+        assert(r == v2)
+
+        r = Spherical.Lerp(v1, v2, 0.5)
+        assert(r == Spherical.Degrees(3, 0, 0))
+
+        r = Spherical.Lerp(v1, v2, -1)
+        assert(r == Spherical.Degrees(9, 135, 0))
+
+        r = Spherical.Lerp(v1, v2, 2)
+        assert(r == Spherical.Degrees(-3, -135, 0))
+
diff --git a/test/SwingTwist_test.py b/test/SwingTwist_test.py
new file mode 100644
index 0000000..382723f
--- /dev/null
+++ b/test/SwingTwist_test.py
@@ -0,0 +1,99 @@
+import unittest
+import sys
+from pathlib import Path
+# Add the project root to sys.path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+                
+from SwingTwist import *
+
+class SwingTwistTest(unittest.TestCase):
+    def test_Constructor(self):
+        s = SwingTwist.Degrees(0, 0, 0)
+        assert(s == SwingTwist.Degrees(0, 0, 0))
+
+        s = SwingTwist.Degrees(0, 180, 0)
+        assert(s == SwingTwist.Degrees(180, 0, 180))
+
+        s = SwingTwist.Degrees(0, 180, 180)
+        assert(s == SwingTwist.Degrees(180, 0, 0))
+
+        s = SwingTwist.Degrees(270, 90, 0)
+        assert(s == SwingTwist.Degrees(-90, 90, 0))
+
+        s = SwingTwist.Degrees(270, 270, 0)
+        assert(s == SwingTwist.Degrees(-90, -90, 0))
+
+        s = SwingTwist.Degrees(270, 225, 0)
+        assert(s == SwingTwist.Degrees(90, -45, -180))
+
+        s = SwingTwist.Degrees(270, 0, 225)
+        assert(s == SwingTwist.Degrees(-90, 0, -135))
+
+    def test_FromQuaternion(self):
+        q = Quaternion.identity
+        r = SwingTwist.FromQuaternion(q)
+        assert(r == SwingTwist.Degrees(0, 0, 0))
+
+        q = Quaternion.Degrees(90, 0, 0)
+        r = SwingTwist.FromQuaternion(q)
+        assert(r == SwingTwist.Degrees(90, 0, 0))
+
+        q = Quaternion.Degrees(0, 90, 0)
+        r = SwingTwist.FromQuaternion(q)
+        assert(r == SwingTwist.Degrees(0, 90, 0))
+
+        q = Quaternion.Degrees(0, 0, 90)
+        r = SwingTwist.FromQuaternion(q)
+        assert(r == SwingTwist.Degrees(0, 0, 90))
+
+        q = Quaternion.Degrees(0, 180, 0)
+        r = SwingTwist.FromQuaternion(q)
+        assert(r == SwingTwist.Degrees(0, 180, 0))
+
+        q = Quaternion.Degrees(0, 135, 0)
+        r = SwingTwist.FromQuaternion(q)
+        assert(r == SwingTwist.Degrees(0, 135, 0))
+
+    def test_FromAngleAxis(self):
+        r = SwingTwist.FromAngleAxis(Angle.zero, Direction.up)
+        assert(r == SwingTwist.Degrees(0, 0, 0))
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.up)
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(90, 0, 0))
+        assert(angle.InDegrees() == 0)
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.up)
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(180, 0, 0))
+        assert(angle.InDegrees() == 0)
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.up);
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(-90, 0, 0))
+        assert(angle.InDegrees() == 0)
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.right)
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 90, 0))
+        assert(angle.InDegrees() == 0)
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.right)
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 180, 0))
+        assert(angle.InDegrees() == 0)
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.right)
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, -90, 0))
+        assert(angle.InDegrees() == 0)
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.forward)
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, 90))
+        assert(angle.InDegrees() == 0)
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.forward)
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, 180))
+        assert(angle.InDegrees() == 0)
+
+        r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.forward)
+        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, -90))
+        assert(angle.InDegrees() == 0)
+
+        # auto r16 = SwingTwist16::AngleAxis(13, Direction16::down);
+        # auto s16 = SwingTwist16::Degrees(-13, 0, 0);
+        # assert(SwingTwist16::Angle(r16, s16), Angle16::Degrees(0))
diff --git a/test/Vector_test.py b/test/Vector_test.py
new file mode 100644
index 0000000..621565a
--- /dev/null
+++ b/test/Vector_test.py
@@ -0,0 +1,551 @@
+import unittest
+import sys
+from pathlib import Path
+# Add the project root to sys.path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+                
+from Vector import *
+
+class Vector2Test(unittest.TestCase):
+    def test_Equality(self):
+        v1: Vector2 = Vector2(4, 5)
+
+        v2: Vector2 = Vector2(1, 2)
+        assert(v1 != v2)
+
+        v2 = Vector2(4, 5)
+        assert(v1 == v2)
+
+    def test_SqrMagnitude(self):
+        v: Vector2 = Vector2(1, 2)
+        m: float = 0;
+
+        m = v.SqrMagnitude()
+        assert(m == 5)
+
+        v = Vector2(-1, -2)
+        m = v.SqrMagnitude()
+        assert(m == 5)
+
+        v = Vector2(0, 0)
+        m = v.SqrMagnitude()
+        assert(m == 0)
+
+    def test_Magnitude(self):
+        v: Vector2 = Vector2(1, 2)
+        m: float = 0;
+
+        m = v.Magnitude()
+        assert(m == 2.23606797749979)
+
+        v = Vector2(-1, -2)
+        m = v.Magnitude()
+        assert(m == 2.23606797749979)
+
+        v = Vector2(0, 0)
+        m = v.Magnitude()
+        assert(m == 0)
+
+    def test_Normalize(self):
+        v1: Vector2 = Vector2(0, 2)
+        v: Vector2 = Vector2.zero
+
+        v = v1.Normalized()
+        assert(v == Vector2(0, 1))
+
+        v1 = Vector2(0, -2)
+        v = v1.Normalized()
+        assert(v == Vector2(0, -1))
+
+        v1 = Vector2(0, 0)
+        v = v1.Normalized()
+        assert(v == Vector2(0, 0))
+
+    def test_Negate(self):
+        v1: Vector2 = Vector2(4, 5)
+        r: Vector2 = Vector2.zero
+
+        r = -v1
+        assert(r == Vector2(-4, -5))
+
+        v1 = Vector2(-4, -5)
+        r = -v1
+        assert(r == Vector2(4, 5))
+
+        v1 = Vector2(0, 0)
+        r = -v1
+        assert(r == Vector2(0, 0))
+
+    def test_Subtract(self):
+        v1: Vector2 = Vector2(4, 5)
+        v2: Vector2 = Vector2(1, 2)
+        r: Vector2 = Vector2.zero
+
+
+        r = v1 - v2
+        assert(v1 - v2 == Vector2(3, 3))
+
+        v2 = Vector2(-1, -2)
+        r = v1 - v2
+        assert(r == Vector2(5, 7))
+
+        v2 = Vector2(4, 5)
+        r = v1 - v2
+        assert(r == Vector2(0, 0))
+
+        v2 = Vector2(0, 0)
+        r = v1 - v2
+        assert(r == Vector2(4, 5))
+
+    def test_Addition(self):
+        v1: Vector2 = Vector2(4, 5)
+        v2: Vector2 = Vector2(1, 2)
+        r: Vector2 = Vector2.zero
+
+        r = v1 + v2
+        assert(r == Vector2(5, 7))
+
+        v2 = Vector2(-1, -2)
+        r = v1 + v2
+        assert(r == Vector2(3, 3))
+
+        v2 = Vector2(0, 0)
+        r = v1 + v2
+        assert(r == Vector2(4, 5))
+
+    def test_Scale(self):
+        v1: Vector2 = Vector2(4, 5)
+        v2: Vector2 = Vector2(1, 2)
+        r: Vector2 = Vector2.zero
+
+        r = v1.Scale(v2)
+        assert(r == Vector2(4, 10))
+
+        v2 = Vector2(-1, -2)
+        r = v1.Scale(v2)
+        assert(r == Vector2(-4, -10))
+
+        v2 = Vector2(0, 0)
+        r = v1.Scale(v2)
+        assert(r == Vector2(0, 0))
+
+    def test_Multiply(self):
+        v1: Vector2 = Vector2(4, 5)
+        f: float = 3
+        r: Vector2 = Vector2.zero
+
+        r = v1 * f
+        assert(r == Vector2(12, 15))
+
+        f = -3
+        r = v1 * f
+        assert(r == Vector2(-12, -15))
+
+        f = 0
+        r = v1 * f
+        assert(r == Vector2(0, 0))
+
+    def test_Divide(self):
+        v1: Vector2 = Vector2(4, 5)
+        f: float = 2
+        v: Vector2 = Vector2.zero
+
+        v = v1 / f
+        assert(v == Vector2(2, 2.5))
+
+        f = -2
+        v = v1 / f
+        assert(v == Vector2(-2, -2.5))
+
+    def test_Distance(self):
+        v1: Vector2 = Vector2(4, 5)
+        v2: Vector2 = Vector2(1, 2)
+        r: float = 0
+
+        r = Vector2.Distance(v1, v2)
+        assert(r == 4.242640687119285)
+
+        v2 = Vector2(-1, -2)
+        r = Vector2.Distance(v1, v2)
+        assert(r == 8.602325267042627)
+
+        v2 = Vector2(0, 0)
+        r = Vector2.Distance(v1, v2)
+        assert(r == 6.4031242374328485)
+
+    def test_Dot(self):
+        v1: Vector2 = Vector2(4, 5)
+        v2: Vector2 = Vector2(1, 2)
+        r: float = 0
+
+        r = Vector2.Dot(v1, v2)
+        assert(r == 14)
+
+        v2 = Vector2(-1, -2)
+        r = Vector2.Dot(v1, v2)
+        assert(r, -14)
+
+        v2 = Vector2(0, 0)
+        r = Vector2.Dot(v1, v2)
+        assert(r, 0)
+
+    def test_Angle(self):
+        v1: Vector2 = Vector2(4, 5)
+        v2: Vector2 = Vector2(1, 2)
+        r: Angle = Angle.Degrees(0)
+
+        r = Vector2.Angle(v1, v2)
+        assert(r.InDegrees() == 12.094757077012119)
+
+        v2 = Vector2(-1, -2)
+        r = Vector2.Angle(v1, v2)
+        assert(r.InDegrees() == 167.9052429229879)
+
+        v2 = Vector2(0, 0)
+        r = Vector2.Angle(v1, v2)
+        assert(r.InDegrees() == 0)
+
+    def test_SignedAngle(self):
+        v1: Vector2 = Vector2(4, 5)
+        v2: Vector2 = Vector2(1, 2)
+        r: float = 0
+
+        r = Vector2.SignedAngle(v1, v2)
+        assert(r.InDegrees() == -12.094757077012098)
+
+        v2 = Vector2(-1, -2)
+        r = Vector2.SignedAngle(v1, v2)
+        assert(r.InDegrees() == 167.90524292298792)
+
+        v2 = Vector2(0, 0);
+        r = Vector2.SignedAngle(v1, v2)
+        assert(r.InDegrees() == 0)
+
+        v1 = Vector2(0, 1)
+        v2 = Vector2(1, 0)
+        r = Vector2.SignedAngle(v1, v2)
+        assert(r.InDegrees(), 90)
+
+        v1 = Vector2(0, 1)
+        v2 = Vector2(0, -1)
+        r = Vector2.SignedAngle(v1, v2)
+        assert(r.InDegrees(), 180)
+
+    def test_Lerp(self):
+        v1: Vector2 = Vector2(4, 5)
+        v2: Vector2 = Vector2(1, 2)
+        r: Vector2 = Vector2.zero
+
+        r = Vector2.Lerp(v1, v2, 0)
+        assert(Vector2.Distance(r, v1), 0)
+
+        r = Vector2.Lerp(v1, v2, 1)
+        assert(Vector2.Distance(r, v2), 0)
+
+        r = Vector2.Lerp(v1, v2, 0.5)
+        assert(Vector2.Distance(r, Vector2(2.5, 3.5)), 0)
+
+        r = Vector2.Lerp(v1, v2, -1)
+        assert(Vector2.Distance(r, Vector2(7.0, 8.0)), 0)
+
+        r = Vector2.Lerp(v1, v2, 2)
+        assert(Vector2.Distance(r, Vector2(-2.0, -1.0)), 0)
+
+class Vector3Test(unittest.TestCase):
+    def test_Equality(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        r: bool = False
+
+        r = v1 == v2
+        assert(r == False)
+
+        v2 = Vector3(4, 5, 6);
+        r = v1 == v2;
+        assert(r == True)
+
+    def test_SqrMagnitude(self):
+        v: Vector3 = Vector3(1, 2, 3)
+        m: float = 0;
+
+        m = v.SqrMagnitude()
+        assert(m == 14)
+
+        v = Vector3(-1, -2, -3)
+        m = v.SqrMagnitude()
+        assert(m == 14)
+
+        v = Vector3(0, 0, 0)
+        m = v.SqrMagnitude()
+        assert(m == 0)
+
+    def test_Magnitude(self):
+        v: Vector3 = Vector3(1, 2, 3)
+        m: float = 0;
+
+        m = v.Magnitude()
+        assert(m == 3.7416573867739413)
+
+        v = Vector3(-1, -2, -3)
+        m = v.Magnitude()
+        assert(m == 3.7416573867739413)
+
+        v = Vector3(0, 0, 0)
+        m = v.Magnitude()
+        assert(m == 0)
+
+    def test_Normalize(self):
+        r: bool = False
+
+        v1: Vector3 = Vector3(0, 2, 0)
+        v: Vector3 = Vector3.zero
+
+        v = v1.Normalized()
+        assert(v == Vector3(0, 1, 0))
+
+        v1 = Vector3(0, -2, 0)
+        v = v1.Normalized()
+        assert(v == Vector3(0, -1, 0))
+
+        v1 = Vector3(0, 0, 0)
+        v = v1.Normalized()
+        assert(v == Vector3(0, 0, 0))
+
+    def test_Negate(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v: Vector3 = Vector3.zero
+
+        v = -v1
+        assert(v == Vector3(-4, -5, -6))
+
+        v1 = Vector3(-4, -5, -6)
+        v = -v1
+        assert(v == Vector3(4, 5, 6))
+
+        v1 = Vector3(0, 0, 0)
+        v = -v1
+        assert(v == Vector3(0, 0, 0))
+
+    def test_Subtract(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        v3: Vector3 = Vector3.zero
+
+        v = v1 - v2
+        assert(v == Vector3(3, 3, 3))
+
+        v2 = Vector3(-1, -2, -3)
+        v = v1 - v2
+        assert(v == Vector3(5, 7, 9))
+
+        v2 = Vector3(4, 5, 6)
+        v = v1 - v2
+        assert(v == Vector3(0, 0, 0))
+
+        v2 = Vector3(0, 0, 0)
+        v = v1 - v2
+        assert(v == Vector3(4, 5, 6))
+
+    def test_Addition(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        v: Vector3 = Vector3.zero
+
+        v = v1 + v2
+        assert(v == Vector3(5, 7, 9))
+
+        v2 = Vector3(-1, -2, -3)
+        v = v1 + v2
+        assert(v == Vector3(3, 3, 3))
+
+        v2 = Vector3(0, 0, 0)
+        v = v1 + v2
+        assert(v == Vector3(4, 5, 6))
+
+    def test_Scale(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        v: Vector3 = Vector3.zero
+
+        v = v1.Scale(v2)
+        assert(v == Vector3(4, 10, 18))
+
+        v2 = Vector3(-1, -2, -3)
+        v = v1.Scale(v2)
+        assert(v == Vector3(-4, -10, -18))
+
+        v2 = Vector3(0, 0, 0)
+        v = v1.Scale(v2)
+        assert(v == Vector3(0, 0, 0))
+
+    def test_Multiply(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        f: float = 3
+        v: Vector3 = Vector3.zero
+
+        v = v1 * f
+        assert(v == Vector3(12, 15, 18))
+
+        f = -3
+        v = v1 * f
+        assert(v == Vector3(-12, -15, -18))
+
+        f = 0
+        v = v1 * f
+        assert(v == Vector3(0, 0, 0))
+
+    def test_Divide(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        f: float = 2
+        v: Vector3 = Vector3.zero
+
+        v = v1 / f
+        assert(v == Vector3(2, 2.5, 3))
+
+        f = -2
+        v = v1 / f
+        assert(v == Vector3(-2, -2.5, -3))
+
+    def test_Distance(self):
+        v1: Vector3 = Vector3(4, 5, 6);
+        v2: Vector3 = Vector3(1, 2, 3);
+        f: float = 0
+
+        f = Vector3.Distance(v1, v2);
+        assert(f == 5.196152422706632)
+
+        v2 = Vector3(-1, -2, -3);
+        f = Vector3.Distance(v1, v2);
+        assert(f == 12.449899597988733)
+
+        v2 = Vector3(0, 0, 0);
+        f = Vector3.Distance(v1, v2);
+        assert(f == 8.774964387392123)
+
+    def test_Dot(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        f: float = 0
+
+        f = Vector3.Dot(v1, v2)
+        assert(f == 32)
+
+        v2 = Vector3(-1, -2, -3)
+        f = Vector3.Dot(v1, v2)
+        assert(f, -32)
+
+        v2 = Vector3(0, 0, 0)
+        f = Vector3.Dot(v1, v2)
+        assert(f, 0)
+
+    def test_Cross(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        v: Vector3 = Vector3.zero
+
+        v = Vector3.Cross(v1, v2)
+        assert(v == Vector3(3, -6, 3))
+
+        v2 = Vector3(-1, -2, -3)
+        v = Vector3.Cross(v1, v2)
+        assert(v == Vector3(-3, 6, -3))
+
+        v2 = Vector3(0, 0, 0)
+        v = Vector3.Cross(v1, v2)
+        assert(v == Vector3(0, 0, 0))
+
+    def test_Project(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        v: Vector3 = Vector3.zero
+
+        v = v1.Project(v2)
+        assert(v == Vector3(2.2857142857142856, 4.571428571428571, 6.857142857142857))
+
+        v2 = Vector3(-1, -2, -3)
+        v = v1.Project(v2)
+        assert(v == Vector3(2.2857142857142856, 4.571428571428571, 6.857142857142857))
+
+        v2 = Vector3(0, 0, 0)
+        v = v1.Project(v2)
+        assert(v == Vector3(0, 0, 0))
+
+    def test_ProjectOnPlane(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        v: Vector3 = Vector3.zero
+
+        v = v1.ProjectOnPlane(v2)
+        assert(v == Vector3(1.7142857142857144, 0.4285714285714288, -0.8571428571428568))
+
+        v2 = Vector3(-1, -2, -3)
+        v = v1.ProjectOnPlane(v2)
+        assert(v == Vector3(1.7142857142857144, 0.4285714285714288, -0.8571428571428568))
+
+        v2 = Vector3(0, 0, 0)
+        v = v1.ProjectOnPlane(v2)
+        assert(v == Vector3(4, 5, 6))
+
+    def test_Angle(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        f: Angle = Angle.Degrees(0)
+
+        f = Vector3.Angle(v1, v2)
+        assert(f.InDegrees() == 12.933154491899135)
+
+        v2 = Vector3(-1, -2, -3)
+        f = Vector3.Angle(v1, v2)
+        assert(f.InDegrees() == 167.06684550810087)
+
+        v2 = Vector3(0, 0, 0)
+        f = Vector3.Angle(v1, v2)
+        assert(f.InDegrees() == 0)
+
+    def test_SignedAngle(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        v3: Vector3 = Vector3(7, 8, -9)
+        f: Angle = Angle.Degrees(0);
+        r: bool = False
+
+        f = Vector3.SignedAngle(v1, v2, v3)
+        assert(f.InDegrees() == -12.933154491899135)
+
+        v2 = Vector3(-1, -2, -3)
+        f = Vector3.SignedAngle(v1, v2, v3)
+        assert(f.InDegrees(), 167.06684550810087)
+
+        v2 = Vector3(0, 0, 0)
+        f = Vector3.SignedAngle(v1, v2, v3)
+        assert(f.InDegrees(), 0)
+
+        v2 = Vector3(1, 2, 3)
+
+        v3 = Vector3(-7, -8, 9)
+        f = Vector3.SignedAngle(v1, v2, v3)
+        assert(f.InDegrees(), 12.933154491899135)
+
+        v3 = Vector3(0, 0, 0)
+        f = Vector3.SignedAngle(v1, v2, v3)
+        assert(f.InDegrees(), 0)
+
+    def test_Lerp(self):
+        v1: Vector3 = Vector3(4, 5, 6)
+        v2: Vector3 = Vector3(1, 2, 3)
+        r: Vector3 = Vector3(0, 0, 0)
+
+        r = Vector3.Lerp(v1, v2, 0)
+        assert(Vector3.Distance(r, v1), 0)
+
+        r = Vector3.Lerp(v1, v2, 1)
+        assert(Vector3.Distance(r, v2), 0)
+
+        r = Vector3.Lerp(v1, v2, 0.5)
+        assert(Vector3.Distance(r, Vector3(2.5, 3.5, 4.5)), 0)
+
+        r = Vector3.Lerp(v1, v2, -1)
+        assert(Vector3.Distance(r, Vector3(7.0, 8.0, 9.0)), 0)
+
+        r = Vector3.Lerp(v1, v2, 2)
+        assert(Vector3.Distance(r, Vector3(-2.0, -1.0, 0.0)), 0)