math: matrix: Add matrix3-inverse! and matrix3-inverse.

1 files changed, 47 insertions, 0 deletions

diff --git a/chickadee/math/matrix.scm b/chickadee/math/matrix.scm
index b79c94d..a075176 100644
--- a/chickadee/math/matrix.scm
+++ b/chickadee/math/matrix.scm
@@ -42,6 +42,8 @@
             matrix3-rotate
             matrix3-transform!
             matrix3-transform
+            matrix3-inverse!
+            matrix3-inverse
             make-matrix4
             make-null-matrix4
             make-identity-matrix4
@@ -289,6 +291,51 @@ column-major format."
     (matrix3-transform! matrix new-v)
     new-v))
 
+;; I honestly found this wikihow page very helpful in explaining the
+;; process of inverting a 3x3 matrix:
+;;
+;; https://www.wikihow.com/Find-the-Inverse-of-a-3x3-Matrix
+(define (matrix3-inverse! matrix target)
+  (let* ((bv (matrix3-bv matrix))
+         (a (matrix3-ref bv 0 0))
+         (b (matrix3-ref bv 0 1))
+         (c (matrix3-ref bv 0 2))
+         (d (matrix3-ref bv 1 0))
+         (e (matrix3-ref bv 1 1))
+         (f (matrix3-ref bv 1 2))
+         (g (matrix3-ref bv 2 0))
+         (h (matrix3-ref bv 2 1))
+         (i (matrix3-ref bv 2 2))
+         ;; Calculate the determinants of the minor matrices of the
+         ;; inverse of the original matrix.
+         (a* (- (* e i) (* f h)))
+         (b* (- (* b i) (* c h)))
+         (c* (- (* b f) (* c e)))
+         (d* (- (* d i) (* f g)))
+         (e* (- (* a i) (* c g)))
+         (f* (- (* a f) (* c d)))
+         (g* (- (* d h) (* e g)))
+         (h* (- (* a h) (* b g)))
+         (i* (- (* a e) (* b d)))
+         ;; Determinant and its inverse.
+         (det (+ (- (* a a*) (* b d*)) (* c g*)))
+         (invdet (/ 1.0 det)))
+    ;; If the matrix cannot be inverted (determinant of 0), then just
+    ;; bail out by resetting target to the identity matrix.
+    (if (= det 0.0)
+        (matrix3-identity! target)
+        ;; Multiply by the inverse of the determinant to get the final
+        ;; inverse matrix.  Every other value is inverted.
+        (init-matrix3 target
+                      (* a* invdet) (* (- b*) invdet) (* c* invdet)
+                      (* (- d*) invdet) (* e* invdet) (* (- f*) invdet)
+                      (* g* invdet) (* (- h*) invdet) (* i* invdet)))))
+
+(define (matrix3-inverse matrix)
+  (let ((new (make-null-matrix3)))
+    (matrix3-inverse! matrix new)
+    new))
+
 
 ;;;
 ;;; 4x4 Matrix