path: root/sly/math/transform.scm

;;; Sly
;;; Copyright (C) 2014 David Thompson <dthompson2@worcester.edu>
;;;
;;; This program 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.
;;;
;;; This program 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/>.

;;; Commentary:
;;
;; 4x4 column-major transformation matrix.
;;
;;; Code:

(define-module (sly math transform)
  #:use-module (system foreign)
  #:use-module (ice-9 match)
  #:use-module (srfi srfi-1)
  #:use-module (srfi srfi-4)
  #:use-module (srfi srfi-9)
  #:use-module (srfi srfi-42)
  #:use-module (sly math)
  #:use-module (sly math vector)
  #:use-module (sly math quaternion)
  #:export (make-transform null-transform identity-transform
            transform? transform-matrix
            transpose transform-vector2
            transform-position
            transform->pointer
            transform+ transform* transform*!
            scale translate rotate-x rotate-y rotate-z rotate
            build-transform
            orthographic-projection perspective-projection
            look-at))

(define-record-type <transform>
  (%make-transform matrix ptr)
  transform?
  (matrix transform-matrix)
  (ptr transform-ptr set-transform-ptr!))

(define (make-4x4-matrix)
  (make-f32vector 16))

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

(define-inlinable (matrix-ref matrix row column)
  (f32vector-ref matrix (+ (* row 4) column)))

