From bf29c9478699f8d578dfbfe9cdea5614f445ff42 Mon Sep 17 00:00:00 2001
From: CoolCat467 <52022020+CoolCat467@users.noreply.github.com>
Date: Fri, 4 Nov 2022 22:12:07 -0500
Subject: [PATCH] Add rotate_axis and rotate_xyz

---
 matrix_functions.py | 34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/matrix_functions.py b/matrix_functions.py
index 21aab07..b5da438 100644
--- a/matrix_functions.py
+++ b/matrix_functions.py
@@ -38,6 +38,38 @@ def rotate_z(a):
         [0, 0, 0, 1]
     ])
 
+def rotate_axis(axis, angle):
+    c = math.cos(angle)
+    s = math.sin(angle)
+    omc = 1 - c
+    x, y, z = axis
+
+    return np.array([
+        [x*x*omc+c,   y*x*omc+z*s, x*z*omc-y*s, 0],
+        [x*y*omc-z*s, y*y*omc+c,   y*z*omc+x*s, 0],
+        [x*z*omc+y*s, y*z*omc-x*s, z*z*omc+c,   0],
+        [0,           0,           0,           1]
+    ])
+
+def rotate_xyz(angle_x: float,
+               angle_y: float,
+               angle_z: float) -> 'Matrix44':
+    cx = math.cos(angle_x)
+    sx = math.sin(angle_x)
+    cy = math.cos(angle_y)
+    sy = math.sin(angle_y)
+    cz = math.cos(angle_z)
+    sz = math.sin(angle_z)
+
+    sxsy = sx*sy
+    cxsy = cx*sy
+
+    return np.array([ 
+        [ cy*cz,  sxsy*cz+cx*sz, -cxsy*cz+sx*sz, 0],
+        [-cy*sz, -sxsy*sz+cx*cz,  cxsy*sz+sx*cz, 0],
+        [    sy,         -sx*cy,          cx*cy, 0],
+        [     0,              0,              0, 1]
+    ])
 
 def scale(n):
     return np.array([
@@ -45,4 +77,4 @@ def scale(n):
         [0, n, 0, 0],
         [0, 0, n, 0],
         [0, 0, 0, 1]
-    ])
\ No newline at end of file
+    ])