Files
jak-project/goal_src/jak2/engine/math/transformq.gc
T
2023-05-18 21:33:46 +01:00

474 lines
16 KiB
Common Lisp

;;-*-Lisp-*-
(in-package goal)
;; name: transformq.gc
;; name in dgo: transformq
;; dgos: ENGINE, GAME
;; DECOMP BEGINS
(defmethod print transformq ((obj transformq))
"Print a transformq"
(format #t "#<transformq @ #x~X~%" obj)
(format #t "~T~Ttrans:~F ~F ~F ~F ~%" (-> obj trans x) (-> obj trans y) (-> obj trans z) (-> obj trans w))
(format #t "~T~Tquat: ~F ~F ~F ~F ~%" (-> obj quat x) (-> obj quat y) (-> obj quat z) (-> obj quat w))
(format #t "~T~Tscale:~F ~F ~F ~F>" (-> obj scale x) (-> obj scale y) (-> obj scale z) (-> obj scale w))
obj
)
(defmethod get-quaternion trsqv ((obj trsqv))
"Get the rotation as a quaternion."
(-> obj quat)
)
(defmethod set-quaternion! trsqv ((obj trsqv) (arg0 quaternion))
"Set the rotation as a quaternion"
(quaternion-copy! (get-quaternion obj) arg0)
)
(defmethod rot->dir-targ! trsqv ((obj trsqv))
"Set the dir-targ to our current orientation"
(quaternion-copy! (-> obj dir-targ) (get-quaternion obj))
)
(defmethod y-angle trsqv ((obj trsqv))
"Get our current y-angle (y is up, so yaw)"
(quaternion-y-angle (get-quaternion obj))
)
(defmethod seek-toward-heading-vec! trsqv ((obj trsqv) (arg0 vector) (arg1 float) (arg2 time-frame))
"Adjust the orientation to point along dir, only changing our yaw.
The vel is a maximum velocity limit.
The frame count is the time constant (first order).
There's some logic to avoid rapidly changing directions"
(with-pp
(let* ((f0-0 (deg-diff (quaternion-y-angle (-> obj quat)) (vector-y-angle arg0)))
(f1-2 (fmin (* arg1 (-> pp clock seconds-per-frame)) (/ (* 5.0 (fabs f0-0)) (the float arg2))))
(f30-0 (fmax (fmin f0-0 f1-2) (- f1-2)))
)
(let ((f0-2 (-> obj old-y-angle-diff)))
(set! f30-0
(cond
((or (= f0-2 0.0)
(and (< 0.0 f30-0) (< 0.0 f0-2))
(or (and (< f30-0 0.0) (< f0-2 0.0)) (>= (- (current-time) (-> obj angle-change-time)) (seconds 0.2)))
)
(set! (-> obj angle-change-time) (current-time))
f30-0
)
(else
(* 0.000000001 f30-0)
)
)
)
)
(set! (-> obj old-y-angle-diff) f30-0)
(let ((a1-2 (get-quaternion obj)))
(quaternion-rotate-y! a1-2 a1-2 f30-0)
)
)
)
)
(defmethod set-heading-vec! trsqv ((obj trsqv) (arg0 vector))
"Makes us look in the arg0 direction immediately. Pitch will be unchanged."
(let ((s3-0 (get-quaternion obj)))
(forward-up-nopitch->quaternion
s3-0
(vector-normalize-copy! (new 'stack-no-clear 'vector) arg0 1.0)
(vector-y-quaternion! (new 'stack-no-clear 'vector) s3-0)
)
)
)
(defmethod seek-to-point-toward-point! trsqv ((obj trsqv) (arg0 vector) (arg1 float) (arg2 time-frame))
"Seek toward pointing toward arg0 from our current location."
(seek-toward-heading-vec! obj (vector-! (new 'stack-no-clear 'vector) arg0 (-> obj trans)) arg1 arg2)
)
(defmethod point-toward-point! trsqv ((obj trsqv) (arg0 vector))
"Immediately point toward arg0"
(let ((s3-0 (get-quaternion obj)))
(forward-up-nopitch->quaternion
s3-0
(vector-normalize! (vector-! (new 'stack-no-clear 'vector) arg0 (-> obj trans)) 1.0)
(vector-y-quaternion! (new 'stack-no-clear 'vector) s3-0)
)
)
)
(defmethod seek-toward-yaw-angle! trsqv ((obj trsqv) (arg0 float) (arg1 float) (arg2 time-frame))
"Seek toward the given yaw angle."
(let ((s3-0 (method-of-object obj seek-toward-heading-vec!))
(s2-0 (new 'stack-no-clear 'vector))
)
(set! (-> s2-0 x) (sin arg0))
(set! (-> s2-0 y) 0.0)
(set! (-> s2-0 z) (cos arg0))
(set! (-> s2-0 w) 1.0)
(s3-0 obj s2-0 arg1 arg2)
)
)
(defmethod set-yaw-angle-clear-roll-pitch! trsqv ((obj trsqv) (arg0 float))
"Immediately clear our roll and pitch and set yaw to the given angle"
(let ((s5-0 (method-of-object obj set-heading-vec-clear-roll-pitch!))
(s4-0 (new 'stack-no-clear 'vector))
)
(set! (-> s4-0 x) (sin arg0))
(set! (-> s4-0 y) 0.0)
(set! (-> s4-0 z) (cos arg0))
(set! (-> s4-0 w) 1.0)
(s5-0 obj s4-0)
)
)
(defmethod set-roll-to-grav! trsqv ((obj trsqv) (arg0 float))
"Set our roll so that our local down aligns with standard gravity"
(set-roll-to-grav-2! obj arg0)
)
(defmethod set-roll-to-grav-2! trsqv ((obj trsqv) (arg0 float))
"Set our roll so that our local down aligns with standard gravity"
(let* ((s5-0 (get-quaternion obj))
(s1-0 (-> *standard-dynamics* gravity-normal))
(s3-0 (quaternion->matrix (new 'stack-no-clear 'matrix) s5-0))
)
(let ((s4-0 (-> s3-0 vector 2)))
(vector-normalize! (vector-flatten! (-> s3-0 vector 1) s1-0 s4-0) 1.0)
(vector-cross! (the-as vector (-> s3-0 vector)) (-> s3-0 vector 1) s4-0)
)
(let ((a1-5 (matrix-rotate-z! (new 'stack-no-clear 'matrix) arg0)))
(matrix*! s3-0 a1-5 s3-0)
)
(matrix->quaternion s5-0 s3-0)
)
)
(defmethod roll-relative-to-gravity trsqv ((obj trsqv))
"Get our roll, relative to 'down' from gravity"
(let* ((s5-0 (get-quaternion obj))
(gp-0 (vector-z-quaternion! (new 'stack-no-clear 'vector) s5-0))
(s5-1 (vector-y-quaternion! (new 'stack-no-clear 'vector) s5-0))
(a1-2 (-> *standard-dynamics* gravity-normal))
(v1-2 (vector-normalize! (vector-flatten! (new 'stack-no-clear 'vector) a1-2 gp-0) 1.0))
(f0-1 (vector-dot v1-2 s5-1))
)
(if (< (vector-dot (vector-cross! (new 'stack-no-clear 'vector) v1-2 s5-1) gp-0) 0.0)
(- (acos f0-1))
(acos f0-1)
)
)
)
(defmethod rotate-toward-orientation! trsqv ((obj trsqv) (arg0 quaternion) (arg1 float) (arg2 float) (arg3 int) (arg4 int) (arg5 float))
"Adjust our orientation toward target, subject to some rate limits.
For jak 1, I said:
I don't think this is a very robust function and probably doesn't work right in cases
where an axis flips by 180 degrees.
But now they use matrix-from-two-vectors-the-long-way-smooth to fix it! Good job.
This additionally uses the fancy logic of matrix-from-two-vectors-smooth."
(local-vars
(f0-4 float)
(sv-192 (function quaternion vector vector float int quaternion))
(sv-208 quaternion)
(sv-224 vector)
(sv-240 int)
(sv-256 vector)
(sv-272 vector)
)
(set! sv-240 arg4)
(let ((s2-0 arg5)
(s5-0 (get-quaternion obj))
)
(let ((gp-0 (new 'stack-no-clear 'quaternion)))
(when (< 0.0 arg2)
(set! sv-192 quaternion-from-two-vectors-smooth!)
(set! sv-208 gp-0)
(set! sv-224 (vector-y-quaternion! (new 'stack-no-clear 'vector) s5-0))
(let ((a2-1 (vector-y-quaternion! (new 'stack-no-clear 'vector) arg0)))
(sv-192 sv-208 sv-224 a2-1 arg2 sv-240)
)
(quaternion-normalize! (quaternion*! s5-0 gp-0 s5-0))
)
(when (< 0.0 arg1)
(set! sv-256 (vector-y-quaternion! (new 'stack-no-clear 'vector) s5-0))
(let ((s1-2 (vector-z-quaternion! (new 'stack-no-clear 'vector) s5-0)))
(set! sv-272 (vector-z-quaternion! (new 'stack-no-clear 'vector) arg0))
(let ((s0-1 (new 'stack-no-clear 'matrix)))
(vector-flatten! s1-2 s1-2 sv-256)
(vector-flatten! sv-272 sv-272 sv-256)
(vector-normalize! s1-2 1.0)
(vector-normalize! sv-272 1.0)
(cond
((!= s2-0 0.0)
(let* ((v1-5 (vector-cross! (new 'stack-no-clear 'vector) s1-2 sv-272))
; (f0-3 (-> sv-256 x))
; (f1-3 (-> sv-256 y))
; (f2-0 (-> sv-256 z))
; (f3-0 (-> v1-5 x))
; (f4-0 (-> v1-5 y))
; (f5-0 (-> v1-5 z))
)
(set! f0-4 (vector-dot sv-256 v1-5))
; (.mula.s f0-3 f3-0)
; (.madda.s f1-3 f4-0)
; (.madd.s f0-4 f2-0 f5-0)
)
(if (< (* f0-4 s2-0) 0.0)
(matrix-from-two-vectors-the-long-way-smooth! s0-1 s1-2 sv-272 arg1 arg3)
(matrix-from-two-vectors-smooth! s0-1 s1-2 sv-272 arg1 arg3)
)
)
(else
(matrix-from-two-vectors-smooth! s0-1 s1-2 sv-272 arg1 arg3)
)
)
(matrix->quaternion gp-0 s0-1)
)
)
(quaternion-normalize! (quaternion*! s5-0 gp-0 s5-0))
)
)
s5-0
)
)
(defmethod set-heading-vec-clear-roll-pitch! trsqv ((obj trsqv) (arg0 vector))
"Set our rotation to point along the given heading, with no roll or pitch."
(forward-up->quaternion
(get-quaternion obj)
(vector-normalize-copy! (new 'stack-no-clear 'vector) arg0 1.0)
(new 'static 'vector :y 1.0 :w 1.0)
)
)
(defmethod point-toward-point-clear-roll-pitch! trsqv ((obj trsqv) (arg0 vector))
"Set our orientation to point toward arg0, clearing roll and pitch"
(forward-up->quaternion
(get-quaternion obj)
(vector-normalize! (vector-! (new 'stack-no-clear 'vector) arg0 (-> obj trans)) 1.0)
(new 'static 'vector :y 1.0 :w 1.0)
)
)
(defun transformq-copy! ((arg0 transformq) (arg1 transformq))
"Set arg0 = arg1"
(let ((v1-0 (-> arg1 trans quad))
(a2-0 (-> arg1 quat quad))
(a1-1 (-> arg1 scale quad))
)
(set! (-> arg0 trans quad) v1-0)
(set! (-> arg0 quat quad) a2-0)
(set! (-> arg0 scale quad) a1-1)
)
arg0
)
(defun matrix<-transformq! ((arg0 matrix) (arg1 transformq))
"Convert to 4x4 affine transform."
(local-vars (v1-1 float))
(rlet ((vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
)
(init-vf0-vector)
(quaternion->matrix arg0 (-> arg1 quat))
(cond
(#f
(set! (-> arg0 trans quad) (-> arg1 trans quad))
)
(else
(.lvf vf1 (&-> arg1 scale quad))
(.lvf vf2 (&-> arg1 trans quad))
(.lvf vf3 (&-> arg0 quad 0))
(.lvf vf4 (&-> arg0 quad 1))
(.lvf vf5 (&-> arg0 quad 2))
(.mov.vf vf2 vf0 :mask #b1000)
(.mul.x.vf vf3 vf3 vf1)
(.mul.y.vf vf4 vf4 vf1)
(.mul.z.vf vf5 vf5 vf1)
(.svf (&-> arg0 trans quad) vf2)
(.svf (&-> arg0 quad 0) vf3)
(.svf (&-> arg0 quad 1) vf4)
(.svf (&-> arg0 quad 2) vf5)
(.mov v1-1 vf5)
)
)
arg0
)
)
(defun matrix<-no-trans-transformq! ((arg0 matrix) (arg1 transformq))
"Create 4x4 affine transform with no translation."
(rlet ((vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
)
(init-vf0-vector)
(quaternion->matrix arg0 (-> arg1 quat))
(.lvf vf1 (&-> arg1 scale quad))
(.lvf vf3 (&-> arg0 quad 0))
(.lvf vf4 (&-> arg0 quad 1))
(.lvf vf5 (&-> arg0 quad 2))
(.mov.vf vf2 vf0)
(.mul.x.vf vf3 vf3 vf1)
(.mul.y.vf vf4 vf4 vf1)
(.mul.z.vf vf5 vf5 vf1)
(.svf (&-> arg0 trans quad) vf2)
(.svf (&-> arg0 quad 0) vf3)
(.svf (&-> arg0 quad 1) vf4)
(.svf (&-> arg0 quad 2) vf5)
arg0
)
)
(defun matrix<-transformq+trans! ((arg0 matrix) (arg1 transformq) (arg2 vector))
"Convert to affine transform with an additional translation (in the local frame)."
(rlet ((acc :class vf)
(vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
)
(init-vf0-vector)
(quaternion->matrix arg0 (-> arg1 quat))
(.lvf vf1 (&-> arg1 scale quad))
(.lvf vf2 (&-> arg1 trans quad))
(.lvf vf6 (&-> arg2 quad))
(.lvf vf3 (&-> arg0 quad 0))
(.lvf vf4 (&-> arg0 quad 1))
(.lvf vf5 (&-> arg0 quad 2))
(.mov.vf vf2 vf0 :mask #b1000)
(.mul.x.vf vf3 vf3 vf1)
(.mul.y.vf vf4 vf4 vf1)
(.mul.z.vf vf5 vf5 vf1)
(.mul.x.vf acc vf3 vf6)
(.add.mul.y.vf acc vf4 vf6 acc)
(.add.mul.z.vf acc vf5 vf6 acc)
(.add.mul.w.vf vf2 vf2 vf0 acc :mask #b111)
(.svf (&-> arg0 trans quad) vf2)
(.svf (&-> arg0 quad 0) vf3)
(.svf (&-> arg0 quad 1) vf4)
(.svf (&-> arg0 quad 2) vf5)
arg0
)
)
(defun matrix<-transformq+world-trans! ((arg0 matrix) (arg1 transformq) (arg2 vector))
"Convert to affine transform with an additional translation in the world frame (not rotated)"
(rlet ((vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
)
(init-vf0-vector)
(quaternion->matrix arg0 (-> arg1 quat))
(.lvf vf1 (&-> arg1 scale quad))
(.lvf vf2 (&-> arg1 trans quad))
(.lvf vf6 (&-> arg2 quad))
(.lvf vf3 (&-> arg0 quad 0))
(.lvf vf4 (&-> arg0 quad 1))
(.lvf vf5 (&-> arg0 quad 2))
(.mov.vf vf2 vf0 :mask #b1000)
(.mul.x.vf vf3 vf3 vf1)
(.mul.y.vf vf4 vf4 vf1)
(.mul.z.vf vf5 vf5 vf1)
(.add.vf vf2 vf2 vf6 :mask #b111)
(.svf (&-> arg0 trans quad) vf2)
(.svf (&-> arg0 quad 0) vf3)
(.svf (&-> arg0 quad 1) vf4)
(.svf (&-> arg0 quad 2) vf5)
arg0
)
)
(defun matrix<-parented-transformq! ((arg0 matrix) (arg1 transformq) (arg2 vector))
"Unused. Seems like the parented thing means there's an inverse scale in arg2."
(local-vars (v1-1 float))
(rlet ((vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
)
(init-vf0-vector)
(quaternion->matrix arg0 (-> arg1 quat))
(let ((v1-0 (new 'stack-no-clear 'vector)))
(set! (-> v1-0 x) (/ 1.0 (-> arg2 x)))
(set! (-> v1-0 y) (/ 1.0 (-> arg2 y)))
(set! (-> v1-0 z) (/ 1.0 (-> arg2 z)))
(.lvf vf1 (&-> arg1 scale quad))
(.lvf vf2 (&-> arg1 trans quad))
(.mov.vf vf2 vf0 :mask #b1000)
(.lvf vf4 (&-> arg0 quad 0))
(.lvf vf5 (&-> arg0 quad 1))
(.lvf vf6 (&-> arg0 quad 2))
(.mul.x.vf vf4 vf4 vf1)
(.mul.y.vf vf5 vf5 vf1)
(.mul.z.vf vf6 vf6 vf1)
(.lvf vf3 (&-> v1-0 quad))
)
(.mul.vf vf4 vf4 vf3)
(.mul.vf vf5 vf5 vf3)
(.mul.vf vf6 vf6 vf3)
(.svf (&-> arg0 trans quad) vf2)
(.svf (&-> arg0 quad 0) vf4)
(.svf (&-> arg0 quad 1) vf5)
(.svf (&-> arg0 quad 2) vf6)
(.mov v1-1 vf6)
arg0
)
)
(defun matrix<-transformq+rot-offset! ((arg0 matrix) (arg1 transformq) (arg2 vector))
"Affine transform for a point offset in the destination frame of the transformq (rather than just
adding an offset at the end)"
(rlet ((acc :class vf)
(vf0 :class vf)
(vf1 :class vf)
(vf2 :class vf)
(vf3 :class vf)
(vf4 :class vf)
(vf5 :class vf)
(vf6 :class vf)
)
(init-vf0-vector)
(quaternion->matrix arg0 (-> arg1 quat))
(.lvf vf1 (&-> arg1 scale quad))
(.lvf vf2 (&-> arg1 trans quad))
(.lvf vf6 (&-> arg2 quad))
(.lvf vf3 (&-> arg0 quad 0))
(.lvf vf4 (&-> arg0 quad 1))
(.lvf vf5 (&-> arg0 quad 2))
(.mov.vf vf2 vf0 :mask #b1000)
(.mul.x.vf vf3 vf3 vf1)
(.mul.y.vf vf4 vf4 vf1)
(.mul.z.vf vf5 vf5 vf1)
(.mul.x.vf acc vf3 vf6)
(.add.mul.y.vf acc vf4 vf6 acc)
(.add.mul.z.vf acc vf5 vf6 acc)
(.sub.mul.w.vf acc vf6 vf0 acc)
(.add.mul.w.vf vf2 vf2 vf0 acc :mask #b111)
(.svf (&-> arg0 trans quad) vf2)
(.svf (&-> arg0 quad 0) vf3)
(.svf (&-> arg0 quad 1) vf4)
(.svf (&-> arg0 quad 2) vf5)
arg0
)
)