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

;;-*-Lisp-*-
(in-package goal)
(bundles "ENGINE.CGO" "GAME.CGO")
(require "engine/gfx/hw/display-h.gc")
(require "engine/physics/dynamics-h.gc")
(require "engine/geometry/geometry.gc")
(require "engine/math/transformq-h.gc")

;; DECOMP BEGINS

(defmethod print ((this transformq))
  "Print a 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))
  "Return the transform's quaternion storage."
  (-> this quat))

(defmethod set-quaternion! ((this trsqv) (rotation quaternion))
  "Copy rotation into this transform."
  (quaternion-copy! (get-quaternion this) rotation))

(defmethod rot->dir-targ! ((this trsqv))
  "Copy the current rotation into dir-targ."
  (quaternion-copy! (-> this dir-targ) (get-quaternion this)))

(defmethod y-angle ((this trsqv))
  "Return the current quaternion's yaw."
  (quaternion-y-angle (get-quaternion this)))

(defmethod seek-toward-heading-vec! ((this trsqv) (heading vector) (max-yaw-rate float) (response-time time-frame))
  "Turn about world Y toward heading's XZ yaw. Clamp each frame's step by max-yaw-rate and
   response-time. A fresh reversal requires one confirming frame unless no full turn has been accepted
   for 0.2 seconds."
  (let* ((yaw-error (deg-diff (quaternion-y-angle (-> this quat)) (vector-y-angle heading)))
         ;; Limit the step by both the angular rate and a first-order response proportional to error.
         (yaw-limit (fmin (* max-yaw-rate (seconds-per-frame)) (/ (* 5.0 (fabs yaw-error)) (the float response-time))))
         (saturated-yaw (fmax (fmin yaw-error yaw-limit) (- yaw-limit))))
    (let ((old-diff (-> this old-y-angle-diff)))
      (set! saturated-yaw
            (cond
              ;; Continue a turn with the same sign. A reversal is accepted immediately only when
              ;; no full step has been accepted for 0.2 seconds.
              ((or (= old-diff 0.0)
                   (and (< 0.0 saturated-yaw) (< 0.0 old-diff))
                   (or (and (< saturated-yaw 0.0) (< old-diff 0.0)) (time-elapsed? (-> this angle-change-time) (seconds 0.2))))
               (set-time! (-> this angle-change-time))
               saturated-yaw)
              (else
               ;; Retain the requested sign without visibly turning. If that sign persists it is
               ;; accepted next frame; alternating signs remain suppressed until the timer expires.
               ;; Zero would take the old-diff=0 path and defeat this one-frame confirmation.
               (* 0.000000001 saturated-yaw)))))
    (set! (-> this old-y-angle-diff) saturated-yaw)
    (let ((rotation (get-quaternion this)))
      (quaternion-rotate-y! rotation rotation saturated-yaw))))

(defmethod set-heading-vec! ((this trsqv) (heading vector))
  "Immediately turn toward heading within the plane perpendicular to the current up direction,
   preserving that up direction."
  (let ((rotation (get-quaternion this)))
    (forward-up-nopitch->quaternion rotation
                                    (vector-normalize-copy! (new 'stack-no-clear 'vector) heading 1.0) ;; forward is the given dir.
                                    (vector-y-quaternion! (new 'stack-no-clear 'vector) rotation) ;; use the old up
                                    )))

(defmethod seek-to-point-toward-point! ((this trsqv) (target-point vector) (max-yaw-rate float) (response-time time-frame))
  "Turn about world Y toward target-point from this position using max-yaw-rate and response-time."
  (seek-toward-heading-vec! this
                            (vector-! (new 'stack-no-clear 'vector) target-point (-> this trans))
                            max-yaw-rate
                            response-time))

(defmethod point-toward-point! ((this trsqv) (target-point vector))
  "Immediately turn toward target-point within the plane perpendicular to the current up
   direction."
  (let ((rotation (get-quaternion this)))
    (forward-up-nopitch->quaternion rotation
                                    (vector-normalize! (vector-! (new 'stack-no-clear 'vector) target-point (-> this trans)) 1.0)
                                    (vector-y-quaternion! (new 'stack-no-clear 'vector) rotation))))

