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

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

;; DECOMP BEGINS

(defun vector-cross! ((out vector) (a vector) (b vector))
  "Write the xyz cross product of a and b to out; w is unspecified."
  (rlet ((acc :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.outer.product.a.vf acc vf1 vf2)
    (.outer.product.b.vf vf3 vf2 vf1 acc)
    (.svf (&-> out quad) vf3)
    out))

(defun vector+float! ((out vector) (value vector) (addend float))
  "Add one scalar to value.xyz, write out.w as 1, and return out."
  (rlet ((vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf))
    (init-vf0-vector)
    (.mov vf6 addend)
    (.lvf vf4 (&-> value quad))
    (.add.x.vf.w vf5 vf0 vf0)
    (.add.x.vf.xyz vf5 vf4 vf6)
    (.svf (&-> out quad) vf5)
    out))

(defun vector*! ((out vector) (a vector) (b vector))
  "Multiply a.xyz and b.xyz componentwise, write out.w as 1, and return out."
  (rlet ((vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf))
    (init-vf0-vector)
    (.lvf vf4 (&-> a quad))
    (.lvf vf5 (&-> b quad))
    (.add.x.vf.w vf6 vf0 vf0)
    (.mul.vf.xyz vf6 vf4 vf5)
    (.svf (&-> out quad) vf6)
    out))

(defun vector+*! ((out vector) (base vector) (value vector) (scale float))
  "Set out.xyz to base.xyz + value.xyz * scale, set out.w to 1, and return out."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf))
    (init-vf0-vector)
    (.mov vf7 scale)
    (.lvf vf5 (&-> value quad))
    (.lvf vf4 (&-> base quad))
    (.add.x.vf.w vf6 vf0 vf0)
    (.mul.x.vf.xyz acc vf5 vf7)
    ;; acts as just an add.
    (.add.mul.w.vf.xyz vf6 vf4 vf0 acc)
    (.svf (&-> out quad) vf6)
    out))

(defun vector-*! ((out vector) (base vector) (value vector) (scale float))
  "Set out.xyz to base.xyz - value.xyz * scale, set out.w to 1, and return out."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf))
    (init-vf0-vector)
    (.mov vf7 scale)
    (.lvf vf5 (&-> value quad))
    (.lvf vf4 (&-> base quad))
    (.add.x.vf.w vf6 vf0 vf0)
    (.mul.w.vf.xyz acc vf4 vf0)
    (.sub.mul.x.vf.xyz vf6 vf5 vf7 acc)
    (.svf (&-> out quad) vf6)
    out))

(defun vector/! ((out vector) (numerator vector) (denominator vector))
  "Divide numerator.xyz by denominator.xyz, overlap the EE and VU divides, set out.w to 1, and return out."
  ;; Division is slow, so launch y on the VU0 divide pipe before numerator is loaded and compute x
  ;; on the EE FPU in parallel. The z divide starts as soon as y is consumed.
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf))
    (init-vf0-vector)
    (.lvf vf5 (&-> denominator quad))
    ;; get started on the first divide ASAP.
    ;; do this before loading the second value.
    ;; q = 1 / denominator.y
    (.div.vf Q vf0 vf5 :fsf #b11 :ftf #b1)
    (.add.x.vf.w vf6 vf0 vf0)
    (.lvf vf4 (&-> numerator quad))
    ;; use FPU to divide x while VU0 is dividing y.
    (let ((v1-0 (/ (-> numerator x) (-> denominator x))))
      (.wait.vf)
      (.mul.vf.y vf6 vf4 Q)
      (.nop.vf)
      (.nop.vf)
      (.div.vf Q vf0 vf5 :fsf #b11 :ftf #b10)
      (.mov vf7 v1-0))
    (.add.x.vf.x vf6 vf0 vf7)
    (.wait.vf)
    (.mul.vf.z vf6 vf4 Q)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> out quad) vf6)
    out))

(defun vector-float*! ((out vector) (value vector) (scale float))
  "Multiply value.xyz by scale, set out.w to 1, and return out."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> value quad))
    (.mov vf2 scale)
    (.add.x.vf.w vf1 vf0 vf0)
    (.mul.x.vf.xyz vf1 vf1 vf2)
    (.svf (&-> out quad) vf1)
    out))

