math: matrix: Add matrix4-inverse and matrix4-inverse! procedures.

2 files changed, 165 insertions, 0 deletions

diff --git a/chickadee/math/matrix.scm b/chickadee/math/matrix.scm
index c42629e..eab6cc6 100644
--- a/chickadee/math/matrix.scm
+++ b/chickadee/math/matrix.scm
@@ -52,6 +52,8 @@
             matrix4-mult!
             matrix4*
             matrix4-identity!
+            matrix4-inverse!
+            matrix4-inverse
             orthographic-projection!
             orthographic-projection
             perspective-projection!
@@ -578,6 +580,157 @@ column-major format."
     (matrix4-identity! matrix)
     matrix))
 
+;; Dear reader (potentially my future self),
+;;
+;; I'm not very good at linear algebra.  I got a D on my first test in
+;; linear algebra class in college and I spent the rest of the
+;; semester attending the professor's office hours whenever possible.
+;; I ended up turning things around and getting an A-.  Not bad.
+;; Anyway, what I'm trying to say is that it took me awhile to read up
+;; on this topic and turn it into working code.  So, I've tried to
+;; document the steps to inverting a 4x4 matrix in as much detail as
+;; this non-mathematician could manage.  Maybe you'll find it useful.
+(define (matrix4-inverse! matrix target)
+  ;; This is a helper.  It will make sense later.
+  (define (3x3-determinant a b c d e f g h i)
+    ;; The determinant of a 3x3 matrix is composed of the determinants
+    ;; of 3 minor 2x2 matrices:
+    ;;
+    ;; a----   --b--   ----c
+    ;; | e f - d | f + d e |
+    ;; | h i   g | i   g h |
+    (+ (* a (- (* e i) (* f h)))
+       (- (* b (- (* d i) (* f g))))
+       (* c (- (* d h) (* e g)))))
+  (let* ((bv (matrix4-bv matrix))
+         ;; Every element of the original matrix gets a letter:
+         ;;
+         ;; a b c d
+         ;; e f g h
+         ;; i j k l
+         ;; m n o p
+         (a (matrix4-ref bv 0 0))
+         (b (matrix4-ref bv 0 1))
+         (c (matrix4-ref bv 0 2))
+         (d (matrix4-ref bv 0 3))
+         (e (matrix4-ref bv 1 0))
+         (f (matrix4-ref bv 1 1))
+         (g (matrix4-ref bv 1 2))
+         (h (matrix4-ref bv 1 3))
+         (i (matrix4-ref bv 2 0))
+         (j (matrix4-ref bv 2 1))
+         (k (matrix4-ref bv 2 2))
+         (l (matrix4-ref bv 2 3))
+         (m (matrix4-ref bv 3 0))
+         (n (matrix4-ref bv 3 1))
+         (o (matrix4-ref bv 3 2))
+         (p (matrix4-ref bv 3 3))
+         ;; Calculate the determinants of the minor matrices of the
+         ;; transpose of the original matrix:
+         ;;
+         ;; a e i m
+         ;; b f j n
+         ;; c g k o
+         ;; d h l p
+         ;;
+         ;; A minor matrix is created by a picking an element of the
+         ;; original matrix and eliminating its row and column.  The
+         ;; remaining 9 elements form a 3x3 minor matrix.  There are
+         ;; 16 of them:
+         ;;
+         ;; a------   --e----  ----i--  ------m
+         ;; | f j n   b | j n  b f | n  b f j |
+         ;; | g k o   c | k o  c g | o  c g k |
+         ;; | h l p   d | l p  d h | p  d h l |
+         ;;
+         ;; | e i m   a | i m  a e | m  a e i |
+         ;; b------   --f----  ----j--  ------n
+         ;; | g k o   c | k o  c g | o  c g k |
+         ;; | h l p   d | l p  d h | p  d h l |
+         ;;
+         ;; | e i m   a | i m  a e | m  a e i |
+         ;; | f j n   b | j n  b f | n  b f j |
+         ;; c------   --g----  ----k--  ------o
+         ;; | h l p   d | l p  d h | p  d h l |
+         ;;
+         ;; | e i m   a | i m  a e | m  a e i |
+         ;; | f j n   b | j n  b f | n  b f j |
+         ;; | g k o   c | k o  c g | o  c g k |
+         ;; d------   --h----  ----l--  ------p
+         ;;
+         ;; The determinant of each 3x3 minor matrix is the
+         ;; combination of the determinants of the 3 2x2 minor-minor
+         ;; matrices within.
+         ;;
+         ;; I'll show just one of these for brevity's sake:
+         ;;
+         ;; f j n    f----   --j--   ----n
+         ;; g k o -> | k o   g | o   g k |
+         ;; h l p    | l p   h | p   h l |
+         ;;
+         ;; So the determinant for this 3x3 minor matrix is:
+         ;;
+         ;; f(kp - ol) - j(gp - oh) + n(gl - kh)
+         ;;
+         ;; The 3x3-determinant helper function takes care of this for
+         ;; all the minor matrices.
+         ;;
+         ;; From these values we create a new matrix of determinants.
+         ;; These matrix elements are given letters, too, but with
+         ;; asterisks at the end because they are more fun:
+         ;;
+         ;; a* b* c* d*
+         ;; e* f* g* h*
+         ;; i* j* k* l*
+         ;; m* n* o* p*
+         (a* (3x3-determinant f j n g k o h l p))
+         (b* (3x3-determinant b j n c k o d l p))
+         (c* (3x3-determinant b f n c g o d h p))
+         (d* (3x3-determinant b f j c g k d h l))
+         (e* (3x3-determinant e i m g k o h l p))
+         (f* (3x3-determinant a i m c k o d l p))
+         (g* (3x3-determinant a e m c g o d h p))
+         (h* (3x3-determinant a e i c g k d h l))
+         (i* (3x3-determinant e i m f j n h l p))
+         (j* (3x3-determinant a i m b j n d l p))
+         (k* (3x3-determinant a e m b f n d h p))
+         (l* (3x3-determinant a e i b f j d h l))
+         (m* (3x3-determinant e i m f j n g k o))
+         (n* (3x3-determinant a i m b j n c k o))
+         (o* (3x3-determinant a e m b f n c g o))
+         (p* (3x3-determinant a e i b f j c h k))
+         ;; Now we can calculate the determinant of the original
+         ;; matrix using the determinants of minor matrices calculated
+         ;; earlier.  The only trick here is that we used a transposed
+         ;; matrix before, so the determinant of the minor matrix of
+         ;; 'd' in the original matrix has been assigned the name
+         ;; 'm*', and so on.
+         (det (+ (* a a*) (- (* b e*)) (* c i*) (- (* d m*))))
+         (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)
+        (matrix4-identity! target)
+        ;; Multiply each element of the adjugate matrix by the inverse
+        ;; of the determinant to get the final inverse matrix.  Half
+        ;; of the values are inverted to form the adjugate matrix:
+        ;;
+        ;; + - + -      +a* -b* +c* -d*
+        ;; - + - +  ->  -e* +f* -g* +h*
+        ;; + - + -      +i* -j* +k* -l*
+        ;; - + - +      -m* +n* -o* +p*
+        (init-matrix4 target
+                      (* a* invdet) (* (- b*) invdet) (* c* invdet) (* (- d*) invdet)
+                      (* (- e*) invdet) (* f* invdet) (* (- g*) invdet) (* h* invdet)
+                      (* i* invdet) (* (- j*) invdet) (* k* invdet) (* (- l*) invdet)
+                      (* (- m*) invdet) (* n* invdet) (* (- o*) invdet) (* p* invdet)))))
+
+(define (matrix4-inverse matrix)
+  "Return the inverse of MATRIX."
+  (let ((new-matrix (make-null-matrix4)))
+    (matrix4-inverse! matrix new-matrix)
+    new-matrix))
+
 (define (orthographic-projection! matrix left right top bottom near far)
   (init-matrix4 matrix
                 (/ 2 (- right left)) 0.0 0.0 0.0
diff --git a/doc/api.texi b/doc/api.texi
index 9afc853..bebef33 100644
--- a/doc/api.texi
+++ b/doc/api.texi
@@ -1134,6 +1134,13 @@ the given @var{matrices}.
 Note: Remember that matrix multiplication is @strong{not} commutative!
 @end deffn
 
+@deffn {Procedure} matrix4-inverse matrix
+Return the inverse of @var{matrix}.
+
+A matrix multiplied by its inverse is the identity matrix, thought not
+always exactly due to the nature of floating point numbers.
+@end deffn
+
 @deffn {Procedure} orthographic-projection left right top bottom near far
 
 Return a new 4x4 matrix that represents an orthographic (2D)
@@ -1180,6 +1187,11 @@ Multiply the 4x4 matrix @var{a} by the 4x4 matrix @var{b} and store
 the result in the 4x4 matrix @var{dest}.
 @end deffn
 
+@deffn {Procedure} matrix4-inverse! matrix target
+Compute the inverse of @var{matrix} and store the result in
+@var{target}.
+@end deffn
+
 @deffn {Procedure} matrix4-translate! matrix x
 Modify @var{matrix} in-place to contain a translation by @var{x}, a 2D
 vector, a 3D vector, or a rectangle (in which case the bottom-left