Archive/jak-project
1
0
Fork 0
mirror of https://github.com/open-goal/jak-project.git synced 2024-10-20 11:26:18 -04:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity
jak-project/goal_src/jak3/engine/math/matrix.gc
Hat Kid 9a4929ac0c
decomp3: some engine files (#3319) 
- `vector-h`
- `gravity-h`
- `bounding-box-h`
- `matrix-h`
- `quaternion-h`
- `euler-h`
- `transform-h`
- `geometry-h`
- `trigonometry-h`
- `transformq-h`
- `bounding-box`
- `matrix`
- `matrix-compose`
- `transform`
- `quaternion`
- `euler`
- `trigonometry`

Not a whole lot of changes, just a couple of new functions and one new
file (`matrix-compose`).
2024-01-20 10:42:51 -05:00

1911 lines
59 KiB
Common Lisp
Raw Permalink Blame History

;;-*-Lisp-*-
(in-package goal)
;; name: matrix.gc
;; name in dgo: matrix
;; dgos: GAME
#|@file
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.
|#
;; DECOMP BEGINS
(defun matrix-identity! ((arg0 matrix))
"Set dst to the identity matrix."
(set! (-> arg0 rvec quad) (the-as uint128 0))
(set! (-> arg0 uvec quad) (the-as uint128 0))
(set! (-> arg0 fvec quad) (the-as uint128 0))
(set! (-> arg0 trans quad) (the-as uint128 0))
(let ((f0-0 1.0))
(set! (-> arg0 trans w) f0-0)
(set! (-> arg0 fvec z) f0-0)
(set! (-> arg0 uvec y) f0-0)
(set! (-> arg0 rvec x) f0-0)
)
arg0
)
(define *identity-matrix* (matrix-identity! (new 'global 'matrix)))
(define *hermite-matrix* (new 'static 'matrix
:rvec (new 'static 'vector :x 2.0 :y -2.0 :z 1.0 :w 1.0)
:uvec (new 'static 'vector :x -3.0 :y 3.0 :z -2.0 :w -1.0)
:fvec (new 'static 'vector :z 1.0)
:trans (new 'static 'vector :x 1.0)
)
)
(defun matrix+! ((arg0 matrix) (arg1 matrix) (arg2 matrix))
"Set dst = src1 + src2. It is okay for any arguments to be the same data.
This is not an efficient implementation."
(dotimes (v1-0 16)
(set! (-> arg0 rvec data v1-0) (+ (-> arg1 rvec data v1-0) (-> arg2 rvec data v1-0)))
)
arg0
)
(defun matrix-! ((arg0 matrix) (arg1 matrix) (arg2 matrix))
"Set dst = src1 - src1. It is okay for any arugments to be the same data.
This is not an efficient implementation."
(dotimes (v1-0 16)
(set! (-> arg0 rvec data v1-0) (- (-> arg1 rvec data v1-0) (-> arg2 rvec data v1-0)))
)
arg0
)
(defun matrix*! ((arg0 matrix) (arg1 matrix) (arg2 matrix))
"Set dst = src1 * src2. It is okay for any arguments to be the same data.
This is a moderately efficient implementation."
(rlet ((acc :class vf)
(vf10 :class vf)
(vf11 :class vf)
(vf12 :class vf)
(vf13 :class vf)
(vf14 :class vf)
(vf15 :class vf)
(vf16 :class vf)
(vf17 :class vf)
(vf18 :class vf)
(vf19 :class vf)
(vf20 :class vf)
(vf21 :class vf)
)
(.lvf vf10 (&-> arg1 rvec quad))
(.lvf vf14 (&-> arg2 rvec quad))
(.lvf vf15 (&-> arg2 uvec quad))
(.lvf vf16 (&-> arg2 fvec quad))
(.lvf vf17 (&-> arg2 trans quad))
(.lvf vf11 (&-> arg1 uvec quad))
(.lvf vf12 (&-> arg1 fvec quad))
(.lvf vf13 (&-> arg1 trans quad))
(.mul.x.vf acc vf14 vf10)
(.add.mul.y.vf acc vf15 vf10 acc)
(.add.mul.z.vf acc vf16 vf10 acc)
(.add.mul.w.vf vf18 vf17 vf10 acc)
(.mul.x.vf acc vf14 vf11)
(.add.mul.y.vf acc vf15 vf11 acc)
(.add.mul.z.vf acc vf16 vf11 acc)
(.add.mul.w.vf vf19 vf17 vf11 acc)
(.mul.x.vf acc vf14 vf12)
(.add.mul.y.vf acc vf15 vf12 acc)
(.add.mul.z.vf acc vf16 vf12 acc)
(.add.mul.w.vf vf20 vf17 vf12 acc)
(.mul.x.vf acc vf14 vf13)
(.add.mul.y.vf acc vf15 vf13 acc)
(.add.mul.z.vf acc vf16 vf13 acc)
(.add.mul.w.vf vf21 vf17 vf13 acc)
(.svf (&-> arg0 rvec quad) vf18)
(.svf (&-> arg0 uvec quad) vf19)
(.svf (&-> arg0 fvec quad) vf20)
(.svf (&-> arg0 trans quad) vf21)
arg0
)
)
(defun matrixp*! ((arg0 matrix) (arg1 matrix) (arg2 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 ((s5-0 (new-stack-matrix0)))
(matrix*! s5-0 arg1 arg2)
(set! (-> arg0 rvec quad) (-> s5-0 rvec quad))
(set! (-> arg0 uvec quad) (-> s5-0 uvec quad))
(set! (-> arg0 fvec quad) (-> s5-0 fvec quad))
(set! (-> arg0 trans quad) (-> s5-0 trans quad))
)
arg0
)
(defun vector-matrix*! ((arg0 vector) (arg1 vector) (arg2 matrix))
"Set dst = vec * mat. dst may be equal to src."
(rlet ((acc :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
)
(.lvf vf1 (&-> arg2 rvec quad))
(.lvf vf2 (&-> arg2 uvec quad))
(.lvf vf3 (&-> arg2 fvec quad))
(.lvf vf4 (&-> arg2 trans quad))
(.lvf vf5 (&-> arg1 quad))
(.mul.x.vf acc vf1 vf5)
(.add.mul.y.vf acc vf2 vf5 acc)
(.add.mul.z.vf acc vf3 vf5 acc)
(.add.mul.w.vf vf5 vf4 vf5 acc)
(.svf (&-> arg0 quad) vf5)
arg0
)
)
(defun vector-norm-matrix*! ((arg0 vector) (arg1 vector) (arg2 matrix))
(rlet ((acc :class vf)
(Q :class vf)
(vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
)
(init-vf0-vector)
(.lvf vf1 (&-> arg2 rvec quad))
(.lvf vf2 (&-> arg2 uvec quad))
(.lvf vf3 (&-> arg2 fvec quad))
(.lvf vf4 (&-> arg2 trans quad))
(.lvf vf5 (&-> arg1 quad))
(.mul.x.vf acc vf1 vf5)
(.add.mul.y.vf acc vf2 vf5 acc)
(.add.mul.z.vf acc vf3 vf5 acc)
(.add.mul.w.vf vf5 vf4 vf5 acc)
(.div.vf Q vf0 vf5 :fsf #b11 :ftf #b11)
(.wait.vf)
(.mul.vf vf5 vf5 Q :mask #b111)
(.nop.vf)
(.nop.vf)
(.mov.vf vf5 vf0 :mask #b1000)
(.svf (&-> arg0 quad) vf5)
arg0
)
)
(defun vector-rotate*! ((arg0 vector) (arg1 vector) (arg2 matrix))
"Set dst to be the input vector rotated by the rotation part of mat.
The input matrix should be a homogeneous transform with a rotation matrix as its upper-left 3x3.
dst may be equal to src."
(rlet ((acc :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
)
(nop!)
(nop!)
(.lvf vf5 (&-> arg1 quad))
(nop!)
(.lvf vf1 (&-> arg2 rvec quad))
(nop!)
(.lvf vf2 (&-> arg2 uvec quad))
(.mul.x.vf acc vf1 vf5)
(.lvf vf3 (&-> arg2 fvec quad))
(.add.mul.y.vf acc vf2 vf5 acc)
(.lvf vf4 (&-> arg2 trans quad))
(.add.mul.z.vf vf5 vf3 vf5 acc)
(nop!)
(nop!)
(.svf (&-> arg0 quad) vf5)
arg0
)
)
(defun vector3s-matrix*! ((arg0 vector3s) (arg1 vector3s) (arg2 matrix))
"Set dst to be ([src 1.0] * mat).xyz. Doesn't touch the w of dst.
dst and vec can be the same memory"
(let ((s5-0 (new-stack-vector0)))
(set-vector! s5-0 (-> arg1 x) (-> arg1 y) (-> arg1 z) 1.0)
(vector-matrix*! s5-0 s5-0 arg2)
(set! (-> arg0 x) (-> s5-0 x))
(set! (-> arg0 y) (-> s5-0 y))
(set! (-> arg0 z) (-> s5-0 z))
)
arg0
)
(defun vector3s-rotate*! ((arg0 vector3s) (arg1 vector3s) (arg2 matrix))
"Set dst to vec rotated by the rotation in the homogeneous transform mat.
mat should not have a scale/shear (the upper 3x3 should be a pure rotation)."
(let ((s5-0 (new-stack-vector0)))
(set-vector! s5-0 (-> arg1 x) (-> arg1 y) (-> arg1 z) 1.0)
(vector-rotate*! s5-0 s5-0 arg2)
(set! (-> arg0 x) (-> s5-0 x))
(set! (-> arg0 y) (-> s5-0 y))
(set! (-> arg0 z) (-> s5-0 z))
)
arg0
)
(defun matrix-transpose! ((dst matrix) (src matrix))
"Set dst = src^T. src and dst can be the same."
(local-vars
(v1-0 uint128)
(v1-1 uint128)
(v1-2 uint128)
(a1-1 uint128)
(a2-1 uint128)
(a3-1 uint128)
(a3-2 uint128)
(t0-1 uint128)
)
(let ((t0-0 (-> src vector 0 quad)))
(let ((t1-0 (-> src vector 1 quad)))
(let ((a2-0 (-> src vector 2 quad)))
(.pextlw v1-0 t1-0 t0-0)
(let ((a3-0 (-> src vector 3 quad)))
(.pextuw a1-1 t1-0 t0-0)
;; (.mov r0-0 f31-0) not sure what this weird thing is.
;; it has no effect, but they use it instead of nop sometimes?
(.pextlw t0-1 a3-0 a2-0)
;; (.mov r0-1 f31-0)
(.pextuw a2-1 a3-0 a2-0)
)
)
)
)
;; (.mov r0-2 f31-0)
(.pcpyld a3-1 t0-1 v1-0)
;; (.mov r0-3 f31-0)
(.pcpyud v1-1 v1-0 t0-1)
(set! (-> dst vector 0 quad) a3-1)
(.pcpyld a3-2 a2-1 a1-1)
(set! (-> dst vector 1 quad) v1-1)
(.pcpyud v1-2 a1-1 a2-1)
(set! (-> dst vector 2 quad) a3-2)
(set! (-> dst vector 3 quad) v1-2)
dst
)
(defun matrix-inverse-of-rot-trans! ((arg0 matrix) (arg1 matrix))
"Set dst = src^-1, assuming src is a homogeneous tranform with only rotation/translation.
NOTE: THIS FUNCTION REQUIRES dst != src"
(rlet ((acc :class vf)
(vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf8 :class vf)
)
(init-vf0-vector)
(matrix-transpose! arg0 arg1)
(.lvf vf1 (&-> arg0 rvec quad))
(.lvf vf2 (&-> arg0 uvec quad))
(.lvf vf3 (&-> arg0 fvec quad))
(.sub.vf vf1 vf1 vf1 :mask #b1000)
(.sub.vf vf2 vf2 vf2 :mask #b1000)
(.sub.vf vf3 vf3 vf3 :mask #b1000)
(.lvf vf8 (&-> arg1 trans quad))
(.mul.x.vf acc vf1 vf8)
(.add.mul.y.vf acc vf2 vf8 acc)
(.add.mul.z.vf vf4 vf3 vf8 acc)
(.sub.vf vf4 vf0 vf4)
(.mov.vf vf4 vf0 :mask #b1000)
(.svf (&-> arg0 rvec quad) vf1)
(.svf (&-> arg0 uvec quad) vf2)
(.svf (&-> arg0 fvec quad) vf3)
(.svf (&-> arg0 trans quad) vf4)
arg0
)
)
;; ERROR: Bad vector register dependency: vf3
;; ERROR: Bad vector register dependency: vf4
;; ERROR: Bad vector register dependency: vf5
(defun matrix-4x4-inverse! ((arg0 matrix) (arg1 matrix))
"Invert a 4x4 matrix. This assumes that the input is a homogeneous transform.
Src and dst can be the same."
(rlet ((acc :class vf)
(Q :class vf)
(vf0 :class vf)
(vf1 :class vf)
(vf10 :class vf)
(vf11 :class vf)
(vf12 :class vf)
(vf13 :class vf)
(vf14 :class vf)
(vf15 :class vf)
(vf16 :class vf)
(vf17 :class vf)
(vf18 :class vf)
(vf19 :class vf)
(vf2 :class vf)
(vf20 :class vf)
(vf23 :class vf)
(vf24 :class vf)
(vf25 :class vf)
(vf26 :class vf)
(vf27 :class vf)
(vf28 :class vf)
(vf29 :class vf)
(vf3 :class vf)
(vf30 :class vf)
(vf31 :class vf)
(vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
(vf7 :class vf)
(vf8 :class vf)
(vf9 :class vf)
)
;; og:preserve-this
;; 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 vf16 vf16 vf16)
(.xor.vf vf17 vf17 vf17)
(.xor.vf vf18 vf18 vf18)
(.xor.vf vf23 vf23 vf23)
(.xor.vf vf19 vf19 vf19)
(.xor.vf vf3 vf3 vf3)
(.xor.vf vf4 vf4 vf4)
(.xor.vf vf5 vf5 vf5)
(init-vf0-vector)
(nop!)
(nop!)
(.lvf vf23 (&-> arg1 quad 0))
(nop!)
(.lvf vf24 (&-> arg1 quad 1))
(nop!)
(.lvf vf25 (&-> arg1 quad 2))
(nop!)
(.lvf vf1 (&-> arg1 trans quad))
(.mul.x.vf vf7 vf24 vf23)
(nop!)
(.mul.y.vf vf8 vf24 vf23)
(nop!)
(.mul.z.vf vf9 vf24 vf23)
(nop!)
(.mul.x.vf vf10 vf25 vf23)
(nop!)
(.mul.y.vf vf11 vf25 vf23)
(nop!)
(.mul.z.vf vf12 vf25 vf23)
(nop!)
(.mul.x.vf vf13 vf25 vf24)
(nop!)
(.mul.y.vf vf14 vf25 vf24)
(nop!)
(.mul.z.vf vf15 vf25 vf24)
(nop!)
(.mul.z.vf vf26 vf7 vf25 :mask #b10)
(nop!)
(.mul.z.vf vf27 vf11 vf24 :mask #b1)
(nop!)
(.mul.y.vf vf28 vf9 vf25 :mask #b1)
(nop!)
(.mul.z.vf vf29 vf14 vf23 :mask #b1)
(nop!)
(.mul.z.vf vf30 vf8 vf25 :mask #b1)
(nop!)
(.mul.y.vf vf31 vf7 vf25 :mask #b100)
(nop!)
(.add.y.vf vf16 vf27 vf26 :mask #b1)
(nop!)
(.sub.vf vf1 vf0 vf1)
(nop!)
(.add.x.vf vf17 vf29 vf30 :mask #b1)
(nop!)
(.sub.z.vf vf18 vf28 vf31 :mask #b1)
(nop!)
(.sub.y.vf vf23 vf14 vf15 :mask #b100)
(nop!)
(.sub.z.vf vf26 vf15 vf13 :mask #b1)
(nop!)
(.sub.x.vf vf29 vf13 vf14 :mask #b10)
(nop!)
(.sub.vf vf19 vf16 vf17 :mask #b1)
(nop!)
(.sub.z.vf vf24 vf12 vf11 :mask #b10)
(nop!)
(.sub.x.vf vf27 vf10 vf12 :mask #b100)
(nop!)
(.sub.y.vf vf30 vf11 vf10 :mask #b1)
(nop!)
(.add.vf vf20 vf19 vf18 :mask #b1)
(nop!)
(.sub.y.vf vf25 vf8 vf9 :mask #b100)
(nop!)
(.sub.z.vf vf28 vf9 vf7 :mask #b1)
(nop!)
(.sub.x.vf vf31 vf7 vf8 :mask #b10)
(nop!)
(.div.vf Q vf0 vf20 :fsf #b11 :ftf #b0)
(nop!)
(.sub.w.vf vf3 vf3 vf3 :mask #b1000)
(nop!)
(.sub.w.vf vf4 vf4 vf4 :mask #b1000)
(nop!)
(.sub.w.vf vf5 vf5 vf5 :mask #b1000)
(nop!)
(.mov.vf vf6 vf0 :mask #b1000)
(nop!)
(.wait.vf)
(nop!)
(.add.vf vf2 vf0 Q :mask #b1)
(nop!)
(.add.x.vf vf2 vf0 vf2 :mask #b111)
(nop!)
(.mul.z.vf vf3 vf2 vf23 :mask #b1)
(nop!)
(.mul.x.vf vf4 vf2 vf26 :mask #b1)
(nop!)
(.mul.y.vf vf5 vf2 vf29 :mask #b1)
(nop!)
(.mul.y.vf vf3 vf2 vf24 :mask #b10)
(nop!)
(.mul.z.vf vf4 vf2 vf27 :mask #b10)
(nop!)
(.mul.x.vf vf5 vf2 vf30 :mask #b10)
(nop!)
(.mul.z.vf vf3 vf2 vf25 :mask #b100)
(nop!)
(.mul.x.vf vf4 vf2 vf28 :mask #b100)
(nop!)
(.mul.y.vf vf5 vf2 vf31 :mask #b100)
(nop!)
(.mul.x.vf acc vf3 vf1)
(.svf (&-> arg0 quad 0) vf3)
(.add.mul.y.vf acc vf4 vf1 acc)
(.svf (&-> arg0 quad 1) vf4)
(.add.mul.z.vf vf6 vf5 vf1 acc :mask #b111)
(.svf (&-> arg0 quad 2) vf5)
(nop!)
(.svf (&-> arg0 trans quad) vf6)
arg0
)
)
(defun matrix-translate! ((arg0 matrix) (arg1 vector))
"Set dst to a homogeneous transform with only a translation of trans."
(matrix-identity! arg0)
(set! (-> arg0 trans x) (-> arg1 x))
(set! (-> arg0 trans y) (-> arg1 y))
(set! (-> arg0 trans z) (-> arg1 z))
arg0
)
(defun matrix-translate+! ((arg0 matrix) (arg1 matrix) (arg2 vector))
"Add the given translation to the translation of homogenous transform mat src
and store in dst. It is okay for dst = src."
(set! (-> arg0 trans x) (+ (-> arg1 trans x) (-> arg2 x)))
(set! (-> arg0 trans y) (+ (-> arg1 trans y) (-> arg2 y)))
(set! (-> arg0 trans z) (+ (-> arg1 trans z) (-> arg2 z)))
(when (!= arg0 arg1)
(set! (-> arg0 rvec quad) (-> arg1 rvec quad))
(set! (-> arg0 uvec quad) (-> arg1 uvec quad))
(set! (-> arg0 fvec quad) (-> arg1 fvec quad))
)
arg0
)
(defun matrix-scale! ((arg0 matrix) (arg1 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! (-> arg0 rvec quad) (the-as uint128 0))
(set! (-> arg0 uvec quad) (the-as uint128 0))
(set! (-> arg0 fvec quad) (the-as uint128 0))
(set! (-> arg0 trans quad) (the-as uint128 0))
(set! (-> arg0 rvec x) (-> arg1 x))
(set! (-> arg0 uvec y) (-> arg1 y))
(set! (-> arg0 fvec z) (-> arg1 z))
(set! (-> arg0 trans w) 1.0)
arg0
)
(defun scale-matrix! ((arg0 matrix) (arg1 vector) (arg2 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."
(rlet ((vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
(vf7 :class vf)
(vf8 :class vf)
)
(.lvf vf4 (&-> arg1 quad))
(.lvf vf5 (&-> arg2 rvec quad))
(.lvf vf6 (&-> arg2 uvec quad))
(.lvf vf7 (&-> arg2 fvec quad))
(.lvf vf8 (&-> arg2 trans quad))
(.mul.x.vf vf5 vf5 vf4)
(.mul.y.vf vf6 vf6 vf4)
(.mul.z.vf vf7 vf7 vf4)
(.mul.w.vf vf8 vf8 vf4)
(.svf (&-> arg0 rvec quad) vf5)
(.svf (&-> arg0 uvec quad) vf6)
(.svf (&-> arg0 fvec quad) vf7)
(.svf (&-> arg0 trans quad) vf8)
arg0
)
)
(defun matrix-inv-scale! ((arg0 matrix) (arg1 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! (-> arg0 rvec quad) (the-as uint128 0))
(set! (-> arg0 uvec quad) (the-as uint128 0))
(set! (-> arg0 fvec quad) (the-as uint128 0))
(set! (-> arg0 trans quad) (the-as uint128 0))
(set! (-> arg0 rvec x) (/ 1.0 (-> arg1 x)))
(set! (-> arg0 uvec y) (/ 1.0 (-> arg1 y)))
(set! (-> arg0 fvec z) (/ 1.0 (-> arg1 z)))
(set! (-> arg0 trans w) 1.0)
arg0
)
(defun column-scale-matrix! ((arg0 matrix) (arg1 vector) (arg2 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."
(rlet ((vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
(vf7 :class vf)
(vf8 :class vf)
)
(.lvf vf4 (&-> arg1 quad))
(.lvf vf5 (&-> arg2 rvec quad))
(.lvf vf6 (&-> arg2 uvec quad))
(.lvf vf7 (&-> arg2 fvec quad))
(.lvf vf8 (&-> arg2 trans quad))
(.mul.vf vf5 vf5 vf4)
(.mul.vf vf6 vf6 vf4)
(.mul.vf vf7 vf7 vf4)
(.mul.vf vf8 vf8 vf4)
(.svf (&-> arg0 rvec quad) vf5)
(.svf (&-> arg0 uvec quad) vf6)
(.svf (&-> arg0 fvec quad) vf7)
(.svf (&-> arg0 trans quad) vf8)
arg0
)
)
(defun matrix-rotate-x! ((arg0 matrix) (arg1 float))
"Set dst to a homogeneous transform matrix for a rotation around the x-axis (degrees)."
(let ((f30-0 (sin arg1))
(f0-0 (cos arg1))
)
(set! (-> arg0 rvec quad) (the-as uint128 0))
(set! (-> arg0 uvec quad) (the-as uint128 0))
(set! (-> arg0 fvec quad) (the-as uint128 0))
(set! (-> arg0 trans quad) (the-as uint128 0))
(set! (-> arg0 rvec x) 1.0)
(set! (-> arg0 uvec y) f0-0)
(set! (-> arg0 uvec z) f30-0)
(set! (-> arg0 fvec y) (- f30-0))
(set! (-> arg0 fvec z) f0-0)
)
(set! (-> arg0 trans w) 1.0)
arg0
)
(defun matrix-rotate-y! ((arg0 matrix) (arg1 float))
"Set dst to a homoegeneous transform matrix for a rotation around the y axis (degrees)."
(let ((f30-0 (sin arg1))
(f0-0 (cos arg1))
)
(set! (-> arg0 rvec quad) (the-as uint128 0))
(set! (-> arg0 uvec quad) (the-as uint128 0))
(set! (-> arg0 fvec quad) (the-as uint128 0))
(set! (-> arg0 trans quad) (the-as uint128 0))
(set! (-> arg0 rvec x) f0-0)
(set! (-> arg0 rvec z) (- f30-0))
(set! (-> arg0 uvec y) 1.0)
(set! (-> arg0 fvec x) f30-0)
(set! (-> arg0 fvec z) f0-0)
)
(set! (-> arg0 trans w) 1.0)
arg0
)
(defun matrix-rotate-z! ((arg0 matrix) (arg1 float))
"Set dst to a homogeneous transform matrix for a rotation around the z-axis (degrees)."
(let ((f30-0 (sin arg1))
(f0-0 (cos arg1))
)
(set! (-> arg0 rvec quad) (the-as uint128 0))
(set! (-> arg0 uvec quad) (the-as uint128 0))
(set! (-> arg0 fvec quad) (the-as uint128 0))
(set! (-> arg0 trans quad) (the-as uint128 0))
(set! (-> arg0 rvec x) f0-0)
(set! (-> arg0 rvec y) f30-0)
(set! (-> arg0 uvec x) (- f30-0))
(set! (-> arg0 uvec y) f0-0)
)
(set! (-> arg0 fvec z) 1.0)
(set! (-> arg0 trans w) 1.0)
arg0
)
(defun matrix-rotate-zyx! ((arg0 matrix) (arg1 vector))
"Rotate in z,y,x order."
(let ((gp-0 (new-stack-matrix0))
(s5-0 (new-stack-matrix0))
)
(matrix-rotate-x! arg0 (-> arg1 x))
(matrix-rotate-y! gp-0 (-> arg1 y))
(matrix*! s5-0 gp-0 arg0)
(matrix-rotate-z! gp-0 (-> arg1 z))
(matrix*! arg0 gp-0 s5-0)
)
arg0
)
(defun-debug matrix-rotate-xyz-2! ((arg0 matrix) (arg1 vector))
"Jak 1 version of matrix-rotate-xyz. Slower than the one below."
(let ((gp-0 (new 'stack-no-clear 'matrix))
(s5-0 (new 'stack-no-clear 'matrix))
)
(matrix-rotate-z! arg0 (-> arg1 z))
(matrix-rotate-y! gp-0 (-> arg1 y))
(matrix*! s5-0 gp-0 arg0)
(matrix-rotate-x! gp-0 (-> arg1 x))
(matrix*! arg0 gp-0 s5-0)
)
arg0
)
(defun matrix-rotate-xyz! ((arg0 matrix) (arg1 vector))
"Rotate in x,y,z order."
(rlet ((vf0 :class vf))
(init-vf0-vector)
(let ((s4-0 (new 'stack-no-clear 'vector))
(s5-0 (new 'stack-no-clear 'vector))
)
(vector-sincos! s4-0 s5-0 arg1)
(let ((f2-0 (-> s4-0 x))
(f5-0 (-> s4-0 y))
(f3-0 (-> s4-0 z))
(f0-0 (-> s5-0 x))
(f1-0 (-> s5-0 y))
)
(let ((f4-0 (-> s5-0 z)))
(set! (-> arg0 rvec x) (* f1-0 f4-0))
(set! (-> arg0 rvec y) (* f1-0 f3-0))
(set! (-> arg0 rvec z) (- f5-0))
(set! (-> arg0 rvec w) 0.0)
(let ((f6-4 (* f2-0 f5-0))
(f5-1 (* f0-0 f5-0))
)
(set! (-> arg0 uvec x) (- (* f6-4 f4-0) (* f0-0 f3-0)))
(set! (-> arg0 uvec y) (+ (* f6-4 f3-0) (* f0-0 f4-0)))
(set! (-> arg0 uvec z) (* f2-0 f1-0))
(set! (-> arg0 uvec w) 0.0)
(set! (-> arg0 fvec x) (+ (* f2-0 f3-0) (* f5-1 f4-0)))
(set! (-> arg0 fvec y) (- (* f5-1 f3-0) (* f2-0 f4-0)))
)
)
(set! (-> arg0 fvec z) (* f0-0 f1-0))
)
)
(set! (-> arg0 fvec w) 0.0)
(.svf (&-> arg0 trans quad) vf0)
arg0
)
)
(defun matrix-rotate-zxy! ((arg0 matrix) (arg1 vector))
"Rotate in z,x,y order."
(let ((gp-0 (new-stack-matrix0))
(s5-0 (new-stack-matrix0))
)
(matrix-rotate-y! arg0 (-> arg1 y))
(matrix-rotate-x! gp-0 (-> arg1 x))
(matrix*! s5-0 gp-0 arg0)
(matrix-rotate-z! gp-0 (-> arg1 z))
(matrix*! arg0 gp-0 s5-0)
)
arg0
)
(defun matrix-rotate-yxz! ((arg0 matrix) (arg1 vector))
"Rotate in y,x,z order."
(let ((gp-0 (new-stack-matrix0))
(s5-0 (new-stack-matrix0))
)
(matrix-rotate-z! arg0 (-> arg1 z))
(matrix-rotate-x! gp-0 (-> arg1 x))
(matrix*! s5-0 gp-0 arg0)
(matrix-rotate-y! gp-0 (-> arg1 y))
(matrix*! arg0 gp-0 s5-0)
)
arg0
)
(defun matrix-rotate-yzx! ((arg0 matrix) (arg1 vector))
"Rotate in y,z,x order."
(let ((gp-0 (new-stack-matrix0))
(s5-0 (new-stack-matrix0))
)
(matrix-rotate-z! arg0 (-> arg1 x))
(matrix-rotate-x! gp-0 (-> arg1 z))
(matrix*! s5-0 gp-0 arg0)
(matrix-rotate-y! gp-0 (-> arg1 y))
(matrix*! arg0 gp-0 s5-0)
)
arg0
)
(defun matrix-rotate-yxy! ((arg0 matrix) (arg1 vector))
"Rotate. I believe in yxy order? Compared to the other rotations, this one
is quite a bit more optimized and avoid repeated trig operations."
(let ((a2-0 (new 'stack-no-clear 'vector))
(s5-0 (new 'stack-no-clear 'vector))
(s4-0 (new 'stack-no-clear 'vector))
)
(set-vector! a2-0 (-> arg1 x) (- (-> arg1 y) (-> arg1 z)) (-> arg1 z) 1.0)
(vector-sincos! s5-0 s4-0 a2-0)
(let ((f1-1 (-> s4-0 y))
(f0-5 (-> s5-0 y))
(f2-0 (-> s4-0 x))
(f5-0 (-> s5-0 x))
(f3-0 (-> s4-0 z))
(f4-0 (-> s5-0 z))
)
(set! (-> arg0 rvec x) (- (* f1-1 f3-0) (* f0-5 f2-0 f4-0)))
(set! (-> arg0 rvec y) (* f0-5 f5-0))
(set! (-> arg0 rvec z) (- (+ (* f1-1 f4-0) (* f0-5 f2-0 f3-0))))
(set! (-> arg0 rvec w) 0.0)
(set! (-> arg0 uvec x) (* f5-0 f4-0))
(set! (-> arg0 uvec y) f2-0)
(set! (-> arg0 uvec z) (* f5-0 f3-0))
(set! (-> arg0 uvec w) 0.0)
(set! (-> arg0 fvec x) (+ (* f0-5 f3-0) (* f1-1 f2-0 f4-0)))
(set! (-> arg0 fvec y) (- (* f1-1 f5-0)))
(set! (-> arg0 fvec z) (- (* f1-1 f2-0 f3-0) (* f0-5 f4-0)))
)
)
(set! (-> arg0 fvec w) 0.0)
(set! (-> arg0 trans x) 0.0)
(set! (-> arg0 trans y) 0.0)
(set! (-> arg0 trans z) 0.0)
(set! (-> arg0 trans w) 1.0)
arg0
)
(defun matrix-rotate-yx! ((arg0 matrix) (arg1 float) (arg2 float))
"Rotate by y then x."
(matrix-rotate-y! arg0 arg1)
(let ((a1-2 (matrix-rotate-x! (new-stack-matrix0) arg2)))
(matrix*! arg0 a1-2 arg0)
)
arg0
)
;; WARN: Return type mismatch matrix vs none.
;; ERROR: Unsupported inline assembly instruction kind - [prot3w a1, v1]
;; ERROR: Unsupported inline assembly instruction kind - [prot3w a2, a1]
(defun matrix-axis-sin-cos-vu! ((arg0 matrix) (arg1 vector) (arg2 float) (arg3 float))
"Create an axis-angle rotation matrix. But given the sin/cos of the angle. Uses the VU."
;; og:preserve-this unused and basically identital to the non-VU version, so I'm leaving this out.
(/ 0 0)
(none)
)
;; ERROR: Unsupported inline assembly instruction kind - [prot3w a0, v1]
;; ERROR: Unsupported inline assembly instruction kind - [prot3w a1, a0]
(defun matrix-axis-sin-cos! ((dst matrix) (axis vector) (s float) (c float))
"Create an axis-angle rotation matrix. But given the sin/cos of the angle."
;;or v0, a0, r0
;;or v1, a1, r0
;;mtc1 f0, a2
;;mtc1 f1, a3
;;sll r0, r0, 0
;; input scramble:
;; - v0 = dst
;; - v1 = axis
;; - f0 = sine
;; - f1 = cosine
(rlet ((vf5 :class vf) ;; vf5.x will be sine, vf5.w will be -cosine
(vf6 :class vf) ;; vf6.x will be cosine
(vf1 :class vf) ;; vector.
(vf0 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf11 :class vf)
(vf7 :class vf)
(vf8 :class vf)
(vf9 :class vf)
(vf10 :class vf)
(a1 :class i128 :type uint128)
(r0 :class i128 :type uint128)) ;; will be used for zero check.
;; initialize constant vectors
(init-vf0-vector)
(.xor.p r0 r0 r0)
;; this is an overly complicated check to see if xyz = 0.
(let ((a0 65535) ;; ori a0, r0, 65535
(v1 (-> axis quad))) ;; lq v1, 0(v1)
;; this a0 constant has ffff in the upper 16 bits to mask out w, later on.
;; dsll32 a0, a0, 16
(set! a0 (shl a0 48)) ;; a0 = ffff'0000'0000'0000
;;mfc1 a1, f1
;;qmtc2.i vf6, a1
(.mov vf6 c) ;; will set vf6.x to cosine.
;;pceqw a1, v1, r0
;; compare for equality with zero.
;; this will set 32-bits for each component.
(.pceqw a1 v1 r0)
;; so we pack: [upper64, w0?16, z0?16, y0?16, x0?16]
(.ppach a1 r0 a1)
;;qmtc2.i vf1, v1
(.mov vf1 v1) ;; should be quad move!
;;or v1, a1, a0
;; make it seem like w is zero. If x,y,z = 0, but w != 0, we want
;; it to act like zero.
(let ((xyz-zero (logior a0 (the uint a1))))
;;mfc1 a0, f0
;;qmtc2.i vf5, a0
(.mov vf5 s)
;;daddiu v1, v1, 1
;; if all is zero, this will overflow and give us 0.
(+! xyz-zero 1)
;;vsubx.w vf5, vf0, vf6
(.sub.x.vf vf5 vf0 vf6 :mask #b1000)
;;beq v1, r0, L15
;;vaddx.y vf5, vf0, vf6
(.add.x.vf vf5 vf0 vf6 :mask #b0010)
(when-goto (zero? xyz-zero) zero-case)
)
)
;; Nonzero case:
;; vsubw.w vf2, vf0, vf0
;; vsubw.w vf3, vf0, vf0
;; vsubw.w vf4, vf0, vf0
(.xor.vf vf2 vf2 vf2)
(.xor.vf vf3 vf3 vf3)
(.xor.vf vf4 vf4 vf4)
;;vmulx.xyz vf11, vf1, vf5
(.mul.x.vf vf11 vf1 vf5 :mask #b0111)
;;vaddy.x vf7, vf0, vf5
(.add.y.vf vf7 vf0 vf5 :mask #b0001)
;;vaddy.y vf8, vf0, vf5
(.add.y.vf vf8 vf0 vf5 :mask #b0010)
;;vaddy.z vf9, vf0, vf5
(.add.y.vf vf9 vf0 vf5 :mask #b0100)
;;vmulw.xyz vf10, vf1, vf5
(.mul.w.vf vf10 vf1 vf5 :mask #b0111)
;;vsubz.y vf7, vf0, vf11
(.sub.z.vf vf7 vf0 vf11 :mask #b0010)
;;vaddy.z vf7, vf0, vf11
(.add.y.vf vf7 vf0 vf11 :mask #b0100)
;;vaddz.x vf8, vf0, vf11
(.add.z.vf vf8 vf0 vf11 :mask #b0001)
;;vsubx.z vf8, vf0, vf11
(.sub.x.vf vf8 vf0 vf11 :mask #b0100)
;;vmulx.xyz vf2, vf10, vf1
(.mul.x.vf vf2 vf10 vf1 :mask #b0111)
;;vmuly.xyz vf3, vf10, vf1
(.mul.y.vf vf3 vf10 vf1 :mask #b0111)
;;vmulz.xyz vf4, vf10, vf1
(.mul.z.vf vf4 vf10 vf1 :mask #b0111)
;;vsuby.x vf9, vf0, vf11
(.sub.y.vf vf9 vf0 vf11 :mask #b0001)
;;vaddx.y vf9, vf0, vf11
(.add.x.vf vf9 vf0 vf11 :mask #b0010)
;;vadd.xyz vf2, vf2, vf7
(.add.vf vf2 vf2 vf7 :mask #b0111)
;;vadd.xyz vf3, vf3, vf8
(.add.vf vf3 vf3 vf8 :mask #b0111)
;;vadd.xyz vf4, vf4, vf9
(.add.vf vf4 vf4 vf9 :mask #b0111)
;;sqc2 vf2, 0(v0)
;;sqc2 vf3, 16(v0)
;;sqc2 vf0, 48(v0)
;;sqc2 vf4, 32(v0)
(.svf (-> dst vector 0) vf2)
(.svf (-> dst vector 1) vf3)
(.svf (-> dst vector 3) vf0)
(.svf (-> dst vector 2) vf4)
(goto end)
(label zero-case)
;; overly clever way to set identity matrix
;;lui v1, 16256
;;sqc2 vf0, 48(v0)
;;pcpyld v1, r0, v1
;;mfc1 r0, f31
;;prot3w a0, v1
;;mfc1 r0, f31
;;prot3w a1, a0
;;sq v1, 0(v0)
;;sll r0, r0, 0
;;sq a0, 32(v0)
;;sll r0, r0, 0
;;sq a1, 16(v0)
(matrix-identity! dst)
(label end)
)
dst
)
(defun matrix-axis-angle! ((arg0 matrix) (arg1 vector) (arg2 float))
"Create an axis-angle rotation matrix."
(matrix-axis-sin-cos! arg0 arg1 (sin arg2) (cos arg2))
)
(defun matrix-lerp! ((arg0 matrix) (arg1 matrix) (arg2 matrix) (arg3 float))
"Lerp an entire matrix, coefficient-wise."
(rlet ((vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
(vf7 :class vf)
(vf8 :class vf)
(vf9 :class vf)
)
(.mov vf9 arg3)
(.lvf vf1 (&-> arg1 rvec quad))
(.lvf vf2 (&-> arg1 uvec quad))
(.lvf vf3 (&-> arg1 fvec quad))
(.lvf vf4 (&-> arg1 trans quad))
(.lvf vf5 (&-> arg2 rvec quad))
(.lvf vf6 (&-> arg2 uvec quad))
(.lvf vf7 (&-> arg2 fvec quad))
(.lvf vf8 (&-> arg2 trans quad))
(.sub.vf vf5 vf5 vf1)
(.sub.vf vf6 vf6 vf2)
(.sub.vf vf7 vf7 vf3)
(.sub.vf vf8 vf8 vf4)
(.mul.x.vf vf5 vf5 vf9)
(.mul.x.vf vf6 vf6 vf9)
(.mul.x.vf vf7 vf7 vf9)
(.mul.x.vf vf8 vf8 vf9)
(.add.vf vf1 vf1 vf5)
(.add.vf vf2 vf2 vf6)
(.add.vf vf3 vf3 vf7)
(.add.vf vf4 vf4 vf8)
(.svf (&-> arg0 rvec quad) vf1)
(.svf (&-> arg0 uvec quad) vf2)
(.svf (&-> arg0 fvec quad) vf3)
(.svf (&-> arg0 trans quad) vf4)
arg0
)
)
(defun matrix-3x3-determinant ((arg0 matrix))
"Compute the determinant of a 3x3 matrix."
(let ((f8-0 (-> arg0 rvec x))
(f1-0 (-> arg0 rvec y))
(f4-0 (-> arg0 rvec z))
(f2-0 (-> arg0 uvec x))
(f5-0 (-> arg0 uvec y))
(f9-0 (-> arg0 uvec z))
(f3-0 (-> arg0 fvec x))
(f6-0 (-> arg0 fvec y))
(f0-0 (-> arg0 fvec z))
)
(- (+ (* f8-0 f5-0 f0-0) (* f1-0 f9-0 f3-0) (* f4-0 f2-0 f6-0))
(+ (* f8-0 f9-0 f6-0) (* f4-0 f5-0 f3-0) (* f1-0 f2-0 f0-0))
)
)
)
(defun matrix3-determinant ((arg0 matrix))
"Unused. Not sure if this has limitations compared to the above version."
(local-vars (v0-0 float))
(rlet ((acc :class vf)
(vf10 :class vf)
(vf11 :class vf)
(vf12 :class vf)
(vf13 :class vf)
)
(.lvf vf11 (&-> arg0 uvec quad))
(.lvf vf12 (&-> arg0 fvec quad))
(.lvf vf10 (&-> arg0 rvec quad))
(.outer.product.a.vf acc vf11 vf12)
(.outer.product.b.vf vf13 vf12 vf11 acc)
(.mul.vf vf13 vf13 vf10 :mask #b111)
(.add.y.vf vf13 vf13 vf13 :mask #b1)
(.add.z.vf vf13 vf13 vf13 :mask #b1)
(.mov v0-0 vf13)
v0-0
)
)
(defun matrix-3x3-inverse! ((arg0 matrix) (arg1 matrix))
"Compute the inverse of a 3x3 matrix. Not very efficient.
Requires src != dst."
(let ((f0-0 (matrix-3x3-determinant arg1)))
(set! (-> arg0 rvec x)
(/ (- (* (-> arg1 uvec y) (-> arg1 fvec z)) (* (-> arg1 uvec z) (-> arg1 fvec y))) f0-0)
)
(set! (-> arg0 uvec x)
(/ (- (* (-> arg1 uvec z) (-> arg1 fvec x)) (* (-> arg1 uvec x) (-> arg1 fvec z))) f0-0)
)
(set! (-> arg0 fvec x)
(/ (- (* (-> arg1 uvec x) (-> arg1 fvec y)) (* (-> arg1 uvec y) (-> arg1 fvec x))) f0-0)
)
(set! (-> arg0 rvec y)
(/ (- (* (-> arg1 fvec y) (-> arg1 rvec z)) (* (-> arg1 fvec z) (-> arg1 rvec y))) f0-0)
)
(set! (-> arg0 uvec y)
(/ (- (* (-> arg1 fvec z) (-> arg1 rvec x)) (* (-> arg1 fvec x) (-> arg1 rvec z))) f0-0)
)
(set! (-> arg0 fvec y)
(/ (- (* (-> arg1 fvec x) (-> arg1 rvec y)) (* (-> arg1 fvec y) (-> arg1 rvec x))) f0-0)
)
(set! (-> arg0 rvec z)
(/ (- (* (-> arg1 rvec y) (-> arg1 uvec z)) (* (-> arg1 rvec z) (-> arg1 uvec y))) f0-0)
)
(set! (-> arg0 uvec z)
(/ (- (* (-> arg1 rvec z) (-> arg1 uvec x)) (* (-> arg1 rvec x) (-> arg1 uvec z))) f0-0)
)
(set! (-> arg0 fvec z)
(/ (- (* (-> arg1 rvec x) (-> arg1 uvec y)) (* (-> arg1 rvec y) (-> arg1 uvec x))) f0-0)
)
)
arg0
)
(defun matrix-3x3-inverse-transpose! ((arg0 matrix) (arg1 matrix))
"Invert and transpose.
Requires dst != src."
(let ((f0-0 (matrix-3x3-determinant arg1)))
(set! (-> arg0 rvec x)
(/ (- (* (-> arg1 uvec y) (-> arg1 fvec z)) (* (-> arg1 uvec z) (-> arg1 fvec y))) f0-0)
)
(set! (-> arg0 rvec y)
(/ (- (* (-> arg1 uvec z) (-> arg1 fvec x)) (* (-> arg1 uvec x) (-> arg1 fvec z))) f0-0)
)
(set! (-> arg0 rvec z)
(/ (- (* (-> arg1 uvec x) (-> arg1 fvec y)) (* (-> arg1 uvec y) (-> arg1 fvec x))) f0-0)
)
(set! (-> arg0 uvec x)
(/ (- (* (-> arg1 fvec y) (-> arg1 rvec z)) (* (-> arg1 fvec z) (-> arg1 rvec y))) f0-0)
)
(set! (-> arg0 uvec y)
(/ (- (* (-> arg1 fvec z) (-> arg1 rvec x)) (* (-> arg1 fvec x) (-> arg1 rvec z))) f0-0)
)
(set! (-> arg0 uvec z)
(/ (- (* (-> arg1 fvec x) (-> arg1 rvec y)) (* (-> arg1 fvec y) (-> arg1 rvec x))) f0-0)
)
(set! (-> arg0 fvec x)
(/ (- (* (-> arg1 rvec y) (-> arg1 uvec z)) (* (-> arg1 rvec z) (-> arg1 uvec y))) f0-0)
)
(set! (-> arg0 fvec y)
(/ (- (* (-> arg1 rvec z) (-> arg1 uvec x)) (* (-> arg1 rvec x) (-> arg1 uvec z))) f0-0)
)
(set! (-> arg0 fvec z)
(/ (- (* (-> arg1 rvec x) (-> arg1 uvec y)) (* (-> arg1 rvec y) (-> arg1 uvec x))) f0-0)
)
)
arg0
)
(defun matrix3-inverse-transpose! ((arg0 matrix) (arg1 matrix))
"Unused. Not sure if this has limitations compared to other version."
(rlet ((acc :class vf)
(Q :class vf)
(vf0 :class vf)
(vf1 :class vf)
(vf10 :class vf)
(vf11 :class vf)
(vf12 :class vf)
(vf13 :class vf)
(vf14 :class vf)
(vf15 :class vf)
)
(init-vf0-vector)
(.lvf vf10 (&-> arg1 rvec quad))
(.lvf vf11 (&-> arg1 uvec quad))
(.lvf vf12 (&-> arg1 fvec quad))
(.outer.product.a.vf acc vf11 vf12)
(.outer.product.b.vf vf13 vf12 vf11 acc)
(.outer.product.a.vf acc vf12 vf10)
(.outer.product.b.vf vf14 vf10 vf12 acc)
(.mul.vf vf1 vf10 vf13 :mask #b111)
(.outer.product.a.vf acc vf10 vf11)
(.outer.product.b.vf vf15 vf11 vf10 acc)
(.nop.vf)
(.add.y.vf vf1 vf1 vf1 :mask #b1)
(.nop.vf)
(.nop.vf)
(.nop.vf)
(.add.z.vf vf1 vf1 vf1 :mask #b1)
(.nop.vf)
(.nop.vf)
(.nop.vf)
(.div.vf Q vf0 vf1 :fsf #b11 :ftf #b0)
(.wait.vf)
(.mul.vf vf13 vf13 Q)
(.mul.vf vf14 vf14 Q)
(.mul.vf vf15 vf15 Q)
(.nop.vf)
(.nop.vf)
(.svf (&-> arg0 rvec quad) vf13)
(.svf (&-> arg0 uvec quad) vf14)
(.svf (&-> arg0 fvec quad) vf15)
arg0
)
)
;; ERROR: Unsupported inline assembly instruction kind - [mula.s f2, f2]
;; ERROR: Unsupported inline assembly instruction kind - [madda.s f0, f0]
;; ERROR: Unsupported inline assembly instruction kind - [madd.s f0, f1, f1]
;; ERROR: Unsupported inline assembly instruction kind - [rsqrt.s f0, f2, f0]
(defun matrix-3x3-normalize! ((arg0 matrix) (arg1 matrix))
"Unused."
(/ 0 0)
arg0
#|
(local-vars (f0-1 float) (f0-2 float))
(rlet ((acc :class vf)
(Q :class vf)
(vf0 :class vf)
(vf10 :class vf)
(vf11 :class vf)
(vf12 :class vf)
(vf13 :class vf)
(vf14 :class vf)
(vf15 :class vf)
(vf16 :class vf)
)
(init-vf0-vector)
(let ((f2-0 (-> arg1 data 0))
(f0-0 (-> arg1 data 1))
(f1-0 (-> arg1 data 2))
(v1-0 #x3f800000)
)
(.lvf vf10 (&-> arg1 quad 0))
(.mula.s f2-0 f2-0)
(.lvf vf11 (&-> arg1 quad 1))
(let ((f2-1 (gpr->fpr v1-0)))
(.lvf vf12 (&-> arg1 quad 2))
(.madda.s f0-0 f0-0)
(.sub.w.vf vf13 vf0 vf0 :mask #b1000)
(.madd.s f0-1 f1-0 f1-0)
(.sub.w.vf vf14 vf0 vf0 :mask #b1000)
(.rsqrt.s f0-2 f2-1 f0-1)
)
)
(.sub.w.vf vf15 vf0 vf0 :mask #b1000)
(let ((v1-1 f0-2))
(.mov vf16 v1-1)
)
(.mul.x.vf vf13 vf10 vf16 :mask #b111)
(.outer.product.a.vf acc vf11 vf13)
(.outer.product.b.vf vf14 vf13 vf11 acc)
(.outer.product.a.vf acc vf13 vf14)
(.outer.product.b.vf vf14 vf14 vf13 acc)
(.mul.vf vf16 vf14 vf14 :mask #b111)
(.mul.x.vf acc vf0 vf16 :mask #b1000)
(.add.mul.y.vf acc vf0 vf16 acc :mask #b1000)
(.add.mul.z.vf vf16 vf0 vf16 acc :mask #b1000)
(.isqrt.vf Q vf0 vf16 :fsf #b11 :ftf #b11)
(.wait.vf)
(.mul.vf vf14 vf14 Q :mask #b111)
(.outer.product.a.vf acc vf13 vf14)
(.outer.product.b.vf vf15 vf14 vf13 acc)
(.svf (&-> arg0 quad 0) vf13)
(.svf (&-> arg0 quad 1) vf14)
(.svf (&-> arg0 quad 2) vf15)
arg0
)
|#
)
(defun matrix-4x4-determinant ((arg0 matrix))
"Take the determinant of a 4x4 matrix, but this is wrong."
(let ((f15-0 (-> arg0 rvec x))
(f14-0 (-> arg0 rvec y))
(f10-0 (-> arg0 rvec z))
(f2-0 (-> arg0 rvec w))
(f9-0 (-> arg0 uvec x))
(f6-0 (-> arg0 uvec y))
(f3-0 (-> arg0 uvec z))
(f11-0 (-> arg0 uvec w))
(f5-0 (-> arg0 fvec x))
(f1-0 (-> arg0 fvec y))
(f8-0 (-> arg0 fvec z))
(f13-0 (-> arg0 fvec w))
(f0-0 (-> arg0 trans x))
(f7-0 (-> arg0 trans y))
(f4-0 (-> arg0 trans z))
(f12-0 (-> arg0 trans w))
)
(- (+ (* f15-0 f6-0 f8-0 f12-0)
(* f15-0 f3-0 f13-0 f7-0)
(* f15-0 f11-0 f1-0 f4-0)
(* f14-0 f9-0 f13-0 f4-0)
(* f14-0 f3-0 f5-0 f4-0)
(* f14-0 f11-0 f8-0 f0-0)
(* f10-0 f9-0 f1-0 f12-0)
(* f10-0 f6-0 f13-0 f0-0)
(* f10-0 f11-0 f5-0 f7-0)
(* f2-0 f9-0 f1-0 f4-0)
(* f2-0 f6-0 f8-0 f0-0)
(* f2-0 f3-0 f5-0 f7-0)
)
(+ (* f15-0 f6-0 f13-0 f4-0)
(* f15-0 f3-0 f1-0 f12-0)
(* f15-0 f11-0 f8-0 f7-0)
(* f14-0 f9-0 f8-0 f12-0)
(* f14-0 f3-0 f13-0 f0-0)
(* f14-0 f11-0 f5-0 f4-0)
(* f10-0 f9-0 f13-0 f7-0)
(* f10-0 f6-0 f5-0 f12-0)
(* f10-0 f11-0 f1-0 f0-0)
(* f2-0 f9-0 f8-0 f7-0)
(* f2-0 f6-0 f5-0 f4-0)
(* f2-0 f3-0 f1-0 f0-0)
)
)
)
)
(defun matrix-4x4-inverse-transpose! ((arg0 matrix) (arg1 matrix))
"Invert and transpose an entire 4x4. I think has no restrictions, other than dst != src. Unused.
The answer is wrong. The determinant function is wrong."
(let ((f0-0 (matrix-4x4-determinant arg1)))
(let ((f9-0 (-> arg1 uvec y))
(f2-0 (-> arg1 uvec z))
(f5-0 (-> arg1 uvec w))
(f3-0 (-> arg1 fvec y))
(f6-0 (-> arg1 fvec z))
(f10-0 (-> arg1 fvec w))
(f4-0 (-> arg1 trans y))
(f7-0 (-> arg1 trans z))
(f1-0 (-> arg1 trans w))
)
(set! (-> arg0 rvec x) (/ (- (+ (* f9-0 f6-0 f1-0) (* f2-0 f10-0 f4-0) (* f5-0 f3-0 f7-0))
(+ (* f9-0 f10-0 f7-0) (* f5-0 f6-0 f4-0) (* f2-0 f3-0 f1-0))
)
f0-0
)
)
)
(let ((f9-2 (-> arg1 uvec x))
(f2-2 (-> arg1 uvec z))
(f5-2 (-> arg1 uvec w))
(f3-1 (-> arg1 fvec x))
(f6-1 (-> arg1 fvec z))
(f10-1 (-> arg1 fvec w))
(f4-3 (-> arg1 trans x))
(f7-2 (-> arg1 trans z))
(f1-6 (-> arg1 trans w))
)
(set! (-> arg0 rvec y) (- (/ (- (+ (* f9-2 f6-1 f1-6) (* f2-2 f10-1 f4-3) (* f5-2 f3-1 f7-2))
(+ (* f9-2 f10-1 f7-2) (* f5-2 f6-1 f4-3) (* f2-2 f3-1 f1-6))
)
f0-0
)
)
)
)
(let ((f9-4 (-> arg1 uvec x))
(f2-4 (-> arg1 uvec y))
(f5-4 (-> arg1 uvec w))
(f3-2 (-> arg1 fvec x))
(f6-2 (-> arg1 fvec y))
(f10-2 (-> arg1 fvec w))
(f4-6 (-> arg1 trans x))
(f7-4 (-> arg1 trans y))
(f1-13 (-> arg1 trans w))
)
(set! (-> arg0 rvec z) (/ (- (+ (* f9-4 f6-2 f1-13) (* f2-4 f10-2 f4-6) (* f5-4 f3-2 f7-4))
(+ (* f9-4 f10-2 f7-4) (* f5-4 f6-2 f4-6) (* f2-4 f3-2 f1-13))
)
f0-0
)
)
)
(let ((f9-6 (-> arg1 uvec x))
(f2-6 (-> arg1 uvec y))
(f5-6 (-> arg1 uvec z))
(f3-3 (-> arg1 fvec x))
(f6-3 (-> arg1 fvec y))
(f10-3 (-> arg1 fvec z))
(f4-9 (-> arg1 trans x))
(f7-6 (-> arg1 trans y))
(f1-19 (-> arg1 trans z))
)
(set! (-> arg0 rvec w) (- (/ (- (+ (* f9-6 f6-3 f1-19) (* f2-6 f10-3 f4-9) (* f5-6 f3-3 f7-6))
(+ (* f9-6 f10-3 f7-6) (* f5-6 f6-3 f4-9) (* f2-6 f3-3 f1-19))
)
f0-0
)
)
)
)
(let ((f9-8 (-> arg1 rvec y))
(f2-8 (-> arg1 rvec z))
(f5-8 (-> arg1 rvec w))
(f3-4 (-> arg1 fvec y))
(f6-4 (-> arg1 fvec z))
(f10-4 (-> arg1 fvec w))
(f4-12 (-> arg1 trans y))
(f7-8 (-> arg1 trans z))
(f1-26 (-> arg1 trans w))
)
(set! (-> arg0 uvec x) (- (/ (- (+ (* f9-8 f6-4 f1-26) (* f2-8 f10-4 f4-12) (* f5-8 f3-4 f7-8))
(+ (* f9-8 f10-4 f7-8) (* f5-8 f6-4 f4-12) (* f2-8 f3-4 f1-26))
)
f0-0
)
)
)
)
(let ((f9-10 (-> arg1 rvec x))
(f2-10 (-> arg1 rvec z))
(f5-10 (-> arg1 rvec w))
(f3-5 (-> arg1 fvec x))
(f6-5 (-> arg1 fvec z))
(f10-5 (-> arg1 fvec w))
(f4-15 (-> arg1 trans x))
(f7-10 (-> arg1 trans z))
(f1-33 (-> arg1 trans w))
)
(set! (-> arg0 uvec y) (/ (- (+ (* f9-10 f6-5 f1-33) (* f2-10 f10-5 f4-15) (* f5-10 f3-5 f7-10))
(+ (* f9-10 f10-5 f7-10) (* f5-10 f6-5 f4-15) (* f2-10 f3-5 f1-33))
)
f0-0
)
)
)
(let ((f9-12 (-> arg1 rvec x))
(f2-12 (-> arg1 rvec y))
(f5-12 (-> arg1 rvec w))
(f3-6 (-> arg1 fvec x))
(f6-6 (-> arg1 fvec y))
(f10-6 (-> arg1 fvec w))
(f4-18 (-> arg1 trans x))
(f7-12 (-> arg1 trans y))
(f1-39 (-> arg1 trans w))
)
(set! (-> arg0 uvec z) (- (/ (- (+ (* f9-12 f6-6 f1-39) (* f2-12 f10-6 f4-18) (* f5-12 f3-6 f7-12))
(+ (* f9-12 f10-6 f7-12) (* f5-12 f6-6 f4-18) (* f2-12 f3-6 f1-39))
)
f0-0
)
)
)
)
(let ((f9-14 (-> arg1 rvec x))
(f2-14 (-> arg1 rvec y))
(f5-14 (-> arg1 rvec z))
(f3-7 (-> arg1 fvec x))
(f6-7 (-> arg1 fvec y))
(f10-7 (-> arg1 fvec z))
(f4-21 (-> arg1 trans x))
(f7-14 (-> arg1 trans y))
(f1-46 (-> arg1 trans z))
)
(set! (-> arg0 uvec w) (/ (- (+ (* f9-14 f6-7 f1-46) (* f2-14 f10-7 f4-21) (* f5-14 f3-7 f7-14))
(+ (* f9-14 f10-7 f7-14) (* f5-14 f6-7 f4-21) (* f2-14 f3-7 f1-46))
)
f0-0
)
)
)
(let ((f9-16 (-> arg1 rvec y))
(f2-16 (-> arg1 rvec z))
(f5-16 (-> arg1 rvec w))
(f3-8 (-> arg1 uvec y))
(f6-8 (-> arg1 uvec z))
(f10-8 (-> arg1 uvec w))
(f4-24 (-> arg1 trans y))
(f7-16 (-> arg1 trans z))
(f1-52 (-> arg1 trans w))
)
(set! (-> arg0 fvec x) (/ (- (+ (* f9-16 f6-8 f1-52) (* f2-16 f10-8 f4-24) (* f5-16 f3-8 f7-16))
(+ (* f9-16 f10-8 f7-16) (* f5-16 f6-8 f4-24) (* f2-16 f3-8 f1-52))
)
f0-0
)
)
)
(let ((f9-18 (-> arg1 rvec x))
(f2-18 (-> arg1 rvec z))
(f5-18 (-> arg1 rvec w))
(f3-9 (-> arg1 uvec x))
(f6-9 (-> arg1 uvec z))
(f10-9 (-> arg1 uvec w))
(f4-27 (-> arg1 trans x))
(f7-18 (-> arg1 trans z))
(f1-58 (-> arg1 trans w))
)
(set! (-> arg0 fvec y) (- (/ (- (+ (* f9-18 f6-9 f1-58) (* f2-18 f10-9 f4-27) (* f5-18 f3-9 f7-18))
(+ (* f9-18 f10-9 f7-18) (* f5-18 f6-9 f4-27) (* f2-18 f3-9 f1-58))
)
f0-0
)
)
)
)
(let ((f9-20 (-> arg1 rvec x))
(f2-20 (-> arg1 rvec y))
(f5-20 (-> arg1 rvec w))
(f3-10 (-> arg1 uvec x))
(f6-10 (-> arg1 uvec y))
(f10-10 (-> arg1 uvec w))
(f4-30 (-> arg1 trans x))
(f7-20 (-> arg1 trans y))
(f1-65 (-> arg1 trans w))
)
(set! (-> arg0 fvec z) (/ (- (+ (* f9-20 f6-10 f1-65) (* f2-20 f10-10 f4-30) (* f5-20 f3-10 f7-20))
(+ (* f9-20 f10-10 f7-20) (* f5-20 f6-10 f4-30) (* f2-20 f3-10 f1-65))
)
f0-0
)
)
)
(let ((f9-22 (-> arg1 rvec x))
(f2-22 (-> arg1 rvec y))
(f5-22 (-> arg1 rvec z))
(f3-11 (-> arg1 uvec x))
(f6-11 (-> arg1 uvec y))
(f10-11 (-> arg1 uvec z))
(f4-33 (-> arg1 trans x))
(f7-22 (-> arg1 trans y))
(f1-71 (-> arg1 trans z))
)
(set! (-> arg0 fvec w) (- (/ (- (+ (* f9-22 f6-11 f1-71) (* f2-22 f10-11 f4-33) (* f5-22 f3-11 f7-22))
(+ (* f9-22 f10-11 f7-22) (* f5-22 f6-11 f4-33) (* f2-22 f3-11 f1-71))
)
f0-0
)
)
)
)
(let ((f9-24 (-> arg1 rvec y))
(f2-24 (-> arg1 rvec z))
(f5-24 (-> arg1 rvec w))
(f3-12 (-> arg1 uvec y))
(f6-12 (-> arg1 uvec z))
(f10-12 (-> arg1 uvec w))
(f4-36 (-> arg1 fvec y))
(f7-24 (-> arg1 fvec z))
(f1-78 (-> arg1 fvec w))
)
(set! (-> arg0 trans x) (- (/ (- (+ (* f9-24 f6-12 f1-78) (* f2-24 f10-12 f4-36) (* f5-24 f3-12 f7-24))
(+ (* f9-24 f10-12 f7-24) (* f5-24 f6-12 f4-36) (* f2-24 f3-12 f1-78))
)
f0-0
)
)
)
)
(let ((f9-26 (-> arg1 rvec x))
(f2-26 (-> arg1 rvec z))
(f5-26 (-> arg1 rvec w))
(f3-13 (-> arg1 uvec x))
(f6-13 (-> arg1 uvec z))
(f10-13 (-> arg1 uvec w))
(f4-39 (-> arg1 fvec x))
(f7-26 (-> arg1 fvec z))
(f1-85 (-> arg1 fvec w))
)
(set! (-> arg0 trans y) (/ (- (+ (* f9-26 f6-13 f1-85) (* f2-26 f10-13 f4-39) (* f5-26 f3-13 f7-26))
(+ (* f9-26 f10-13 f7-26) (* f5-26 f6-13 f4-39) (* f2-26 f3-13 f1-85))
)
f0-0
)
)
)
(let ((f9-28 (-> arg1 rvec x))
(f2-28 (-> arg1 rvec y))
(f5-28 (-> arg1 rvec w))
(f3-14 (-> arg1 uvec x))
(f6-14 (-> arg1 uvec y))
(f10-14 (-> arg1 uvec w))
(f4-42 (-> arg1 fvec x))
(f7-28 (-> arg1 fvec y))
(f1-91 (-> arg1 fvec w))
)
(set! (-> arg0 trans z) (- (/ (- (+ (* f9-28 f6-14 f1-91) (* f2-28 f10-14 f4-42) (* f5-28 f3-14 f7-28))
(+ (* f9-28 f10-14 f7-28) (* f5-28 f6-14 f4-42) (* f2-28 f3-14 f1-91))
)
f0-0
)
)
)
)
(let ((f8-60 (-> arg1 rvec x))
(f1-98 (-> arg1 rvec y))
(f5-30 (-> arg1 rvec z))
(f2-30 (-> arg1 uvec x))
(f6-15 (-> arg1 uvec y))
(f9-30 (-> arg1 uvec z))
(f4-45 (-> arg1 fvec x))
(f7-30 (-> arg1 fvec y))
(f3-15 (-> arg1 fvec z))
)
(set! (-> arg0 trans w) (/ (- (+ (* f8-60 f6-15 f3-15) (* f1-98 f9-30 f4-45) (* f5-30 f2-30 f7-30))
(+ (* f8-60 f9-30 f7-30) (* f5-30 f6-15 f4-45) (* f1-98 f2-30 f3-15))
)
f0-0
)
)
)
)
arg0
)
(defun matrix-y-angle ((arg0 matrix))
"If mat has its upper 3x3 as a rotation, gets the y axis rotation."
(let ((v1-0 (-> arg0 fvec)))
(atan (-> v1-0 x) (-> v1-0 z))
)
)
(defun matrix->trans ((arg0 matrix) (arg1 vector))
"Multiply xyz by 1/w."
(rlet ((Q :class vf)
(vf0 :class vf)
(vf2 :class vf)
)
(init-vf0-vector)
(.lvf vf2 (&-> arg0 trans quad))
(.div.vf Q vf0 vf2 :fsf #b11 :ftf #b11)
(.wait.vf)
(.mul.vf vf2 vf2 Q :mask #b111)
(.nop.vf)
(.nop.vf)
(.mov.vf vf2 vf0 :mask #b1000)
(.svf (&-> arg1 quad) vf2)
arg1
)
)
(defun matrix<-trans ((arg0 matrix) (arg1 vector))
"Set the translation."
(set! (-> arg0 trans quad) (-> arg1 quad))
arg0
)
(defun matrix->scale ((arg0 matrix) (arg1 vector))
"Get the scale of a matrix."
(set! (-> arg1 x) (vector-length (-> arg0 rvec)))
(set! (-> arg1 y) (vector-length (-> arg0 uvec)))
(set! (-> arg1 z) (vector-length (-> arg0 fvec)))
(set! (-> arg1 w) 1.0)
arg1
)
(defun matrix<-scale ((arg0 matrix) (arg1 vector))
"Set the scale of a matrix by rescaling."
(vector-normalize! (-> arg0 rvec) (-> arg1 x))
(vector-normalize! (-> arg0 uvec) (-> arg1 y))
(vector-normalize! (-> arg0 fvec) (-> arg1 z))
arg0
)
(defun matrix->quat ((arg0 matrix) (arg1 quaternion))
"Convert matrix to quaternion, works for matrix with scale.
unlike matrix->quaternion."
(let ((s5-0 (new 'stack-no-clear 'matrix)))
(let* ((a2-0 arg0)
(v1-0 (-> a2-0 rvec quad))
(a0-1 (-> a2-0 uvec quad))
(a1-1 (-> a2-0 fvec quad))
(a2-1 (-> a2-0 trans quad))
)
(set! (-> s5-0 rvec quad) v1-0)
(set! (-> s5-0 uvec quad) a0-1)
(set! (-> s5-0 fvec quad) a1-1)
(set! (-> s5-0 trans quad) a2-1)
)
(vector-reset! (-> s5-0 trans))
(vector-normalize! (-> s5-0 rvec) 1.0)
(vector-normalize! (-> s5-0 uvec) 1.0)
(vector-normalize! (-> s5-0 fvec) 1.0)
(matrix->quaternion arg1 s5-0)
)
)
(defun matrix<-quat ((arg0 matrix) (arg1 quaternion))
"Modify the rotation part of a transform."
(let ((s5-0 (matrix->scale arg0 (new 'stack-no-clear 'vector)))
(s4-1 (quaternion->matrix (new 'stack-no-clear 'matrix) arg1))
)
(vector-normalize-copy! (-> arg0 rvec) (-> s4-1 rvec) (-> s5-0 x))
(vector-normalize-copy! (-> arg0 uvec) (-> s4-1 uvec) (-> s5-0 y))
(vector-normalize-copy! (-> arg0 fvec) (-> s4-1 fvec) (-> s5-0 z))
)
(set! (-> arg0 rvec w) 0.0)
(set! (-> arg0 uvec w) 0.0)
(set! (-> arg0 fvec w) 0.0)
arg0
)
(defun matrix->transformq ((arg0 transformq) (arg1 matrix))
"Create a transformq from a matrix. Allowing scale, etc."
(rlet ((Q :class vf)
(vf0 :class vf)
(vf2 :class vf)
)
(init-vf0-vector)
(.lvf vf2 (&-> arg1 trans quad))
(.div.vf Q vf0 vf2 :fsf #b11 :ftf #b11)
(.wait.vf)
(.mul.vf vf2 vf2 Q :mask #b111)
(.nop.vf)
(.nop.vf)
(.mov.vf vf2 vf0 :mask #b1000)
(.svf (&-> arg0 trans quad) vf2)
(let ((s4-0 (new 'stack-no-clear 'matrix)))
(let* ((a2-0 arg1)
(v1-0 (-> a2-0 rvec quad))
(a0-1 (-> a2-0 uvec quad))
(a1-1 (-> a2-0 fvec quad))
(a2-1 (-> a2-0 trans quad))
)
(set! (-> s4-0 rvec quad) v1-0)
(set! (-> s4-0 uvec quad) a0-1)
(set! (-> s4-0 fvec quad) a1-1)
(set! (-> s4-0 trans quad) a2-1)
)
(vector-normalize! (-> s4-0 rvec) 1.0)
(vector-normalize! (-> s4-0 uvec) 1.0)
(vector-normalize! (-> s4-0 fvec) 1.0)
(vector-cross! (-> s4-0 fvec) (-> s4-0 rvec) (-> s4-0 uvec))
(vector-cross! (-> s4-0 uvec) (-> s4-0 fvec) (-> s4-0 rvec))
(matrix->quaternion (the-as quaternion (-> arg0 rot)) s4-0)
)
(set-vector!
(-> arg0 scale)
(vector-length (-> arg1 rvec))
(vector-length (-> arg1 uvec))
(vector-length (-> arg1 fvec))
1.0
)
arg0
)
)
(defmethod transform-vectors! matrix ((this _type_) (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."
(rlet ((vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
(vf7 :class vf)
(vf8 :class vf)
(acc :class vf)
(v1 :type int)
)
(init-vf0-vector)
;; blez a3, L3
(when-goto (<= count 0) end)
;; lqc2 vf1, 0(a0)
(.lvf vf1 (-> this vector 0))
;; lqc2 vf2, 16(a0)
(.lvf vf2 (-> this vector 1))
;; lqc2 vf3, 32(a0)
(.lvf vf3 (-> this vector 2))
;; lqc2 vf4, 48(a0)
(.lvf vf4 (-> this vector 3))
;; lqc2 vf5, 0(a2)
(.lvf vf5 (-> src 0))
;; lqc2 vf6, 16(a2)
(.lvf vf6 (-> src 1))
(label loop-top)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;vmulaw.xyzw acc, vf4, vf0
(.mul.w.vf acc vf4 vf0)
;; lqc2 vf7, 32(a2)
(.lvf vf7 (-> src 2))
;;vmaddax.xyzw acc, vf1, vf5
(.add.mul.x.vf acc vf1 vf5 acc)
;;lqc2 vf8, 48(a2)
(.lvf vf8 (-> src 3))
;;vmadday.xyzw acc, vf2, vf5
(.add.mul.y.vf acc vf2 vf5 acc)
;;daddiu a2, a2, 64
;;(&+! src 64)
(set! src (the (inline-array vector) (+ 64 (the int src))))
;;vmaddz.xyz vf5, vf3, vf5
(.add.mul.z.vf vf5 vf3 vf5 acc :mask #b0111)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;daddiu v1, a3, -1
(set! v1 (- count 1))
;;vmulaw.xyzw acc, vf4, vf0
(.mul.w.vf acc vf4 vf0)
;;vmaddax.xyzw acc, vf1, vf6
(.add.mul.x.vf acc vf1 vf6 acc)
;; vmadday.xyzw acc, vf2, vf6
(.add.mul.y.vf acc vf2 vf6 acc)
;; vmaddz.xyz vf6, vf3, vf6
(.add.mul.z.vf vf6 vf3 vf6 acc :mask #b0111)
;;sqc2 vf5, 0(a1)
(.svf (-> dst 0) vf5)
;;beq v1, r0, L3
;;vmulaw.xyzw acc, vf4, vf0
(.mul.w.vf acc vf4 vf0)
(when-goto (zero? v1) end)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;vmaddax.xyzw acc, vf1, vf7
(.add.mul.x.vf acc vf1 vf7 acc)
;;daddiu v1, v1, -1
(+! v1 -1)
;;vmadday.xyzw acc, vf2, vf7
(.add.mul.y.vf acc vf2 vf7 acc)
;;lqc2 vf5, 0(a2)
(.lvf vf5 (-> src 0))
;;vmaddz.xyz vf7, vf3, vf7
(.add.mul.z.vf vf7 vf3 vf7 acc :mask #b0111)
;;sqc2 vf6, 16(a1)
(.svf (-> dst 1) vf6)
;;beq v1, r0, L3
;;vmulaw.xyzw acc, vf4, vf0
(.mul.w.vf acc vf4 vf0)
(when-goto (zero? v1) end)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;vmaddax.xyzw acc, vf1, vf8
(.add.mul.x.vf acc vf1 vf8 acc)
;;daddiu v1, v1, -1
(+! v1 -1)
;;vmadday.xyzw acc, vf2, vf8
(.add.mul.y.vf acc vf2 vf8 acc)
;;lqc2 vf6, 16(a2)
(.lvf vf6 (-> src 1))
;;vmaddz.xyz vf8, vf3, vf8
(.add.mul.z.vf vf8 vf3 vf8 acc :mask #b0111)
;;sqc2 vf7, 32(a1)
(.svf (-> dst 2) vf7)
;;beq v1, r0, L3
;;daddiu a1, a1, 64
;;(&!+ dst 64)
(set! dst (the (inline-array vector) (+ 64 (the int dst))))
(when-goto (zero? v1) end)
;;daddiu a3, v1, -1
(set! count (- v1 1))
;;sqc2 vf8, -16(a1)
(.svf dst vf8 :offset -16)
(when-goto (not (zero? count)) loop-top)
(label end)
(none)
)
)
(defun matrix-mirror! ((arg0 matrix) (arg1 vector) (arg2 vector))
"Make a matrix that mirrors. Arguments are unclear."
(let ((f10-0 -2.0)
(f0-0 1.0)
)
(let* ((f1-0 0.0)
(f9-0 (-> arg2 x))
(f12-0 (-> arg2 y))
(f11-0 (-> arg2 z))
(f6-0 (-> arg1 x))
(f5-0 (-> arg1 y))
(f2-0 (-> arg1 z))
(f3-2 (+ f0-0 (* f10-0 f9-0 f9-0)))
(f4-2 (+ f0-0 (* f10-0 f12-0 f12-0)))
(f7-2 (+ f0-0 (* f10-0 f11-0 f11-0)))
(f8-1 (* f10-0 f9-0 f12-0))
(f9-2 (* f10-0 f9-0 f11-0))
(f10-2 (* f10-0 f12-0 f11-0))
(f11-4 (- f6-0 (+ (* f6-0 f3-2) (* f5-0 f8-1) (* f2-0 f9-2))))
(f12-6 (- f5-0 (+ (* f6-0 f8-1) (* f5-0 f4-2) (* f2-0 f10-2))))
(f2-1 (- f2-0 (+ (* f6-0 f9-2) (* f5-0 f10-2) (* f2-0 f7-2))))
)
(set! (-> arg0 rvec x) f3-2)
(set! (-> arg0 rvec y) f8-1)
(set! (-> arg0 rvec z) f9-2)
(set! (-> arg0 rvec w) f1-0)
(set! (-> arg0 uvec x) f8-1)
(set! (-> arg0 uvec y) f4-2)
(set! (-> arg0 uvec z) f10-2)
(set! (-> arg0 uvec w) f1-0)
(set! (-> arg0 fvec x) f9-2)
(set! (-> arg0 fvec y) f10-2)
(set! (-> arg0 fvec z) f7-2)
(set! (-> arg0 fvec w) f1-0)
(set! (-> arg0 trans x) f11-4)
(set! (-> arg0 trans y) f12-6)
(set! (-> arg0 trans z) f2-1)
)
(set! (-> arg0 trans w) f0-0)
)
arg0
)
Reference in a new issue View git blame Copy permalink