mirror of
https://github.com/open-goal/jak-project
synced 2026-08-06 18:03:30 -04:00
water111
1505 lines
62 KiB
Common Lisp
1505 lines
62 KiB
Common Lisp
;;-*-Lisp-*-
|
|
(in-package goal)
|
|
(bundles "ENGINE.CGO" "GAME.CGO")
|
|
(require "engine/math/vector-h.gc")
|
|
(require "engine/math/matrix-h.gc")
|
|
|
|
;; The PC implementation of the register-argument entry point delegates forward
|
|
;; to the ordinary axis/sine/cosine routine defined later in this file.
|
|
(defun-extern matrix-axis-sin-cos! matrix vector float float matrix)
|
|
|
|
;; This file implements the GOAL matrix library.
|
|
|
|
;; In general, a vector is always treated as a row vector.
|
|
;; Chaining transformation is done like v_row * T1_goal * T2_goal, which means do T1 before T2.
|
|
;; This is the opposite of the usual convention:
|
|
;; T2_normal * T1_normal * v_col
|
|
;; However, there is good news:
|
|
;; T1_normal = T1_goal ^ T
|
|
;; T2_normal = T2_goal ^ T
|
|
;; This is due to the property
|
|
;; (A * B)^T = B^T * A^T
|
|
|
|
;; So a homogeneous transformation is:
|
|
;; R_xx R_xy R_xz 0
|
|
;; R_yx R_yy R_yz 0
|
|
;; R_zx R_zy R_zz 0
|
|
;; T_x T_y T_z 1
|
|
|
|
;; which is probably the transpose of what you're used to.
|
|
|
|
;; note that they also used row-major storage, so the 3rd qword of a matrix is the translation
|
|
;; part of the affine transform.
|
|
|
|
;; In general, be careful with using these functions as they often have strange
|
|
;; requirements for the form of the input matrix or if the input/output matrix are
|
|
;; allowed to be the same memory.
|
|
|
|
;; matrix*! computes dst = src1 * src2 in the row-vector sense described above, which means src1
|
|
;; is applied first. (matrix*! out rot trans) rotates and then translates. The rotate-in-order
|
|
;; helpers below are all built that way, passing the newer rotation on the left.
|
|
|
|
;; The multiply is four broadcast multiply-adds, one per source row, and every one of them writes
|
|
;; all four lanes. A function that only wants xyz either ignores the input w, as vector-rotate*!
|
|
;; does, or masks its final write to xyz. A w of 1.0 always comes from vf0, which
|
|
;; init-vf0-vector loads with (0, 0, 0, 1), the same value VU0's hardwired vf0 holds.
|
|
|
|
;; dst may alias a source in matrix+!, matrix-!, matrix*!, matrix-transpose!,
|
|
;; matrix-4x4-inverse!, matrix-translate+!, scale-matrix!, column-scale-matrix!, matrix-lerp!,
|
|
;; vector-matrix*!, vector-rotate*!, vector3s-matrix*! and vector3s-rotate*!. It must differ from
|
|
;; src in matrix-inverse-of-rot-trans!, matrix-3x3-inverse!, matrix-3x3-inverse-transpose! and
|
|
;; matrix-4x4-inverse-transpose!, all of which read src after they have started writing dst.
|
|
|
|
;; matrix3 shares the layout, but only the upper 3x3 means anything. Nothing here writes its
|
|
;; fourth column, so reading one back as a matrix finds garbage in every w.
|
|
|
|
;; A few idioms recur. (.mul.x.vf acc a b) opens the VU0 accumulator, .add.mul.<lane>.vf keeps
|
|
;; adding broadcast products, and the last one writes a register; one matrix row is four of those
|
|
;; and never needs a temporary. (.mov gpr vf) and (.mov vf gpr) are bit moves between the
|
|
;; register files rather than conversions, which is how an FPU float becomes a broadcast lane and
|
|
;; how a lane's sign gets tested with an integer branch. pceqw, ppach, add one, branch on zero
|
|
;; asks whether a group of lanes were all zero in a single test. (.xor.vf r r r) stands where the
|
|
;; EE cleared a register by subtracting it from itself: subtraction preserves NaN, xor does not,
|
|
;; and NaN lanes are very slow on the host.
|
|
|
|
;; DECOMP BEGINS
|
|
|
|
(defmethod inspect ((this matrix))
|
|
"Print out all the values in a matrix."
|
|
(format #t "[~8x] matrix~%" this)
|
|
(format #t "~T[~F] [~F] [~F] [~F]~%" (-> this data 0) (-> this data 1) (-> this data 2) (-> this data 3))
|
|
(format #t "~T[~F] [~F] [~F] [~F]~%" (-> this data 4) (-> this data 5) (-> this data 6) (-> this data 7))
|
|
(format #t "~T[~F] [~F] [~F] [~F]~%" (-> this data 8) (-> this data 9) (-> this data 10) (-> this data 11))
|
|
(format #t "~T[~F] [~F] [~F] [~F]~%" (-> this data 12) (-> this data 13) (-> this data 14) (-> this data 15))
|
|
this)
|
|
|
|
(defmethod inspect ((this matrix3))
|
|
"Print out the values in a 3x3 matrix."
|
|
(format #t "[~8x] matrix3~%" this)
|
|
(format #t "~T[~F] [~F] [~F]~%" (-> this data 0) (-> this data 1) (-> this data 2))
|
|
(format #t "~T[~F] [~F] [~F]~%" (-> this data 4) (-> this data 5) (-> this data 6))
|
|
(format #t "~T[~F] [~F] [~F]~%" (-> this data 8) (-> this data 9) (-> this data 10))
|
|
this)
|
|
|
|
(defun matrix-identity! ((dst matrix))
|
|
"Set dst to the identity matrix."
|
|
;; zero matrix
|
|
(set! (-> dst vector 0 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 1 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 2 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 3 quad) (the-as uint128 0))
|
|
;; set diagonal to 1.0
|
|
;; doing it like this is a slight optimization and avoids
|
|
;; reloading the 1.0 constant from memory each time.
|
|
(let ((one 1.0))
|
|
(set! (-> dst data 15) one)
|
|
(set! (-> dst data 10) one)
|
|
(set! (-> dst data 5) one)
|
|
(set! (-> dst data 0) one))
|
|
dst)
|
|
|
|
;; A 4x4 identity matrix
|
|
(define *identity-matrix* (the-as matrix (new 'global 'matrix)))
|
|
|
|
(matrix-identity! *identity-matrix*)
|
|
|
|
(defun matrix+! ((dst matrix) (src1 matrix) (src2 matrix))
|
|
"Set dst = src1 + src2. It is okay for any arguments to be the same data.
|
|
This is not an efficient implementation."
|
|
(dotimes (i 16)
|
|
(set! (-> dst data i) (+ (-> src1 data i) (-> src2 data i))))
|
|
dst)
|
|
|
|
(defun matrix-! ((dst matrix) (src1 matrix) (src2 matrix))
|
|
"Set dst = src1 - src2. It is okay for any arguments to share storage.
|
|
This is not an efficient implementation."
|
|
(dotimes (i 16)
|
|
(set! (-> dst data i) (- (-> src1 data i) (-> src2 data i))))
|
|
dst)
|
|
|
|
(desfun .vf.vec (reg1 &rest regs)
|
|
"make a vector for assembly regs.
|
|
A vector is simply a list of 4 regs, the locations of each of the 4 components (x, y, z, w).
|
|
If you supply less than 4 regs, the last one will expand to the remaining components.
|
|
Examples: (.vf.vec vf1) -> (vf1 vf1 vf1 vf1); (.vf.vec vf2 vf2 vf5) -> (vf2 vf2 vf5 vf5)
|
|
TODO: make keys"
|
|
(let ((x reg1)
|
|
(y #f)
|
|
(z #f)
|
|
(w #f))
|
|
(cond
|
|
((= (length regs) 1) (set! y (car regs)) (set! z (car regs)) (set! w (car regs)))
|
|
((= (length regs) 2) (set! y (car regs)) (set! z (cadr regs)) (set! w (cadr regs)))
|
|
((>= (length regs) 3) (set! y (car regs)) (set! z (cadr regs)) (set! w (caddr regs)))
|
|
(#t (set! y reg1) (set! z reg1) (set! w reg1)))
|
|
(cons x (cons y (cons z (cons w '()))))))
|
|
|
|
(desfun .vf.x (vec)
|
|
(car vec))
|
|
|
|
(desfun .vf.y (vec)
|
|
(cadr vec))
|
|
|
|
(desfun .vf.z (vec)
|
|
(caddr vec))
|
|
|
|
(desfun .vf.w (vec)
|
|
(cadddr vec))
|
|
|
|
(desfun .vf.vec<-arg (arg)
|
|
(if (pair? arg)
|
|
(if (= (length arg) 1)
|
|
(.vf.vec (car arg))
|
|
(if (= (length arg) 2)
|
|
(.vf.vec (car arg) (cadr arg))
|
|
(if (= (length arg) 3) (.vf.vec (car arg) (cadr arg) (caddr arg)) (.vf.vec (car arg) (cadr arg) (caddr arg) (cadddr arg)))))
|
|
(.vf.vec arg)))
|
|
|
|
(defmacro .matrix*! (acc dest-mat src1-mat src2-mat)
|
|
"macro for assembly matrix multiplication. dest and src2 shall not overlap.
|
|
The mats are a list of 4 vectors."
|
|
(let ((dest-r1 (.vf.x (.vf.vec<-arg (car dest-mat))))
|
|
(dest-r2 (.vf.x (.vf.vec<-arg (cadr dest-mat))))
|
|
(dest-r3 (.vf.x (.vf.vec<-arg (caddr dest-mat))))
|
|
(dest-r4 (.vf.x (.vf.vec<-arg (cadddr dest-mat))))
|
|
(src1-r1 (.vf.vec<-arg (car src1-mat)))
|
|
(src1-r2 (.vf.vec<-arg (cadr src1-mat)))
|
|
(src1-r3 (.vf.vec<-arg (caddr src1-mat)))
|
|
(src1-r4 (.vf.vec<-arg (cadddr src1-mat)))
|
|
(src2-r1 (.vf.vec<-arg (car src2-mat)))
|
|
(src2-r2 (.vf.vec<-arg (cadr src2-mat)))
|
|
(src2-r3 (.vf.vec<-arg (caddr src2-mat)))
|
|
(src2-r4 (.vf.vec<-arg (cadddr src2-mat))))
|
|
`(begin
|
|
(.mul.x.vf ,acc ,(.vf.x src2-r1) ,(.vf.x src1-r1))
|
|
(.add.mul.y.vf ,acc ,(.vf.x src2-r2) ,(.vf.y src1-r1) ,acc)
|
|
(.add.mul.z.vf ,acc ,(.vf.x src2-r3) ,(.vf.z src1-r1) ,acc)
|
|
(.add.mul.w.vf ,dest-r1 ,(.vf.x src2-r4) ,(.vf.w src1-r1) ,acc)
|
|
(.mul.x.vf ,acc ,(.vf.y src2-r1) ,(.vf.x src1-r2))
|
|
(.add.mul.y.vf ,acc ,(.vf.y src2-r2) ,(.vf.y src1-r2) ,acc)
|
|
(.add.mul.z.vf ,acc ,(.vf.y src2-r3) ,(.vf.z src1-r2) ,acc)
|
|
(.add.mul.w.vf ,dest-r2 ,(.vf.y src2-r4) ,(.vf.w src1-r2) ,acc)
|
|
(.mul.x.vf ,acc ,(.vf.z src2-r1) ,(.vf.x src1-r3))
|
|
(.add.mul.y.vf ,acc ,(.vf.z src2-r2) ,(.vf.y src1-r3) ,acc)
|
|
(.add.mul.z.vf ,acc ,(.vf.z src2-r3) ,(.vf.z src1-r3) ,acc)
|
|
(.add.mul.w.vf ,dest-r3 ,(.vf.z src2-r4) ,(.vf.w src1-r3) ,acc)
|
|
(.mul.x.vf ,acc ,(.vf.w src2-r1) ,(.vf.x src1-r4))
|
|
(.add.mul.y.vf ,acc ,(.vf.w src2-r2) ,(.vf.y src1-r4) ,acc)
|
|
(.add.mul.z.vf ,acc ,(.vf.w src2-r3) ,(.vf.z src1-r4) ,acc)
|
|
(.add.mul.w.vf ,dest-r4 ,(.vf.w src2-r4) ,(.vf.w src1-r4) ,acc))))
|
|
|
|
(defun matrix*! ((dst matrix) (src1 matrix) (src2 matrix))
|
|
"Set dst = src1 * src2, so src1 is applied first. All four lanes are multiplied, w included.
|
|
Any argument may be the same data, since all eight rows reach registers before the first
|
|
store. This is a moderately efficient implementation."
|
|
(rlet ((acc :class vf)
|
|
(left-row0 :class vf)
|
|
(left-row1 :class vf)
|
|
(left-row2 :class vf)
|
|
(left-row3 :class vf)
|
|
(right-row0 :class vf)
|
|
(right-row1 :class vf)
|
|
(right-row2 :class vf)
|
|
(right-row3 :class vf)
|
|
(out-row0 :class vf)
|
|
(out-row1 :class vf)
|
|
(out-row2 :class vf)
|
|
(out-row3 :class vf))
|
|
;; This could likely be faster on PS2 with some clever reordering.
|
|
;; These loads will stall.
|
|
(.lvf left-row0 (&-> src1 vector 0 quad))
|
|
(.lvf right-row0 (&-> src2 vector 0 quad))
|
|
(.lvf right-row1 (&-> src2 vector 1 quad))
|
|
(.lvf right-row2 (&-> src2 vector 2 quad))
|
|
(.lvf right-row3 (&-> src2 vector 3 quad))
|
|
(.lvf left-row1 (&-> src1 vector 1 quad))
|
|
(.lvf left-row2 (&-> src1 vector 2 quad))
|
|
(.lvf left-row3 (&-> src1 vector 3 quad))
|
|
(.mul.x.vf acc right-row0 left-row0)
|
|
(.add.mul.y.vf acc right-row1 left-row0 acc)
|
|
(.add.mul.z.vf acc right-row2 left-row0 acc)
|
|
(.add.mul.w.vf out-row0 right-row3 left-row0 acc)
|
|
(.mul.x.vf acc right-row0 left-row1)
|
|
(.add.mul.y.vf acc right-row1 left-row1 acc)
|
|
(.add.mul.z.vf acc right-row2 left-row1 acc)
|
|
(.add.mul.w.vf out-row1 right-row3 left-row1 acc)
|
|
(.mul.x.vf acc right-row0 left-row2)
|
|
(.add.mul.y.vf acc right-row1 left-row2 acc)
|
|
(.add.mul.z.vf acc right-row2 left-row2 acc)
|
|
(.add.mul.w.vf out-row2 right-row3 left-row2 acc)
|
|
(.mul.x.vf acc right-row0 left-row3)
|
|
(.add.mul.y.vf acc right-row1 left-row3 acc)
|
|
(.add.mul.z.vf acc right-row2 left-row3 acc)
|
|
(.add.mul.w.vf out-row3 right-row3 left-row3 acc)
|
|
(.svf (&-> dst vector 0 quad) out-row0)
|
|
(.svf (&-> dst vector 1 quad) out-row1)
|
|
(.svf (&-> dst vector 2 quad) out-row2)
|
|
(.svf (&-> dst vector 3 quad) out-row3)
|
|
dst))
|
|
|
|
(defun matrixp*! ((dst matrix) (src1 matrix) (src2 matrix))
|
|
"Set dst = src1 * src2. NOTE: this function is a wrapper around matrix*!
|
|
that adds no additional functionality. It seems to be a leftover from
|
|
a time when matrix*! wasn't safe to use in place. This is unused."
|
|
(let ((temp-mat (new-stack-matrix0)))
|
|
(matrix*! temp-mat src1 src2)
|
|
(matrix-copy! dst temp-mat))
|
|
dst)
|
|
|
|
(defun vector-matrix*! ((dst vector) (vec vector) (mat matrix))
|
|
"Set dst = vec * mat, treating vec as a row vector. The w lane participates, so pass w = 1.0 to
|
|
pick up mat's translation row and 0.0 to skip it. dst may be vec."
|
|
(rlet ((acc :class vf)
|
|
(row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(row3 :class vf)
|
|
(point :class vf))
|
|
(.lvf row0 (&-> mat vector 0 quad))
|
|
(.lvf row1 (&-> mat vector 1 quad))
|
|
(.lvf row2 (&-> mat vector 2 quad))
|
|
(.lvf row3 (&-> mat vector 3 quad))
|
|
(.lvf point (&-> vec quad))
|
|
(.mul.x.vf acc row0 point)
|
|
(.add.mul.y.vf acc row1 point acc)
|
|
(.add.mul.z.vf acc row2 point acc)
|
|
(.add.mul.w.vf point row3 point acc)
|
|
(.svf (&-> dst quad) point)
|
|
dst))
|
|
|
|
(defun vector-rotate*! ((dst vector) (vec vector) (mat matrix))
|
|
"Set dst to vec rotated by mat's upper 3x3. The translation row is loaded and never used. All
|
|
four lanes are still written, so mat's w column must be (0, 0, 0, x) or it leaks into dst.w.
|
|
dst may be vec."
|
|
(rlet ((acc :class vf)
|
|
(row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(row3-unused :class vf)
|
|
(point :class vf))
|
|
;; this implementation is better than vector-matrix*!
|
|
(.lvf point (&-> vec quad))
|
|
(.lvf row0 (&-> mat vector 0 quad))
|
|
(.lvf row1 (&-> mat vector 1 quad))
|
|
;; the mul's assume the right-most column of the input matrix are 0,0,0,X
|
|
(.mul.x.vf acc row0 point)
|
|
(.lvf row2 (&-> mat vector 2 quad))
|
|
(.add.mul.y.vf acc row1 point acc)
|
|
;; this load doesn't need to be here!
|
|
(.lvf row3-unused (&-> mat vector 3 quad))
|
|
(.add.mul.z.vf point row2 point acc)
|
|
(.svf (&-> dst quad) point)
|
|
dst))
|
|
|
|
(defun vector3s-matrix*! ((dst vector3s) (vec vector3s) (mat matrix))
|
|
"Set dst.xyz to ([vec.xyz, 1.0] * mat).xyz, so mat's translation row applies. dst.w is left
|
|
alone. This goes through a stack vector, so dst and vec can be the same memory."
|
|
(let ((temp-vec3 (new-stack-vector0)))
|
|
(set-vector! temp-vec3 (-> vec x) (-> vec y) (-> vec z) 1.0)
|
|
(vector-matrix*! temp-vec3 temp-vec3 mat)
|
|
(set! (-> dst x) (-> temp-vec3 x))
|
|
(set! (-> dst y) (-> temp-vec3 y))
|
|
(set! (-> dst z) (-> temp-vec3 z)))
|
|
dst)
|
|
|
|
(defun vector3s-rotate*! ((dst vector3s) (vec vector3s) (mat matrix))
|
|
"Set dst.xyz to vec.xyz rotated by mat's upper 3x3, ignoring the translation row and leaving
|
|
dst.w alone. dst and vec can be the same memory. mat should not have a scale/shear (the upper
|
|
3x3 should be a pure rotation)."
|
|
(let ((temp-vec3 (new-stack-vector0)))
|
|
(set-vector! temp-vec3 (-> vec x) (-> vec y) (-> vec z) 1.0)
|
|
(vector-rotate*! temp-vec3 temp-vec3 mat)
|
|
(set! (-> dst x) (-> temp-vec3 x))
|
|
(set! (-> dst y) (-> temp-vec3 y))
|
|
(set! (-> dst z) (-> temp-vec3 z)))
|
|
dst)
|
|
|
|
(defun matrix-transpose! ((dst matrix) (src matrix))
|
|
"Set dst = src^T. src and dst can be the same, because all four columns are built before the
|
|
first store. This is a pure lane shuffle: pextlw and pextuw pair rows 0/1 and rows 2/3 into
|
|
quadwords holding two columns each, then pcpyld and pcpyud select the right 64-bit half.
|
|
Sixteen scalar moves would be far worse."
|
|
(local-vars
|
|
(low-01 uint128)
|
|
(col1 uint128)
|
|
(col3 uint128)
|
|
(high-01 uint128)
|
|
(high-23 uint128)
|
|
(col0 uint128)
|
|
(col2 uint128)
|
|
(low-23 uint128))
|
|
(let ((row0 (-> src vector 0 quad)))
|
|
(let ((row1 (-> src vector 1 quad)))
|
|
(let ((row2 (-> src vector 2 quad)))
|
|
(.pextlw low-01 row1 row0)
|
|
(let ((row3 (-> src vector 3 quad)))
|
|
(.pextuw high-01 row1 row0)
|
|
(#unless PC_PORT
|
|
(mmi-nop!))
|
|
(.pextlw low-23 row3 row2)
|
|
(#unless PC_PORT
|
|
(mmi-nop!))
|
|
(.pextuw high-23 row3 row2)))))
|
|
(#unless PC_PORT
|
|
(mmi-nop!))
|
|
(.pcpyld col0 low-23 low-01)
|
|
(#unless PC_PORT
|
|
(mmi-nop!))
|
|
(.pcpyud col1 low-01 low-23)
|
|
(set! (-> dst vector 0 quad) col0)
|
|
(.pcpyld col2 high-23 high-01)
|
|
(set! (-> dst vector 1 quad) col1)
|
|
(.pcpyud col3 high-01 high-23)
|
|
(set! (-> dst vector 2 quad) col2)
|
|
(set! (-> dst vector 3 quad) col3)
|
|
dst)
|
|
|
|
(defun matrix-inverse-of-rot-trans! ((dst matrix) (src matrix))
|
|
"Set dst = src^-1, assuming src is a homogeneous tranform with only rotation/translation.
|
|
R^-1 = R^T for a rotation, so the upper 3x3 is just the transpose, but the translation still
|
|
has to be rotated into the new frame and negated, giving dst = [R^T, -T * R^T].
|
|
NOTE: THIS FUNCTION REQUIRES dst != src. It transposes into dst and then goes back to src for
|
|
the translation row."
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(rot-row0 :class vf)
|
|
(rot-row1 :class vf)
|
|
(rot-row2 :class vf)
|
|
(inv-trans :class vf)
|
|
(orig-trans :class vf))
|
|
(init-vf0-vector)
|
|
;; this makes the upper 3x3 correct because R^-1 = R^T for any rotation.
|
|
;; but now the transform is in the wrong spot, and needs to be rotated
|
|
;; and inverted.
|
|
(matrix-transpose! dst src)
|
|
;; load the inverted rotation
|
|
(.lvf rot-row0 (&-> dst vector 0 quad))
|
|
(.lvf rot-row1 (&-> dst vector 1 quad))
|
|
(.lvf rot-row2 (&-> dst vector 2 quad))
|
|
;; set the right most column to [0, 0, 0]
|
|
(.sub.vf.w rot-row0 rot-row0 rot-row0)
|
|
(.sub.vf.w rot-row1 rot-row1 rot-row1)
|
|
(.sub.vf.w rot-row2 rot-row2 rot-row2)
|
|
;; load the original [translation, 1]
|
|
(.lvf orig-trans (&-> src vector 3 quad))
|
|
;; rotate the translation
|
|
(.mul.x.vf acc rot-row0 orig-trans)
|
|
(.add.mul.y.vf acc rot-row1 orig-trans acc)
|
|
(.add.mul.z.vf inv-trans rot-row2 orig-trans acc)
|
|
;; negate translation
|
|
(.sub.vf inv-trans vf0 inv-trans)
|
|
;; set the w to 1.0
|
|
(.mov.vf.w inv-trans vf0)
|
|
;; write back the rotation with fixed right column
|
|
(.svf (&-> dst vector 0 quad) rot-row0)
|
|
(.svf (&-> dst vector 1 quad) rot-row1)
|
|
(.svf (&-> dst vector 2 quad) rot-row2)
|
|
;; write back the negated and rotated translation.
|
|
(.svf (&-> dst vector 3 quad) inv-trans)
|
|
dst))
|
|
|
|
(defun matrix-4x4-inverse! ((dst matrix) (src matrix))
|
|
"Invert a homogeneous transform. Unlike matrix-inverse-of-rot-trans!, the upper 3x3 may carry
|
|
scale and shear. src and dst can be the same.
|
|
|
|
This is the adjugate inverse of the 3x3 part, arranged so there are no branches. The
|
|
r<row><lane>-r<row> registers hold the pairwise broadcast row products every cofactor needs;
|
|
the six differences taken from them are the cofactors, and their contraction with row 0 is the
|
|
determinant, which lands in det.x. One divide produces 1/det, which is broadcast and
|
|
multiplied in as each row is written. The cofactors are consumed transposed, which is what
|
|
makes the result the inverse rather than the adjugate. The translation, negated at the top of
|
|
the function, then goes through the new 3x3 and picks up w = 1.0.
|
|
|
|
vf23 through vf31 keep physical names deliberately: each is a determinant partial product in
|
|
the first half and an unrelated cofactor difference in the second."
|
|
(rlet ((acc :class vf)
|
|
(Q :class vf)
|
|
(vf0 :class vf)
|
|
(neg-trans :class vf)
|
|
(r2x-r0 :class vf)
|
|
(r2y-r0 :class vf)
|
|
(r2z-r0 :class vf)
|
|
(r2x-r1 :class vf)
|
|
(r2y-r1 :class vf)
|
|
(r2z-r1 :class vf)
|
|
(det-a :class vf)
|
|
(det-b :class vf)
|
|
(det-c :class vf)
|
|
(det-ab :class vf)
|
|
(inv-det :class vf)
|
|
(det :class vf)
|
|
(vf23 :class vf)
|
|
(vf24 :class vf)
|
|
(vf25 :class vf)
|
|
(vf26 :class vf)
|
|
(vf27 :class vf)
|
|
(vf28 :class vf)
|
|
(vf29 :class vf)
|
|
(inv-row0 :class vf)
|
|
(vf30 :class vf)
|
|
(vf31 :class vf)
|
|
(inv-row1 :class vf)
|
|
(inv-row2 :class vf)
|
|
(inv-trans :class vf)
|
|
(r1x-r0 :class vf)
|
|
(r1y-r0 :class vf)
|
|
(r1z-r0 :class vf))
|
|
;; ADDED: the original implementation does math on vectors where some values are
|
|
;; uninitialized. It doesn't use the result, so there's no problem, but this
|
|
;; may cause strange slowdowns on x86 where doing math on NaNs can be extremely slow.
|
|
;; ideally we should find a better solution.
|
|
(.xor.vf vf26 vf26 vf26)
|
|
(.xor.vf vf27 vf27 vf27)
|
|
(.xor.vf vf28 vf28 vf28)
|
|
(.xor.vf vf29 vf29 vf29)
|
|
(.xor.vf vf30 vf30 vf30)
|
|
(.xor.vf vf31 vf31 vf31)
|
|
(.xor.vf det-a det-a det-a)
|
|
(.xor.vf det-b det-b det-b)
|
|
(.xor.vf det-c det-c det-c)
|
|
(.xor.vf vf23 vf23 vf23)
|
|
(.xor.vf det-ab det-ab det-ab)
|
|
(.xor.vf inv-row0 inv-row0 inv-row0)
|
|
(.xor.vf inv-row1 inv-row1 inv-row1)
|
|
(.xor.vf inv-row2 inv-row2 inv-row2)
|
|
(init-vf0-vector)
|
|
(.lvf vf23 (&-> src vector 0 quad))
|
|
(.lvf vf24 (&-> src vector 1 quad))
|
|
(.lvf vf25 (&-> src vector 2 quad))
|
|
(.lvf neg-trans (&-> src vector 3 quad))
|
|
(.mul.x.vf r1x-r0 vf24 vf23)
|
|
(.mul.y.vf r1y-r0 vf24 vf23)
|
|
(.mul.z.vf r1z-r0 vf24 vf23)
|
|
(.mul.x.vf r2x-r0 vf25 vf23)
|
|
(.mul.y.vf r2y-r0 vf25 vf23)
|
|
(.mul.z.vf r2z-r0 vf25 vf23)
|
|
(.mul.x.vf r2x-r1 vf25 vf24)
|
|
(.mul.y.vf r2y-r1 vf25 vf24)
|
|
(.mul.z.vf r2z-r1 vf25 vf24)
|
|
(.mul.z.vf.y vf26 r1x-r0 vf25)
|
|
(.mul.z.vf.x vf27 r2y-r0 vf24)
|
|
(.mul.y.vf.x vf28 r1z-r0 vf25)
|
|
(.mul.z.vf.x vf29 r2y-r1 vf23)
|
|
(.mul.z.vf.x vf30 r1y-r0 vf25)
|
|
(.mul.y.vf.z vf31 r1x-r0 vf25)
|
|
(.add.y.vf.x det-a vf27 vf26)
|
|
(.sub.vf neg-trans vf0 neg-trans)
|
|
(.add.x.vf.x det-b vf29 vf30)
|
|
(.sub.z.vf.x det-c vf28 vf31)
|
|
(.sub.y.vf.z vf23 r2y-r1 r2z-r1)
|
|
(.sub.z.vf.x vf26 r2z-r1 r2x-r1)
|
|
(.sub.x.vf.y vf29 r2x-r1 r2y-r1)
|
|
(.sub.vf.x det-ab det-a det-b)
|
|
(.sub.z.vf.y vf24 r2z-r0 r2y-r0)
|
|
(.sub.x.vf.z vf27 r2x-r0 r2z-r0)
|
|
(.sub.y.vf.x vf30 r2y-r0 r2x-r0)
|
|
(.add.vf.x det det-ab det-c)
|
|
(.sub.y.vf.z vf25 r1y-r0 r1z-r0)
|
|
(.sub.z.vf.x vf28 r1z-r0 r1x-r0)
|
|
(.sub.x.vf.y vf31 r1x-r0 r1y-r0)
|
|
(.div.vf Q vf0 det :fsf #b11 :ftf #b0)
|
|
;;(.sub.w.vf.w inv-row0 inv-row0 inv-row0)
|
|
(.xor.vf inv-row0 inv-row0 inv-row0)
|
|
;;(.sub.w.vf.w inv-row1 inv-row1 inv-row1)
|
|
(.xor.vf inv-row1 inv-row1 inv-row1)
|
|
;;(.sub.w.vf.w inv-row2 inv-row2 inv-row2)
|
|
(.xor.vf inv-row2 inv-row2 inv-row2)
|
|
(.mov.vf.w inv-trans vf0)
|
|
(.wait.vf)
|
|
(.add.vf.x inv-det vf0 Q)
|
|
(.add.x.vf.xyz inv-det vf0 inv-det)
|
|
(.mul.z.vf.x inv-row0 inv-det vf23)
|
|
(.mul.x.vf.x inv-row1 inv-det vf26)
|
|
(.mul.y.vf.x inv-row2 inv-det vf29)
|
|
(.mul.y.vf.y inv-row0 inv-det vf24)
|
|
(.mul.z.vf.y inv-row1 inv-det vf27)
|
|
(.mul.x.vf.y inv-row2 inv-det vf30)
|
|
(.mul.z.vf.z inv-row0 inv-det vf25)
|
|
(.mul.x.vf.z inv-row1 inv-det vf28)
|
|
(.mul.y.vf.z inv-row2 inv-det vf31)
|
|
(.mul.x.vf acc inv-row0 neg-trans)
|
|
(.svf (&-> dst vector 0 quad) inv-row0)
|
|
(.add.mul.y.vf acc inv-row1 neg-trans acc)
|
|
(.svf (&-> dst vector 1 quad) inv-row1)
|
|
(.add.mul.z.vf.xyz inv-trans inv-row2 neg-trans acc)
|
|
(.svf (&-> dst vector 2 quad) inv-row2)
|
|
(.svf (&-> dst vector 3 quad) inv-trans)
|
|
dst))
|
|
|
|
(defun matrix-translate! ((dst matrix) (trans vector))
|
|
"Set dst to a homogeneous transform with only a translation of trans"
|
|
(matrix-identity! dst)
|
|
(set! (-> dst data 12) (-> trans data 0))
|
|
(set! (-> dst data 13) (-> trans data 1))
|
|
(set! (-> dst data 14) (-> trans data 2))
|
|
dst)
|
|
|
|
(defun matrix-translate+! ((dst matrix) (src matrix) (trans vector))
|
|
"Add the given translation to the translation of homogenous transform mat src
|
|
and store in dst. It is okay for dst = src."
|
|
(set! (-> dst data 12) (+ (-> src data 12) (-> trans data 0)))
|
|
(set! (-> dst data 13) (+ (-> src data 13) (-> trans data 1)))
|
|
(set! (-> dst data 14) (+ (-> src data 14) (-> trans data 2)))
|
|
(when (!= dst src)
|
|
;; only copy the untouched rows if we are storing in somewhere that's not the source
|
|
(set! (-> dst vector 0 quad) (-> src vector 0 quad))
|
|
(set! (-> dst vector 1 quad) (-> src vector 1 quad))
|
|
(set! (-> dst vector 2 quad) (-> src vector 2 quad)))
|
|
dst)
|
|
|
|
(defun matrix-scale! ((dst matrix) (scale vector))
|
|
"Set dst to a homogenous transform with only a scale. The x,y,z components
|
|
of scale become the x,y,z scaling factors"
|
|
(set! (-> dst vector 0 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 1 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 2 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 3 quad) (the-as uint128 0))
|
|
(set! (-> dst data 0) (-> scale data 0))
|
|
(set! (-> dst data 5) (-> scale data 1))
|
|
(set! (-> dst data 10) (-> scale data 2))
|
|
(set! (-> dst data 15) 1.0)
|
|
dst)
|
|
|
|
(defun scale-matrix! ((dst matrix) (scale vector) (src matrix))
|
|
"Scale an existing matrix. Okay for dst = src. The scaling is applied per row.
|
|
This means the x component of scale is used to scale the first row of src.
|
|
The w component of scale IS USED!! It scales the whole translation row, the 1.0 included, so
|
|
pass 1.0 there unless that is intended."
|
|
(rlet ((scale-bcast :class vf)
|
|
(row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(row3 :class vf))
|
|
(.lvf scale-bcast (&-> scale quad))
|
|
(.lvf row0 (&-> src vector 0 quad))
|
|
(.lvf row1 (&-> src vector 1 quad))
|
|
(.lvf row2 (&-> src vector 2 quad))
|
|
(.lvf row3 (&-> src vector 3 quad))
|
|
(.mul.x.vf row0 row0 scale-bcast)
|
|
(.mul.y.vf row1 row1 scale-bcast)
|
|
(.mul.z.vf row2 row2 scale-bcast)
|
|
(.mul.w.vf row3 row3 scale-bcast)
|
|
(.svf (&-> dst vector 0 quad) row0)
|
|
(.svf (&-> dst vector 1 quad) row1)
|
|
(.svf (&-> dst vector 2 quad) row2)
|
|
(.svf (&-> dst vector 3 quad) row3)
|
|
dst))
|
|
|
|
(defun matrix-inv-scale! ((dst matrix) (scale vector))
|
|
"Set dst to a homogeneous transform with only a scale.
|
|
The x,y,z components of scale are inverted and used as the x,y,z scaling factors"
|
|
(set! (-> dst vector 0 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 1 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 2 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 3 quad) (the-as uint128 0))
|
|
(set! (-> dst data 0) (/ 1.0 (-> scale data 0)))
|
|
(set! (-> dst data 5) (/ 1.0 (-> scale data 1)))
|
|
(set! (-> dst data 10) (/ 1.0 (-> scale data 2)))
|
|
(set! (-> dst data 15) 1.0)
|
|
dst)
|
|
|
|
(defun column-scale-matrix! ((dst matrix) (scale vector) (src matrix))
|
|
"Scale an existing matrix. Okay for dst = src. The scaling is applied column-wise.
|
|
Meaning the x component of scale will scale the first column of src. scale.w reaches the w
|
|
column, including the translation row's 1.0, so keep it at 1.0."
|
|
(rlet ((scale-bcast :class vf)
|
|
(row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(row3 :class vf))
|
|
(.lvf scale-bcast (&-> scale quad))
|
|
(.lvf row0 (&-> src vector 0 quad))
|
|
(.lvf row1 (&-> src vector 1 quad))
|
|
(.lvf row2 (&-> src vector 2 quad))
|
|
(.lvf row3 (&-> src vector 3 quad))
|
|
(.mul.vf row0 row0 scale-bcast)
|
|
(.mul.vf row1 row1 scale-bcast)
|
|
(.mul.vf row2 row2 scale-bcast)
|
|
(.mul.vf row3 row3 scale-bcast)
|
|
(.svf (&-> dst vector 0 quad) row0)
|
|
(.svf (&-> dst vector 1 quad) row1)
|
|
(.svf (&-> dst vector 2 quad) row2)
|
|
(.svf (&-> dst vector 3 quad) row3)
|
|
dst))
|
|
|
|
(defun matrix-rotate-x! ((dst matrix) (angle float))
|
|
"Set dst to a homogeneous rotation about X by angle in 65,536-units-per-turn rotation units."
|
|
(let ((rot-sin (sin angle))
|
|
(rot-cos (cos angle)))
|
|
(set! (-> dst vector 0 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 1 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 2 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 3 quad) (the-as uint128 0))
|
|
(set! (-> dst data 0) 1.0)
|
|
(set! (-> dst data 5) rot-cos)
|
|
(set! (-> dst data 6) rot-sin)
|
|
(set! (-> dst data 9) (- rot-sin))
|
|
(set! (-> dst data 10) rot-cos))
|
|
(set! (-> dst data 15) 1.0)
|
|
dst)
|
|
|
|
(defun matrix-rotate-y! ((dst matrix) (angle float))
|
|
"Set dst to a homogeneous rotation about Y by angle in 65,536-units-per-turn rotation units."
|
|
(let ((rot-sin (sin angle))
|
|
(rot-cos (cos angle)))
|
|
(set! (-> dst vector 0 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 1 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 2 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 3 quad) (the-as uint128 0))
|
|
(set! (-> dst data 0) rot-cos)
|
|
(set! (-> dst data 2) (- rot-sin))
|
|
(set! (-> dst data 5) 1.0)
|
|
(set! (-> dst data 8) rot-sin)
|
|
(set! (-> dst data 10) rot-cos))
|
|
(set! (-> dst data 15) 1.0)
|
|
dst)
|
|
|
|
(defun matrix-rotate-z! ((dst matrix) (angle float))
|
|
"Set dst to a homogeneous rotation about Z by angle in 65,536-units-per-turn rotation units."
|
|
(let ((rot-sin (sin angle))
|
|
(rot-cos (cos angle)))
|
|
(set! (-> dst vector 0 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 1 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 2 quad) (the-as uint128 0))
|
|
(set! (-> dst vector 3 quad) (the-as uint128 0))
|
|
(set! (-> dst data 0) rot-cos)
|
|
(set! (-> dst data 1) rot-sin)
|
|
(set! (-> dst data 4) (- rot-sin))
|
|
(set! (-> dst data 5) rot-cos))
|
|
(set! (-> dst data 10) 1.0)
|
|
(set! (-> dst data 15) 1.0)
|
|
dst)
|
|
|
|
(defun matrix-rotate-zyx! ((dst matrix) (rot-xyz-deg vector))
|
|
"Rotate in z,y,x order."
|
|
(let ((temp-mat (new-stack-matrix0))
|
|
(rot-mat (new-stack-matrix0)))
|
|
;; x-rot
|
|
(matrix-rotate-x! dst (-> rot-xyz-deg data 0))
|
|
;; y-rot
|
|
(matrix-rotate-y! temp-mat (-> rot-xyz-deg data 1))
|
|
;; rot-mat = yx
|
|
(matrix*! rot-mat temp-mat dst)
|
|
(matrix-rotate-z! temp-mat (-> rot-xyz-deg data 2))
|
|
;; dst = z*yz
|
|
(matrix*! dst temp-mat rot-mat))
|
|
dst)
|
|
|
|
(defun matrix-rotate-xyz! ((dst matrix) (rot-xyz-deg vector))
|
|
"Rotate in x,y,z order"
|
|
(let ((temp-mat (new-stack-matrix0))
|
|
(rot-mat (new-stack-matrix0)))
|
|
(matrix-rotate-z! dst (-> rot-xyz-deg data 2))
|
|
(matrix-rotate-y! temp-mat (-> rot-xyz-deg data 1))
|
|
(matrix*! rot-mat temp-mat dst)
|
|
(matrix-rotate-x! temp-mat (-> rot-xyz-deg data 0))
|
|
(matrix*! dst temp-mat rot-mat))
|
|
dst)
|
|
|
|
(defun matrix-rotate-zxy! ((dst matrix) (rot-xyz-deg vector))
|
|
"Rotate in z,x,y order"
|
|
(let ((temp-mat (new-stack-matrix0))
|
|
(rot-mat (new-stack-matrix0)))
|
|
(matrix-rotate-y! dst (-> rot-xyz-deg data 1))
|
|
(matrix-rotate-x! temp-mat (-> rot-xyz-deg data 0))
|
|
(matrix*! rot-mat temp-mat dst)
|
|
(matrix-rotate-z! temp-mat (-> rot-xyz-deg data 2))
|
|
(matrix*! dst temp-mat rot-mat))
|
|
dst)
|
|
|
|
(defun matrix-rotate-yxz! ((dst matrix) (rot-xyz-deg vector))
|
|
"Rotate in y,x,z order."
|
|
(let ((temp-mat (new-stack-matrix0))
|
|
(rot-mat (new-stack-matrix0)))
|
|
(matrix-rotate-z! dst (-> rot-xyz-deg data 2))
|
|
(matrix-rotate-x! temp-mat (-> rot-xyz-deg data 0))
|
|
(matrix*! rot-mat temp-mat dst)
|
|
(matrix-rotate-y! temp-mat (-> rot-xyz-deg data 1))
|
|
(matrix*! dst temp-mat rot-mat))
|
|
dst)
|
|
|
|
(defun matrix-rotate-yzx! ((dst matrix) (rot-xyz-deg vector))
|
|
"Rotate in y,z,x order. Unlike the other ordered-rotation helpers, this reads the z angle from
|
|
rot-xyz-deg.x and the x angle from rot-xyz-deg.z."
|
|
(let ((temp-mat (new-stack-matrix0))
|
|
(rot-mat (new-stack-matrix0)))
|
|
(matrix-rotate-z! dst (-> rot-xyz-deg data 0))
|
|
(matrix-rotate-x! temp-mat (-> rot-xyz-deg data 2))
|
|
(matrix*! rot-mat temp-mat dst)
|
|
(matrix-rotate-y! temp-mat (-> rot-xyz-deg data 1))
|
|
(matrix*! dst temp-mat rot-mat))
|
|
dst)
|
|
|
|
(defun matrix-rotate-yxy! ((dst matrix) (rots-deg vector))
|
|
"Rotate in y,x,y order. The two y angles are rots-deg.y - rots-deg.z and rots-deg.z. Compared
|
|
to the other rotations this one is quite a bit more optimized: it writes the product's
|
|
coefficients out directly instead of composing three matrices, and takes all six trig values
|
|
from one vector-sincos! call."
|
|
(let ((sincos-input (new 'stack-no-clear 'vector))
|
|
(sin-vec (new 'stack-no-clear 'vector))
|
|
(cos-vec (new 'stack-no-clear 'vector)))
|
|
;; the vector-sincos! lets us take the sine and cosine of 4 floats at a time.
|
|
;; it uses vector operations and reuses the taylor series coefficients to do this quickly
|
|
(set-vector! sincos-input (-> rots-deg x) (- (-> rots-deg y) (-> rots-deg z)) (-> rots-deg z) 1.0)
|
|
(vector-sincos! sin-vec cos-vec sincos-input)
|
|
(let ((cos-y (-> cos-vec data 1))
|
|
(sin-y (-> sin-vec data 1))
|
|
(cos-x (-> cos-vec data 0))
|
|
(sin-x (-> sin-vec data 0))
|
|
(cos-z (-> cos-vec data 2))
|
|
(sin-z (-> sin-vec data 2)))
|
|
;; just directly set the coefficients instead of doing wasteful multiplies.
|
|
(set! (-> dst data 0) (- (* cos-y cos-z) (* (* sin-y cos-x) sin-z)))
|
|
(set! (-> dst data 1) (* sin-y sin-x))
|
|
(set! (-> dst data 2) (- (+ (* cos-y sin-z) (* (* sin-y cos-x) cos-z))))
|
|
(set! (-> dst data 3) 0.0)
|
|
(set! (-> dst data 4) (* sin-x sin-z))
|
|
(set! (-> dst data 5) cos-x)
|
|
(set! (-> dst data 6) (* sin-x cos-z))
|
|
(set! (-> dst data 7) 0.0)
|
|
(set! (-> dst data 8) (+ (* sin-y cos-z) (* (* cos-y cos-x) sin-z)))
|
|
(set! (-> dst data 9) (- (* cos-y sin-x)))
|
|
(set! (-> dst data 10) (- (* (* cos-y cos-x) cos-z) (* sin-y sin-z)))))
|
|
(set! (-> dst data 11) 0.0)
|
|
(set! (-> dst data 12) 0.0)
|
|
(set! (-> dst data 13) 0.0)
|
|
(set! (-> dst data 14) 0.0)
|
|
(set! (-> dst data 15) 1.0)
|
|
dst)
|
|
|
|
(defun matrix-rotate-yx! ((dst matrix) (rot-y-deg float) (rot-x-deg float))
|
|
"Set dst to a Y rotation followed by an X rotation."
|
|
(matrix-rotate-y! dst rot-y-deg)
|
|
;; Construct the temporary X rotation directly in the multiply argument.
|
|
(matrix*! dst (matrix-rotate-x! (new-stack-matrix0) rot-x-deg) dst)
|
|
dst)
|
|
|
|
(defun matrix-axis-sin-cos-vu! ((dst matrix) (axis vector) (sin-angle float) (cos-angle float))
|
|
"Build an axis-angle rotation matrix from precomputed sine and cosine passed in EE registers."
|
|
(#unless PC_PORT
|
|
;; This entry point is the register-argument twin of matrix-axis-sin-cos!.
|
|
;; Preserve the original schedule, including the VU pipeline spacing.
|
|
(rlet ((dst-reg :reg a0)
|
|
(axis-reg :reg a1)
|
|
(sin-bits :reg a2)
|
|
(cos-bits :reg a3)
|
|
(half-mask :reg t0)
|
|
(axis-bits :reg v1 :class i128)
|
|
(xyz-mask :reg a1 :class i128)
|
|
(zero-lanes :reg a3 :class i128)
|
|
(identity-row0 :reg v1 :class i128)
|
|
(identity-row1 :reg a2 :class i128)
|
|
(identity-row2 :reg a1 :class i128)
|
|
(axis-vf :reg vf1)
|
|
(row0 :reg vf2)
|
|
(row1 :reg vf3)
|
|
(row2 :reg vf4)
|
|
(sin-cos :reg vf5)
|
|
(cos-vf :reg vf6)
|
|
(skew0 :reg vf7)
|
|
(skew1 :reg vf8)
|
|
(skew2 :reg vf9)
|
|
(one-minus-cos-axis :reg vf10)
|
|
(sin-axis :reg vf11))
|
|
(nop!)
|
|
(set! half-mask #xffff)
|
|
(l.q axis-bits axis-reg)
|
|
(set! xyz-mask (shl half-mask 48))
|
|
(m cos-vf cos-bits)
|
|
(.pceqw zero-lanes axis-bits r0)
|
|
(mmi-nop!)
|
|
(.ppach zero-lanes r0 zero-lanes)
|
|
(mmi-nop!)
|
|
(nop!)
|
|
(m axis-vf axis-bits)
|
|
(set! axis-bits (logior zero-lanes xyz-mask))
|
|
(m sin-cos sin-bits)
|
|
(+! axis-bits 1)
|
|
(.sub.x.vf.w sin-cos vf0 cos-vf)
|
|
(b.z axis-bits matrix-axis-vu-zero :delay (.add.x.vf.y sin-cos vf0 cos-vf))
|
|
(.sub.w.vf.w row0 vf0 vf0)
|
|
(nop!)
|
|
(.sub.w.vf.w row1 vf0 vf0)
|
|
(nop!)
|
|
(.sub.w.vf.w row2 vf0 vf0)
|
|
(nop!)
|
|
(.mul.x.vf.xyz sin-axis axis-vf sin-cos)
|
|
(nop!)
|
|
(.add.y.vf.x skew0 vf0 sin-cos)
|
|
(nop!)
|
|
(.add.y.vf.y skew1 vf0 sin-cos)
|
|
(nop!)
|
|
(.add.y.vf.z skew2 vf0 sin-cos)
|
|
(nop!)
|
|
(.mul.w.vf.xyz one-minus-cos-axis axis-vf sin-cos)
|
|
(nop!)
|
|
(.sub.z.vf.y skew0 vf0 sin-axis)
|
|
(nop!)
|
|
(.add.y.vf.z skew0 vf0 sin-axis)
|
|
(nop!)
|
|
(.add.z.vf.x skew1 vf0 sin-axis)
|
|
(nop!)
|
|
(.sub.x.vf.z skew1 vf0 sin-axis)
|
|
(nop!)
|
|
(.mul.x.vf.xyz row0 one-minus-cos-axis axis-vf)
|
|
(nop!)
|
|
(.mul.y.vf.xyz row1 one-minus-cos-axis axis-vf)
|
|
(nop!)
|
|
(.mul.z.vf.xyz row2 one-minus-cos-axis axis-vf)
|
|
(nop!)
|
|
(.sub.y.vf.x skew2 vf0 sin-axis)
|
|
(nop!)
|
|
(.add.x.vf.y skew2 vf0 sin-axis)
|
|
(nop!)
|
|
(.add.vf.xyz row0 row0 skew0)
|
|
(nop!)
|
|
(.add.vf.xyz row1 row1 skew1)
|
|
(nop!)
|
|
(nop!)
|
|
(nop!)
|
|
(.add.vf.xyz row2 row2 skew2)
|
|
(nop!)
|
|
(nop!)
|
|
(.svf dst-reg row0)
|
|
(nop!)
|
|
(.svf dst-reg row1 :offset 16)
|
|
(nop!)
|
|
(.svf dst-reg vf0 :offset 48)
|
|
(b matrix-axis-vu-done :delay (.svf dst-reg row2 :offset 32))
|
|
matrix-axis-vu-zero
|
|
;; Form the identity rows by rotating the words of [1, 0, 0, 0].
|
|
(lui identity-row0 #x3f80)
|
|
(.svf dst-reg vf0 :offset 48)
|
|
(.pcpyld identity-row0 r0 identity-row0)
|
|
(mmi-nop!)
|
|
(.prot3w identity-row2 identity-row0)
|
|
(mmi-nop!)
|
|
(.prot3w identity-row1 identity-row2)
|
|
(s.q identity-row0 dst-reg)
|
|
(nop!)
|
|
(s.q identity-row2 dst-reg 32)
|
|
(nop!)
|
|
(s.q identity-row1 dst-reg 16)
|
|
matrix-axis-vu-done
|
|
(m v0 dst-reg)
|
|
(j ra :delay (m sp sp))
|
|
(nop!)
|
|
(nop!)
|
|
(nop!)))
|
|
(#when PC_PORT
|
|
;; The special register calling convention is irrelevant on the host.
|
|
(matrix-axis-sin-cos! dst axis sin-angle cos-angle))
|
|
(none))
|
|
|
|
(defun matrix-axis-sin-cos! ((dst matrix) (axis vector) (sin-angle float) (cos-angle float))
|
|
"Build an axis-angle rotation matrix from precomputed sine and cosine. axis must be unit
|
|
length; its w is ignored, and a zero xyz axis gives the identity.
|
|
|
|
Rodrigues' formula, R = (1-cos)(a outer a) + cos*I + sin*[a]x. The outer-product rows come
|
|
from broadcasting one lane of (1-cos)*axis against the axis. skew0 through skew2 hold the
|
|
cross-product term and the cosine diagonal together, built from lanes of sin*axis and the
|
|
cosine itself. matrix-axis-sin-cos-vu! above is the same code scheduled for the real VU0
|
|
latencies."
|
|
(rlet ((sin-cos :class vf) ;; x = sine, y = cosine, w = -cosine
|
|
(cos-bcast :class vf) ;; x = cosine, as it arrives from the FPU
|
|
(axis-vf :class vf) ;; the rotation axis
|
|
(vf0 :class vf)
|
|
(row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(sin-axis :class vf)
|
|
(skew0 :class vf)
|
|
(skew1 :class vf)
|
|
(skew2 :class vf)
|
|
(one-minus-cos-axis :class vf)
|
|
(zero-lanes :class i128 :type uint128)
|
|
(zero :class i128 :type uint128)) ;; will be used for zero check.
|
|
;; initialize constant vectors
|
|
(init-vf0-vector)
|
|
(.xor.p zero zero zero)
|
|
;; this is an overly complicated check to see if xyz = 0.
|
|
(let ((w-mask 65535)
|
|
(axis-bits (-> axis quad)))
|
|
;; ffff in the halfword lane that the packed compare below puts w into.
|
|
(set! w-mask (shl w-mask 48)) ;; ffff'0000'0000'0000
|
|
(.mov cos-bcast cos-angle) ;; bits go straight across and land in x
|
|
;; Is xyz zero? pceqw gives all ones for each zero word, and ppach folds the four high
|
|
;; halfwords down into [w0? z0? y0? x0?].
|
|
(.pceqw zero-lanes axis-bits zero)
|
|
(.ppach zero-lanes zero zero-lanes)
|
|
(.mov axis-vf axis-bits) ;; should be quad move!
|
|
;; Force the packed w lane on, so a nonzero w cannot rescue a zero xyz.
|
|
(let ((xyz-zero (logior w-mask (the uint zero-lanes))))
|
|
(.mov sin-cos sin-angle)
|
|
;; Only all ones wraps to zero, so one branch covers all three lanes.
|
|
(+! xyz-zero 1)
|
|
(.sub.x.vf.w sin-cos vf0 cos-bcast)
|
|
(.add.x.vf.y sin-cos vf0 cos-bcast)
|
|
(when-goto (zero? xyz-zero) zero-case)))
|
|
;; Nonzero axis. The original cleared these by subtracting vf0's w from itself. The xor is
|
|
;; here so a stale NaN in an untouched lane cannot survive.
|
|
(.xor.vf row0 row0 row0)
|
|
(.xor.vf row1 row1 row1)
|
|
(.xor.vf row2 row2 row2)
|
|
(.mul.x.vf.xyz sin-axis axis-vf sin-cos)
|
|
(.add.y.vf.x skew0 vf0 sin-cos)
|
|
(.add.y.vf.y skew1 vf0 sin-cos)
|
|
(.add.y.vf.z skew2 vf0 sin-cos)
|
|
(.mul.w.vf.xyz one-minus-cos-axis axis-vf sin-cos)
|
|
(.sub.z.vf.y skew0 vf0 sin-axis)
|
|
(.add.y.vf.z skew0 vf0 sin-axis)
|
|
(.add.z.vf.x skew1 vf0 sin-axis)
|
|
(.sub.x.vf.z skew1 vf0 sin-axis)
|
|
(.mul.x.vf.xyz row0 one-minus-cos-axis axis-vf)
|
|
(.mul.y.vf.xyz row1 one-minus-cos-axis axis-vf)
|
|
(.mul.z.vf.xyz row2 one-minus-cos-axis axis-vf)
|
|
(.sub.y.vf.x skew2 vf0 sin-axis)
|
|
(.add.x.vf.y skew2 vf0 sin-axis)
|
|
(.add.vf.xyz row0 row0 skew0)
|
|
(.add.vf.xyz row1 row1 skew1)
|
|
(.add.vf.xyz row2 row2 skew2)
|
|
(.svf (-> dst vector 0) row0)
|
|
(.svf (-> dst vector 1) row1)
|
|
(.svf (-> dst vector 3) vf0)
|
|
(.svf (-> dst vector 2) row2)
|
|
(goto end)
|
|
(label zero-case)
|
|
;; Zero axis, so write the identity. This builds [1, 0, 0, 0] from a single lui (0x3f80 is
|
|
;; the top half of 1.0) and rotates its words for the y and z rows, avoiding a constant load.
|
|
(#unless PC_PORT
|
|
(rlet ((dst-reg :reg v0)
|
|
(identity-row0 :reg v1 :class i128)
|
|
(identity-row1 :reg a1 :class i128)
|
|
(identity-row2 :reg a0 :class i128))
|
|
(lui identity-row0 #x3f80)
|
|
(.svf dst-reg vf0 :offset 48)
|
|
(.pcpyld identity-row0 r0 identity-row0)
|
|
(mmi-nop!)
|
|
(.prot3w identity-row2 identity-row0)
|
|
(mmi-nop!)
|
|
(.prot3w identity-row1 identity-row2)
|
|
(s.q identity-row0 dst-reg)
|
|
(nop!)
|
|
(s.q identity-row2 dst-reg 32)
|
|
(nop!)
|
|
(s.q identity-row1 dst-reg 16)))
|
|
(#when PC_PORT
|
|
(matrix-identity! dst))
|
|
(label end))
|
|
dst)
|
|
|
|
(defun matrix-axis-angle! ((dst matrix) (axis vector) (angle-deg float))
|
|
"Create an axis-angle rotation matrix."
|
|
(matrix-axis-sin-cos! dst axis (sin angle-deg) (cos angle-deg))
|
|
(none))
|
|
|
|
(defun matrix-lerp! ((dst matrix) (src1 matrix) (src2 matrix) (alpha float))
|
|
"Lerp an entire matrix, coefficient-wise. All sixteen entries, w column included. This is not
|
|
a rotation blend, since halfway between two rotations is not a rotation, but it is fine for
|
|
matrices that are already close together. dst may alias either source."
|
|
(rlet ((row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(row3 :class vf)
|
|
(delta0 :class vf)
|
|
(delta1 :class vf)
|
|
(delta2 :class vf)
|
|
(delta3 :class vf)
|
|
(alpha-bcast :class vf))
|
|
;; this uses src1 + alpha * (src2 - src1)
|
|
;; it seems like src1 * (1 - alpha) + src2 * alpha might have been faster?
|
|
(.mov alpha-bcast alpha)
|
|
(.lvf row0 (&-> src1 vector 0 quad))
|
|
(.lvf row1 (&-> src1 vector 1 quad))
|
|
(.lvf row2 (&-> src1 vector 2 quad))
|
|
(.lvf row3 (&-> src1 vector 3 quad))
|
|
(.lvf delta0 (&-> src2 vector 0 quad))
|
|
(.lvf delta1 (&-> src2 vector 1 quad))
|
|
(.lvf delta2 (&-> src2 vector 2 quad))
|
|
(.lvf delta3 (&-> src2 vector 3 quad))
|
|
(.sub.vf delta0 delta0 row0)
|
|
(.sub.vf delta1 delta1 row1)
|
|
(.sub.vf delta2 delta2 row2)
|
|
(.sub.vf delta3 delta3 row3)
|
|
(.mul.x.vf delta0 delta0 alpha-bcast)
|
|
(.mul.x.vf delta1 delta1 alpha-bcast)
|
|
(.mul.x.vf delta2 delta2 alpha-bcast)
|
|
(.mul.x.vf delta3 delta3 alpha-bcast)
|
|
(.add.vf row0 row0 delta0)
|
|
(.add.vf row1 row1 delta1)
|
|
(.add.vf row2 row2 delta2)
|
|
(.add.vf row3 row3 delta3)
|
|
(.svf (&-> dst vector 0 quad) row0)
|
|
(.svf (&-> dst vector 1 quad) row1)
|
|
(.svf (&-> dst vector 2 quad) row2)
|
|
(.svf (&-> dst vector 3 quad) row3)
|
|
dst))
|
|
|
|
(defun matrix-3x3-determinant ((mat matrix))
|
|
"Compute the determinant of a 3x3 matrix. Scalar FPU code; matrix3-determinant does the same
|
|
thing on VU0."
|
|
(let ((m00 (-> mat data 0))
|
|
(m01 (-> mat data 1))
|
|
(m02 (-> mat data 2))
|
|
(m10 (-> mat data 4))
|
|
(m11 (-> mat data 5))
|
|
(m12 (-> mat data 6))
|
|
(m20 (-> mat data 8))
|
|
(m21 (-> mat data 9))
|
|
(m22 (-> mat data 10)))
|
|
(- (+ (+ (* (* m00 m11) m22) (* (* m01 m12) m20)) (* (* m02 m10) m21))
|
|
(+ (+ (* (* m00 m12) m21) (* (* m02 m11) m20)) (* (* m01 m10) m22)))))
|
|
|
|
(defun matrix3-determinant ((mat matrix))
|
|
"3x3 determinant as row0 . (row1 x row2), on VU0. Unused in Jak 1. The result crosses back
|
|
through a bit move, so the local is declared int and then reinterpreted; it is a float."
|
|
(local-vars (result-bits int))
|
|
(rlet ((acc :class vf)
|
|
(row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(cross12 :class vf))
|
|
(.lvf row1 (&-> mat vector 1 quad))
|
|
(.lvf row2 (&-> mat vector 2 quad))
|
|
(.lvf row0 (&-> mat vector 0 quad))
|
|
(.outer.product.a.vf acc row1 row2)
|
|
(.outer.product.b.vf cross12 row2 row1 acc)
|
|
(.mul.vf.xyz cross12 cross12 row0)
|
|
(.add.y.vf.x cross12 cross12 cross12)
|
|
(.add.z.vf.x cross12 cross12 cross12)
|
|
(.mov result-bits cross12)
|
|
(the-as float result-bits)))
|
|
|
|
(defun matrix-3x3-inverse! ((dst matrix) (src matrix))
|
|
"Compute the inverse of a 3x3 matrix, dst[i][j] = C[j][i] / det. Only the nine entries are
|
|
written; dst's fourth row and column are left alone. Not very efficient, since it divides nine
|
|
times where once would do. Requires src != dst."
|
|
(let ((determinant (matrix-3x3-determinant src)))
|
|
(set! (-> dst vector 0 x)
|
|
(/ (- (* (-> src vector 1 y) (-> src vector 2 z)) (* (-> src vector 1 z) (-> src vector 2 y))) determinant))
|
|
(set! (-> dst vector 1 x)
|
|
(/ (- (* (-> src vector 1 z) (-> src vector 2 x)) (* (-> src vector 1 x) (-> src vector 2 z))) determinant))
|
|
(set! (-> dst vector 2 x)
|
|
(/ (- (* (-> src vector 1 x) (-> src vector 2 y)) (* (-> src vector 1 y) (-> src vector 2 x))) determinant))
|
|
(set! (-> dst vector 0 y)
|
|
(/ (- (* (-> src vector 2 y) (-> src vector 0 z)) (* (-> src vector 2 z) (-> src vector 0 y))) determinant))
|
|
(set! (-> dst vector 1 y)
|
|
(/ (- (* (-> src vector 2 z) (-> src vector 0 x)) (* (-> src vector 2 x) (-> src vector 0 z))) determinant))
|
|
(set! (-> dst vector 2 y)
|
|
(/ (- (* (-> src vector 2 x) (-> src vector 0 y)) (* (-> src vector 2 y) (-> src vector 0 x))) determinant))
|
|
(set! (-> dst vector 0 z)
|
|
(/ (- (* (-> src vector 0 y) (-> src vector 1 z)) (* (-> src vector 0 z) (-> src vector 1 y))) determinant))
|
|
(set! (-> dst vector 1 z)
|
|
(/ (- (* (-> src vector 0 z) (-> src vector 1 x)) (* (-> src vector 0 x) (-> src vector 1 z))) determinant))
|
|
(set! (-> dst vector 2 z)
|
|
(/ (- (* (-> src vector 0 x) (-> src vector 1 y)) (* (-> src vector 0 y) (-> src vector 1 x))) determinant)))
|
|
dst)
|
|
|
|
(defun matrix-3x3-inverse-transpose! ((dst matrix) (src matrix))
|
|
"Invert and transpose, dst[i][j] = C[i][j] / det. Same nine products as matrix-3x3-inverse!
|
|
with the destination indices swapped. This is the matrix to apply to normals when the
|
|
transform carries scale or shear.
|
|
Requires dst != src."
|
|
(let ((determinant (matrix-3x3-determinant src)))
|
|
(set! (-> dst vector 0 x)
|
|
(/ (- (* (-> src vector 1 y) (-> src vector 2 z)) (* (-> src vector 1 z) (-> src vector 2 y))) determinant))
|
|
(set! (-> dst vector 0 y)
|
|
(/ (- (* (-> src vector 1 z) (-> src vector 2 x)) (* (-> src vector 1 x) (-> src vector 2 z))) determinant))
|
|
(set! (-> dst vector 0 z)
|
|
(/ (- (* (-> src vector 1 x) (-> src vector 2 y)) (* (-> src vector 1 y) (-> src vector 2 x))) determinant))
|
|
(set! (-> dst vector 1 x)
|
|
(/ (- (* (-> src vector 2 y) (-> src vector 0 z)) (* (-> src vector 2 z) (-> src vector 0 y))) determinant))
|
|
(set! (-> dst vector 1 y)
|
|
(/ (- (* (-> src vector 2 z) (-> src vector 0 x)) (* (-> src vector 2 x) (-> src vector 0 z))) determinant))
|
|
(set! (-> dst vector 1 z)
|
|
(/ (- (* (-> src vector 2 x) (-> src vector 0 y)) (* (-> src vector 2 y) (-> src vector 0 x))) determinant))
|
|
(set! (-> dst vector 2 x)
|
|
(/ (- (* (-> src vector 0 y) (-> src vector 1 z)) (* (-> src vector 0 z) (-> src vector 1 y))) determinant))
|
|
(set! (-> dst vector 2 y)
|
|
(/ (- (* (-> src vector 0 z) (-> src vector 1 x)) (* (-> src vector 0 x) (-> src vector 1 z))) determinant))
|
|
(set! (-> dst vector 2 z)
|
|
(/ (- (* (-> src vector 0 x) (-> src vector 1 y)) (* (-> src vector 0 y) (-> src vector 1 x))) determinant)))
|
|
dst)
|
|
|
|
(defun matrix3-inverse-transpose! ((dst matrix) (src matrix))
|
|
"Invert and transpose the upper 3x3, on VU0. Unused in Jak 1. The three cofactor rows are the
|
|
three pairwise cross products, and row0 . cofactor0 is the determinant, so this needs one
|
|
divide and three multiplies. The runs of .nop.vf are the original's pipeline spacing around
|
|
the dependent broadcast adds and the divide."
|
|
(rlet ((acc :class vf)
|
|
(Q :class vf)
|
|
(vf0 :class vf)
|
|
(det-terms :class vf)
|
|
(row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(cofactor0 :class vf)
|
|
(cofactor1 :class vf)
|
|
(cofactor2 :class vf))
|
|
(init-vf0-vector)
|
|
;; og:preserve-this
|
|
;; The original leaves the unused fourth lanes undefined. Clear them on PC so
|
|
;; host SIMD cannot propagate stale values or NaNs into the stored matrix.
|
|
(.xor.vf cofactor0 cofactor0 cofactor0)
|
|
(.xor.vf cofactor1 cofactor1 cofactor1)
|
|
(.xor.vf cofactor2 cofactor2 cofactor2)
|
|
(.lvf row0 (&-> src vector 0 quad))
|
|
(.lvf row1 (&-> src vector 1 quad))
|
|
(.lvf row2 (&-> src vector 2 quad))
|
|
(.outer.product.a.vf acc row1 row2)
|
|
(.outer.product.b.vf cofactor0 row2 row1 acc)
|
|
(.outer.product.a.vf acc row2 row0)
|
|
(.outer.product.b.vf cofactor1 row0 row2 acc)
|
|
(.mul.vf.xyz det-terms row0 cofactor0)
|
|
(.outer.product.a.vf acc row0 row1)
|
|
(.outer.product.b.vf cofactor2 row1 row0 acc)
|
|
(.nop.vf)
|
|
(.add.y.vf.x det-terms det-terms det-terms)
|
|
(.nop.vf)
|
|
(.nop.vf)
|
|
(.nop.vf)
|
|
(.add.z.vf.x det-terms det-terms det-terms)
|
|
(.nop.vf)
|
|
(.nop.vf)
|
|
(.nop.vf)
|
|
(.div.vf Q vf0 det-terms :fsf #b11 :ftf #b0)
|
|
(.wait.vf)
|
|
(.mul.vf cofactor0 cofactor0 Q)
|
|
(.mul.vf cofactor1 cofactor1 Q)
|
|
(.mul.vf cofactor2 cofactor2 Q)
|
|
(.nop.vf)
|
|
(.nop.vf)
|
|
(.svf (&-> dst vector 0 quad) cofactor0)
|
|
(.svf (&-> dst vector 1 quad) cofactor1)
|
|
(.svf (&-> dst vector 2 quad) cofactor2)
|
|
dst))
|
|
|
|
(defun matrix-4x4-determinant ((mat matrix))
|
|
"4x4 determinant. Broken for a general matrix; see the two bad terms below."
|
|
(let ((m00 (-> mat vector 0 x))
|
|
(m01 (-> mat vector 0 y))
|
|
(m02 (-> mat vector 0 z))
|
|
(m03 (-> mat vector 0 w))
|
|
(m10 (-> mat vector 1 x))
|
|
(m11 (-> mat vector 1 y))
|
|
(m12 (-> mat vector 1 z))
|
|
(m13 (-> mat vector 1 w))
|
|
(m20 (-> mat vector 2 x))
|
|
(m21 (-> mat vector 2 y))
|
|
(m22 (-> mat vector 2 z))
|
|
(m23 (-> mat vector 2 w))
|
|
(m30 (-> mat vector 3 x))
|
|
(m31 (-> mat vector 3 y))
|
|
(m32 (-> mat vector 3 z))
|
|
(m33 (-> mat vector 3 w)))
|
|
;; Cofactor expansion along row 0: m00*C00 - m01*C01 + m02*C02 - m03*C03, with each Cij
|
|
;; spelled out as the six products of its minor. Two things are wrong with it.
|
|
;; (* m01 m12 m20 m32) uses column 2 twice and column 3 not at all; it should be m33, and it
|
|
;; belongs in the other group. And all six m03 products are on the wrong side, so this adds
|
|
;; +m03*C03 where the alternating sign calls for -m03*C03 and every m03 term comes out
|
|
;; negated. Either defect alone spoils a general matrix. A rotation with no translation
|
|
;; survives the sign problem, since its m03 is zero.
|
|
(- (+ (* m00 m11 m22 m33)
|
|
(* m00 m12 m23 m31)
|
|
(* m00 m13 m21 m32)
|
|
(* m01 m10 m23 m32)
|
|
(* m01 m12 m20 m32)
|
|
(* m01 m13 m22 m30)
|
|
(* m02 m10 m21 m33)
|
|
(* m02 m11 m23 m30)
|
|
(* m02 m13 m20 m31)
|
|
(* m03 m10 m21 m32)
|
|
(* m03 m11 m22 m30)
|
|
(* m03 m12 m20 m31))
|
|
(+ (* m00 m11 m23 m32)
|
|
(* m00 m12 m21 m33)
|
|
(* m00 m13 m22 m31)
|
|
(* m01 m10 m22 m33)
|
|
(* m01 m12 m23 m30)
|
|
(* m01 m13 m20 m32)
|
|
(* m02 m10 m23 m31)
|
|
(* m02 m11 m20 m33)
|
|
(* m02 m13 m21 m30)
|
|
(* m03 m10 m22 m31)
|
|
(* m03 m11 m20 m32)
|
|
(* m03 m12 m21 m30)))))
|
|
|
|
(defun matrix-4x4-inverse-transpose! ((dst matrix) (src matrix))
|
|
"Invert and transpose the full 4x4, dst[i][j] = C[i][j] / det. All sixteen entries are the same
|
|
3x3 determinant applied to the minor left after dropping row i and column j, with the
|
|
alternating sign carried by the outer negate. Requires dst != src. Unused, and it could not
|
|
work as written: the divisor comes from matrix-4x4-determinant, which is broken."
|
|
(let ((determinant (matrix-4x4-determinant src)))
|
|
(let ((a00 (-> src vector 1 y))
|
|
(a01 (-> src vector 1 z))
|
|
(a02 (-> src vector 1 w))
|
|
(a10 (-> src vector 2 y))
|
|
(a11 (-> src vector 2 z))
|
|
(a12 (-> src vector 2 w))
|
|
(a20 (-> src vector 3 y))
|
|
(a21 (-> src vector 3 z))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 0 x)
|
|
(/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant)))
|
|
(let ((a00 (-> src vector 1 x))
|
|
(a01 (-> src vector 1 z))
|
|
(a02 (-> src vector 1 w))
|
|
(a10 (-> src vector 2 x))
|
|
(a11 (-> src vector 2 z))
|
|
(a12 (-> src vector 2 w))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 z))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 0 y)
|
|
(- (/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
(let ((a00 (-> src vector 1 x))
|
|
(a01 (-> src vector 1 y))
|
|
(a02 (-> src vector 1 w))
|
|
(a10 (-> src vector 2 x))
|
|
(a11 (-> src vector 2 y))
|
|
(a12 (-> src vector 2 w))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 y))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 0 z)
|
|
(/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant)))
|
|
(let ((a00 (-> src vector 1 x))
|
|
(a01 (-> src vector 1 y))
|
|
(a02 (-> src vector 1 z))
|
|
(a10 (-> src vector 2 x))
|
|
(a11 (-> src vector 2 y))
|
|
(a12 (-> src vector 2 z))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 y))
|
|
(a22 (-> src vector 3 z)))
|
|
(set! (-> dst vector 0 w)
|
|
(- (/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
(let ((a00 (-> src vector 0 y))
|
|
(a01 (-> src vector 0 z))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 2 y))
|
|
(a11 (-> src vector 2 z))
|
|
(a12 (-> src vector 2 w))
|
|
(a20 (-> src vector 3 y))
|
|
(a21 (-> src vector 3 z))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 1 x)
|
|
(- (/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 z))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 2 x))
|
|
(a11 (-> src vector 2 z))
|
|
(a12 (-> src vector 2 w))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 z))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 1 y)
|
|
(/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant)))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 y))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 2 x))
|
|
(a11 (-> src vector 2 y))
|
|
(a12 (-> src vector 2 w))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 y))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 1 z)
|
|
(- (/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 y))
|
|
(a02 (-> src vector 0 z))
|
|
(a10 (-> src vector 2 x))
|
|
(a11 (-> src vector 2 y))
|
|
(a12 (-> src vector 2 z))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 y))
|
|
(a22 (-> src vector 3 z)))
|
|
(set! (-> dst vector 1 w)
|
|
(/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant)))
|
|
(let ((a00 (-> src vector 0 y))
|
|
(a01 (-> src vector 0 z))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 1 y))
|
|
(a11 (-> src vector 1 z))
|
|
(a12 (-> src vector 1 w))
|
|
(a20 (-> src vector 3 y))
|
|
(a21 (-> src vector 3 z))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 2 x)
|
|
(/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant)))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 z))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 1 x))
|
|
(a11 (-> src vector 1 z))
|
|
(a12 (-> src vector 1 w))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 z))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 2 y)
|
|
(- (/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 y))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 1 x))
|
|
(a11 (-> src vector 1 y))
|
|
(a12 (-> src vector 1 w))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 y))
|
|
(a22 (-> src vector 3 w)))
|
|
(set! (-> dst vector 2 z)
|
|
(/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant)))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 y))
|
|
(a02 (-> src vector 0 z))
|
|
(a10 (-> src vector 1 x))
|
|
(a11 (-> src vector 1 y))
|
|
(a12 (-> src vector 1 z))
|
|
(a20 (-> src vector 3 x))
|
|
(a21 (-> src vector 3 y))
|
|
(a22 (-> src vector 3 z)))
|
|
(set! (-> dst vector 2 w)
|
|
(- (/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
(let ((a00 (-> src vector 0 y))
|
|
(a01 (-> src vector 0 z))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 1 y))
|
|
(a11 (-> src vector 1 z))
|
|
(a12 (-> src vector 1 w))
|
|
(a20 (-> src vector 2 y))
|
|
(a21 (-> src vector 2 z))
|
|
(a22 (-> src vector 2 w)))
|
|
(set! (-> dst vector 3 x)
|
|
(- (/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 z))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 1 x))
|
|
(a11 (-> src vector 1 z))
|
|
(a12 (-> src vector 1 w))
|
|
(a20 (-> src vector 2 x))
|
|
(a21 (-> src vector 2 z))
|
|
(a22 (-> src vector 2 w)))
|
|
(set! (-> dst vector 3 y)
|
|
(/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant)))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 y))
|
|
(a02 (-> src vector 0 w))
|
|
(a10 (-> src vector 1 x))
|
|
(a11 (-> src vector 1 y))
|
|
(a12 (-> src vector 1 w))
|
|
(a20 (-> src vector 2 x))
|
|
(a21 (-> src vector 2 y))
|
|
(a22 (-> src vector 2 w)))
|
|
(set! (-> dst vector 3 z)
|
|
(- (/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
(let ((a00 (-> src vector 0 x))
|
|
(a01 (-> src vector 0 y))
|
|
(a02 (-> src vector 0 z))
|
|
(a10 (-> src vector 1 x))
|
|
(a11 (-> src vector 1 y))
|
|
(a12 (-> src vector 1 z))
|
|
(a20 (-> src vector 2 x))
|
|
(a21 (-> src vector 2 y))
|
|
(a22 (-> src vector 2 z)))
|
|
(set! (-> dst vector 3 w)
|
|
(/ (- (+ (* a00 a11 a22) (* a01 a12 a20) (* a02 a10 a21)) (+ (* a00 a12 a21) (* a02 a11 a20) (* a01 a10 a22)))
|
|
determinant))))
|
|
dst)
|
|
|
|
(defun matrix-y-angle ((mat matrix))
|
|
"If mat has its upper 3x3 as a rotation, gets the y axis rotation. This is the atan of the
|
|
rotated z axis, which is row 2."
|
|
(let ((z-row (&-> mat data 8))) (atan (-> z-row 0) (-> z-row 2))))
|
|
|
|
(defmethod transform-vectors! ((this matrix) (dst (inline-array vector)) (src (inline-array vector)) (count int))
|
|
"Transform many vectors. This acts like w = 1, even if it isn't. The value of w is copied,
|
|
because the last multiply-add of each vector writes xyz only. dst and src may be the same
|
|
array.
|
|
|
|
The loop is unrolled four wide and pipelined: the next pair of source quadwords is already
|
|
loading while the current pair is in the accumulator, and each count test sits one vector
|
|
ahead of the store it guards. That is where the three separate decrements of remaining come
|
|
from, and it does not survive reordering."
|
|
(rlet ((vf0 :class vf)
|
|
(row0 :class vf)
|
|
(row1 :class vf)
|
|
(row2 :class vf)
|
|
(row3 :class vf)
|
|
(point0 :class vf)
|
|
(point1 :class vf)
|
|
(point2 :class vf)
|
|
(point3 :class vf)
|
|
(acc :class vf)
|
|
(remaining :type int))
|
|
(init-vf0-vector)
|
|
(when-goto (<= count 0) end)
|
|
(.lvf row0 (-> this vector 0))
|
|
(.lvf row1 (-> this vector 1))
|
|
(.lvf row2 (-> this vector 2))
|
|
(.lvf row3 (-> this vector 3))
|
|
(.lvf point0 (-> src 0))
|
|
(.lvf point1 (-> src 1))
|
|
(label loop-top)
|
|
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
|
|
(.mul.w.vf acc row3 vf0)
|
|
(.lvf point2 (-> src 2))
|
|
(.add.mul.x.vf acc row0 point0 acc)
|
|
(.lvf point3 (-> src 3))
|
|
(.add.mul.y.vf acc row1 point0 acc)
|
|
;;(&+! src 64)
|
|
(set! src (the (inline-array vector) (+ 64 (the int src))))
|
|
(.add.mul.z.vf.xyz point0 row2 point0 acc)
|
|
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
|
|
(set! remaining (- count 1))
|
|
(.mul.w.vf acc row3 vf0)
|
|
(.add.mul.x.vf acc row0 point1 acc)
|
|
;; vmadday.xyzw acc, row1, point1
|
|
(.add.mul.y.vf acc row1 point1 acc)
|
|
;; vmaddz.xyz point1, row2, point1
|
|
(.add.mul.z.vf.xyz point1 row2 point1 acc)
|
|
(.svf (-> dst 0) point0)
|
|
(.mul.w.vf acc row3 vf0)
|
|
(when-goto (zero? remaining) end)
|
|
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
|
|
(.add.mul.x.vf acc row0 point2 acc)
|
|
(+! remaining -1)
|
|
(.add.mul.y.vf acc row1 point2 acc)
|
|
(.lvf point0 (-> src 0))
|
|
(.add.mul.z.vf.xyz point2 row2 point2 acc)
|
|
(.svf (-> dst 1) point1)
|
|
(.mul.w.vf acc row3 vf0)
|
|
(when-goto (zero? remaining) end)
|
|
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
|
|
(.add.mul.x.vf acc row0 point3 acc)
|
|
(+! remaining -1)
|
|
(.add.mul.y.vf acc row1 point3 acc)
|
|
(.lvf point1 (-> src 1))
|
|
(.add.mul.z.vf.xyz point3 row2 point3 acc)
|
|
(.svf (-> dst 2) point2)
|
|
;;(&!+ dst 64)
|
|
(set! dst (the (inline-array vector) (+ 64 (the int dst))))
|
|
(when-goto (zero? remaining) end)
|
|
(set! count (- remaining 1))
|
|
(.svf dst point3 :offset -16)
|
|
(when-goto (not (zero? count)) loop-top)
|
|
(label end)
|
|
(none)))
|