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