math: matrix: Replace matrix4-transform! with matrix4-transform-{vec2,vec3}!

1 files changed, 41 insertions, 5 deletions

diff --git a/chickadee/math/matrix.scm b/chickadee/math/matrix.scm
index a04972f..c42629e 100644
--- a/chickadee/math/matrix.scm
+++ b/chickadee/math/matrix.scm
@@ -73,7 +73,10 @@
             matrix4-2d-transform!
             matrix4-transform-x
             matrix4-transform-y
-            matrix4-transform!
+            matrix4-transform-vec2!
+            matrix4-transform-vec3!
+            matrix4-transform-vec2
+            matrix4-transform-vec3
             matrix4-x
             matrix4-y
             matrix4-z))
@@ -808,11 +811,44 @@ happens with respect to ORIGIN, a 2D vector."
        (* y (matrix4-ref bv 1 1))
        (matrix4-ref bv 3 1))))
 
-(define-inlinable (matrix4-transform! matrix v)
-  (let ((x (vec2-x v))
+(define-inlinable (matrix4-transform-vec2! matrix v)
+  (let ((bv (matrix4-bv matrix))
+        (x (vec2-x v))
         (y (vec2-y v)))
-    (set-vec2-x! v (matrix4-transform-x matrix x y))
-    (set-vec2-y! v (matrix4-transform-y matrix x y))))
+    (set-vec2-x! v (+ (* x (matrix4-ref bv 0 0))
+                      (* y (matrix4-ref bv 1 0))
+                      (matrix4-ref bv 3 0)))
+    (set-vec2-y! v (+ (* x (matrix4-ref bv 0 1))
+                      (* y (matrix4-ref bv 1 1))
+                      (matrix4-ref bv 3 1)))))
+
+(define-inlinable (matrix4-transform-vec3! matrix v)
+  (let ((bv (matrix4-bv matrix))
+        (x (vec3-x v))
+        (y (vec3-y v))
+        (z (vec3-z v)))
+    (set-vec3-x! v (+ (* x (matrix4-ref bv 0 0))
+                      (* y (matrix4-ref bv 1 0))
+                      (* z (matrix4-ref bv 2 0))
+                      (matrix4-ref bv 3 0)))
+    (set-vec3-y! v (+ (* x (matrix4-ref bv 0 1))
+                      (* y (matrix4-ref bv 1 1))
+                      (* z (matrix4-ref bv 2 1))
+                      (matrix4-ref bv 3 1)))
+    (set-vec3-z! v (+ (* x (matrix4-ref bv 0 2))
+                      (* y (matrix4-ref bv 1 2))
+                      (* z (matrix4-ref bv 2 2))
+                      (matrix4-ref bv 3 2)))))
+
+(define-inlinable (matrix4-transform-vec2 matrix v)
+  (let ((new-v (vec2-copy v)))
+    (matrix4-transform-vec2! matrix new-v)
+    new-v))
+
+(define-inlinable (matrix4-transform-vec3 matrix v)
+  (let ((new-v (vec3-copy v)))
+    (matrix4-transform-vec3! matrix new-v)
+    new-v))
 
 (define-inlinable (matrix4-x matrix)
   (matrix4-ref (matrix4-bv matrix) 3 0))