(defun vector-average! ((out vector) (a vector) (b vector))
  "Average a.xyz and b.xyz, set out.w to 1, and return out."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (let ((v1-0 #x3f000000)) (.lvf vf1 (&-> a quad)) (.lvf vf2 (&-> b quad)) (.mov vf3 v1-0))
    (.add.x.vf.w vf4 vf0 vf0)
    (.mul.x.vf acc vf1 vf3)
    (.add.mul.x.vf.xyz vf4 vf2 vf3 acc)
    (.svf (&-> out quad) vf4)
    out))

(defun vector+float*! ((out vector) (base vector) (value vector) (scale float))
  "Set out.xyz to base.xyz + value.xyz * scale, set out.w to 1, and return out."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf2 (&-> value quad))
    (.lvf vf1 (&-> base quad))
    (.mov vf3 scale)
    (.add.x.vf.w vf4 vf0 vf0)
    (.mul.x.vf acc vf2 vf3)
    (.add.mul.w.vf.xyz vf4 vf1 vf0 acc)
    (.svf (&-> out quad) vf4)
    out))

(defun vector--float*! ((out vector) (base vector) (value vector) (scale float))
  "Set out.xyz to base.xyz - value.xyz * scale, set out.w to 1, and return out."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf2 (&-> value quad))
    (.lvf vf1 (&-> base quad))
    (.mov vf3 scale)
    (.add.x.vf.w vf4 vf0 vf0)
    (.mul.w.vf acc vf1 vf0)
    (.sub.mul.x.vf.xyz vf4 vf2 vf3 acc)
    (.svf (&-> out quad) vf4)
    out))

(defun vector-float/! ((out vector) (value vector) (divisor float))
  "Divide value.xyz by divisor, set out.w to 1, and return out."
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.mov vf3 divisor)
    (.div.vf Q vf0 vf3 :fsf #b11 :ftf #b0)
    (.lvf vf1 (&-> value quad))
    (.add.x.vf.w vf4 vf0 vf0)
    (.wait.vf)
    (.mul.vf.xyz vf4 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> out quad) vf4)
    out))

(defun vector-negate! ((out vector) (value vector))
  "Negate value.xyz into out, set out.w to 1, and return out."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> value quad))
    (.sub.vf.xyz vf4 vf0 vf1)
    (.add.x.vf.w vf4 vf0 vf0)
    (.svf (&-> out quad) vf4)
    out))

(defun vector-negate-in-place! ((value vector))
  "Negate value.xyz in place without changing w."
  (rlet ((vf0 :class vf)
         (vf1 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> value quad))
    (.sub.vf.xyz vf1 vf0 vf1)
    (.svf (&-> value quad) vf1)
    value))

(#when PC_PORT
  (defun vector= ((a vector) (b vector))
    "Return true when the bit patterns of a.xyz and b.xyz match; w is ignored."
    (let* ((low-mask #xffff)
           (a-quad (-> a quad))
           (xyz-mask (shl low-mask 48))
           (b-quad (-> b quad))
           (equal-words (the uint128 0))
           (zero-quad (the uint128 0)))
      (.pceqw equal-words a-quad b-quad)
      (.ppach equal-words zero-quad equal-words)
      (set! xyz-mask (logior (the int equal-words) xyz-mask))
      ;; Adding one overflows only when all three packed comparisons produced all-one words.
      (set! xyz-mask (+ xyz-mask 1))
      (zero? xyz-mask))))

(#unless PC_PORT
  (defun vector= ((a vector) (b vector))
    "Return true when the bit patterns of a.xyz and b.xyz match; w is ignored."
    ;; Collapse the three all-one comparison words into one 64-bit overflow test. The high 16 mask
    ;; discards the packed w comparison without branching on individual components.
    (rlet ((result)
           (xyz-mask)
           (a-quad :class i128)
           (b-quad :class i128)
           (equal-words :class i128))
      (m! result #t)
      (m! xyz-mask #xffff)
      (l.q a-quad a)
      (sll xyz-mask xyz-mask 48)
      (l.q b-quad b)
      (pceqw equal-words a-quad b-quad)
      (ppach equal-words r0 equal-words)
      (logior! xyz-mask equal-words)
      (+! xyz-mask 1)
      (b.z xyz-mask equal :delay (nop!))
      (m! result #f)
      (label equal)
      result)))

(defun vector-delta ((a vector) (b vector))
  "Return the Manhattan distance between a.xyz and b.xyz."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.sub.vf vf1 vf2 vf1)
    (.abs.vf vf1 vf1)
    ;; put abs.x in acc.w
    (.mul.x.vf.w acc vf0 vf1)
    ;; add abs.y
    (.add.mul.y.vf.w acc vf0 vf1 acc)
    ;; add abs.z
    (.add.mul.z.vf.w vf3 vf0 vf1 acc)
    ;; set acc.x = acc.w
    (.add.w.vf.x vf3 vf0 vf3)
    (.mov v0-0 vf3)
    v0-0))

(defun vector-seek! ((value vector) (target vector) (max-step float))
  "Move each component of value.xyz toward target.xyz by at most max-step and set w to 1."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf))
    (init-vf0-vector)
    (.mov vf4 max-step)
    (.lvf vf1 (&-> target quad))
    (.lvf vf2 (&-> value quad))
    (.add.x.vf.w vf1 vf0 vf0)
    (.sub.x.vf.x vf5 vf0 vf4)
    (.sub.vf.xyz vf3 vf1 vf2)
    (.min.x.vf.xyz vf3 vf3 vf4)
    (.max.x.vf.xyz vf3 vf3 vf5)
    (.add.vf.xyz vf1 vf2 vf3)
    (.svf (&-> value quad) vf1)
    value))

