jak-project/goal_src/jak2/engine/math/transformq.gc

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

;; name: transformq.gc
;; name in dgo: transformq
;; dgos: ENGINE, GAME

;; DECOMP BEGINS

(defmethod print ((this transformq))
  (format #t "#<transformq @ #x~X~%" this)
  (format #t "~T~Ttrans:~F ~F ~F ~F ~%" (-> this trans x) (-> this trans y) (-> this trans z) (-> this trans w))
  (format #t "~T~Tquat:  ~F ~F ~F ~F ~%" (-> this quat x) (-> this quat y) (-> this quat z) (-> this quat w))
  (format #t "~T~Tscale:~F ~F ~F ~F>" (-> this scale x) (-> this scale y) (-> this scale z) (-> this scale w))
  this
  )

(defmethod get-quaternion ((this trsqv))
  (-> this quat)
  )

(defmethod set-quaternion! ((this trsqv) (arg0 quaternion))
  (quaternion-copy! (get-quaternion this) arg0)
  )

(defmethod rot->dir-targ! ((this trsqv))
  (quaternion-copy! (-> this dir-targ) (get-quaternion this))
  )

(defmethod y-angle ((this trsqv))
  (quaternion-y-angle (get-quaternion this))
  )

(defmethod seek-toward-heading-vec! ((this 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"
  (let* ((f0-0 (deg-diff (quaternion-y-angle (-> this quat)) (vector-y-angle arg0)))
         (f1-2 (fmin (* arg1 (seconds-per-frame)) (/ (* 5.0 (fabs f0-0)) (the float arg2))))
         (f30-0 (fmax (fmin f0-0 f1-2) (- f1-2)))
         )
    (let ((f0-2 (-> this 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)) (time-elapsed? (-> this angle-change-time) (seconds 0.2)))
                   )
               (set-time! (-> this angle-change-time))
               f30-0
               )
              (else
                (* 0.000000001 f30-0)
                )
              )
            )
      )
    (set! (-> this old-y-angle-diff) f30-0)
    (let ((a1-2 (get-quaternion this)))
      (quaternion-rotate-y! a1-2 a1-2 f30-0)
      )
    )
  )

(defmethod set-heading-vec! ((this trsqv) (arg0 vector))
  "Makes us look in the arg0 direction immediately.  Pitch will be unchanged."
  (let ((s3-0 (get-quaternion this)))
    (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! ((this trsqv) (arg0 vector) (arg1 float) (arg2 time-frame))
  "Seek toward pointing toward arg0 from our current location."
  (seek-toward-heading-vec! this (vector-! (new 'stack-no-clear 'vector) arg0 (-> this trans)) arg1 arg2)
  )

(defmethod point-toward-point! ((this trsqv) (arg0 vector))
  "Immediately point toward arg0"
  (let ((s3-0 (get-quaternion this)))
    (forward-up-nopitch->quaternion
      s3-0
      (vector-normalize! (vector-! (new 'stack-no-clear 'vector) arg0 (-> this trans)) 1.0)
      (vector-y-quaternion! (new 'stack-no-clear 'vector) s3-0)
      )
    )
  )

(defmethod seek-toward-yaw-angle! ((this trsqv) (arg0 float) (arg1 float) (arg2 time-frame))
  "Seek toward the given yaw angle."
  (let ((s3-0 (method-of-object this 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 this s2-0 arg1 arg2)
    )
  )

(defmethod set-yaw-angle-clear-roll-pitch! ((this trsqv) (arg0 float))
  "Immediately clear our roll and pitch and set yaw to the given angle"
  (let ((s5-0 (method-of-object this 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 this s4-0)
    )
  )

(defmethod set-roll-to-grav! ((this trsqv) (arg0 float))
  "Set our roll so that our local down aligns with standard gravity"
  (set-roll-to-grav-2! this arg0)
  )

(defmethod set-roll-to-grav-2! ((this trsqv) (arg0 float))
  "Set our roll so that our local down aligns with standard gravity"
  (let* ((s5-0 (get-quaternion this))
         (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! (-> s3-0 vector 0) (-> 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 ((this trsqv))
  "Get our roll, relative to 'down' from gravity"
  (let* ((s5-0 (get-quaternion this))
         (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)
        )
    )
  )

;; ERROR: Unsupported inline assembly instruction kind - [mula.s f0, f3]
;; ERROR: Unsupported inline assembly instruction kind - [madda.s f1, f4]
;; ERROR: Unsupported inline assembly instruction kind - [madd.s f0, f2, f5]
(defmethod rotate-toward-orientation! ((this 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))
  (let ((sv-240 arg4)
        (s2-0 arg5)
        (s5-0 (get-quaternion this))
        )
    (let ((gp-0 (new 'stack-no-clear 'quaternion)))
      (when (< 0.0 arg2)
        (quaternion-from-two-vectors-smooth!
          gp-0
          (vector-y-quaternion! (new 'stack-no-clear 'vector) s5-0)
          (vector-y-quaternion! (new 'stack-no-clear 'vector) arg0)
          arg2
          sv-240
          )
        (quaternion-normalize! (quaternion*! s5-0 gp-0 s5-0))
        )
      (when (< 0.0 arg1)
        (let ((sv-256 (vector-y-quaternion! (new 'stack-no-clear 'vector) s5-0))
              (s1-2 (vector-z-quaternion! (new 'stack-no-clear 'vector) s5-0))
              (sv-272 (vector-z-quaternion! (new 'stack-no-clear 'vector) arg0))
              (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)))
               (set! f0-4 (vector-dot sv-256 v1-5))
               )
             (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! ((this trsqv) (arg0 vector))
  "Set our rotation to point along the given heading, with no roll or pitch."
  (forward-up->quaternion
    (get-quaternion this)
    (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! ((this trsqv) (arg0 vector))
  "Set our orientation to point toward arg0, clearing roll and pitch"
  (forward-up->quaternion
    (get-quaternion this)
    (vector-normalize! (vector-! (new 'stack-no-clear 'vector) arg0 (-> this 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.w 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)
        (.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.w vf2 vf0)
    (.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.xyz vf2 vf2 vf0 acc)
    (.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.w vf2 vf0)
    (.mul.x.vf vf3 vf3 vf1)
    (.mul.y.vf vf4 vf4 vf1)
    (.mul.z.vf vf5 vf5 vf1)
    (.add.vf.xyz vf2 vf2 vf6)
    (.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.w vf2 vf0)
      (.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.w vf2 vf0)
    (.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.xyz vf2 vf2 vf0 acc)
    (.svf (&-> arg0 trans quad) vf2)
    (.svf (&-> arg0 quad 0) vf3)
    (.svf (&-> arg0 quad 1) vf4)
    (.svf (&-> arg0 quad 2) vf5)
    arg0
    )
  )