diff options
| -rw-r--r-- | chickadee/math/matrix.scm | 374 |
1 files changed, 193 insertions, 181 deletions
diff --git a/chickadee/math/matrix.scm b/chickadee/math/matrix.scm index 8ec7767..e411b48 100644 --- a/chickadee/math/matrix.scm +++ b/chickadee/math/matrix.scm @@ -274,17 +274,17 @@ m)) (define-inlinable (matrix3-transform! matrix v) - (let ((x (vec2-x v)) - (y (vec2-y v))) - (call-with-values (lambda () - (match matrix - (($ <matrix3> bv offset) + (match matrix + (($ <matrix3> bv (? exact-integer? offset)) + (let ((x (vec2-x v)) + (y (vec2-y v))) + (call-with-values (lambda () (bytestruct-unpack <matrix3> ((0) (1) (3) (4) (6) (7)) - bv offset)))) - (lambda (aa ab ba bb ca cb) - (set-vec2-x! v (+ (* x aa) (* y ba) ca)) - (set-vec2-y! v (+ (* x ab) (* y bb) cb)))))) + bv offset)) + (lambda (aa ab ba bb ca cb) + (set-vec2-x! v (+ (* x aa) (* y ba) ca)) + (set-vec2-y! v (+ (* x ab) (* y bb) cb)))))))) (define (matrix3-transform matrix v) (let ((new-v (vec2-copy v))) @@ -296,37 +296,39 @@ ;; ;; https://www.wikihow.com/Find-the-Inverse-of-a-3x3-Matrix (define (matrix3-inverse! matrix target) - (call-with-values (lambda () - (match matrix - (($ <matrix3> bv offset) + (match matrix + (($ <matrix3> bv (? exact-integer? offset)) + (call-with-values (lambda () (bytestruct-unpack <matrix3> - ((0) (1) (2) (3) (4) (5) (6) (7) (8)) - bv offset)))) - (lambda (a b c d e f g h i) - ;; Calculate the determinants of the minor matrices of the - ;; transpose of the original matrix. - (let* ((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. - (matrix3-init! target - (* a* invdet) (* (- b*) invdet) (* c* invdet) - (* (- d*) invdet) (* e* invdet) (* (- f*) invdet) - (* g* invdet) (* (- h*) invdet) (* i* invdet))))))) + ((0) (1) (2) + (3) (4) (5) + (6) (7) (8)) + bv offset)) + (lambda (a b c d e f g h i) + ;; Calculate the determinants of the minor matrices of the + ;; transpose of the original matrix. + (let* ((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. + (matrix3-init! target + (* a* invdet) (* (- b*) invdet) (* c* invdet) + (* (- d*) invdet) (* e* invdet) (* (- f*) invdet) + (* g* invdet) (* (- h*) invdet) (* i* invdet))))))))) (define (matrix3-inverse matrix) "Return the inverse of MATRIX." @@ -368,121 +370,121 @@ (+ (* a (- (* e i) (* f h))) (- (* b (- (* d i) (* f g)))) (* c (- (* d h) (* e g))))) - ;; 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 - (call-with-values (lambda () - (match matrix - (($ <matrix4> bv offset) + (match matrix + (($ <matrix4> bv (? exact-integer? offset)) + ;; 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 + (call-with-values (lambda () (bytestruct-unpack <matrix4> ((0) (1) (2) (3) (4) (5) (6) (7) (8) (9) (10) (11) (12) (13) (14) (15)) - bv offset)))) - (lambda (a b c d e f g h i j k l m n o p) - ;; 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* - (let* ((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* - (matrix4-init! 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))))))) + bv offset)) + (lambda (a b c d e f g h i j k l m n o p) + ;; 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* + (let* ((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* + (matrix4-init! 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." @@ -715,56 +717,66 @@ happens with respect to ORIGIN, a 2D vector." (error "expected inexact real" angle)) (f32vector-set! bv 0 (cos angle))) +(define (index? i) + (and (>= i 0) (<= most-positive-fixnum))) + +(define-inlinable (matrix4-blah matrix) + (match matrix + (($ <matrix4> bv (? exact-integer? offset)) + (bytestruct-unpack <matrix4> + ((0) (4) (12)) + bv offset)))) + (define-inlinable (matrix4-transform-x matrix x y) - (call-with-values (lambda () - (match matrix - (($ <matrix4> bv offset) + (match matrix + (($ <matrix4> bv (? exact-integer? offset)) + (call-with-values (lambda () (bytestruct-unpack <matrix4> ((0) (4) (12)) - bv offset)))) - (lambda (aa ba da) - (+ (* x aa) (* y ba) da)))) + bv offset)) + (lambda (aa ba da) + (+ (* x aa) (* y ba) da)))))) (define-inlinable (matrix4-transform-y matrix x y) - (call-with-values (lambda () - (match matrix - (($ <matrix4> bv offset) + (match matrix + (($ <matrix4> bv (? exact-integer? offset)) + (call-with-values (lambda () (bytestruct-unpack <matrix4> ((1) (5) (13)) - bv offset)))) - (lambda (ab bb db) - (+ (* x ab) (* y bb) db)))) + bv offset)) + (lambda (ab bb db) + (+ (* x ab) (* y bb) db)))))) (define-inlinable (matrix4-transform-vec2! matrix v) - (call-with-values (lambda () - (match matrix - (($ <matrix4> bv offset) + (match matrix + (($ <matrix4> bv (? exact-integer? offset)) + (call-with-values (lambda () (bytestruct-unpack <matrix4> ((0) (1) (4) (5) (12) (13)) - bv offset)))) - (lambda (aa ab ba bb da db) - (let ((x (vec2-x v)) - (y (vec2-y v))) - (set-vec2-x! v (+ (* x aa) (* y ba) da)) - (set-vec2-y! v (+ (* x ab) (* y bb) db)))))) + bv offset)) + (lambda (aa ab ba bb da db) + (let ((x (vec2-x v)) + (y (vec2-y v))) + (set-vec2-x! v (+ (* x aa) (* y ba) da)) + (set-vec2-y! v (+ (* x ab) (* y bb) db)))))))) (define-inlinable (matrix4-transform-vec3! matrix v) - (call-with-values (lambda () - (match matrix - (($ <matrix4> bv offset) + (match matrix + (($ <matrix4> bv (? exact-integer? offset)) + (call-with-values (lambda () (bytestruct-unpack <matrix4> ((0) (1) (2) (4) (5) (6) (8) (9) (10) (12) (13) (14)) - bv offset)))) - (lambda (aa ab ac ba bb bc ca cb cc da db dc) - (let ((x (vec3-x v)) - (y (vec3-y v)) - (z (vec3-z v))) - (set-vec3-x! v (+ (* x aa) (* y ba) (* z ca) da)) - (set-vec3-y! v (+ (* x ab) (* y bb) (* z cb) db)) - (set-vec3-z! v (+ (* x ac) (* y bc) (* z cc) dc)))))) + bv offset)) + (lambda (aa ab ac ba bb bc ca cb cc da db dc) + (let ((x (vec3-x v)) + (y (vec3-y v)) + (z (vec3-z v))) + (set-vec3-x! v (+ (* x aa) (* y ba) (* z ca) da)) + (set-vec3-y! v (+ (* x ab) (* y bb) (* z cb) db)) + (set-vec3-z! v (+ (* x ac) (* y bc) (* z cc) dc)))))))) (define-inlinable (matrix4-transform-vec2 matrix v) (let ((new-v (vec2-copy v))) |