From d6f3dc5a398040847d53bdeeb9d7ef5f5eab0dfc Mon Sep 17 00:00:00 2001
From: David Thompson <dthompson2@worcester.edu>
Date: Tue, 7 Sep 2021 18:27:55 -0400
Subject: math: matrix: Add matrix3-inverse! and matrix3-inverse.

---
 chickadee/math/matrix.scm | 47 +++++++++++++++++++++++++++++++++++++++++++++++
 doc/api.texi              |  9 +++++++++
 2 files changed, 56 insertions(+)

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
diff --git a/doc/api.texi b/doc/api.texi
index f6333d6..0a15586 100644
--- a/doc/api.texi
+++ b/doc/api.texi
@@ -1093,6 +1093,10 @@ Return a new 2D vector that is @var{v} as transformed by the 3x3
 matrix @var{matrix}.
 @end deffn
 
+@deffn {Procedure} matrix3-inverse matrix
+Return the inverse of @var{matrix}.
+@end deffn
+
 The following procedures perform in-place, destructive updates to 3x3
 matrix objects:
 
@@ -1125,6 +1129,11 @@ Modify the 2D vector @var{v} in-place to contain @var{v} as
 transformed by the 3x3 matrix @var{matrix}.
 @end deffn
 
+@deffn {Procedure} matrix3-inverse! matrix target
+Compute the inverse of @var{matrix} and store the results in
+@var{target}.
+@end deffn
+
 @subsubsection 4x4 Matrices
 
 @deffn {Procedure} make-matrix4 aa ab ac ad @
-- 
cgit v1.2.3