path: root/chickadee/math/matrix.scm

;;; Chickadee Game Toolkit
;;; Copyright © 2016, 2024 David Thompson <dthompson2@worcester.edu>
;;;
;;; Licensed under the Apache License, Version 2.0 (the "License");
;;; you may not use this file except in compliance with the License.
;;; You may obtain a copy of the License at
;;;
;;;    http://www.apache.org/licenses/LICENSE-2.0
;;;
;;; Unless required by applicable law or agreed to in writing, software
;;; distributed under the License is distributed on an "AS IS" BASIS,
;;; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
;;; See the License for the specific language governing permissions and
;;; limitations under the License.

(define-module (chickadee math matrix)
  #:use-module (ice-9 format)
  #:use-module (ice-9 match)
  #:use-module (rnrs bytevectors)
  #:use-module (srfi srfi-1)
  #:use-module (srfi srfi-9)
  #:use-module (srfi srfi-9 gnu)
  #:use-module (srfi srfi-4)
  #:use-module (system foreign)
  #:use-module (chickadee data bytestruct)
  #:use-module (chickadee math)
  #:use-module (chickadee math quaternion)
  #:use-module (chickadee math rect)
  #:use-module (chickadee math vector)
  #:export (<matrix3>
            make-matrix3
            make-null-matrix3
            make-identity-matrix3
            matrix3?
            matrix3=
            matrix3-copy!
            matrix3-copy
            matrix3-mult!
            matrix3*
            matrix3-identity!
            matrix3-translate!
            matrix3-translate
            matrix3-scale!
            matrix3-scale
            matrix3-rotate!
            matrix3-rotate
            matrix3-transform!
            matrix3-transform
            matrix3-inverse!
            matrix3-inverse

            <matrix4>
            make-matrix4
            make-null-matrix4
            make-identity-matrix4
            matrix4?
            matrix4=
            matrix4-copy!
            matrix4-copy
            matrix4-mult!
            matrix4*
            matrix4-identity!
            matrix4-inverse!
            matrix4-inverse
            orthographic-projection!
            orthographic-projection
            perspective-projection!
            perspective-projection
            look-at!
            look-at
            matrix4-translate!
            matrix4-translate
            matrix4-scale!
            matrix4-scale
            matrix4-rotate!
            matrix4-rotate
            matrix4-rotate-x!
            matrix4-rotate-x
            matrix4-rotate-y!
            matrix4-rotate-y
            matrix4-rotate-z!
            matrix4-rotate-z
            matrix4-2d-transform!
            matrix4-transform-x
            matrix4-transform-y
            matrix4-transform-vec2!
            matrix4-transform-vec3!
            matrix4-transform-vec2
            matrix4-transform-vec3
            matrix4-x
            matrix4-y
            matrix4-z))