(defmethod seek-toward-yaw-angle! ((this trsqv) (yaw float) (max-yaw-rate float) (response-time time-frame))
  "Turn toward yaw using max-yaw-rate and response-time."
  (seek-toward-heading-vec! this
                            ;; make a vector that points toward +z (forward) rotated by the yaw.
                            (set-vector! (new 'stack-no-clear 'vector) (sin yaw) 0.0 (cos yaw) 1.0)
                            max-yaw-rate
                            response-time))

(defmethod set-yaw-angle-clear-roll-pitch! ((this trsqv) (yaw float))
  "Set yaw immediately with a horizontal forward direction and world +Y up."
  (set-heading-vec-clear-roll-pitch! this (set-vector! (new 'stack-no-clear 'vector) (sin yaw) 0.0 (cos yaw) 1.0)))

(defmethod set-roll-to-grav! ((this trsqv) (roll-offset float))
  "Align local up with standard world up while retaining forward, then apply roll-offset."
  (set-roll-to-grav-2! this roll-offset))

(defmethod set-roll-to-grav-2! ((this trsqv) (roll-offset float))
  "Align local up with standard world up projected perpendicular to forward, rebuild the right
   axis, then apply roll-offset about local Z."
  (let* ((rotation (get-quaternion this)) ;; our orientation
         (world-up (-> *standard-dynamics* gravity-normal))
         (rotation-matrix (quaternion->matrix (new 'stack-no-clear 'matrix) rotation)))
    (let ((forward (-> rotation-matrix vector 2)))
      ;; Remove the forward component from world up, then use the result as local up.
      (vector-normalize! (vector-flatten! (-> rotation-matrix vector 1) world-up forward) 1.0)
      ;; Rebuild local right so the three axes remain orthonormal.
      (vector-cross! (the-as vector (-> rotation-matrix vector)) (-> rotation-matrix vector 1) forward))
    (let ((roll-matrix (matrix-rotate-z! (new 'stack-no-clear 'matrix) roll-offset)))
      (matrix*! rotation-matrix roll-matrix rotation-matrix))
    (matrix->quaternion rotation rotation-matrix)))

(defmethod roll-relative-to-gravity ((this trsqv))
  "Return signed roll relative to standard world up after projecting world up perpendicular to
   the current forward axis."
  (let* ((rotation (get-quaternion this))
         (forward (vector-z-quaternion! (new 'stack-no-clear 'vector) rotation))
         (up (vector-y-quaternion! (new 'stack-no-clear 'vector) rotation))
         (world-up (-> *standard-dynamics* gravity-normal))
         (projected-up (vector-normalize! (vector-flatten! (new 'stack-no-clear 'vector) world-up forward) 1.0))
         (roll-cosine (vector-dot projected-up up)))
    ;; acos supplies the magnitude; the cross product's direction supplies the sign around forward.
    (if (< (vector-dot (vector-cross! (new 'stack-no-clear 'vector) projected-up up) forward) 0.0)
      (- (acos roll-cosine))
      (acos roll-cosine))))

(defmethod rotate-toward-orientation! ((this trsqv) (target quaternion) (y-rate float) (z-rate float))
  "Rotate the current local Y axis toward target's Y axis at z-rate, then the resulting local Z
   axis toward target's Z axis at y-rate. Each rate is converted to a per-frame maximum."
  ;; The shortest-arc helper is singular for exactly opposite axes, so this is not robust across a
  ;; 180-degree axis flip.
  (let ((rotation (get-quaternion this)))
    (let ((increment (new 'stack-no-clear 'quaternion)))
      (when (< 0.0 z-rate)
        (quaternion-from-two-vectors-max-angle! increment
                                                (vector-y-quaternion! (new 'stack-no-clear 'vector) rotation)
                                                (vector-y-quaternion! (new 'stack-no-clear 'vector) target)
                                                (* z-rate (seconds-per-frame)))
        (quaternion-normalize! (quaternion*! rotation increment rotation)))
      (when (< 0.0 y-rate)
        (quaternion-from-two-vectors-max-angle! increment
                                                (vector-z-quaternion! (new 'stack-no-clear 'vector) rotation)
                                                (vector-z-quaternion! (new 'stack-no-clear 'vector) target)
                                                (* y-rate (seconds-per-frame)))
        (quaternion-normalize! (quaternion*! rotation increment rotation))))
    rotation))

