jak-project/test/decompiler/reference/engine/math/transformq_REF.gc

;;-*-Lisp-*-
(in-package goal)

;; definition for method 2 of type transformq
(defmethod print transformq ((obj 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
  )

;; definition for method 27 of type trsqv
(defmethod get-quaternion trsqv ((obj trsqv))
  (-> obj quat)
  )

;; definition for method 18 of type trsqv
(defmethod set-quaternion! trsqv ((obj trsqv) (arg0 quaternion))
  (quaternion-copy! (get-quaternion obj) arg0)
  )

;; definition for method 21 of type trsqv
(defmethod rot->dir-targ! trsqv ((obj trsqv))
  (quaternion-copy! (-> obj dir-targ) (get-quaternion obj))
  )

;; definition for method 22 of type trsqv
(defmethod y-angle trsqv ((obj trsqv))
  (quaternion-y-angle (get-quaternion obj))
  )

;; definition for method 9 of type trsqv
(defmethod
  seek-toward-heading-vec!
  trsqv
  ((obj trsqv) (dir vector) (vel float) (frame-count int))
  (let*
   ((yaw-error
     (deg-diff (quaternion-y-angle (-> obj quat)) (vector-y-angle dir))
     )
    (yaw-limit
     (fmin
      (* vel (-> *display* seconds-per-frame))
      (/ (* 5.0 (fabs yaw-error)) (the float frame-count))
      )
     )
    (saturated-yaw (fmax (fmin yaw-error yaw-limit) (- yaw-limit)))
    )
   (let ((old-diff (-> obj old-y-angle-diff)))
    (set! saturated-yaw (cond
                         ((or
                           (= old-diff 0.0)
                           (and (< 0.0 saturated-yaw) (< 0.0 old-diff))
                           (or
                            (and (< saturated-yaw 0.0) (< old-diff 0.0))
                            (>=
                             (-
                              (-> *display* base-frame-counter)
                              (-> obj angle-change-time)
                              )
                             60
                             )
                            )
                           )
                          (set!
                           (-> obj angle-change-time)
                           (-> *display* base-frame-counter)
                           )
                          saturated-yaw
                          )
                         (else
                          (* 0.000000001 saturated-yaw)
                          )
                         )
     )
    )
   (set! (-> obj old-y-angle-diff) saturated-yaw)
   (let ((quat (get-quaternion obj)))
    (quaternion-rotate-y! quat quat saturated-yaw)
    )
   )
  )

;; definition for method 10 of type trsqv
(defmethod set-heading-vec! trsqv ((obj trsqv) (arg0 vector))
  (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)
    )
   )
  )

;; definition for method 11 of type trsqv
(defmethod
  seek-to-point-toward-point!
  trsqv
  ((obj trsqv) (arg0 vector) (arg1 float) (arg2 int))
  (seek-toward-heading-vec!
   obj
   (vector-! (new 'stack-no-clear 'vector) arg0 (-> obj trans))
   arg1
   arg2
   )
  )

;; definition for method 12 of type trsqv
(defmethod point-toward-point! trsqv ((obj trsqv) (arg0 vector))
  (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)
    )
   )
  )

;; definition for method 13 of type trsqv
(defmethod
  seek-toward-yaw-angle!
  trsqv
  ((obj trsqv) (yaw float) (vel float) (frame-count int))
  (let ((s3-0 (method-of-object obj seek-toward-heading-vec!))
        (s2-0 (new 'stack-no-clear 'vector))
        )
   (set! (-> s2-0 x) (sin yaw))
   (set! (-> s2-0 y) 0.0)
   (set! (-> s2-0 z) (cos yaw))
   (set! (-> s2-0 w) 1.0)
   (s3-0 obj s2-0 vel frame-count)
   )
  )

;; definition for method 14 of type trsqv
(defmethod set-yaw-angle-clear-roll-pitch! trsqv ((obj trsqv) (arg0 float))
  (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)
   )
  )

