From f75b06869606036099a531bb0f54a0e3826b6272 Mon Sep 17 00:00:00 2001 From: David Thompson Date: Wed, 3 Aug 2022 17:06:53 -0400 Subject: math: matrix: Add matrix4-inverse and matrix4-inverse! procedures. --- chickadee/math/matrix.scm | 153 ++++++++++++++++++++++++++++++++++++++++++++++ doc/api.texi | 12 ++++ 2 files changed, 165 insertions(+) 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 -- cgit v1.2.3