;;; Chickadee Game Toolkit
;;; Copyright © 2016 David Thompson <davet@gnu.org>
;;;
;;; Chickadee is free software: you can redistribute it and/or modify
;;; it under the terms of the GNU General Public License as published
;;; by the Free Software Foundation, either version 3 of the License,
;;; or (at your option) any later version.
;;;
;;; Chickadee is distributed in the hope that it will be useful, but
;;; WITHOUT ANY WARRANTY; without even the implied warranty of
;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
;;; General Public License for more details.
;;;
;;; You should have received a copy of the GNU General Public License
;;; along with this program.  If not, see
;;; <http://www.gnu.org/licenses/>.

(define-module (chickadee math matrix)
  #:use-module (ice-9 format)
  #:use-module (ice-9 match)
  #:use-module (rnrs bytevectors)
  #:use-module (srfi srfi-9)
  #:use-module (srfi srfi-9 gnu)
  #:use-module (srfi srfi-4)
  #:use-module (system foreign)
  #:use-module (chickadee math)
  #:use-module (chickadee math quaternion)
  #:use-module (chickadee math rect)
  #:use-module (chickadee math vector)
  #: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
            matrix3-inverse!
            matrix3-inverse
            make-matrix4
            make-null-matrix4
            make-identity-matrix4
            matrix4?
            matrix4-mult!
            matrix4*
            matrix4-identity!
            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!
            matrix4-x
            matrix4-y
            matrix4-z))

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

;; 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)
  (let* ((bv (matrix3-bv matrix))
         (a (matrix3-ref bv 0 0))
         (b (matrix3-ref bv 0 1))
         (c (matrix3-ref bv 0 2))
         (d (matrix3-ref bv 1 0))
         (e (matrix3-ref bv 1 1))
         (f (matrix3-ref bv 1 2))
         (g (matrix3-ref bv 2 0))
         (h (matrix3-ref bv 2 1))
         (i (matrix3-ref bv 2 2))
         ;; Calculate the determinants of the minor matrices of the
         ;; inverse of the original matrix.
         (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.
        (init-matrix3 target
                      (* a* invdet) (* (- b*) invdet) (* c* invdet)
                      (* (- d*) invdet) (* e* invdet) (* (- f*) invdet)
                      (* g* invdet) (* (- h*) invdet) (* i* invdet)))))

(define (matrix3-inverse matrix)
  (let ((new (make-null-matrix3)))
    (matrix3-inverse! matrix new)
    new))

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

(define-record-type <matrix4>
  (%make-matrix4 bv ptr)
  matrix4?
  (bv matrix4-bv)
  (ptr matrix4-ptr))

(define-inlinable (matrix4-set! matrix row column x)
  (f32vector-set! matrix (+ (* row 4) column) x))

(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]]>"
            (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)

(define (init-matrix4 matrix
                      aa ab ac ad
                      ba bb bc bd
                      ca cb cc cd
                      da db dc dd)
  (let ((bv (matrix4-bv matrix)))
    (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)))
    (%make-matrix4 bv (bytevector->pointer bv))))

(define (make-matrix4 aa ab ac ad
                      ba bb bc bd
                      ca cb cc cd
                      da db dc dd)
  "Return a new 4x4 matrix initialized with the given 16 values in
column-major format."
  (let ((matrix (make-null-matrix4)))
    (init-matrix4 matrix
                  aa ab ac ad
                  ba bb bc bd
                  ca cb cc cd
                  da db dc dd)
    matrix))

(define (matrix4-mult! dest a b)
  "Multiply matrices A and B, storing the result in DEST."
  (let ((m1 (matrix4-bv a))
        (m2 (matrix4-bv b))
        (m3 (matrix4-bv dest)))
    (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (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)))
      (matrix4-set! m3 3 3
                   (+ (* m1-3-0 m2-0-3)
                      (* m1-3-1 m2-1-3)
                      (* m1-3-2 m2-2-3)
                      (* m1-3-3 m2-3-3))))))

(define (matrix4-copy matrix)
  (let ((bv (bytevector-copy (matrix4-bv matrix))))
    (%make-matrix4 bv (bytevector->pointer bv))))

(define (matrix4* . matrices)
  "Return the product of MATRICES."
  (match matrices
    (() (make-identity-matrix4))
    ((a b)
     (let ((result (make-identity-matrix4)))
       (matrix4-mult! result a b)
       result))
    ((first . rest)
     (let loop ((temp (make-identity-matrix4))
                (prev (matrix4-copy first))
                (matrices rest))
       (match matrices
         (() prev)
         ((current . rest)
          (matrix4-mult! temp prev current)
          (loop prev temp rest)))))))

(define (matrix4-identity! matrix)
  (init-matrix4 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
                0.0 0.0 0.0 1.0))

(define (make-identity-matrix4)
  (let ((matrix (make-null-matrix4)))
    (matrix4-identity! matrix)
    matrix))

(define (orthographic-projection! matrix left right top bottom near far)
  (init-matrix4 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))))
    (init-matrix4 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)))
    (init-matrix4 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)
    (init-matrix4 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)
    (init-matrix4 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)
    (init-matrix4 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)))
        (init-matrix4  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))
      (init-matrix4  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)))))
    (init-matrix4 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)))
    (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."
  (let ((matrix (make-null-matrix4)))
    (matrix4-rotate-x! matrix angle)
    matrix))

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

(define (matrix4-rotate-z! matrix angle)
  (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."
  (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."
  (let* ((bv (matrix4-bv matrix))
         (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)))
    (bytevector-fill! bv 0)
    (f32vector-set! bv 0 q)
    (f32vector-set! bv 1 r)
    (f32vector-set! bv 4 s)
    (f32vector-set! bv 5 t)
    (f32vector-set! bv 10 1.0)
    (f32vector-set! bv 12 (- x (* ox q) (* oy s)))
    (f32vector-set! bv 13 (- y (* ox r) (* oy t)))
    (f32vector-set! bv 15 1.0)))

(define-inlinable (matrix4-transform-x matrix x y)
  (let ((bv (matrix4-bv matrix)))
    (+ (* x (matrix4-ref bv 0 0))
       (* y (matrix4-ref bv 1 0))
       (matrix4-ref bv 3 0))))

(define-inlinable (matrix4-transform-y matrix x y)
  (let ((bv (matrix4-bv matrix)))
    (+ (* x (matrix4-ref bv 0 1))
       (* y (matrix4-ref bv 1 1))
       (matrix4-ref bv 3 1))))

(define-inlinable (matrix4-transform! matrix v)
  (let ((x (vec2-x v))
        (y (vec2-y v)))
    (set-vec2-x! v (matrix4-transform-x matrix x y))
    (set-vec2-y! v (matrix4-transform-y matrix x y))))

(define-inlinable (matrix4-x matrix)
  (matrix4-ref (matrix4-bv matrix) 3 0))

(define-inlinable (matrix4-y matrix)
  (matrix4-ref (matrix4-bv matrix) 3 1))

(define-inlinable (matrix4-z matrix)
  (matrix4-ref (matrix4-bv matrix) 3 2))