diff options
Diffstat (limited to 'chickadee/math/matrix.scm')
-rw-r--r-- | chickadee/math/matrix.scm | 315 |
1 files changed, 315 insertions, 0 deletions
diff --git a/chickadee/math/matrix.scm b/chickadee/math/matrix.scm new file mode 100644 index 0000000..be307ab --- /dev/null +++ b/chickadee/math/matrix.scm @@ -0,0 +1,315 @@ +;;; 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 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 vector) + #:export (make-matrix4 + make-null-matrix4 + matrix4? + matrix4-mult! + matrix4* + matrix4-identity! + make-identity-matrix4 + orthographic-projection + matrix4-translate! + matrix4-translate + matrix4-scale! + matrix4-scale + matrix4-rotate-z! + matrix4-rotate-z + transform)) + +;; 4x4 matrix +(define-record-type <matrix4> + (%make-matrix4 bv ptr) + matrix4? + (bv matrix4-bv) + (ptr matrix4-ptr)) + +(define-inlinable (matrix-set! matrix row column x) + (f32vector-set! matrix (+ (* row 4) column) x)) + +(define-inlinable (matrix-ref matrix row column) + (f32vector-ref matrix (+ (* row 4) column))) + +(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))) + (matrix-set! bv 0 0 aa) + (matrix-set! bv 0 1 ab) + (matrix-set! bv 0 2 ac) + (matrix-set! bv 0 3 ad) + (matrix-set! bv 1 0 ba) + (matrix-set! bv 1 1 bb) + (matrix-set! bv 1 2 bc) + (matrix-set! bv 1 3 bd) + (matrix-set! bv 2 0 ca) + (matrix-set! bv 2 1 cb) + (matrix-set! bv 2 2 cc) + (matrix-set! bv 2 3 cd) + (matrix-set! bv 3 0 da) + (matrix-set! bv 3 1 db) + (matrix-set! bv 3 2 dc) + (matrix-set! bv 3 3 dd))) + +(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 (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 (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 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-matrix4 (/ 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 (matrix4-translate! matrix v) + (cond + ((vector2? v) + (init-matrix4 matrix + 1 0 0 0 + 0 1 0 0 + 0 0 1 0 + (vx v) (vy v) 0 1)) + ((vector3? v) + (init-matrix4 matrix + 1 0 0 0 + 0 1 0 0 + 0 0 1 0 + (vx v) (vy v) (vz v) 1)) + (else + (error "invalid translation vector" v)))) + +(define (matrix4-translate v) + (let ((matrix (make-null-matrix4))) + (matrix4-translate! matrix v) + matrix)) + +(define (matrix4-scale! matrix s) + (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-z! matrix angle) + (init-matrix4 matrix + (cos angle) (- (sin angle)) 0.0 0.0 + (sin angle) (cos angle) 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0)) + +(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 (transform matrix x y) + (let ((bv (matrix4-bv matrix))) + (values (+ (* x (matrix-ref bv 0 0)) + (* y (matrix-ref bv 1 0)) + (matrix-ref bv 3 0)) + (+ (* x (matrix-ref bv 0 1)) + (* y (matrix-ref bv 1 1)) + (matrix-ref bv 3 1))))) |