(defmethod set-heading-vec-clear-roll-pitch! ((this trsqv) (heading vector))
  "Build an orientation from normalized heading and world +Y. This removes roll; a horizontal
   heading also removes pitch."
  (forward-up->quaternion (get-quaternion this)
                          (vector-normalize-copy! (new 'stack-no-clear 'vector) heading 1.0)
                          (new 'static 'vector :y 1.0 :w 1.0)))

(defmethod point-toward-point-clear-roll-pitch! ((this trsqv) (target-point vector))
  "Build an orientation toward target-point using world +Y. This removes roll; a target at the
   same height also removes pitch."
  (forward-up->quaternion (get-quaternion this)
                          (vector-normalize! (vector-! (new 'stack-no-clear 'vector) target-point (-> this trans)) 1.0)
                          (new 'static 'vector :y 1.0 :w 1.0)))

(defun transformq-copy! ((dst transformq) (src transformq))
  "Copy translation, quaternion rotation, and scale from src to dst."
  (vector-copy! (-> dst trans) (-> src trans))
  (vector-copy! (-> dst rot) (-> src rot))
  (vector-copy! (-> dst scale) (-> src scale))
  dst)

(defun matrix<-transformq! ((dst matrix) (src transformq))
  "Convert src to an affine matrix, scaling its three rotation axes and forcing translation.w to
   one."
  (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 dst (-> src quat))
    (cond
      (#f (set! (-> dst vector 3 quad) (-> src trans quad)))
      (else
       (.lvf vf1 (&-> src scale quad))
       (.lvf vf2 (&-> src trans quad))
       (.lvf vf3 (&-> dst vector 0 quad))
       (.lvf vf4 (&-> dst vector 1 quad))
       (.lvf vf5 (&-> dst vector 2 quad))
       (.mov.vf.w vf2 vf0)      ;; set trans.w = 1
       (.mul.x.vf vf3 vf3 vf1)  ;; apply scale
       (.mul.y.vf vf4 vf4 vf1)
       (.mul.z.vf vf5 vf5 vf1)
       (.svf (&-> dst vector 3 quad) vf2)
       (.svf (&-> dst vector 0 quad) vf3)
       (.svf (&-> dst vector 1 quad) vf4)
       (.svf (&-> dst vector 2 quad) vf5)
       (.mov v1-1 vf5)))
    dst))

(defun matrix<-no-trans-transformq! ((dst matrix) (src transformq))
  "Convert src rotation and scale to an affine matrix with zero translation and homogeneous w equal
   to one."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf))
    (init-vf0-vector)
    (quaternion->matrix dst (-> src quat))
    (.lvf vf1 (&-> src scale quad))
    (.lvf vf3 (&-> dst vector 0 quad))
    (.lvf vf4 (&-> dst vector 1 quad))
    (.lvf vf5 (&-> dst vector 2 quad))
    (.mov.vf vf2 vf0)       ;; set trans = [0, 0, 0, 1]
    (.mul.x.vf vf3 vf3 vf1) ;; apply scale
    (.mul.y.vf vf4 vf4 vf1)
    (.mul.z.vf vf5 vf5 vf1)
    (.svf (&-> dst vector 3 quad) vf2)
    (.svf (&-> dst vector 0 quad) vf3)
    (.svf (&-> dst vector 1 quad) vf4)
    (.svf (&-> dst vector 2 quad) vf5)
    dst))