;; definition for method 15 of type trsqv
(defmethod set-roll-to-grav! trsqv ((obj trsqv) (arg0 float))
  (set-roll-to-grav-2! obj arg0)
  )

;; definition for method 16 of type trsqv
(defmethod set-roll-to-grav-2! trsqv ((obj trsqv) (arg0 float))
  (let* ((quat (get-quaternion obj))
         (grav (-> *standard-dynamics* gravity-normal))
         (rot-mat (quaternion->matrix (new 'stack-no-clear 'matrix) quat))
         )
   (let ((dir-z (-> rot-mat vector 2)))
    (vector-normalize! (vector-flatten! (-> rot-mat vector 1) grav dir-z) 1.0)
    (vector-cross!
     (the-as vector (-> rot-mat vector))
     (-> rot-mat vector 1)
     dir-z
     )
    )
   (let ((a1-5 (matrix-rotate-z! (new 'stack-no-clear 'matrix) arg0)))
    (matrix*! rot-mat a1-5 rot-mat)
    )
   (matrix->quaternion quat rot-mat)
   )
  )

;; definition for method 25 of type trsqv
(defmethod roll-relative-to-gravity trsqv ((obj trsqv))
  (let* ((quat (get-quaternion obj))
         (dir-z (vector-z-quaternion! (new 'stack-no-clear 'vector) quat))
         (dir-y (vector-y-quaternion! (new 'stack-no-clear 'vector) quat))
         (dir-grav (-> *standard-dynamics* gravity-normal))
         (grav-z-plane
          (vector-normalize!
           (vector-flatten! (new 'stack-no-clear 'vector) dir-grav dir-z)
           1.0
           )
          )
         (grav-dot (vector-dot grav-z-plane dir-y))
         )
   (if
    (<
     (vector-dot
      (vector-cross! (new 'stack-no-clear 'vector) grav-z-plane dir-y)
      dir-z
      )
     0.0
     )
    (- (acos grav-dot))
    (acos grav-dot)
    )
   )
  )

;; definition for method 17 of type trsqv
;; Used lq/sq
(defmethod
  rotate-toward-orientation!
  trsqv
  ((obj trsqv) (target quaternion) (y-rate float) (z-rate float))
  (local-vars (sv-96 vector))
  (let ((quat (get-quaternion obj)))
   (let ((temp-quat (new 'stack-no-clear 'quaternion)))
    (when (< 0.0 z-rate)
     (let ((s1-0 quaternion-from-two-vectors-max-angle!)
           (s0-0 temp-quat)
           )
      (set! sv-96 (vector-y-quaternion! (new 'stack-no-clear 'vector) quat))
      (let ((a2-1 (vector-y-quaternion! (new 'stack-no-clear 'vector) target))
            (a3-1 (* z-rate (-> *display* seconds-per-frame)))
            )
       (s1-0 s0-0 sv-96 a2-1 a3-1)
       )
      )
     (quaternion-normalize! (quaternion*! quat temp-quat quat))
     )
    (when (< 0.0 y-rate)
     (quaternion-from-two-vectors-max-angle!
      temp-quat
      (vector-z-quaternion! (new 'stack-no-clear 'vector) quat)
      (vector-z-quaternion! (new 'stack-no-clear 'vector) target)
      (* y-rate (-> *display* seconds-per-frame))
      )
     (quaternion-normalize! (quaternion*! quat temp-quat quat))
     )
    )
   quat
   )
  )

;; definition for method 19 of type trsqv
(defmethod set-heading-vec-clear-roll-pitch! trsqv ((obj trsqv) (arg0 vector))
  (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)
   )
  )

;; definition for method 20 of type trsqv
(defmethod point-toward-point-clear-roll-pitch! trsqv ((obj trsqv) (arg0 vector))
  (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)
   )
  )

