summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--chickadee/math/matrix.scm374
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)))