(defun vector-seek-2d-xz-smooth! ((vec vector) (target vector) (max-step float) (alpha float))
  "Move vec toward target in xz by alpha times the error, limiting the step length to max-step."
  ;; how much we have to go to get to the target
  (let ((x-diff (- (-> target x) (-> vec x)))
        (z-diff (- (-> target z) (-> vec z))))
    ;; do we have to move?
    (if (or (!= x-diff 0.0) (!= z-diff 0.0))
      ;; if so, scale by alpha,
      (let* ((x-step (* x-diff alpha))
             (z-step (* z-diff alpha))
             ;; and get the length of this step
             (step-len (sqrtf (+ (square x-step) (square z-step)))))
        (cond
          ((>= max-step step-len)
           ;; step is within max-step, just do it.
           (+! (-> vec x) x-step)
           (+! (-> vec z) z-step))
          (else
           ;; not in range.
           (let ((step-scale (/ max-step step-len))) (+! (-> vec x) (* step-scale x-step)) (+! (-> vec z) (* step-scale z-step))))))))
  vec)

(defun vector-seek-2d-yz-smooth! ((vec vector) (target vector) (max-step float) (alpha float))
  "Move vec toward target in yz by alpha times the error, limiting the step length to max-step."
  (let ((y-diff (- (-> target y) (-> vec y)))
        (z-diff (- (-> target z) (-> vec z))))
    (if (or (!= y-diff 0.0) (!= z-diff 0.0))
      (let* ((y-step (* y-diff alpha))
             (z-step (* z-diff alpha))
             (step-len (sqrtf (+ (square y-step) (square z-step)))))
        (cond
          ((>= max-step step-len) (+! (-> vec y) y-step) (+! (-> vec z) z-step))
          (else
           (let ((step-scale (/ max-step step-len))) (+! (-> vec y) (* step-scale y-step)) (+! (-> vec z) (* step-scale z-step))))))))
  vec)

(defun vector-seek-3d-smooth! ((vec vector) (target vector) (max-step float) (alpha float))
  "Move vec toward target in xyz by alpha times the error, limiting the step length to max-step."
  (let ((x-diff (- (-> target x) (-> vec x)))
        (y-diff (- (-> target y) (-> vec y)))
        (z-diff (- (-> target z) (-> vec z))))
    (if (or (!= x-diff 0.0) (!= y-diff 0.0) (!= z-diff 0.0))
      (let* ((x-step (* x-diff alpha))
             (y-step (* y-diff alpha))
             (z-step (* z-diff alpha))
             (step-len (sqrtf (+ (+ (square x-step) (square y-step)) (square z-step)))))
        (cond
          ((>= max-step step-len) (+! (-> vec x) x-step) (+! (-> vec y) y-step) (+! (-> vec z) z-step))
          (else
           (let ((step-scale (/ max-step step-len)))
             (+! (-> vec x) (* step-scale x-step))
             (+! (-> vec y) (* step-scale y-step))
             (+! (-> vec z) (* step-scale z-step))))))))
  vec)

(defun seek-with-smooth ((value float) (target float) (max-step float) (alpha float) (deadband float))
  "Move value toward target by alpha times the error, snap inside deadband, and clamp the step to max-step."
  (let ((diff (- target value)))
    (if (>= deadband (fabs diff))
      target
      (let ((step (* diff alpha)))
        (let ((min-step (- max-step)))
          (cond
            ((< step min-step) (set! step min-step))
            ((< max-step step) (set! step max-step))))
        (+ step value)))))

(defun vector-identity! ((value vector))
  "Set all four lanes of value to 1."
  (set! (-> value x) 1.0)
  (set! (-> value y) 1.0)
  (set! (-> value z) 1.0)
  (set! (-> value w) 1.0)
  value)