;; definition for function transformq-copy!
;; Used lq/sq
(defun transformq-copy! ((arg0 transformq) (arg1 transformq))
  (set! (-> arg0 trans quad) (-> arg1 trans quad))
  (set! (-> arg0 quat vec quad) (-> arg1 quat vec quad))
  (set! (-> arg0 scale quad) (-> arg1 scale quad))
  arg0
  )

;; definition for function matrix<-transformq!
;; Used lq/sq
(defun matrix<-transformq! ((arg0 matrix) (arg1 transformq))
  (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 vector 3 quad) (-> arg1 trans quad))
     )
    (else
     (.lvf vf1 (&-> arg1 scale quad))
     (.lvf vf2 (&-> arg1 trans quad))
     (.lvf vf3 (&-> arg0 vector 0 quad))
     (.lvf vf4 (&-> arg0 vector 1 quad))
     (.lvf vf5 (&-> arg0 vector 2 quad))
     (.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 vector 3 quad) vf2)
     (.svf (&-> arg0 vector 0 quad) vf3)
     (.svf (&-> arg0 vector 1 quad) vf4)
     (.svf (&-> arg0 vector 2 quad) vf5)
     (.mov v1-1 vf5)
     )
    )
   arg0
   )
  )

;; definition for function matrix<-no-trans-transformq!
(defun matrix<-no-trans-transformq! ((arg0 matrix) (arg1 transformq))
  (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 vector 0 quad))
   (.lvf vf4 (&-> arg0 vector 1 quad))
   (.lvf vf5 (&-> arg0 vector 2 quad))
   (.mov.vf vf2 vf0)
   (.mul.x.vf vf3 vf3 vf1)
   (.mul.y.vf vf4 vf4 vf1)
   (.mul.z.vf vf5 vf5 vf1)
   (.svf (&-> arg0 vector 3 quad) vf2)
   (.svf (&-> arg0 vector 0 quad) vf3)
   (.svf (&-> arg0 vector 1 quad) vf4)
   (.svf (&-> arg0 vector 2 quad) vf5)
   arg0
   )
  )

;; definition for function matrix<-transformq+trans!
(defun matrix<-transformq+trans! ((arg0 matrix) (arg1 transformq) (arg2 vector))
  (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 vector 0 quad))
   (.lvf vf4 (&-> arg0 vector 1 quad))
   (.lvf vf5 (&-> arg0 vector 2 quad))
   (.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 vector 3 quad) vf2)
   (.svf (&-> arg0 vector 0 quad) vf3)
   (.svf (&-> arg0 vector 1 quad) vf4)
   (.svf (&-> arg0 vector 2 quad) vf5)
   arg0
   )
  )

;; definition for function matrix<-transformq+world-trans!
(defun
  matrix<-transformq+world-trans!
  ((arg0 matrix) (arg1 transformq) (arg2 vector))
  (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 vector 0 quad))
   (.lvf vf4 (&-> arg0 vector 1 quad))
   (.lvf vf5 (&-> arg0 vector 2 quad))
   (.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 vector 3 quad) vf2)
   (.svf (&-> arg0 vector 0 quad) vf3)
   (.svf (&-> arg0 vector 1 quad) vf4)
   (.svf (&-> arg0 vector 2 quad) vf5)
   arg0
   )
  )

;; definition for function matrix<-parented-transformq!
(defun
  matrix<-parented-transformq!
  ((arg0 matrix) (arg1 transformq) (arg2 vector))
  (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 vector 0 quad))
    (.lvf vf5 (&-> arg0 vector 1 quad))
    (.lvf vf6 (&-> arg0 vector 2 quad))
    (.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 vector 3 quad) vf2)
   (.svf (&-> arg0 vector 0 quad) vf4)
   (.svf (&-> arg0 vector 1 quad) vf5)
   (.svf (&-> arg0 vector 2 quad) vf6)
   (.mov v1-1 vf6)
   arg0
   )
  )