(define (make-transform aa ab ac ad
                        ba bb bc bd
                        ca cb cc cd
                        da db dc dd)
  "Return a new transform initialized with the given 16 values in
column-major format."
  (let ((matrix (make-4x4-matrix)))
    (matrix-set! matrix 0 0 aa)
    (matrix-set! matrix 0 1 ab)
    (matrix-set! matrix 0 2 ac)
    (matrix-set! matrix 0 3 ad)
    (matrix-set! matrix 1 0 ba)
    (matrix-set! matrix 1 1 bb)
    (matrix-set! matrix 1 2 bc)
    (matrix-set! matrix 1 3 bd)
    (matrix-set! matrix 2 0 ca)
    (matrix-set! matrix 2 1 cb)
    (matrix-set! matrix 2 2 cc)
    (matrix-set! matrix 2 3 cd)
    (matrix-set! matrix 3 0 da)
    (matrix-set! matrix 3 1 db)
    (matrix-set! matrix 3 2 dc)
    (matrix-set! matrix 3 3 dd)
    (%make-transform matrix #f)))

(define null-transform
  (%make-transform (make-4x4-matrix) #f))

(define identity-transform
  (make-transform 1 0 0 0
                  0 1 0 0
                  0 0 1 0
                  0 0 0 1))

(define (transpose transform)
  "Return a transform that is the transpose of TRANSFORM."
  (let ((m1 (transform-matrix transform))
        (m2 (make-4x4-matrix)))
    (do-ec (: r 4) (: c 4)
           (matrix-set! m2 c r
                        (matrix-ref m1 r c)))
    (%make-transform m2 #f)))

(define (transform-vector2 transform v)
  "Apply TRANSFORM to the 2D vector V."
  (let ((m (transform-matrix transform))
        (x (vx v))
        (y (vy v)))
    (vector2 (+ (* x (matrix-ref m 0 0))
                (* y (matrix-ref m 0 1))
                (matrix-ref m 0 3))
             (+ (* x (matrix-ref m 1 0))
                (* y (matrix-ref m 1 1))
                (matrix-ref m 1 3)))))

(define (transform-position transform)
  "Return a vector3 containing the positional data stored in
TRANSFORM."
  (let ((matrix (transform-matrix transform)))
    (vector3 (matrix-ref matrix 3 0)
             (matrix-ref matrix 3 1)
             (matrix-ref matrix 3 2))))

(define (transform+ . transforms)
  "Return the sum of TRANSFORM.  Return 'null-transform' if called
without any arguments."
  (define (add a b)
    (let ((m1 (transform-matrix a))
          (m2 (transform-matrix b))
          (m3 (make-4x4-matrix)))
      (do-ec (: r 4) (: c 4)
             (let ((x (+ (matrix-ref m1 r c)
                         (matrix-ref m2 r c))))
               (matrix-set! m3 r c x)))
      (%make-transform m3 #f)))
  (reduce add null-transform transforms))

(define (transform->pointer t)
  (let ((ptr (transform-ptr t)))
    (or ptr
        (let ((ptr (bytevector->pointer (transform-matrix t))))
          (set-transform-ptr! t ptr)
          ptr))))

(define (transform* . transforms)
  "Return the product of TRANSFORMS.  Return identity-transform if
called without any arguments."
  (define (mul a b)
    (let ((result (%make-transform (make-4x4-matrix) #f)))
      (transform*! result a b)
      result))
  (reduce mul identity-transform transforms))

;; This could be described as a loop, but the result would be much
;; slower matrix multiplication. To enable Guile's optimizer to unbox
;; floating point ops and reduce reads/writes, each matrix element is
;; bound as a local, the loop is completely unrolled, and matrix-ref
;; and matrix-set! are inlined.
(define (transform*! dest a b)
  (let ((m1 (transform-matrix a))
        (m2 (transform-matrix b))
        (m3 (transform-matrix dest)))
    (let ((m1-0-0 (matrix-ref m1 0 0))
          (m1-0-1 (matrix-ref m1 0 1))
          (m1-0-2 (matrix-ref m1 0 2))
          (m1-0-3 (matrix-ref m1 0 3))
          (m1-1-0 (matrix-ref m1 1 0))
          (m1-1-1 (matrix-ref m1 1 1))
          (m1-1-2 (matrix-ref m1 1 2))
          (m1-1-3 (matrix-ref m1 1 3))
          (m1-2-0 (matrix-ref m1 2 0))
          (m1-2-1 (matrix-ref m1 2 1))
          (m1-2-2 (matrix-ref m1 2 2))
          (m1-2-3 (matrix-ref m1 2 3))
          (m1-3-0 (matrix-ref m1 3 0))
          (m1-3-1 (matrix-ref m1 3 1))
          (m1-3-2 (matrix-ref m1 3 2))
          (m1-3-3 (matrix-ref m1 3 3))
          (m2-0-0 (matrix-ref m2 0 0))
          (m2-0-1 (matrix-ref m2 0 1))
          (m2-0-2 (matrix-ref m2 0 2))
          (m2-0-3 (matrix-ref m2 0 3))
          (m2-1-0 (matrix-ref m2 1 0))
          (m2-1-1 (matrix-ref m2 1 1))
          (m2-1-2 (matrix-ref m2 1 2))
          (m2-1-3 (matrix-ref m2 1 3))
          (m2-2-0 (matrix-ref m2 2 0))
          (m2-2-1 (matrix-ref m2 2 1))
          (m2-2-2 (matrix-ref m2 2 2))
          (m2-2-3 (matrix-ref m2 2 3))
          (m2-3-0 (matrix-ref m2 3 0))
          (m2-3-1 (matrix-ref m2 3 1))
          (m2-3-2 (matrix-ref m2 3 2))
          (m2-3-3 (matrix-ref m2 3 3)))

      (matrix-set! m3 0 0
                   (+ (* m1-0-0 m2-0-0)
                      (* m1-0-1 m2-1-0)
                      (* m1-0-2 m2-2-0)
                      (* m1-0-3 m2-3-0)))
      (matrix-set! m3 0 1
                   (+ (* m1-0-0 m2-0-1)
                      (* m1-0-1 m2-1-1)
                      (* m1-0-2 m2-2-1)
                      (* m1-0-3 m2-3-1)))
      (matrix-set! m3 0 2
                   (+ (* m1-0-0 m2-0-2)
                      (* m1-0-1 m2-1-2)
                      (* m1-0-2 m2-2-2)
                      (* m1-0-3 m2-3-2)))
      (matrix-set! m3 0 3
                   (+ (* m1-0-0 m2-0-3)
                      (* m1-0-1 m2-1-3)
                      (* m1-0-2 m2-2-3)
                      (* m1-0-3 m2-3-3)))
      (matrix-set! m3 1 0
                   (+ (* m1-1-0 m2-0-0)
                      (* m1-1-1 m2-1-0)
                      (* m1-1-2 m2-2-0)
                      (* m1-1-3 m2-3-0)))
      (matrix-set! m3 1 1
                   (+ (* m1-1-0 m2-0-1)
                      (* m1-1-1 m2-1-1)
                      (* m1-1-2 m2-2-1)
                      (* m1-1-3 m2-3-1)))
      (matrix-set! m3 1 2
                   (+ (* m1-1-0 m2-0-2)
                      (* m1-1-1 m2-1-2)
                      (* m1-1-2 m2-2-2)
                      (* m1-1-3 m2-3-2)))
      (matrix-set! m3 1 3
                   (+ (* m1-1-0 m2-0-3)
                      (* m1-1-1 m2-1-3)
                      (* m1-1-2 m2-2-3)
                      (* m1-1-3 m2-3-3)))
      (matrix-set! m3 2 0
                   (+ (* m1-2-0 m2-0-0)
                      (* m1-2-1 m2-1-0)
                      (* m1-2-2 m2-2-0)
                      (* m1-2-3 m2-3-0)))
      (matrix-set! m3 2 1
                   (+ (* m1-2-0 m2-0-1)
                      (* m1-2-1 m2-1-1)
                      (* m1-2-2 m2-2-1)
                      (* m1-2-3 m2-3-1)))
      (matrix-set! m3 2 2
                   (+ (* m1-2-0 m2-0-2)
                      (* m1-2-1 m2-1-2)
                      (* m1-2-2 m2-2-2)
                      (* m1-2-3 m2-3-2)))
      (matrix-set! m3 2 3
                   (+ (* m1-2-0 m2-0-3)
                      (* m1-2-1 m2-1-3)
                      (* m1-2-2 m2-2-3)
                      (* m1-2-3 m2-3-3)))
      (matrix-set! m3 3 0
                   (+ (* m1-3-0 m2-0-0)
                      (* m1-3-1 m2-1-0)
                      (* m1-3-2 m2-2-0)
                      (* m1-3-3 m2-3-0)))
      (matrix-set! m3 3 1
                   (+ (* m1-3-0 m2-0-1)
                      (* m1-3-1 m2-1-1)
                      (* m1-3-2 m2-2-1)
                      (* m1-3-3 m2-3-1)))
      (matrix-set! m3 3 2
                   (+ (* m1-3-0 m2-0-2)
                      (* m1-3-1 m2-1-2)
                      (* m1-3-2 m2-2-2)
                      (* m1-3-3 m2-3-2)))
      (matrix-set! m3 3 3
                   (+ (* 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 (translate! t . args)
  (match args
    ((? transform? v)
     (let ((x (vector2-x v))
           (y (vector2-y v)))
       (make-transform  1 0 0 0
                        0 1 0 0
                        0 0 1 0
                        x y 0 1)))
    ((? vector3? v)
     (let ((x (vector3-x v))
           (y (vector3-y v))
           (z (vector3-z v)))
       (make-transform 1 0 0 0
                       0 1 0 0
                       0 0 1 0
                       x y z 1)))))

(define translate
  (match-lambda
    ((? vector2? v)
     (let ((x (vector2-x v))
           (y (vector2-y v)))
       (make-transform  1 0 0 0
                        0 1 0 0
                        0 0 1 0
                        x y 0 1)))
    ((? vector3? v)
     (let ((x (vector3-x v))
           (y (vector3-y v))
           (z (vector3-z v)))
       (make-transform 1 0 0 0
                       0 1 0 0
                       0 0 1 0
                       x y z 1)))
    (v (error "Invalid translation vector: " v))))

(define scale
  (match-lambda
   ((? number? v)
    (make-transform v 0 0 0
                    0 v 0 0
                    0 0 v 0
                    0 0 0 1))
   ((? vector2? v)
    (let ((x (vector2-x v))
          (y (vector2-y v)))
      (make-transform x 0 0 0
                      0 y 0 0
                      0 0 1 0
                      0 0 0 1)))
   ((? vector3? v)
    (let ((x (vector3-x v))
          (y (vector3-y v))
          (z (vector3-z v)))
      (make-transform x 0 0 0
                      0 y 0 0
                      0 0 z 0
                      0 0 0 1)))
   (v (error "Invalid scaling vector: " v))))

(define (rotate-x angle)
  "Return a new transform that rotates the X axis by ANGLE radians."
  (make-transform 1 0           0               0
                  0 (cos angle) (- (sin angle)) 0
                  0 (sin angle) (cos angle)     0
                  0 0           0               1))

(define (rotate-y angle)
  "Return a new transform that rotates the Y axis by ANGLE radians."
  (make-transform (cos angle)     0 (sin angle) 0
                  0               1 0           0
                  (- (sin angle)) 0 (cos angle) 0
                  0               0 0           1))

(define (rotate-z angle)
  "Return a new transform that rotates the Z axis by ANGLE radians."
  (make-transform (cos angle) (- (sin angle)) 0 0
                  (sin angle) (cos angle)     0 0
                  0           0               1 0
                  0           0               0 1))

(define rotate
  (match-lambda
   (($ <quaternion> w x y z)
    (make-transform
     (- 1 (* 2 (square y)) (* 2 (square z)))
     (- (* 2 x y) (* 2 w z))
     (+ (* 2 x z) (* 2 w y))
     0
     (+ (* 2 x y) (* 2 w z))
     (- 1 (* 2 (square x)) (* 2 (square z)))
     (- (* 2 y z) (* 2 w x))
     0
     (- (* 2 x z) (* 2 w y))
     (+ (* 2 y z) (* 2 w x))
     (- 1 (* 2 (square x)) (* 2 (square y)))
     0 0 0 0 1))))

(define build-transform
  (let ((%scale scale))
    (lambda* (#:optional #:key (position (vector3 0 0 0))
              (scale 1) (rotation null-quaternion))
      (transform* (translate position)
                  (%scale scale)
                  (rotate rotation)))))

(define (orthographic-projection left right top bottom near far)
  "Return a new transform that represents an orthographic projection
for the vertical clipping plane LEFT and RIGHT, the horizontal
clipping plane TOP and BOTTOM, and the depth clipping plane NEAR and
FAR."
  (make-transform (/ 2 (- right left)) 0 0 0
                  0 (/ 2 (- top bottom)) 0 0
                  0 0 (/ 2 (- far near)) 0
                  (- (/ (+ right left) (- right left)))
                  (- (/ (+ top bottom) (- top bottom)))
                  (- (/ (+ far near) (- far near)))
                  1))

(define (perspective-projection field-of-vision aspect-ratio near far)
  "Return a new transform that represents a perspective projection
with a FIELD-OF-VISION in degrees, the desired ASPECT-RATIO, and the
depth clipping plane NEAR and FAR."
  (let ((f (cotan (/ (degrees->radians field-of-vision) 2))))
    (make-transform (/ 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* (look-at eye center #:optional (up (vector3 0 1 0)))
  (let* ((forward (normalize (v- center eye)))
         (side (normalize (vcross forward up)))
         (up (normalize (vcross side forward))))
    (transform*
     (make-transform (vx side) (vx up) (- (vx forward)) 0
                     (vy side) (vy up) (- (vy forward)) 0
                     (vz side) (vz up) (- (vz forward)) 0
                     0         0       0                1)
     (translate (v- eye)))))