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


;;;
;;; 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-bytevector 36)))
    (%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= m1 m2)
  "Return #t if M1 is the same matrix as M2"
  (equal? (matrix3-bv m1) (matrix3-bv m2)))

(define (matrix3-copy! src dest)
  "Copy the contents of matrix SRC to DEST."
  (bytevector-copy! (matrix3-bv src) 0 (matrix3-bv dest) 0 36))

(define (matrix3-copy matrix)
  "Return a new 3x3 matrix that is a copy of MATRIX."
  (%make-matrix3 (bytevector-copy (matrix3-bv 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* . 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
         ;; transpose 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)
  "Return the inverse of 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-bytevector 64)))
    (%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= m1 m2)
  "Return #t if M1 is the same matrix as M2"
  (equal? (matrix4-bv m1) (matrix4-bv m2)))

(define (matrix4-copy! src dest)
  "Copy the contents of matrix SRC to DEST."
  (bytevector-copy! (matrix4-bv src) 0 (matrix4-bv dest) 0 64))

(define (matrix4-copy matrix)
  "Return a new 4x4 matrix that is a copy of MATRIX."
  (let ((bv (bytevector-copy (matrix4-bv matrix))))
    (%make-matrix4 bv (bytevector->pointer bv))))

(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* . 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))

;; 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)))))
  (let* ((bv (matrix4-bv matrix))
         ;; 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
         (a (matrix4-ref bv 0 0))
         (b (matrix4-ref bv 0 1))
         (c (matrix4-ref bv 0 2))
         (d (matrix4-ref bv 0 3))
         (e (matrix4-ref bv 1 0))
         (f (matrix4-ref bv 1 1))
         (g (matrix4-ref bv 1 2))
         (h (matrix4-ref bv 1 3))
         (i (matrix4-ref bv 2 0))
         (j (matrix4-ref bv 2 1))
         (k (matrix4-ref bv 2 2))
         (l (matrix4-ref bv 2 3))
         (m (matrix4-ref bv 3 0))
         (n (matrix4-ref bv 3 1))
         (o (matrix4-ref bv 3 2))
         (p (matrix4-ref bv 3 3))
         ;; 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*
         (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*
        (init-matrix4 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)
  (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-vec2! matrix v)
  (let ((bv (matrix4-bv matrix))
        (x (vec2-x v))
        (y (vec2-y v)))
    (set-vec2-x! v (+ (* x (matrix4-ref bv 0 0))
                      (* y (matrix4-ref bv 1 0))
                      (matrix4-ref bv 3 0)))
    (set-vec2-y! v (+ (* x (matrix4-ref bv 0 1))
                      (* y (matrix4-ref bv 1 1))
                      (matrix4-ref bv 3 1)))))

(define-inlinable (matrix4-transform-vec3! matrix v)
  (let ((bv (matrix4-bv matrix))
        (x (vec3-x v))
        (y (vec3-y v))
        (z (vec3-z v)))
    (set-vec3-x! v (+ (* x (matrix4-ref bv 0 0))
                      (* y (matrix4-ref bv 1 0))
                      (* z (matrix4-ref bv 2 0))
                      (matrix4-ref bv 3 0)))
    (set-vec3-y! v (+ (* x (matrix4-ref bv 0 1))
                      (* y (matrix4-ref bv 1 1))
                      (* z (matrix4-ref bv 2 1))
                      (matrix4-ref bv 3 1)))
    (set-vec3-z! v (+ (* x (matrix4-ref bv 0 2))
                      (* y (matrix4-ref bv 1 2))
                      (* z (matrix4-ref bv 2 2))
                      (matrix4-ref bv 3 2)))))

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