(defun matrix<-transformq+trans! ((dst matrix) (src transformq) (local-offset vector))
  "Convert src to an affine matrix and add local-offset.xyz after scaling and rotating it into world
   space. local-offset.w is ignored."
  (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 dst (-> src quat))
    (.lvf vf1 (&-> src scale quad))
    (.lvf vf2 (&-> src trans quad))
    (.lvf vf6 (&-> local-offset quad))
    (.lvf vf3 (&-> dst vector 0 quad))
    (.lvf vf4 (&-> dst vector 1 quad))
    (.lvf vf5 (&-> dst vector 2 quad))
    (.mov.vf.w vf2 vf0)      ;; set original-trans.w = 1
    (.mul.x.vf vf3 vf3 vf1)  ;; scale matrix
    (.mul.y.vf vf4 vf4 vf1)
    (.mul.z.vf vf5 vf5 vf1)
    (.mul.x.vf acc vf3 vf6)          ;; rotate local-offset by scaled matrix
    (.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) ;; adds the offset and original translation
    (.svf (&-> dst vector 3 quad) vf2)
    (.svf (&-> dst vector 0 quad) vf3)
    (.svf (&-> dst vector 1 quad) vf4)
    (.svf (&-> dst vector 2 quad) vf5)
    dst))

(defun matrix<-transformq+world-trans! ((dst matrix) (src transformq) (world-offset vector))
  "Convert src to an affine matrix and add world-offset.xyz directly to its world-space translation.
   world-offset.w is ignored."
  (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 dst (-> src quat))
    (.lvf vf1 (&-> src scale quad))
    (.lvf vf2 (&-> src trans quad))
    (.lvf vf6 (&-> world-offset quad))
    (.lvf vf3 (&-> dst vector 0 quad))
    (.lvf vf4 (&-> dst vector 1 quad))
    (.lvf vf5 (&-> dst vector 2 quad))
    (.mov.vf.w vf2 vf0)        ;; set trans.w = 1
    (.mul.x.vf vf3 vf3 vf1)    ;; scale matrix
    (.mul.y.vf vf4 vf4 vf1)
    (.mul.z.vf vf5 vf5 vf1)
    (.add.vf.xyz vf2 vf2 vf6)  ;; add world trans offset
    (.svf (&-> dst vector 3 quad) vf2)
    (.svf (&-> dst vector 0 quad) vf3)
    (.svf (&-> dst vector 1 quad) vf4)
    (.svf (&-> dst vector 2 quad) vf5)
    dst))

(defun matrix<-parented-transformq! ((dst matrix) (src transformq) (parent-scale vector))
  "Prepare src for composition with a nonuniformly scaled parent. Divide every scaled rotation-axis
   component by the corresponding nonzero parent-scale component so the later parent multiplication
   does not apply that scale again; copy translation unchanged."
  (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 dst (-> src quat))
    (let ((inverse-parent-scale (new 'stack-no-clear 'vector)))
      (set! (-> inverse-parent-scale x) (/ 1.0 (-> parent-scale x)))
      (set! (-> inverse-parent-scale y) (/ 1.0 (-> parent-scale y)))
      (set! (-> inverse-parent-scale z) (/ 1.0 (-> parent-scale z)))
      (.lvf vf1 (&-> src scale quad))
      (.lvf vf2 (&-> src trans quad))
      (.mov.vf.w vf2 vf0) ;; trans.w = 0
      (.lvf vf4 (&-> dst vector 0 quad))
      (.lvf vf5 (&-> dst vector 1 quad))
      (.lvf vf6 (&-> dst vector 2 quad))
      (.mul.x.vf vf4 vf4 vf1)  ;; apply our scale
      (.mul.y.vf vf5 vf5 vf1)
      (.mul.z.vf vf6 vf6 vf1)
      (.lvf vf3 (&-> inverse-parent-scale quad)))
    (.mul.vf vf4 vf4 vf3) ;; apply inverse parent scale
    (.mul.vf vf5 vf5 vf3)
    (.mul.vf vf6 vf6 vf3)
    (.svf (&-> dst vector 3 quad) vf2)
    (.svf (&-> dst vector 0 quad) vf4)
    (.svf (&-> dst vector 1 quad) vf5)
    (.svf (&-> dst vector 2 quad) vf6)
    (.mov v1-1 vf6)
    dst))