AtlatestRepositorysigil-geometry
sigil-geometry / tree / src / sigil / geometrymat4.sgl
1
;;; (sigil geometry mat4) - 4x4 float32 matrices, column-major2
;;;3
;;; A mat4 is a 64-byte bytevector of 16 float32 values in native byte4
;;; order, stored column by column (the OpenGL convention): the element5
;;; at column `c`, row `r` is float `c*4 + r`. A translation therefore6
;;; sits in floats 12, 13 and 14. These bytes are what a GL uniform7
;;; `mat4` expects with no transpose.8
;;;9
;;; Points are column vectors and matrices multiply on the left:10
;;; `(mat4-mul a b)` is the transform that applies `b` first, then `a`.11
;;;12
;;; Every operation that produces a matrix has an allocating form and13
;;; a `!` form that writes into a destination and returns it. The14
;;; destination may be the same bytevector as any input.16
(define-library (sigil geometry mat4)17
(import (sigil core) (sigil math))18
(export19
mat4 make-mat4 mat4? mat4-ref mat4-set! list->mat4 mat4->list20
mat4-identity mat4-identity! mat4-copy mat4-copy!21
mat4-mul mat4-mul! mat4-transpose mat4-transpose!22
mat4-determinant mat4-inverse mat4-inverse!23
mat4-translation mat4-translation! mat4-scaling mat4-scaling!24
mat4-rotation-x mat4-rotation-x! mat4-rotation-y mat4-rotation-y!25
mat4-rotation-z mat4-rotation-z! mat4-rotation mat4-rotation!26
mat4-from-quat mat4-from-quat! mat4-from-trs mat4-from-trs!27
mat4-get-translation mat4-get-translation!28
mat4-transform-point mat4-transform-point!29
mat4-transform-direction mat4-transform-direction!30
mat4-transform-vec4 mat4-transform-vec4!31
mat4-approx=?)33
(begin34
;; Float index -> float32 at byte offset 4*i, native byte order.35
;; Procedures, not syntax-rules macros: native codegen looks up a36
;; macro-introduced global through %hygiene-ref at run time, which37
;; allocates on every call.38
(define (f@ v i) (bytevector-ieee-single-ref v (* 4 i)))39
(define (f! v i x) (bytevector-ieee-single-set! v (* 4 i) x))41
;; Write all 16 floats, in storage (column-major) order; returns `d`.42
;; Every argument is evaluated before the first write, which is what43
;; lets a `!` form's destination alias its inputs.44
(define (set16! d a0 a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15)45
(f! d 0 a0) (f! d 1 a1) (f! d 2 a2) (f! d 3 a3)46
(f! d 4 a4) (f! d 5 a5) (f! d 6 a6) (f! d 7 a7)47
(f! d 8 a8) (f! d 9 a9) (f! d 10 a10) (f! d 11 a11)48
(f! d 12 a12) (f! d 13 a13) (f! d 14 a14) (f! d 15 a15)49
d)51
(define (new) (make-bytevector 64 0))53
;;; A new mat4 from 16 values in column-major order: the first four54
;;; are column 0 (rows 0 to 3), and so on.55
(define (mat4 a0 a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15)56
(set16! (new) a0 a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15))58
;;; A new identity mat4.59
(define (make-mat4) (mat4-identity! (new)))61
;;; True for a bytevector of mat4 size (64 bytes).62
(define (mat4? m) (and (bytevector? m) (= (bytevector-length m) 64)))64
;;; The element at column `col`, row `row`.65
(define (mat4-ref m col row) (f@ m (+ (* col 4) row)))67
;;; Set the element at column `col`, row `row`; returns `m`.68
(define (mat4-set! m col row x) (f! m (+ (* col 4) row) x) m)70
;;; A new mat4 from a list of 16 numbers in column-major order.71
(define (list->mat4 lst)72
(unless (= (length lst) 16)73
(error "list->mat4: expected 16 numbers" (length lst)))74
(let ((m (new)))75
(let loop ((i 0) (xs lst))76
(if (null? xs)77
m78
(begin (f! m i (car xs)) (loop (+ i 1) (cdr xs)))))))80
;;; The 16 floats in column-major order.81
(define (mat4->list m)82
(let loop ((i 15) (acc '()))83
(if (< i 0) acc (loop (- i 1) (cons (f@ m i) acc)))))85
(define (mat4-identity! dst)86
(set16! dst 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))87
(define (mat4-identity) (mat4-identity! (new)))89
(define (mat4-copy! dst m)90
(unless (eq? dst m) (bytevector-copy! dst 0 m 0 64))91
dst)92
(define (mat4-copy m) (mat4-copy! (new) m))94
;;; `a * b`: the transform that applies `b`, then `a`.95
;;;96
;;; ```scheme97
;;; ;; model-view-projection, written into one reused matrix98
;;; (mat4-mul! mvp view model)99
;;; (mat4-mul! mvp proj mvp)100
;;; ```101
(define (mat4-mul! dst a b)102
(let ((a00 (f@ a 0)) (a01 (f@ a 1)) (a02 (f@ a 2)) (a03 (f@ a 3))103
(a10 (f@ a 4)) (a11 (f@ a 5)) (a12 (f@ a 6)) (a13 (f@ a 7))104
(a20 (f@ a 8)) (a21 (f@ a 9)) (a22 (f@ a 10)) (a23 (f@ a 11))105
(a30 (f@ a 12)) (a31 (f@ a 13)) (a32 (f@ a 14)) (a33 (f@ a 15))106
(b00 (f@ b 0)) (b01 (f@ b 1)) (b02 (f@ b 2)) (b03 (f@ b 3))107
(b10 (f@ b 4)) (b11 (f@ b 5)) (b12 (f@ b 6)) (b13 (f@ b 7))108
(b20 (f@ b 8)) (b21 (f@ b 9)) (b22 (f@ b 10)) (b23 (f@ b 11))109
(b30 (f@ b 12)) (b31 (f@ b 13)) (b32 (f@ b 14)) (b33 (f@ b 15)))110
;; aCR is column C, row R. Result column j = a * (column j of b).111
(set16! dst112
(+ (* a00 b00) (* a10 b01) (* a20 b02) (* a30 b03))113
(+ (* a01 b00) (* a11 b01) (* a21 b02) (* a31 b03))114
(+ (* a02 b00) (* a12 b01) (* a22 b02) (* a32 b03))115
(+ (* a03 b00) (* a13 b01) (* a23 b02) (* a33 b03))116
(+ (* a00 b10) (* a10 b11) (* a20 b12) (* a30 b13))117
(+ (* a01 b10) (* a11 b11) (* a21 b12) (* a31 b13))118
(+ (* a02 b10) (* a12 b11) (* a22 b12) (* a32 b13))119
(+ (* a03 b10) (* a13 b11) (* a23 b12) (* a33 b13))120
(+ (* a00 b20) (* a10 b21) (* a20 b22) (* a30 b23))121
(+ (* a01 b20) (* a11 b21) (* a21 b22) (* a31 b23))122
(+ (* a02 b20) (* a12 b21) (* a22 b22) (* a32 b23))123
(+ (* a03 b20) (* a13 b21) (* a23 b22) (* a33 b23))124
(+ (* a00 b30) (* a10 b31) (* a20 b32) (* a30 b33))125
(+ (* a01 b30) (* a11 b31) (* a21 b32) (* a31 b33))126
(+ (* a02 b30) (* a12 b31) (* a22 b32) (* a32 b33))127
(+ (* a03 b30) (* a13 b31) (* a23 b32) (* a33 b33)))))128
(define (mat4-mul a b) (mat4-mul! (new) a b))130
(define (mat4-transpose! dst m)131
(let ((m0 (f@ m 0)) (m1 (f@ m 1)) (m2 (f@ m 2)) (m3 (f@ m 3))132
(m4 (f@ m 4)) (m5 (f@ m 5)) (m6 (f@ m 6)) (m7 (f@ m 7))133
(m8 (f@ m 8)) (m9 (f@ m 9)) (m10 (f@ m 10)) (m11 (f@ m 11))134
(m12 (f@ m 12)) (m13 (f@ m 13)) (m14 (f@ m 14)) (m15 (f@ m 15)))135
(set16! dst m0 m4 m8 m12 m1 m5 m9 m13 m2 m6 m10 m14 m3 m7 m11 m15)))136
(define (mat4-transpose m) (mat4-transpose! (new) m))138
;;; The determinant.139
(define (mat4-determinant m)140
(let ((a00 (f@ m 0)) (a01 (f@ m 1)) (a02 (f@ m 2)) (a03 (f@ m 3))141
(a10 (f@ m 4)) (a11 (f@ m 5)) (a12 (f@ m 6)) (a13 (f@ m 7))142
(a20 (f@ m 8)) (a21 (f@ m 9)) (a22 (f@ m 10)) (a23 (f@ m 11))143
(a30 (f@ m 12)) (a31 (f@ m 13)) (a32 (f@ m 14)) (a33 (f@ m 15)))144
(let ((b00 (- (* a00 a11) (* a01 a10)))145
(b01 (- (* a00 a12) (* a02 a10)))146
(b02 (- (* a00 a13) (* a03 a10)))147
(b03 (- (* a01 a12) (* a02 a11)))148
(b04 (- (* a01 a13) (* a03 a11)))149
(b05 (- (* a02 a13) (* a03 a12)))150
(b06 (- (* a20 a31) (* a21 a30)))151
(b07 (- (* a20 a32) (* a22 a30)))152
(b08 (- (* a20 a33) (* a23 a30)))153
(b09 (- (* a21 a32) (* a22 a31)))154
(b10 (- (* a21 a33) (* a23 a31)))155
(b11 (- (* a22 a33) (* a23 a32))))156
(+ (- (* b00 b11) (* b01 b10)) (* b02 b09) (* b03 b08)157
(- (* b04 b07)) (* b05 b06)))))159
;;; Write the inverse of `m` into `dst` and return `dst`, or return #f160
;;; and leave `dst` untouched when `m` is singular (determinant 0).161
(define (mat4-inverse! dst m)162
(let ((a00 (f@ m 0)) (a01 (f@ m 1)) (a02 (f@ m 2)) (a03 (f@ m 3))163
(a10 (f@ m 4)) (a11 (f@ m 5)) (a12 (f@ m 6)) (a13 (f@ m 7))164
(a20 (f@ m 8)) (a21 (f@ m 9)) (a22 (f@ m 10)) (a23 (f@ m 11))165
(a30 (f@ m 12)) (a31 (f@ m 13)) (a32 (f@ m 14)) (a33 (f@ m 15)))166
(let* ((b00 (- (* a00 a11) (* a01 a10)))167
(b01 (- (* a00 a12) (* a02 a10)))168
(b02 (- (* a00 a13) (* a03 a10)))169
(b03 (- (* a01 a12) (* a02 a11)))170
(b04 (- (* a01 a13) (* a03 a11)))171
(b05 (- (* a02 a13) (* a03 a12)))172
(b06 (- (* a20 a31) (* a21 a30)))173
(b07 (- (* a20 a32) (* a22 a30)))174
(b08 (- (* a20 a33) (* a23 a30)))175
(b09 (- (* a21 a32) (* a22 a31)))176
(b10 (- (* a21 a33) (* a23 a31)))177
(b11 (- (* a22 a33) (* a23 a32)))178
(det (+ (- (* b00 b11) (* b01 b10)) (* b02 b09) (* b03 b08)179
(- (* b04 b07)) (* b05 b06))))180
(if (= det 0.0)181
#f182
(let ((d (/ 1.0 det)))183
(set16! dst184
(* d (+ (- (* a11 b11) (* a12 b10)) (* a13 b09)))185
(* d (- (- (* a02 b10) (* a01 b11)) (* a03 b09)))186
(* d (+ (- (* a31 b05) (* a32 b04)) (* a33 b03)))187
(* d (- (- (* a22 b04) (* a21 b05)) (* a23 b03)))188
(* d (- (- (* a12 b08) (* a10 b11)) (* a13 b07)))189
(* d (+ (- (* a00 b11) (* a02 b08)) (* a03 b07)))190
(* d (- (- (* a32 b02) (* a30 b05)) (* a33 b01)))191
(* d (+ (- (* a20 b05) (* a22 b02)) (* a23 b01)))192
(* d (+ (- (* a10 b10) (* a11 b08)) (* a13 b06)))193
(* d (- (- (* a01 b08) (* a00 b10)) (* a03 b06)))194
(* d (+ (- (* a30 b04) (* a31 b02)) (* a33 b00)))195
(* d (- (- (* a21 b02) (* a20 b04)) (* a23 b00)))196
(* d (- (- (* a11 b07) (* a10 b09)) (* a12 b06)))197
(* d (+ (- (* a00 b09) (* a01 b07)) (* a02 b06)))198
(* d (- (- (* a31 b01) (* a30 b03)) (* a32 b00)))199
(* d (+ (- (* a20 b03) (* a21 b01)) (* a22 b00)))))))))201
;;; The inverse of `m` as a new matrix, or #f when `m` is singular.202
(define (mat4-inverse m) (mat4-inverse! (new) m))204
;;; A translation by the vec3 `v`.205
(define (mat4-translation! dst v)206
(set16! dst 1.0 0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 1.0 0.0207
(f@ v 0) (f@ v 1) (f@ v 2) 1.0))208
(define (mat4-translation v) (mat4-translation! (new) v))210
;;; A scale by the vec3 `v` along x, y and z.211
(define (mat4-scaling! dst v)212
(set16! dst (f@ v 0) 0.0 0.0 0.0 0.0 (f@ v 1) 0.0 0.0 0.0 0.0 (f@ v 2) 0.0213
0.0 0.0 0.0 1.0))214
(define (mat4-scaling v) (mat4-scaling! (new) v))216
;;; A rotation of `angle` radians about the x axis, counterclockwise217
;;; when looking from +x toward the origin (y turns toward z).218
(define (mat4-rotation-x! dst angle)219
(let ((c (cos angle)) (s (sin angle)))220
(set16! dst 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)))221
(define (mat4-rotation-x angle) (mat4-rotation-x! (new) angle))223
;;; A rotation of `angle` radians about the y axis (z turns toward x).224
(define (mat4-rotation-y! dst angle)225
(let ((c (cos angle)) (s (sin angle)))226
(set16! dst 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)))227
(define (mat4-rotation-y angle) (mat4-rotation-y! (new) angle))229
;;; A rotation of `angle` radians about the z axis (x turns toward y).230
(define (mat4-rotation-z! dst angle)231
(let ((c (cos angle)) (s (sin angle)))232
(set16! dst 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)))233
(define (mat4-rotation-z angle) (mat4-rotation-z! (new) angle))235
;;; A rotation of `angle` radians about the vec3 `axis`, which need236
;;; not be unit length. A zero axis gives the identity.237
(define (mat4-rotation! dst axis angle)238
(let* ((x (f@ axis 0)) (y (f@ axis 1)) (z (f@ axis 2))239
(len (sqrt (+ (* x x) (* y y) (* z z)))))240
(if (= len 0.0)241
(mat4-identity! dst)242
(let* ((x (/ x len)) (y (/ y len)) (z (/ z len))243
(c (cos angle)) (s (sin angle)) (t (- 1.0 c)))244
(set16! dst245
(+ (* x x t) c) (+ (* y x t) (* z s)) (- (* z x t) (* y s)) 0.0246
(- (* x y t) (* z s)) (+ (* y y t) c) (+ (* z y t) (* x s)) 0.0247
(+ (* x z t) (* y s)) (- (* y z t) (* x s)) (+ (* z z t) c) 0.0248
0.0 0.0 0.0 1.0)))))249
(define (mat4-rotation axis angle) (mat4-rotation! (new) axis angle))251
;;; The rotation matrix of the unit quaternion `q` (x y z w).252
(define (mat4-from-quat! dst q)253
(mat4-from-trs-values! dst 0.0 0.0 0.0 q 1.0 1.0 1.0))254
(define (mat4-from-quat q) (mat4-from-quat! (new) q))256
;;; Translation * rotation * scale: scale by the vec3 `s`, rotate by257
;;; the quaternion `q`, then move by the vec3 `t`. The usual model258
;;; matrix for an object with a position, orientation and size.259
(define (mat4-from-trs! dst t q s)260
(mat4-from-trs-values! dst (f@ t 0) (f@ t 1) (f@ t 2) q (f@ s 0) (f@ s 1) (f@ s 2)))261
(define (mat4-from-trs t q s) (mat4-from-trs! (new) t q s))263
(define (mat4-from-trs-values! dst tx ty tz q sx sy sz)264
(let* ((x (f@ q 0)) (y (f@ q 1)) (z (f@ q 2)) (w (f@ q 3))265
(x2 (+ x x)) (y2 (+ y y)) (z2 (+ z z))266
(xx (* x x2)) (yx (* y x2)) (yy (* y y2))267
(zx (* z x2)) (zy (* z y2)) (zz (* z z2))268
(wx (* w x2)) (wy (* w y2)) (wz (* w z2)))269
(set16! dst270
(* sx (- 1.0 yy zz)) (* sx (+ yx wz)) (* sx (- zx wy)) 0.0271
(* sy (- yx wz)) (* sy (- 1.0 xx zz)) (* sy (+ zy wx)) 0.0272
(* sz (+ zx wy)) (* sz (- zy wx)) (* sz (- 1.0 xx yy)) 0.0273
tx ty tz 1.0)))275
;;; The translation part of `m` (floats 12, 13, 14) as a vec3.276
(define (mat4-get-translation! dst m)277
(f! dst 0 (f@ m 12)) (f! dst 1 (f@ m 13)) (f! dst 2 (f@ m 14))278
dst)279
(define (mat4-get-translation m) (mat4-get-translation! (make-bytevector 12 0) m))281
;;; Transform the vec3 point `p` (w = 1) and divide by the resulting282
;;; w, so a projection matrix yields normalized device coordinates.283
;;; When the resulting w is 0 the undivided x, y, z are written.284
(define (mat4-transform-point! dst m p)285
(let* ((x (f@ p 0)) (y (f@ p 1)) (z (f@ p 2))286
(rx (+ (* (f@ m 0) x) (* (f@ m 4) y) (* (f@ m 8) z) (f@ m 12)))287
(ry (+ (* (f@ m 1) x) (* (f@ m 5) y) (* (f@ m 9) z) (f@ m 13)))288
(rz (+ (* (f@ m 2) x) (* (f@ m 6) y) (* (f@ m 10) z) (f@ m 14)))289
(rw (+ (* (f@ m 3) x) (* (f@ m 7) y) (* (f@ m 11) z) (f@ m 15))))290
(if (or (= rw 1.0) (= rw 0.0))291
(begin (f! dst 0 rx) (f! dst 1 ry) (f! dst 2 rz))292
(let ((inv (/ 1.0 rw)))293
(f! dst 0 (* rx inv)) (f! dst 1 (* ry inv)) (f! dst 2 (* rz inv))))294
dst))295
(define (mat4-transform-point m p) (mat4-transform-point! (make-bytevector 12 0) m p))297
;;; Transform the vec3 direction `d` (w = 0): rotation and scale298
;;; apply, translation does not.299
(define (mat4-transform-direction! dst m d)300
(let ((x (f@ d 0)) (y (f@ d 1)) (z (f@ d 2)))301
(let ((rx (+ (* (f@ m 0) x) (* (f@ m 4) y) (* (f@ m 8) z)))302
(ry (+ (* (f@ m 1) x) (* (f@ m 5) y) (* (f@ m 9) z)))303
(rz (+ (* (f@ m 2) x) (* (f@ m 6) y) (* (f@ m 10) z))))304
(f! dst 0 rx) (f! dst 1 ry) (f! dst 2 rz)305
dst)))306
(define (mat4-transform-direction m d)307
(mat4-transform-direction! (make-bytevector 12 0) m d))309
;;; `m * v` for the vec4 `v`, with no divide.310
(define (mat4-transform-vec4! dst m v)311
(let ((x (f@ v 0)) (y (f@ v 1)) (z (f@ v 2)) (w (f@ v 3)))312
(let ((rx (+ (* (f@ m 0) x) (* (f@ m 4) y) (* (f@ m 8) z) (* (f@ m 12) w)))313
(ry (+ (* (f@ m 1) x) (* (f@ m 5) y) (* (f@ m 9) z) (* (f@ m 13) w)))314
(rz (+ (* (f@ m 2) x) (* (f@ m 6) y) (* (f@ m 10) z) (* (f@ m 14) w)))315
(rw (+ (* (f@ m 3) x) (* (f@ m 7) y) (* (f@ m 11) z) (* (f@ m 15) w))))316
(f! dst 0 rx) (f! dst 1 ry) (f! dst 2 rz) (f! dst 3 rw)317
dst)))318
(define (mat4-transform-vec4 m v) (mat4-transform-vec4! (make-bytevector 16 0) m v))320
;;; True when every element differs by at most `eps` (default 1e-5,321
;;; relative above magnitude 1).322
(define (mat4-approx=? a b . eps)323
(all-close? a b (if (null? eps) 1e-5 (car eps)) 0))325
;; Floats i..15 of a and b all within e. `<`, not `<=`, so NaN fails326
;; (sigil 0.22.7's `<=` answers #t for NaN). A top-level recursive327
;; procedure rather than a named let: a named let allocates a closure328
;; on every call.329
(define (all-close? a b e i)330
(or (= i 16)331
(and (let ((x (f@ a i)) (y (f@ b i)))332
(< (abs (- x y)) (* e (max 1.0 (abs x) (abs y)))))333
(all-close? a b e (+ i 1)))))))