(defun vector-seconds ((out vector) (seconds vector))
  "Convert seconds.xyz to the engine's time units and write out.xyz."
  (set! (-> out x) (fsec (-> seconds x)))
  (set! (-> out y) (fsec (-> seconds y)))
  (set! (-> out z) (fsec (-> seconds z)))
  out)

(defun vector-seconds! ((seconds vector))
  "Convert seconds.xyz to the engine's time units in place."
  (set! (-> seconds x) (fsec (-> seconds x)))
  (set! (-> seconds y) (fsec (-> seconds y)))
  (set! (-> seconds z) (fsec (-> seconds z)))
  seconds)

(defun vector-v! ((velocity vector))
  "Convert a per-second velocity to displacement per frame in place."
  (vector-float*! velocity velocity (-> *display* seconds-per-frame))
  velocity)

(defun vector-v+! ((result vector) (position vector) (velocity vector))
  "Advance position by one frame of velocity and write result."
  (vector+float*! result position velocity (-> *display* seconds-per-frame))
  result)

(defun vector-v*float+! ((result vector) (position vector) (velocity vector) (velocity-scale float))
  "Advance position by one frame of scaled velocity and write result."
  (vector+float*! result position velocity (* velocity-scale (-> *display* seconds-per-frame)))
  result)

(defun vector-v++! ((position vector) (velocity vector))
  "Advance position in place by two frame displacements of velocity."
  (vector+float*! position position velocity (-> *display* seconds-per-frame))
  position)

(defun vector-v*float! ((delta-p vector) (velocity vector) (scale float))
  "Convert velocity to a scaled displacement per frame and write delta-p."
  (vector-float*! delta-p velocity (* scale (-> *display* seconds-per-frame))))

(defun vector-v*float++! ((position vector) (velocity vector) (scale float))
  "Advance position in place by one frame of scaled velocity."
  (vector+float*! position position velocity (* scale (-> *display* seconds-per-frame)))
  position)

(defun vector-to-ups! ((out vector) (per-frame vector))
  "Convert per-frame.xyz to units per second, set out.w to 1, and return out."
  (local-vars (at-0 int))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> per-frame quad))
    (let ((f0-0 (-> *display* frames-per-second))) (.mov at-0 f0-0))
    (.mov vf2 at-0)
    (.mov.vf.w vf1 vf0)
    (.mul.x.vf.xyz vf1 vf1 vf2)
    (.svf (&-> out quad) vf1)
    out))

(defun vector-from-ups! ((out vector) (per-second vector))
  "Convert per-second.xyz to units per frame, set out.w to 1, and return out."
  (local-vars (at-0 int))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> per-second quad))
    (let ((f0-0 (-> *display* seconds-per-frame))) (.mov at-0 f0-0))
    (.mov vf2 at-0)
    (.mov.vf.w vf1 vf0)
    (.mul.x.vf.xyz vf1 vf1 vf2)
    (.svf (&-> out quad) vf1)
    out))

(defun vector-length ((value vector))
  "Return the Euclidean length of value.xyz."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> value quad))
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.w acc vf0 vf1)
    (.add.mul.y.vf.w acc vf0 vf1 acc)
    (.add.mul.z.vf.w vf1 vf0 vf1 acc)
    (.sqrt.vf Q vf1 :ftf #b11)
    (.add.w.vf.x vf1 vf0 vf0)
    (.wait.vf)
    (.mul.vf.x vf1 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-length-squared ((value vector))
  "Return the squared Euclidean length of value.xyz."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> value quad))
    (.add.w.vf.x vf2 vf0 vf0)
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.x acc vf2 vf1)
    (.add.mul.y.vf.x acc vf2 vf1 acc)
    (.add.mul.z.vf.x vf1 vf2 vf1 acc)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-xz-length-squared ((value vector))
  "Return the squared Euclidean length of value.xz."
  (+ (* (-> value x) (-> value x)) (* (-> value z) (-> value z))))

(defun vector-xz-length ((value vector))
  "Return the Euclidean length of value.xz."
  (sqrtf (+ (* (-> value x) (-> value x)) (* (-> value z) (-> value z)))))