(define-syntax define-square-matrix-type
  (lambda (stx)
    (syntax-case stx ()
      ((_ (<matrix> dimension)
          make-matrix make-null-matrix make-identity-matrix
          matrix? matrix=?
          matrix-ref matrix-set! matrix-init! matrix-identity!
          matrix-copy matrix-copy!
          matrix:* matrix-mul!)
       (and (identifier? #'<matrix>)
            (exact-integer? (syntax->datum #'dimension))
            (identifier? #'make-matrix)
            (identifier? #'make-null-matrix)
            (identifier? #'make-identity-matrix)
            (identifier? #'matrix?)
            (identifier? #'matrix-ref)
            (identifier? #'matrix-set!)
            (identifier? #'matrix-init!)
            (identifier? #'matrix-identity!)
            (identifier? #'matrix-copy)
            (identifier? #'matrix3-copy!)
            (identifier? #'matrix:*)
            (identifier? #'matrix-mul!))
       (let* ((dim (syntax->datum #'dimension))
              (n (* dim dim))
              (name (format #f "matrix~a" dim)))
         (define (index row column)
           (+ (* row dim) column))
         (define (matrix-map proc)
           (append-map
            (lambda (row)
              (map (lambda (column)
                     (proc row column))
                   (iota dim)))
            (iota dim)))
         (define (make-args prefix)
           (matrix-map
            (lambda (row column)
              (datum->syntax #f (string->symbol
                                 (format #f "~a:~a-~a" prefix row column))))))
         (let ((args (make-args 'e)))
           #`(begin
               (define-bytestruct <matrix> (array #,n f32)
                 #:printer
                 (lambda (m port)
                   (display "#<" port)
                   (display #,name port)
                   #,@(map (lambda (i)
                             #`(let ((x (bytestruct-ref <matrix> (#,i) m)))
                                 (display " " port)
                                 (display x port)))
                           (iota n))
                   (display ">" port)))
               (define-bytestruct-predicate matrix? <matrix>)
               (define (matrix=? a b)
                 (bytestruct=? <matrix> a b))
               (define-inlinable (matrix-init! m #,@args)
                 (match m
                   (($ <matrix> bv offset)
                    (bytestruct-pack!
                     <matrix>
                     #,(map (lambda (i)
                              #`((#,i) #,(list-ref args i)))
                            (iota n))
                     bv offset))))
               (define-inlinable (matrix-identity! m)
                 #,@(matrix-map
                     (lambda (row column)
                       (let ((i (index row column))
                             (x (if (= row column) 1.0 0.0)))
                         #`(bytestruct-set! <matrix> (#,i) m #,x)))))
               (define (make-matrix #,@args)
                 (bytestruct-alloc <matrix>
                                   #,@(map (lambda (i arg)
                                             #`((#,i) #,arg))
                                           (iota n) args)))
               (define (make-null-matrix)
                 (bytestruct-alloc <matrix>))
               (define (make-identity-matrix)
                 (let ((m (make-null-matrix)))
                   (matrix-identity! m)
                   m))
               (define (matrix-copy! src dst)
                 (bytestruct-copy! <matrix> src dst))
               (define (matrix-copy m)
                 (bytestruct-copy <matrix> m))
               (define-inlinable (matrix-mul! dst a b)
                 #,(let ((args-a (make-args 'a))
                         (args-b (make-args 'b)))
                     #`(match a
                         (($ <matrix> bv-a offset-a)
                          (match b
                            (($ <matrix> bv-b offset-b)
                             (call-with-values
                                 (lambda ()
                                   (bytestruct-unpack <matrix>
                                                      #,(map list (iota n))
                                                      bv-a offset-a))
                               (lambda #,args-a
                                 (call-with-values
                                     (lambda ()
                                       (bytestruct-unpack <matrix>
                                                          #,(map list (iota n))
                                                          bv-b offset-b))
                                   (lambda #,args-b
                                     (matrix-init!
                                      dst
                                      #,@(matrix-map
                                          (lambda (row column)
                                            (let ((i (index row column)))
                                              #`(+ #,@(map (lambda (column-a row-b)
                                                             #`(* #,(list-ref args-a
                                                                              (index row column-a))
                                                                  #,(list-ref args-b
                                                                              (index row-b column))))
                                                           (iota dim)
                                                           (iota dim)))))))))))))))))
               (define matrix:*
                 (case-lambda
                   (() (make-identity-matrix))
                   ((a) a)
                   ((a b)
                    (let ((dst (make-null-matrix)))
                      (matrix-mul! dst a b)
                      dst))
                   ((a b . rest)
                    (apply matrix:* (matrix:* a b) rest)))))))))))


;;;
;;; 3x3 Matrix
;;;

(define-square-matrix-type (<matrix3> 3)
  make-matrix3 make-null-matrix3 make-identity-matrix3
  matrix3? matrix3=
  matrix3-ref matrix3-set! matrix3-init! matrix3-identity!
  matrix3-copy matrix3-copy!
  matrix3* matrix3-mult!)

(define (matrix3-translate! matrix v)
  (matrix3-init! 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)
  (match s
    ((? real?)
     (matrix3-init! matrix
                    s    0.0  0.0
                    0.0  s    0.0
                    0.0  0.0  1.0))
    ((? vec2?)
     (matrix3-init! 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)))
    (matrix3-init! 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 ((x (vec2-x v))
        (y (vec2-y v)))
    (call-with-values (lambda ()
                        (match matrix
                          (($ <matrix3> bv offset)
                           (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))))))

(define (matrix3-transform matrix v)
  (let ((new-v (vec2-copy v)))
    (matrix3-transform! matrix new-v)
    new-v))

;; I honestly found this wikihow page very helpful in explaining the
;; process of inverting a 3x3 matrix:
;;
;; 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)
                         (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)))))))

(define (matrix3-inverse matrix)
  "Return the inverse of MATRIX."
  (let ((new (make-null-matrix3)))
    (matrix3-inverse! matrix new)
    new))


;;;
;;; 4x4 Matrix
;;;

(define-square-matrix-type (<matrix4> 4)
  make-matrix4 make-null-matrix4 make-identity-matrix4
  matrix4? matrix4=
  matrix4-ref matrix4-set! matrix4-init! matrix4-identity!
  matrix4-copy matrix4-copy!
  matrix4* matrix4-mult!)

;; 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)))))
  ;; 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)
                         (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)))))))

(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)
  (matrix4-init! matrix
                 (/ 2 (- right left)) 0.0 0.0 0.0
                 0.0 (/ 2 (- top bottom)) 0.0 0.0
                 0.0 0.0 (/ 2 (- far near)) 0.0
                 (- (/ (+ right left) (- right left)))
                 (- (/ (+ top bottom) (- top bottom)))
                 (- (/ (+ far near) (- far near)))
                 1.0))

(define (orthographic-projection left right top bottom near far)
  "Return a new matrix4 that represents an orthographic projection for
the horizontal clipping plane LEFT and RIGHT, the vertical clipping
plane TOP and BOTTOM, and the depth clipping plane NEAR and FAR."
  (let ((matrix (make-null-matrix4)))
    (orthographic-projection! matrix left right top bottom near far)
    matrix))

(define (perspective-projection! matrix field-of-vision aspect-ratio near far)
  (let ((f (cotan (/ field-of-vision 2))))
    (matrix4-init! matrix
                   (/ f aspect-ratio) 0 0 0
                   0 f 0 0
                   0 0 (/ (+ far near) (- near far)) -1
                   0 0 (/ (* 2 far near) (- near far)) 0)))

(define (perspective-projection field-of-vision aspect-ratio near far)
  "Return a new matrix4 that represents a perspective projection with
a FIELD-OF-VISION in radians, the desired ASPECT-RATIO, and the depth
clipping plane NEAR and FAR."
  (let ((matrix (make-null-matrix4)))
    (perspective-projection! matrix field-of-vision aspect-ratio near far)
    matrix))

(define (look-at! matrix eye at up)
  ;; TODO: Eliminate allocation of vectors.
  (let* ((zaxis (vec3-normalize (vec3- at eye)))
         (xaxis (vec3-normalize (vec3-cross zaxis (vec3-normalize up))))
         (yaxis (vec3-cross xaxis zaxis)))
    (matrix4-init! matrix
                   (vec3-x xaxis) (vec3-x yaxis) (- (vec3-x zaxis)) 0.0
                   (vec3-y xaxis) (vec3-y yaxis) (- (vec3-y zaxis)) 0.0
                   (vec3-z xaxis) (vec3-z yaxis) (- (vec3-z zaxis)) 0.0
                   (- (vec3-dot xaxis eye))
                   (- (vec3-dot yaxis eye))
                   (vec3-dot zaxis eye)
                   1.0)))

(define (look-at eye at up)
  "Return a new matrix4 that looks toward the position AT from the
position EYE, with the top of the viewport facing UP."
  (let ((matrix (make-null-matrix4)))
    (look-at! matrix eye at up)
    matrix))

(define (matrix4-translate! matrix v)
  (cond
   ((vec2? v)
    (matrix4-init! matrix
                   1.0 0.0 0.0 0.0
                   0.0 1.0 0.0 0.0
                   0.0 0.0 1.0 0.0
                   (vec2-x v) (vec2-y v) 0.0 1.0))
   ((rect? v)
    (matrix4-init! matrix
                   1.0 0.0 0.0 0.0
                   0.0 1.0 0.0 0.0
                   0.0 0.0 1.0 0.0
                   (rect-x v) (rect-y v) 0.0 1.0))
   ((vec3? v)
    (matrix4-init! matrix
                   1.0 0.0 0.0 0.0
                   0.0 1.0 0.0 0.0
                   0.0 0.0 1.0 0.0
                   (vec3-x v) (vec3-y v) (vec3-z v) 1.0))))

(define (matrix4-translate v)
  (let ((matrix (make-null-matrix4)))
    (matrix4-translate! matrix v)
    matrix))

(define (matrix4-scale! matrix s)
  (if (vec3? s)
      (let ((x (vec3-x s))
            (y (vec3-y s))
            (z (vec3-z s)))
        (matrix4-init!  matrix
                        x    0.0  0.0  0.0
                        0.0  y    0.0  0.0
                        0.0  0.0  z    0.0
                        0.0  0.0  0.0  1.0))
      (matrix4-init!  matrix
                      s    0.0  0.0  0.0
                      0.0  s    0.0  0.0
                      0.0  0.0  s    0.0
                      0.0  0.0  0.0  1.0)))

(define (matrix4-scale s)
  (let ((matrix (make-null-matrix4)))
    (matrix4-scale! matrix s)
    matrix))

(define (matrix4-rotate! matrix q)
  "Return a new rotation matrix for the quaternion Q."
  ;; Math based on this StackOverflow answer:
  ;; https://stackoverflow.com/a/1556470
  ;;
  ;; sqrt elimination thanks to this comment on the above answer:
  ;; https://stackoverflow.com/questions/1556260/convert-quaternion-rotation-to-rotation-matrix#comment74466994_1556470
  (let* ((x (quaternion-x q))
         (y (quaternion-y q))
         (z (quaternion-z q))
         (w (quaternion-w q))
         (n (/ 2.0 (+ (* x x) (* y y) (* z z) (* w w)))))
    (matrix4-init! matrix
                   (- 1.0 (* n y y) (* n z z))
                   (- (* n x y) (* n z w))
                   (+ (* n x z) (* n y w))
                   0.0
                   (+ (* n x y) (* n z w))
                   (- 1.0 (* n x x) (* n z z))
                   (- (* n y z) (* n x w))
                   0.0
                   (- (* n x z) (* n y w))
                   (+ (* n y z) (* n x w))
                   (- 1.0 (* n x x) (* n y y))
                   0.0
                   0.0
                   0.0
                   0.0
                   1.0)))

(define (matrix4-rotate q)
  (let ((matrix (make-null-matrix4)))
    (matrix4-rotate! matrix q)
    matrix))

(define (matrix4-rotate-x! matrix angle)
  (let ((c (cos angle))
        (s (sin angle)))
    (matrix4-init! 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."
  (let ((matrix (make-null-matrix4)))
    (matrix4-rotate-x! matrix angle)
    matrix))

(define (matrix4-rotate-y! matrix angle)
  (let ((c (cos angle))
        (s (sin angle)))
    (matrix4-init! 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."
  (let ((matrix (make-null-matrix4)))
    (matrix4-rotate-y! matrix angle)
    matrix))

(define (matrix4-rotate-z! matrix angle)
  (let ((c (cos angle))
        (s (sin angle)))
    (matrix4-init! 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."
  (let ((matrix (make-null-matrix4)))
    (matrix4-rotate-z! matrix angle)
    matrix))

(define %null-vec2 (vec2 0.0 0.0))
(define %default-scale (vec2 1.0 1.0))

(define* (matrix4-2d-transform! matrix
                                #:key
                                (origin %null-vec2)
                                (position %null-vec2)
                                (rotation 0.0)
                                (scale %default-scale)
                                (shear %null-vec2))
  "Store in MATRIX the transformation described by POSITION, a 2D
vector or rect, ROTATION, a scalar representing a rotation about the Z
axis, SCALE, a 2D vector, and SHEAR, a 2D vector.  The transformation
happens with respect to ORIGIN, a 2D vector."
  (unless (real? rotation)
    (error "expected real number for rotation" rotation))
  (let* ((x (vec2-x position))
         (y (vec2-y position))
         (ox (vec2-x origin))
         (oy (vec2-y origin))
         (sx (vec2-x scale))
         (sy (vec2-y scale))
         (kx (vec2-x shear))
         (ky (vec2-y shear))
         (c (cos rotation))
         (s (sin rotation))
         (q (* c sx))
         (r (+ (* s sx) (* c sy ky)))
         (s (- (* c sx kx) (* s sy)))
         (t (* c sy)))
    (matrix4-init! matrix
                   q r 0.0 0.0
                   s t 0.0 0.0
                   0.0 0.0 1.0 0.0
                   (- x (* ox q) (* oy s))
                   (- y (* ox r) (* oy t))
                   0.0 1.0)))

(lambda (bv angle)
  (unless (and (real? angle) (inexact? angle))
    (error "expected inexact real" angle))
  (f32vector-set! bv 0 (cos angle)))

(define-inlinable (matrix4-transform-x matrix x y)
  (call-with-values (lambda ()
                      (match matrix
                        (($ <matrix4> bv offset)
                         (bytestruct-unpack <matrix4>
                                            ((0) (4) (12))
                                            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)
                         (bytestruct-unpack <matrix4>
                                            ((1) (5) (13))
                                            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)
                         (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))))))

(define-inlinable (matrix4-transform-vec3! matrix v)
  (call-with-values (lambda ()
                      (match matrix
                        (($ <matrix4> bv offset)
                         (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))))))

(define-inlinable (matrix4-transform-vec2 matrix v)
  (let ((new-v (vec2-copy v)))
    (matrix4-transform-vec2! matrix new-v)
    new-v))

(define-inlinable (matrix4-transform-vec3 matrix v)
  (let ((new-v (vec3-copy v)))
    (matrix4-transform-vec3! matrix new-v)
    new-v))

(define-inlinable (matrix4-x matrix)
  (bytestruct-ref <matrix4> (12) matrix))

(define-inlinable (matrix4-y matrix)
  (bytestruct-ref <matrix4> (13) matrix))

(define-inlinable (matrix4-z matrix)
  (bytestruct-ref <matrix4> (14) matrix))