diff options
| -rw-r--r-- | chickadee/math/matrix.scm | 451 |
1 files changed, 345 insertions, 106 deletions
diff --git a/chickadee/math/matrix.scm b/chickadee/math/matrix.scm index 27b2fc7..17c5f49 100644 --- a/chickadee/math/matrix.scm +++ b/chickadee/math/matrix.scm @@ -27,13 +27,28 @@ #:use-module (chickadee math quaternion) #:use-module (chickadee math rect) #:use-module (chickadee math vector) - #:export (make-matrix4 + #:export (make-matrix3 + make-null-matrix3 + make-identity-matrix3 + matrix3? + matrix3-mult! + matrix3* + matrix3-identity! + matrix3-translate! + matrix3-translate + matrix3-scale! + matrix3-scale + matrix3-rotate! + matrix3-rotate + matrix3-transform! + matrix3-transform + make-matrix4 make-null-matrix4 + make-identity-matrix4 matrix4? matrix4-mult! matrix4* matrix4-identity! - make-identity-matrix4 orthographic-projection perspective-projection matrix4-translate! @@ -53,39 +68,257 @@ transform-y transform!)) -;; 4x4 matrix + +;;; +;;; 3x3 Matrix +;;; + +(define-record-type <matrix3> + (%make-matrix3 bv) + matrix3? + (bv matrix3-bv)) + +(define-inlinable (matrix3-set! matrix row column x) + (f32vector-set! matrix (+ (* row 3) column) x)) + +(define-inlinable (matrix3-ref matrix row column) + (f32vector-ref matrix (+ (* row 3) column))) + +(define (display-matrix3 matrix port) + (let ((m (matrix3-bv matrix))) + (format port + "#<matrix3 [[~f ~f ~f] [~f ~f ~f] [~f ~f ~f]]>" + (matrix3-ref m 0 0) + (matrix3-ref m 0 1) + (matrix3-ref m 0 2) + (matrix3-ref m 1 0) + (matrix3-ref m 1 1) + (matrix3-ref m 1 2) + (matrix3-ref m 2 0) + (matrix3-ref m 2 1) + (matrix3-ref m 2 2)))) + +(set-record-type-printer! <matrix3> display-matrix3) + +(define (init-matrix3 matrix + aa ab ac + ba bb bc + ca cb cc) + (let ((bv (matrix3-bv matrix))) + (matrix3-set! bv 0 0 aa) + (matrix3-set! bv 0 1 ab) + (matrix3-set! bv 0 2 ac) + (matrix3-set! bv 1 0 ba) + (matrix3-set! bv 1 1 bb) + (matrix3-set! bv 1 2 bc) + (matrix3-set! bv 2 0 ca) + (matrix3-set! bv 2 1 cb) + (matrix3-set! bv 2 2 cc))) + +(define (make-null-matrix3) + (let ((bv (make-f32vector 9))) + (%make-matrix3 bv))) + +(define (make-matrix3 aa ab ac + ba bb bc + ca cb cc) + "Return a new 3x3 matrix initialized with the given 9 values in +column-major format." + (let ((matrix (make-null-matrix3))) + (init-matrix3 matrix + aa ab ac + ba bb bc + ca cb cc) + matrix)) + +(define (matrix3-mult! dest a b) + "Multiply matrices A and B, storing the result in DEST." + (let ((m1 (matrix3-bv a)) + (m2 (matrix3-bv b)) + (m3 (matrix3-bv dest))) + (let ((m1-0-0 (matrix3-ref m1 0 0)) + (m1-0-1 (matrix3-ref m1 0 1)) + (m1-0-2 (matrix3-ref m1 0 2)) + (m1-1-0 (matrix3-ref m1 1 0)) + (m1-1-1 (matrix3-ref m1 1 1)) + (m1-1-2 (matrix3-ref m1 1 2)) + (m1-2-0 (matrix3-ref m1 2 0)) + (m1-2-1 (matrix3-ref m1 2 1)) + (m1-2-2 (matrix3-ref m1 2 2)) + (m2-0-0 (matrix3-ref m2 0 0)) + (m2-0-1 (matrix3-ref m2 0 1)) + (m2-0-2 (matrix3-ref m2 0 2)) + (m2-1-0 (matrix3-ref m2 1 0)) + (m2-1-1 (matrix3-ref m2 1 1)) + (m2-1-2 (matrix3-ref m2 1 2)) + (m2-2-0 (matrix3-ref m2 2 0)) + (m2-2-1 (matrix3-ref m2 2 1)) + (m2-2-2 (matrix3-ref m2 2 2))) + (matrix3-set! m3 0 0 + (+ (* m1-0-0 m2-0-0) + (* m1-0-1 m2-1-0) + (* m1-0-2 m2-2-0))) + (matrix3-set! m3 0 1 + (+ (* m1-0-0 m2-0-1) + (* m1-0-1 m2-1-1) + (* m1-0-2 m2-2-1))) + (matrix3-set! m3 0 2 + (+ (* m1-0-0 m2-0-2) + (* m1-0-1 m2-1-2) + (* m1-0-2 m2-2-2))) + (matrix3-set! m3 1 0 + (+ (* m1-1-0 m2-0-0) + (* m1-1-1 m2-1-0) + (* m1-1-2 m2-2-0))) + (matrix3-set! m3 1 1 + (+ (* m1-1-0 m2-0-1) + (* m1-1-1 m2-1-1) + (* m1-1-2 m2-2-1))) + (matrix3-set! m3 1 2 + (+ (* m1-1-0 m2-0-2) + (* m1-1-1 m2-1-2) + (* m1-1-2 m2-2-2))) + (matrix3-set! m3 2 0 + (+ (* m1-2-0 m2-0-0) + (* m1-2-1 m2-1-0) + (* m1-2-2 m2-2-0))) + (matrix3-set! m3 2 1 + (+ (* m1-2-0 m2-0-1) + (* m1-2-1 m2-1-1) + (* m1-2-2 m2-2-1))) + (matrix3-set! m3 2 2 + (+ (* m1-2-0 m2-0-2) + (* m1-2-1 m2-1-2) + (* m1-2-2 m2-2-2)))))) + +(define (matrix3-copy matrix) + (%make-matrix3 (bytevector-copy (matrix3-bv matrix)))) + +(define (matrix3* . matrices) + "Return the product of MATRICES." + (match matrices + (() (make-identity-matrix3)) + ((a b) + (let ((result (make-identity-matrix3))) + (matrix3-mult! result a b) + result)) + ((first . rest) + (let loop ((temp (make-identity-matrix3)) + (prev (matrix3-copy first)) + (matrices rest)) + (match matrices + (() prev) + ((current . rest) + (matrix3-mult! temp prev current) + (loop prev temp rest))))))) + +(define (matrix3-identity! matrix) + (init-matrix3 matrix + 1.0 0.0 0.0 + 0.0 1.0 0.0 + 0.0 0.0 1.0)) + +(define (make-identity-matrix3) + (let ((m (make-null-matrix3))) + (matrix3-identity! m) + m)) + +;; matrix3-transform! +(define (matrix3-translate! matrix v) + (init-matrix3 matrix + 1.0 0.0 0.0 + 0.0 1.0 0.0 + (vec2-x v) (vec2-y v) 1.0)) + +(define (matrix3-translate v) + (let ((m (make-null-matrix3))) + (matrix3-translate! m v) + m)) + +(define (matrix3-scale! matrix s) + (cond + ((number? s) + (init-matrix3 matrix + s 0.0 0.0 + 0.0 s 0.0 + 0.0 0.0 1.0)) + ((vec2? s) + (init-matrix3 matrix + (vec2-x s) 0.0 0.0 + 0.0 (vec2-y s) 0.0 + 0.0 0.0 1.0)))) + +(define (matrix3-scale s) + (let ((m (make-null-matrix3))) + (matrix3-scale! m s) + m)) + +(define (matrix3-rotate! matrix angle) + (let ((s (sin angle)) + (c (cos angle))) + (init-matrix3 matrix + c (- s) 0.0 + s c 0.0 + 0.0 0.0 1.0))) + +(define (matrix3-rotate angle) + (let ((m (make-null-matrix3))) + (matrix3-rotate! m angle) + m)) + +(define-inlinable (matrix3-transform! matrix v) + (let ((bv (matrix3-bv matrix)) + (x (vec2-x v)) + (y (vec2-y v))) + (set-vec2-x! v (+ (* x (matrix3-ref bv 0 0)) + (* y (matrix3-ref bv 1 0)) + (matrix3-ref bv 2 0))) + (set-vec2-y! v (+ (* x (matrix3-ref bv 0 1)) + (* y (matrix3-ref bv 1 1)) + (matrix3-ref bv 2 1))))) + +(define (matrix3-transform matrix v) + (let ((new-v (vec2-copy v))) + (matrix3-transform! matrix new-v) + new-v)) + + +;;; +;;; 4x4 Matrix +;;; + (define-record-type <matrix4> (%make-matrix4 bv ptr) matrix4? (bv matrix4-bv) (ptr matrix4-ptr)) -(define-inlinable (matrix-set! matrix row column x) +(define-inlinable (matrix4-set! matrix row column x) (f32vector-set! matrix (+ (* row 4) column) x)) -(define-inlinable (matrix-ref matrix row column) +(define-inlinable (matrix4-ref matrix row column) (f32vector-ref matrix (+ (* row 4) column))) (define (display-matrix4 matrix port) (let ((m (matrix4-bv matrix))) (format port "#<matrix4 [[~f ~f ~f ~f] [~f ~f ~f ~f] [~f ~f ~f ~f] [~f ~f ~f ~f]]>" - (matrix-ref m 0 0) - (matrix-ref m 0 1) - (matrix-ref m 0 2) - (matrix-ref m 0 3) - (matrix-ref m 1 0) - (matrix-ref m 1 1) - (matrix-ref m 1 2) - (matrix-ref m 1 3) - (matrix-ref m 2 0) - (matrix-ref m 2 1) - (matrix-ref m 2 2) - (matrix-ref m 2 3) - (matrix-ref m 3 0) - (matrix-ref m 3 1) - (matrix-ref m 3 2) - (matrix-ref m 3 3)))) + (matrix4-ref m 0 0) + (matrix4-ref m 0 1) + (matrix4-ref m 0 2) + (matrix4-ref m 0 3) + (matrix4-ref m 1 0) + (matrix4-ref m 1 1) + (matrix4-ref m 1 2) + (matrix4-ref m 1 3) + (matrix4-ref m 2 0) + (matrix4-ref m 2 1) + (matrix4-ref m 2 2) + (matrix4-ref m 2 3) + (matrix4-ref m 3 0) + (matrix4-ref m 3 1) + (matrix4-ref m 3 2) + (matrix4-ref m 3 3)))) (set-record-type-printer! <matrix4> display-matrix4) @@ -95,22 +328,22 @@ ca cb cc cd da db dc dd) (let ((bv (matrix4-bv matrix))) - (matrix-set! bv 0 0 aa) - (matrix-set! bv 0 1 ab) - (matrix-set! bv 0 2 ac) - (matrix-set! bv 0 3 ad) - (matrix-set! bv 1 0 ba) - (matrix-set! bv 1 1 bb) - (matrix-set! bv 1 2 bc) - (matrix-set! bv 1 3 bd) - (matrix-set! bv 2 0 ca) - (matrix-set! bv 2 1 cb) - (matrix-set! bv 2 2 cc) - (matrix-set! bv 2 3 cd) - (matrix-set! bv 3 0 da) - (matrix-set! bv 3 1 db) - (matrix-set! bv 3 2 dc) - (matrix-set! bv 3 3 dd))) + (matrix4-set! bv 0 0 aa) + (matrix4-set! bv 0 1 ab) + (matrix4-set! bv 0 2 ac) + (matrix4-set! bv 0 3 ad) + (matrix4-set! bv 1 0 ba) + (matrix4-set! bv 1 1 bb) + (matrix4-set! bv 1 2 bc) + (matrix4-set! bv 1 3 bd) + (matrix4-set! bv 2 0 ca) + (matrix4-set! bv 2 1 cb) + (matrix4-set! bv 2 2 cc) + (matrix4-set! bv 2 3 cd) + (matrix4-set! bv 3 0 da) + (matrix4-set! bv 3 1 db) + (matrix4-set! bv 3 2 dc) + (matrix4-set! bv 3 3 dd))) (define (make-null-matrix4) (let ((bv (make-f32vector 16))) @@ -135,114 +368,114 @@ column-major format." (let ((m1 (matrix4-bv a)) (m2 (matrix4-bv b)) (m3 (matrix4-bv dest))) - (let ((m1-0-0 (matrix-ref m1 0 0)) - (m1-0-1 (matrix-ref m1 0 1)) - (m1-0-2 (matrix-ref m1 0 2)) - (m1-0-3 (matrix-ref m1 0 3)) - (m1-1-0 (matrix-ref m1 1 0)) - (m1-1-1 (matrix-ref m1 1 1)) - (m1-1-2 (matrix-ref m1 1 2)) - (m1-1-3 (matrix-ref m1 1 3)) - (m1-2-0 (matrix-ref m1 2 0)) - (m1-2-1 (matrix-ref m1 2 1)) - (m1-2-2 (matrix-ref m1 2 2)) - (m1-2-3 (matrix-ref m1 2 3)) - (m1-3-0 (matrix-ref m1 3 0)) - (m1-3-1 (matrix-ref m1 3 1)) - (m1-3-2 (matrix-ref m1 3 2)) - (m1-3-3 (matrix-ref m1 3 3)) - (m2-0-0 (matrix-ref m2 0 0)) - (m2-0-1 (matrix-ref m2 0 1)) - (m2-0-2 (matrix-ref m2 0 2)) - (m2-0-3 (matrix-ref m2 0 3)) - (m2-1-0 (matrix-ref m2 1 0)) - (m2-1-1 (matrix-ref m2 1 1)) - (m2-1-2 (matrix-ref m2 1 2)) - (m2-1-3 (matrix-ref m2 1 3)) - (m2-2-0 (matrix-ref m2 2 0)) - (m2-2-1 (matrix-ref m2 2 1)) - (m2-2-2 (matrix-ref m2 2 2)) - (m2-2-3 (matrix-ref m2 2 3)) - (m2-3-0 (matrix-ref m2 3 0)) - (m2-3-1 (matrix-ref m2 3 1)) - (m2-3-2 (matrix-ref m2 3 2)) - (m2-3-3 (matrix-ref m2 3 3))) - (matrix-set! m3 0 0 + (let ((m1-0-0 (matrix4-ref m1 0 0)) + (m1-0-1 (matrix4-ref m1 0 1)) + (m1-0-2 (matrix4-ref m1 0 2)) + (m1-0-3 (matrix4-ref m1 0 3)) + (m1-1-0 (matrix4-ref m1 1 0)) + (m1-1-1 (matrix4-ref m1 1 1)) + (m1-1-2 (matrix4-ref m1 1 2)) + (m1-1-3 (matrix4-ref m1 1 3)) + (m1-2-0 (matrix4-ref m1 2 0)) + (m1-2-1 (matrix4-ref m1 2 1)) + (m1-2-2 (matrix4-ref m1 2 2)) + (m1-2-3 (matrix4-ref m1 2 3)) + (m1-3-0 (matrix4-ref m1 3 0)) + (m1-3-1 (matrix4-ref m1 3 1)) + (m1-3-2 (matrix4-ref m1 3 2)) + (m1-3-3 (matrix4-ref m1 3 3)) + (m2-0-0 (matrix4-ref m2 0 0)) + (m2-0-1 (matrix4-ref m2 0 1)) + (m2-0-2 (matrix4-ref m2 0 2)) + (m2-0-3 (matrix4-ref m2 0 3)) + (m2-1-0 (matrix4-ref m2 1 0)) + (m2-1-1 (matrix4-ref m2 1 1)) + (m2-1-2 (matrix4-ref m2 1 2)) + (m2-1-3 (matrix4-ref m2 1 3)) + (m2-2-0 (matrix4-ref m2 2 0)) + (m2-2-1 (matrix4-ref m2 2 1)) + (m2-2-2 (matrix4-ref m2 2 2)) + (m2-2-3 (matrix4-ref m2 2 3)) + (m2-3-0 (matrix4-ref m2 3 0)) + (m2-3-1 (matrix4-ref m2 3 1)) + (m2-3-2 (matrix4-ref m2 3 2)) + (m2-3-3 (matrix4-ref m2 3 3))) + (matrix4-set! m3 0 0 (+ (* m1-0-0 m2-0-0) (* m1-0-1 m2-1-0) (* m1-0-2 m2-2-0) (* m1-0-3 m2-3-0))) - (matrix-set! m3 0 1 + (matrix4-set! m3 0 1 (+ (* m1-0-0 m2-0-1) (* m1-0-1 m2-1-1) (* m1-0-2 m2-2-1) (* m1-0-3 m2-3-1))) - (matrix-set! m3 0 2 + (matrix4-set! m3 0 2 (+ (* m1-0-0 m2-0-2) (* m1-0-1 m2-1-2) (* m1-0-2 m2-2-2) (* m1-0-3 m2-3-2))) - (matrix-set! m3 0 3 + (matrix4-set! m3 0 3 (+ (* m1-0-0 m2-0-3) (* m1-0-1 m2-1-3) (* m1-0-2 m2-2-3) (* m1-0-3 m2-3-3))) - (matrix-set! m3 1 0 + (matrix4-set! m3 1 0 (+ (* m1-1-0 m2-0-0) (* m1-1-1 m2-1-0) (* m1-1-2 m2-2-0) (* m1-1-3 m2-3-0))) - (matrix-set! m3 1 1 + (matrix4-set! m3 1 1 (+ (* m1-1-0 m2-0-1) (* m1-1-1 m2-1-1) (* m1-1-2 m2-2-1) (* m1-1-3 m2-3-1))) - (matrix-set! m3 1 2 + (matrix4-set! m3 1 2 (+ (* m1-1-0 m2-0-2) (* m1-1-1 m2-1-2) (* m1-1-2 m2-2-2) (* m1-1-3 m2-3-2))) - (matrix-set! m3 1 3 + (matrix4-set! m3 1 3 (+ (* m1-1-0 m2-0-3) (* m1-1-1 m2-1-3) (* m1-1-2 m2-2-3) (* m1-1-3 m2-3-3))) - (matrix-set! m3 2 0 + (matrix4-set! m3 2 0 (+ (* m1-2-0 m2-0-0) (* m1-2-1 m2-1-0) (* m1-2-2 m2-2-0) (* m1-2-3 m2-3-0))) - (matrix-set! m3 2 1 + (matrix4-set! m3 2 1 (+ (* m1-2-0 m2-0-1) (* m1-2-1 m2-1-1) (* m1-2-2 m2-2-1) (* m1-2-3 m2-3-1))) - (matrix-set! m3 2 2 + (matrix4-set! m3 2 2 (+ (* m1-2-0 m2-0-2) (* m1-2-1 m2-1-2) (* m1-2-2 m2-2-2) (* m1-2-3 m2-3-2))) - (matrix-set! m3 2 3 + (matrix4-set! m3 2 3 (+ (* m1-2-0 m2-0-3) (* m1-2-1 m2-1-3) (* m1-2-2 m2-2-3) (* m1-2-3 m2-3-3))) - (matrix-set! m3 3 0 + (matrix4-set! m3 3 0 (+ (* m1-3-0 m2-0-0) (* m1-3-1 m2-1-0) (* m1-3-2 m2-2-0) (* m1-3-3 m2-3-0))) - (matrix-set! m3 3 1 + (matrix4-set! m3 3 1 (+ (* m1-3-0 m2-0-1) (* m1-3-1 m2-1-1) (* m1-3-2 m2-2-1) (* m1-3-3 m2-3-1))) - (matrix-set! m3 3 2 + (matrix4-set! m3 3 2 (+ (* m1-3-0 m2-0-2) (* m1-3-1 m2-1-2) (* m1-3-2 m2-2-2) (* m1-3-3 m2-3-2))) - (matrix-set! m3 3 3 + (matrix4-set! m3 3 3 (+ (* m1-3-0 m2-0-3) (* m1-3-1 m2-1-3) (* m1-3-2 m2-2-3) @@ -372,11 +605,13 @@ clipping plane NEAR and FAR." matrix)) (define (matrix4-rotate-x! matrix angle) - (init-matrix4 matrix - 1.0 0.0 0.0 0.0 - 0.0 (cos angle) (- (sin angle)) 0.0 - 0.0 (sin angle) (cos angle) 0.0 - 0.0 0.0 0.0 1.0)) + (let ((c (cos angle)) + (s (sin angle))) + (init-matrix4 matrix + 1.0 0.0 0.0 0.0 + 0.0 c (- s) 0.0 + 0.0 s c 0.0 + 0.0 0.0 0.0 1.0))) (define (matrix4-rotate-x angle) "Return a new matrix that rotates about the X axis by ANGLE radians." @@ -385,11 +620,13 @@ clipping plane NEAR and FAR." matrix)) (define (matrix4-rotate-y! matrix angle) - (init-matrix4 matrix - (cos angle) 0.0 (- (sin angle)) 0.0 - 0.0 1.0 0.0 0.0 - (sin angle) 0.0 (cos angle) 0.0 - 0.0 0.0 0.0 1.0)) + (let ((c (cos angle)) + (s (sin angle))) + (init-matrix4 matrix + c 0.0 (- s) 0.0 + 0.0 1.0 0.0 0.0 + s 0.0 c 0.0 + 0.0 0.0 0.0 1.0))) (define (matrix4-rotate-y angle) "Return a new matrix that rotates about the Y axis by ANGLE radians." @@ -398,11 +635,13 @@ clipping plane NEAR and FAR." matrix)) (define (matrix4-rotate-z! matrix angle) - (init-matrix4 matrix - (cos angle) (- (sin angle)) 0.0 0.0 - (sin angle) (cos angle) 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 0.0 0.0 1.0)) + (let ((c (cos angle)) + (s (sin angle))) + (init-matrix4 matrix + c (- s) 0.0 0.0 + s c 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0))) (define (matrix4-rotate-z angle) "Return a new matrix that rotates the Z axis by ANGLE radians." @@ -453,15 +692,15 @@ happens with respect to ORIGIN, a 2D vector." (define-inlinable (transform-x matrix x y) (let ((bv (matrix4-bv matrix))) - (+ (* x (matrix-ref bv 0 0)) - (* y (matrix-ref bv 1 0)) - (matrix-ref bv 3 0)))) + (+ (* x (matrix4-ref bv 0 0)) + (* y (matrix4-ref bv 1 0)) + (matrix4-ref bv 3 0)))) (define-inlinable (transform-y matrix x y) (let ((bv (matrix4-bv matrix))) - (+ (* x (matrix-ref bv 0 1)) - (* y (matrix-ref bv 1 1)) - (matrix-ref bv 3 1)))) + (+ (* x (matrix4-ref bv 0 1)) + (* y (matrix4-ref bv 1 1)) + (matrix4-ref bv 3 1)))) (define-inlinable (transform! matrix v) (let ((x (vec2-x v)) |