(defun vector-vector-distance ((a vector) (b vector))
  "Return the Euclidean distance between a.xyz and b.xyz."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (init-vf0-vector)
    (.lvf vf2 (&-> a quad))
    (.lvf vf3 (&-> b quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.w acc vf0 vf1)
    (.add.mul.y.vf.w acc vf0 vf1 acc)
    (.add.mul.z.vf.w vf1 vf0 vf1 acc)
    (.sqrt.vf Q vf1 :ftf #b11)
    (.add.w.vf.x vf1 vf0 vf0)
    (.wait.vf)
    (.mul.vf.x vf1 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-vector-distance-squared ((a vector) (b vector))
  "Return the squared Euclidean distance between a.xyz and b.xyz."
  (local-vars (v0-0 float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (.lvf vf2 (&-> a quad))
    (.lvf vf3 (&-> b quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.add.y.vf.x vf1 vf1 vf1)
    (.add.z.vf.x vf1 vf1 vf1)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-vector-xz-distance ((a vector) (b vector))
  "Return the Euclidean distance between a and b in the xz plane."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (init-vf0-vector)
    (.lvf vf2 (&-> a quad))
    (.lvf vf3 (&-> b quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.w acc vf0 vf1)
    (.add.mul.z.vf.w vf1 vf0 vf1 acc)
    (.sqrt.vf Q vf1 :ftf #b11)
    (.add.w.vf.x vf1 vf0 vf0)
    (.wait.vf)
    (.mul.vf.x vf1 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-vector-xz-distance-squared ((a vector) (b vector))
  "Return the squared distance between a and b in the xz plane."
  (local-vars (v0-0 float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (.lvf vf2 (&-> a quad))
    (.lvf vf3 (&-> b quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.add.z.vf.x vf1 vf1 vf1)
    (.mov v0-0 vf1)
    v0-0))

(#when PC_PORT
  (defun vector-normalize! ((value vector) (target-length float))
    "Scale value.xyz in place to the requested length without changing w."
    (let ((old-length (vector-length value)))
      (let ((scale (/ target-length old-length)))
        (set! (-> value x) (* (-> value x) scale))
        (set! (-> value y) (* (-> value y) scale))
        (set! (-> value z) (* (-> value z) scale))))
    value))

(#unless PC_PORT
  (defun vector-normalize! ((value vector) (target-length float))
    "Scale value.xyz in place to the requested length without changing w."
    ;; VU0 computes target-length / sqrt(dot(value, value)) in Q, then scales xyz while leaving w
    ;; untouched. The three vnops retain the original reciprocal-square-root pipeline schedule.
    (rlet ((source :class vf)
           (squared-length :class vf)
           (target :class vf)
           (Q :reg Q))
      (init-vf0-vector)
      (l.vf source value)
      (mul.vf.xyz squared-length source source)
      (m! target target-length)
      (mula.x.vf.w vf0 squared-length)
      (madda.y.vf.w vf0 squared-length)
      (madd.z.vf.w squared-length vf0 squared-length)
      (rsqrt.x.w Q target squared-length)
      (waitq)
      (mulq.vf.xyz source source Q)
      (vnop)
      (vnop)
      (vnop)
      (s.vf source value)
      value)))

(#when PC_PORT
  (defun vector-normalize-ret-len! ((value vector) (target-length float))
    "Scale value.xyz in place to the requested length and return its original length."
    (let ((old-length (vector-length value)))
      (let ((scale (/ target-length old-length)))
        (set! (-> value x) (* (-> value x) scale))
        (set! (-> value y) (* (-> value y) scale))
        (set! (-> value z) (* (-> value z) scale)))
      old-length)))

(#unless PC_PORT
  (defun vector-normalize-ret-len! ((value vector) (target-length float))
    "Scale value.xyz in place to the requested length and return its original length."
    ;; Start the VU reciprocal square root before the EE square root. Both consume the same
    ;; squared length, so the scalar unit recovers the return value while VU0 prepares the scale.
    (rlet ((source :class vf)
           (squared-length :class vf)
           (target :class vf)
           (old-length)
           (Q :reg Q))
      (init-vf0-vector)
      (l.vf source value)
      (mul.vf.xyz squared-length source source)
      (m! target target-length)
      (mula.x.vf.w vf0 squared-length)
      (madda.y.vf.w vf0 squared-length)
      (madd.z.vf.w squared-length vf0 squared-length)
      (rsqrt.x.w Q target squared-length)
      (add.w.vf.x squared-length vf0 squared-length)
      (m old-length squared-length)
      (sqrt.s old-length old-length)
      (waitq)
      (mulq.vf.xyz source source Q)
      (vnop)
      (vnop)
      (vnop)
      (s.vf source value)
      old-length)))

(defun vector-normalize-copy! ((out vector) (value vector) (target-length float))
  "Scale value.xyz to the requested length into out, copy zero vectors unchanged, and set out.w to 1."
  (let ((old-length (vector-length value)))
    (if (= old-length 0.0)
      (vector-copy! out value)
      (let ((scale (/ target-length old-length)))
        (set! (-> out x) (* (-> value x) scale))
        (set! (-> out y) (* (-> value y) scale))
        (set! (-> out z) (* (-> value z) scale)))))
  (set! (-> out w) 1.0)
  out)

(defun vector-xz-normalize! ((value vector) (target-length float))
  "Scale value.xz in place to the requested length when its current xz length is nonzero."
  (let ((old-length (vector-xz-length value)))
    (if (!= old-length 0.0)
      (let ((scale (/ target-length old-length)))
        (set! (-> value x) (* (-> value x) scale))
        (set! (-> value z) (* (-> value z) scale)))))
  value)

(defun vector-length-max! ((value vector) (maximum float))
  "Limit value.xyz to maximum length without changing its direction or w."
  (let ((current-length (vector-length value)))
    (when (not (or (= current-length 0.0) (< current-length maximum)))
      (set! current-length (/ current-length maximum))
      (when (!= current-length 0.0)
        (set! (-> value x) (/ (-> value x) current-length))
        (set! (-> value y) (/ (-> value y) current-length))
        (set! (-> value z) (/ (-> value z) current-length)))))
  value)

(defun vector-xz-length-max! ((value vector) (maximum float))
  "Limit value.xz to maximum length without changing its direction, y, or w."
  (let ((current-length (vector-xz-length value)))
    (when (not (or (= current-length 0.0) (< current-length maximum)))
      (set! current-length (/ current-length maximum))
      (when (!= current-length 0.0)
        (set! (-> value x) (/ (-> value x) current-length))
        (set! (-> value z) (/ (-> value z) current-length)))))
  value)

(defun vector-rotate-around-y! ((out vector) (value vector) (angle float))
  "Rotate value around the y axis by angle and write out."
  (let ((z (-> value z))
        (x (-> value x))
        (cosine (cos angle))
        (sine (sin angle)))
    (vector-copy! out value)
    (set! (-> out z) (- (* z cosine) (* x sine)))
    (set! (-> out x) (+ (* z sine) (* x cosine))))
  out)

(defun rotate-y<-vector+vector ((from vector) (to vector))
  "Return the signed y rotation from the first vector to the second."
  (atan (- (-> to x) (-> from x)) (- (-> to z) (-> from z))))

(defun vector-cvt.w.s! ((out vector) (value vector))
  "Truncate four floating-point lanes to signed 32-bit integers."
  (rlet ((vf1 :class vf)) (.lvf vf1 (&-> value quad)) (.ftoi.vf vf1 vf1) (.svf (&-> out quad) vf1) out))

(defun vector-cvt.s.w! ((out vector) (value vector))
  "Convert four signed 32-bit integer lanes to floating point."
  (rlet ((vf1 :class vf)) (.lvf vf1 (&-> value quad)) (.itof.vf vf1 vf1) (.svf (&-> out quad) vf1) out))

(defun rot-zxy-from-vector! ((out vector) (forward vector))
  "Compute yaw and pitch that orient a forward vector along forward using ZXY rotation order; roll is zero."
  ;; Solve yaw first from x/z, undo it to recover horizontal length, then solve pitch from -y.
  (let* ((z (-> forward z))
         (x (-> forward x))
         (yaw (atan x z)))
    (set! (-> out y) yaw)
    (let* ((inverse-yaw (- yaw))
           (horizontal-length (- (* z (cos inverse-yaw)) (* x (sin inverse-yaw))))
           (pitch (atan (- (-> forward y)) horizontal-length)))
      (set! (-> out x) pitch)))
  (set! (-> out z) 0.0)
  out)

(defun rot-zyx-from-vector! ((out vector) (forward vector))
  "Compute pitch and yaw that orient a forward vector along forward using ZYX rotation order; roll is zero."
  ;; Solve pitch first from -y/z, undo it to recover horizontal length, then solve yaw from x.
  (let* ((z (-> forward z))
         (negative-y (- (-> forward y)))
         (pitch (atan negative-y z)))
    (set! (-> out x) pitch)
    (let* ((inverse-pitch (- pitch))
           (horizontal-length (- (* z (cos inverse-pitch)) (* negative-y (sin inverse-pitch))))
           (yaw (atan (-> forward x) horizontal-length)))
      (set! (-> out y) yaw)))
  (set! (-> out z) 0.0)
  out)

(defun vector-lerp! ((out vector) (a vector) (b vector) (alpha float))
  "Interpolate a.xyz toward b.xyz by unclamped alpha, set out.w to 1, and return out."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.mov vf4 alpha)
    (.add.x.vf.w vf3 vf0 vf0)
    (.sub.vf vf2 vf2 vf1)
    (.mul.x.vf vf2 vf2 vf4)
    (.add.vf.xyz vf3 vf1 vf2)
    (.svf (&-> out quad) vf3)
    out))

(defun vector-lerp-clamp! ((out vector) (a vector) (b vector) (alpha float))
  "Interpolate a.xyz toward b.xyz by alpha clamped to [0, 1], set out.w to 1, and return out."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (cond
      ((>= 0.0 alpha) (vector-copy! out a))
      ((>= alpha 1.0) (vector-copy! out b))
      (else
       (let ((v1-2 out))
         (let ((f0-2 alpha)) (.lvf vf1 (&-> a quad)) (.lvf vf2 (&-> b quad)) (let ((a1-1 f0-2)) (.mov vf4 a1-1)))
         (.add.x.vf.w vf3 vf0 vf0)
         (.sub.vf vf2 vf2 vf1)
         (.mul.x.vf vf2 vf2 vf4)
         (.add.vf.xyz vf3 vf1 vf2)
         (.svf (&-> v1-2 quad) vf3))))
    out))

(defun vector4-lerp! ((out vector) (a vector) (b vector) (alpha float))
  "Interpolate all four lanes from a to b by unclamped alpha."
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.mov vf4 alpha)
    (.sub.vf vf2 vf2 vf1)
    (.mul.x.vf vf2 vf2 vf4)
    (.add.vf vf3 vf1 vf2)
    (.svf (&-> out quad) vf3)
    out))

(defun vector4-lerp-clamp! ((out vector) (a vector) (b vector) (alpha float))
  "Interpolate all four lanes from a to b by alpha clamped to [0, 1]."
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (cond
      ((>= 0.0 alpha) (vector-copy! out a))
      ((>= alpha 1.0) (vector-copy! out b))
      (else
       (let ((v1-2 out))
         (let ((f0-2 alpha)) (.lvf vf1 (&-> a quad)) (.lvf vf2 (&-> b quad)) (let ((a1-1 f0-2)) (.mov vf4 a1-1)))
         (.sub.vf vf2 vf2 vf1)
         (.mul.x.vf vf2 vf2 vf4)
         (.add.vf vf3 vf1 vf2)
         (.svf (&-> v1-2 quad) vf3))))
    out))

(defun vector-degi ((out vector) (rotations vector))
  "Truncate rotation-unit floats, shift each packed word left 16 bits, and write the uncommon integer angle form."
  (local-vars (v1-0 uint128) (v1-1 uint128))
  (rlet ((vf1 :class vf))
    (.lvf vf1 (&-> rotations quad))
    (.ftoi.vf vf1 vf1)
    (.mov v1-0 vf1)
    (.pw.sll v1-1 v1-0 16)
    (set! (-> out quad) (the-as uint128 v1-1))
    out))

(defun vector-degf ((out vector) (packed-angles vector))
  "Arithmetic-shift packed angle words right 16 bits and convert them to rotation-unit floats."
  (local-vars (v1-1 uint128))
  (rlet ((vf1 :class vf))
    (let ((v1-0 (-> packed-angles quad))) (.pw.sra v1-1 v1-0 16))
    (.mov vf1 v1-1)
    (.itof.vf vf1 vf1)
    (.svf (&-> out quad) vf1)
    out))

(defun vector-degmod ((out vector) (angles vector))
  "Wrap four rotation-unit floats to signed 16-bit angular range."
  (local-vars (v1-0 uint128) (v1-1 uint128) (v1-2 uint128))
  (rlet ((vf1 :class vf))
    (.lvf vf1 (&-> angles quad))
    (.ftoi.vf vf1 vf1)
    (.mov v1-0 vf1)
    (.pw.sll v1-1 v1-0 16)
    (.pw.sra v1-2 v1-1 16)
    (.mov vf1 v1-2)
    (.itof.vf vf1 vf1)
    (.svf (&-> out quad) vf1)
    out))

(defun vector-deg-diff ((out vector) (a vector) (b vector))
  "Write the signed wrapped 16-bit angular difference a - b for all four lanes."
  ;; A full rotation is 2^16 units. Shifting each integer angle left 16 bits, subtracting packed
  ;; words, then arithmetic-shifting right sign-extends the low 16-bit result. The wrap into the
  ;; shortest signed angular interval therefore comes directly from packed integer overflow.
  (local-vars (v0-0 float) (v1-0 uint128) (v1-1 uint128) (v1-2 uint128) (v1-3 uint128) (a1-1 uint128) (a1-2 uint128))
  (rlet ((vf1 :class vf)
         (vf2 :class vf))
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.ftoi.vf vf1 vf1)
    (.ftoi.vf vf2 vf2)
    (.mov a1-1 vf1)
    (.mov v1-0 vf2)
    (.pw.sll a1-2 a1-1 16)
    (.pw.sll v1-1 v1-0 16)
    (.psubw v1-2 a1-2 v1-1)
    (.pw.sra v1-3 v1-2 16)
    (.mov vf1 v1-3)
    (.itof.vf vf1 vf1)
    (.svf (&-> out quad) vf1)
    (.mov v0-0 vf1)
    (none)))

(defun vector-deg-lerp-clamp! ((out vector) (minimum vector) (maximum vector) (amount float))
  "Apply clamped shortest-angle interpolation to three lanes and set out.w to 1."
  (cond
    ((>= 0.0 amount) (vector-copy! out minimum))
    ((>= amount 1.0) (vector-copy! out maximum))
    (else
     (set! (-> out x) (deg-lerp-clamp (-> minimum x) (-> maximum x) amount))
     (set! (-> out y) (deg-lerp-clamp (-> minimum y) (-> maximum y) amount))
     (set! (-> out z) (deg-lerp-clamp (-> minimum z) (-> maximum z) amount))
     (set! (-> out w) 1.0)))
  out)

;; The weird docstrings for the next 4 functions were left behind in the game.
;; We suspect they accidentally put something before the docstring, turning it
;; into a string constant. GOAL doesn't eliminate dead code and
;; loads into registers greedily, so it decompiles into a variable assignment.

(defun vector3s-copy! ((out vector) (value vector))
  "Copy value.xyz to out without changing out.w."
  (let ((v1-0 "Copy a vector3s")))
  (set! (-> out x) (-> value x))
  (set! (-> out y) (-> value y))
  (set! (-> out z) (-> value z))
  out)

(defun vector3s+! ((out vector) (a vector) (b vector))
  "Add a.xyz and b.xyz into out without changing out.w."
  (let ((v1-0 "Add 2 vectors3.")))
  (set! (-> out x) (+ (-> a x) (-> b x)))
  (set! (-> out y) (+ (-> a y) (-> b y)))
  (set! (-> out z) (+ (-> a z) (-> b z)))
  out)

(defun vector3s*float! ((out vector) (value vector) (scale float))
  "Multiply value.xyz by scale into out without changing out.w."
  (let ((v1-0 "mult vectors3 by float")))
  (set! (-> out x) (* (-> value x) scale))
  (set! (-> out y) (* (-> value y) scale))
  (set! (-> out z) (* (-> value z) scale))
  out)

(defun vector3s-! ((out vector) (a vector) (b vector))
  "Subtract b.xyz from a.xyz into out without changing out.w."
  (let ((v1-0 "Subtract 2 vectors3: c = (a - b).")))
  (set! (-> out x) (- (-> a x) (-> b x)))
  (set! (-> out y) (- (-> a y) (-> b y)))
  (set! (-> out z) (- (-> a z) (-> b z)))
  out)

(defun spheres-overlap? ((a sphere) (b sphere))
  "Return true when the center distance is no greater than the sum of the radii."
  (local-vars (distance-squared float) (radius-squared float))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.sub.vf.xyz vf3 vf1 vf2)
    (.mul.vf.xyz vf3 vf3 vf3)
    (.add.w.vf.w vf4 vf1 vf2)
    (.mul.w.vf.w vf4 vf4 vf4)
    (.add.y.vf.x vf3 vf3 vf3)
    (.add.z.vf.x vf3 vf3 vf3)
    (.add.w.vf.x vf4 vf0 vf4)
    (.mov radius-squared vf4)
    (.mov distance-squared vf3)
    (>= radius-squared distance-squared)))

(defun sphere<-vector! ((out sphere) (center vector))
  "Copy center.xyz into out while preserving its radius."
  (let ((f0-0 (-> out w))) (vector-copy! out center) (set! (-> out w) f0-0))
  out)

(defun sphere<-vector+r! ((out sphere) (center vector) (radius float))
  "Copy center.xyz and radius into out."
  (vector-copy! out center)
  (set! (-> out w) radius)
  out)

(defun rand-vu-sphere-point! ((out vector) (radius float))
  "Choose a cube-sampled direction, normalize it to a random length in [0, radius], and write out."
  (set-vector! out (rand-vu-float-range -1.0 1.0) (rand-vu-float-range -1.0 1.0) (rand-vu-float-range -1.0 1.0) 1.0)
  (vector-normalize! out (rand-vu-float-range 0.0 radius)))