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

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

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

;; DECOMP BEGINS

(defun vector-cross! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Compute the cross product. The w component is set to junk."
  (rlet ((acc :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.outer.product.a.vf acc vf1 vf2)
    (.outer.product.b.vf vf3 vf2 vf1 acc)
    (.svf (&-> arg0 quad) vf3)
    arg0
    )
  )

(defun vector-xz-cross! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Compute the cross product of the xz components of inputs."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.add.x.vf.y vf1 vf0 vf0)
    (.add.x.vf.y vf2 vf0 vf0)
    (.outer.product.a.vf acc vf1 vf2)
    (.outer.product.b.vf vf3 vf2 vf1 acc)
    (.svf (&-> arg0 quad) vf3)
    arg0
    )
  )

(defun vector+float! ((arg0 vector) (arg1 vector) (arg2 float))
  "Add float to each component of vector. The w component is set to 1"
  (rlet ((vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (init-vf0-vector)
    (.mov vf6 arg2)
    (.lvf vf4 (&-> arg1 quad))
    (.add.x.vf.w vf5 vf0 vf0)
    (.add.x.vf.xyz vf5 vf4 vf6)
    (.svf (&-> arg0 quad) vf5)
    arg0
    )
  )

(defun vector*! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Elementwise product. Set w = 1"
  (rlet ((vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf4 (&-> arg1 quad))
    (.lvf vf5 (&-> arg2 quad))
    (.add.x.vf.w vf6 vf0 vf0)
    (.mul.vf.xyz vf6 vf4 vf5)
    (.svf (&-> arg0 quad) vf6)
    arg0
    )
  )

(defun vector+*! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "set arg0 = arg1 + (arg3 * arg2). The w component will be set to 1"
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         )
    (init-vf0-vector)
    (.mov vf7 arg3)
    (.lvf vf5 (&-> arg2 quad))
    (.lvf vf4 (&-> arg1 quad))
    (.add.x.vf.w vf6 vf0 vf0)
    (.mul.x.vf.xyz acc vf5 vf7)
    (.add.mul.w.vf.xyz vf6 vf4 vf0 acc)
    (.svf (&-> arg0 quad) vf6)
    arg0
    )
  )

(defun vector-*! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Set arg0 = arg1 - (arg3 * arg2). The w component will be set to 1."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         )
    (init-vf0-vector)
    (.mov vf7 arg3)
    (.lvf vf5 (&-> arg2 quad))
    (.lvf vf4 (&-> arg1 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 (&-> arg0 quad) vf6)
    arg0
    )
  )

(defun vector/! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Set arg0 = arg1 / arg2. The w component will be set to 1.
   The implementation is kind of crazy."
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf5 (&-> arg2 quad))
    (.div.vf Q vf0 vf5 :fsf #b11 :ftf #b1)
    (.add.x.vf.w vf6 vf0 vf0)
    (.lvf vf4 (&-> arg1 quad))
    (let ((v1-0 (/ (-> arg1 x) (-> arg2 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 (&-> arg0 quad) vf6)
    arg0
    )
  )

(defun vector-float*! ((arg0 vector) (arg1 vector) (arg2 float))
  "Multiply all values in a vector by arg2. Set w to 1."
  (declare (inline))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (.mov vf2 arg2)
    (.add.x.vf.w vf1 vf0 vf0)
    (.mul.x.vf.xyz vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun vector-average! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Set arg0 to the average of arg1 and arg2. Set w to 1."
  (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 (&-> arg1 quad))
      (.lvf vf2 (&-> arg2 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 (&-> arg0 quad) vf4)
    arg0
    )
  )

(defun vector+float*! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "arg0 = arg1 + arg2 * arg3."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf2 (&-> arg2 quad))
    (.lvf vf1 (&-> arg1 quad))
    (.mov vf3 arg3)
    (.add.x.vf.w vf4 vf0 vf0)
    (.mul.x.vf acc vf2 vf3)
    (.add.mul.w.vf.xyz vf4 vf1 vf0 acc)
    (.svf (&-> arg0 quad) vf4)
    arg0
    )
  )

(defun vector--float*! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Set arg0 = arg1 - (arg2 * arg3). The w component will be set to 1
   Is this different from vector-*!"
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf2 (&-> arg2 quad))
    (.lvf vf1 (&-> arg1 quad))
    (.mov vf3 arg3)
    (.add.x.vf.w vf4 vf0 vf0)
    (.mul.w.vf acc vf1 vf0)
    (.sub.mul.x.vf.xyz vf4 vf2 vf3 acc)
    (.svf (&-> arg0 quad) vf4)
    arg0
    )
  )

(defun vector-float/! ((arg0 vector) (arg1 vector) (arg2 float))
  "Divide all components by arg2. The w component will be set to 1."
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (init-vf0-vector)
    (.mov vf3 arg2)
    (.div.vf Q vf0 vf3 :fsf #b11 :ftf #b0)
    (.lvf vf1 (&-> arg1 quad))
    (.add.x.vf.w vf4 vf0 vf0)
    (.wait.vf)
    (.mul.vf.xyz vf4 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> arg0 quad) vf4)
    arg0
    )
  )

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

(defun vector-negate-in-place! ((arg0 vector))
  "Negate xyz. Doesn't touch w."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.sub.vf.xyz vf1 vf0 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun vector= ((arg0 vector) (arg1 vector))
  "Are the two vectors equal? Does not compare the w component.
   The implementation is cool."

  ;; (label L91)
  ;;   (set! v0-0 #t)
  ;;   (set! v1-0 #xffff)
  (let* ((v1-0 #xffff)
         ;;   (set! a0-1 (l.q a0-0))
         (a0-1 (-> arg0 quad))
         ;;   (set! v1-1 (sll v1-0 48))
         (v1-1 (shl v1-0 48))
         ;;   (set! a1-1 (l.q a1-0))
         (a1-1 (-> arg1 quad))
         (a0-2 (the uint128 0))
         (r0 (the uint128 0))
         )
    ;;   (.pceqw a0-2 a0-1 a1-1)
    (.pceqw a0-2 a0-1 a1-1)
    ;;   (.ppach a0-3 r0-0 a0-2)
    (.ppach a0-2 r0 a0-2)
    ;;   (set! v1-2 (logior a0-3 v1-1))
    (set! v1-1 (logior (the int a0-2) v1-1))
    ;;   (set! v1-3 (+ v1-2 1))
    ;; will overflow the 64-bit integer if xyz is equal.
    (set! v1-1 (+ v1-1 1))
    (zero? v1-1)
    )

  ;;   (b! (zero? v1-3) L92 (nop!))
  ;;   (set! v0-0 #f)
  ;; (label L92)
  ;;   (ret-value v0-0)

  )

(defun vector-delta ((arg0 vector) (arg1 vector))
  "Sum of the elementwise absolute value of differences"
  (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 (&-> arg0 quad))
    (.lvf vf2 (&-> arg1 quad))
    (.sub.vf vf1 vf2 vf1)
    (.abs.vf vf1 vf1)
    (.mul.x.vf.w acc vf0 vf1)
    (.add.mul.y.vf.w acc vf0 vf1 acc)
    (.add.mul.z.vf.w vf3 vf0 vf1 acc)
    (.add.w.vf.x vf3 vf0 vf3)
    (.mov v0-0 vf3)
    v0-0
    )
  )

(defun vector-seek! ((arg0 vector) (arg1 vector) (arg2 float))
  "Seek arg0 toward arg1. The arg0 is both read and written.
   arg2 is saturated to (0, 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 arg2)
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg0 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 (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun vector-smooth-seek! ((arg0 vector) (arg1 vector) (arg2 float))
  "Smoothly seek vec toward target.
   The step always points toward the target and has length (dist * alpha)
   If the step is longer than max-step, the step is projected onto a _square_ with side length arg2.
   Note that this doesn't project to a circle like the function below..."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         )
    (init-vf0-vector)
    (let ((v1-0 (new 'stack-no-clear 'vector)))
      (vector-! v1-0 arg1 arg0)
      (let ((v0-0 arg0))
        (let ((a1-2 (fmin 1.0 arg2)))
          (.mov vf7 a1-2)
          )
        (.lvf vf5 (&-> v1-0 quad))
        (.lvf vf4 (&-> arg0 quad))
        (.add.x.vf.w vf6 vf0 vf0)
        (.mul.x.vf.xyz acc vf5 vf7)
        (.add.mul.w.vf.xyz vf6 vf4 vf0 acc)
        (.svf (&-> v0-0 quad) vf6)
        v0-0
        )
      )
    )
  )

(defun vector-seek-2d-xz-smooth! ((arg0 vector) (arg1 vector) (arg2 float) (arg3 float))
  "Smoothly seek vec's x and z components toward target.
   The step always points toward the target and has length (dist * alpha)
   If the step is longer than max-step, the step is projected onto a circle of radius max-step.
   Doesn't touch y or w."
  (let ((f0-1 (- (-> arg1 x) (-> arg0 x)))
        (f2-1 (- (-> arg1 z) (-> arg0 z)))
        )
    (when (or (!= f0-1 0.0) (!= f2-1 0.0))
      (let* ((f1-5 (* f0-1 arg3))
             (f0-3 (* f2-1 arg3))
             (f2-4 (sqrtf (+ (* f1-5 f1-5) (* f0-3 f0-3))))
             )
        (cond
          ((>= arg2 f2-4)
           (+! (-> arg0 x) f1-5)
           (+! (-> arg0 z) f0-3)
           )
          (else
            (let ((f2-6 (/ arg2 f2-4)))
              (+! (-> arg0 x) (* f2-6 f1-5))
              (+! (-> arg0 z) (* f2-6 f0-3))
              )
            )
          )
        )
      )
    )
  arg0
  )

(defun vector-seek-2d-yz-smooth! ((arg0 vector) (arg1 vector) (arg2 float) (arg3 float))
  "Smoothly seek vec's y and z components toward target.
   The step always points toward the target and has length (dist * alpha)
   If the step is longer than max-step, the step is projected onto a circle of radius max-step.
   Doesn't touch x or w."
  (let ((f0-1 (- (-> arg1 y) (-> arg0 y)))
        (f2-1 (- (-> arg1 z) (-> arg0 z)))
        )
    (when (or (!= f0-1 0.0) (!= f2-1 0.0))
      (let* ((f1-5 (* f0-1 arg3))
             (f0-3 (* f2-1 arg3))
             (f2-4 (sqrtf (+ (* f1-5 f1-5) (* f0-3 f0-3))))
             )
        (cond
          ((>= arg2 f2-4)
           (+! (-> arg0 y) f1-5)
           (+! (-> arg0 z) f0-3)
           )
          (else
            (let ((f2-6 (/ arg2 f2-4)))
              (+! (-> arg0 y) (* f2-6 f1-5))
              (+! (-> arg0 z) (* f2-6 f0-3))
              )
            )
          )
        )
      )
    )
  arg0
  )

(defun vector-seek-3d-smooth! ((arg0 vector) (arg1 vector) (arg2 float) (arg3 float))
  "Smoothly seek vec's x, y, and z components toward target.
   The step always points toward the target and has length (dist * alpha)
   If the step is longer than max-step, the step is projected onto a circle of radius max-step.
   Doesn't touch  w."
  (let ((f0-1 (- (-> arg1 x) (-> arg0 x)))
        (f1-2 (- (-> arg1 y) (-> arg0 y)))
        (f3-1 (- (-> arg1 z) (-> arg0 z)))
        )
    (when (or (!= f0-1 0.0) (!= f1-2 0.0) (!= f3-1 0.0))
      (let* ((f2-6 (* f0-1 arg3))
             (f1-3 (* f1-2 arg3))
             (f0-4 (* f3-1 arg3))
             (f3-5 (sqrtf (+ (* f2-6 f2-6) (* f1-3 f1-3) (* f0-4 f0-4))))
             )
        (cond
          ((>= arg2 f3-5)
           (+! (-> arg0 x) f2-6)
           (+! (-> arg0 y) f1-3)
           (+! (-> arg0 z) f0-4)
           )
          (else
            (let ((f3-7 (/ arg2 f3-5)))
              (+! (-> arg0 x) (* f3-7 f2-6))
              (+! (-> arg0 y) (* f3-7 f1-3))
              (+! (-> arg0 z) (* f3-7 f0-4))
              )
            )
          )
        )
      )
    )
  arg0
  )

(defun seek-with-smooth ((arg0 float) (arg1 float) (arg2 float) (arg3 float) (arg4 float))
  "Move value closer to target.
   If we are within deadband, just go straight to target.
   If not, try to go alpha*err. If that is a larger step than max-step, limit to max-step"
  (let ((f0-1 (- arg1 arg0)))
    (cond
      ((>= arg4 (fabs f0-1))
       arg1
       )
      (else
        (let ((f0-2 (* f0-1 arg3)))
          (let ((f1-4 (- arg2)))
            (cond
              ((< f0-2 f1-4)
               (set! f0-2 f1-4)
               )
              ((< arg2 f0-2)
               (set! f0-2 arg2)
               )
              )
            )
          (+ f0-2 arg0)
          )
        )
      )
    )
  )

(defun vector-identity! ((arg0 vector))
  "Set arg0 to 1, 1, 1, 1"
  (set! (-> arg0 x) 1.0)
  (set! (-> arg0 y) 1.0)
  (set! (-> arg0 z) 1.0)
  (set! (-> arg0 w) 1.0)
  arg0
  )

(defun vector-seconds ((arg0 vector) (arg1 vector))
  "Convert from actual seconds to the seconds unit."
  (set! (-> arg0 x) (* 300.0 (-> arg1 x)))
  (set! (-> arg0 y) (* 300.0 (-> arg1 y)))
  (set! (-> arg0 z) (* 300.0 (-> arg1 z)))
  arg0
  )

(defun vector-seconds! ((arg0 vector))
  "Convert from actual seconds to seconds, in place"
  (set! (-> arg0 x) (* 300.0 (-> arg0 x)))
  (set! (-> arg0 y) (* 300.0 (-> arg0 y)))
  (set! (-> arg0 z) (* 300.0 (-> arg0 z)))
  arg0
  )

(defun vector-v! ((arg0 vector))
  "Convert a velocity to a displacement per frame. The velocity should be in X/actual_second.
   Uses the current process clock."
  (vector-float*! arg0 arg0 (seconds-per-frame))
  arg0
  )

(defun vector-v+! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Euler forward step, using the current display time settings"
  (vector+float*! arg0 arg1 arg2 (seconds-per-frame))
  arg0
  )

(defun vector-v*float+! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Euler forward step, scaling velocity by velocity-scale"
  (vector+float*! arg0 arg1 arg2 (* arg3 (seconds-per-frame)))
  arg0
  )

(defun vector-v++! ((arg0 vector) (arg1 vector))
  "Update position in place, using display's current timing"
  (vector+float*! arg0 arg0 arg1 (seconds-per-frame))
  arg0
  )

(defun vector-v*float! ((arg0 vector) (arg1 vector) (arg2 float))
  "Go from velocity to delta-p per frame, scaling by scale"
  (vector-float*! arg0 arg1 (* arg2 (seconds-per-frame)))
  )

(defun vector-v*float++! ((arg0 vector) (arg1 vector) (arg2 float))
  "update position with given velocity, scaled by scale."
  (vector+float*! arg0 arg0 arg1 (* arg2 (seconds-per-frame)))
  arg0
  )

(defun vector-to-ups! ((arg0 vector) (arg1 vector))
  "Go from units per frame to units per second?"
  (local-vars (at-0 int))
  (with-pp
    (rlet ((vf0 :class vf)
           (vf1 :class vf)
           (vf2 :class vf)
           )
      (init-vf0-vector)
      (.lvf vf1 (&-> arg1 quad))
      (let ((f0-0 (-> pp clock 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 (&-> arg0 quad) vf1)
      arg0
      )
    )
  )

(defun vector-from-ups! ((arg0 vector) (arg1 vector))
  "Go from units per second to units per frame?"
  (local-vars (at-0 int))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (let ((f0-0 (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 (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun vector-length ((arg0 vector))
  "Get the length of the xyz part."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 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 ((arg0 vector))
  "Get the squared length of the xyz part."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 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 ((arg0 vector))
  "Get the length of the xz part, squared."
  (+ (* (-> arg0 x) (-> arg0 x)) (* (-> arg0 z) (-> arg0 z)))
  )

(defun vector-xz-length ((arg0 vector))
  "Get the length of the xz part"
  (sqrtf (+ (* (-> arg0 x) (-> arg0 x)) (* (-> arg0 z) (-> arg0 z))))
  )

(defun vector-vector-distance ((arg0 vector) (arg1 vector))
  "Subtract the xyz parts and get the norm"
  (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 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 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 ((arg0 vector) (arg1 vector))
  "Squared norm of the difference of the xyz parts"
  (local-vars (v0-0 float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (.lvf vf2 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 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 ((arg0 vector) (arg1 vector))
  "Distance on 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 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 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-xy-distance ((arg0 vector) (arg1 vector))
  "distance on the xy 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 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.w acc vf0 vf1)
    (.add.mul.y.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 ((arg0 vector) (arg1 vector))
  "Distance on the xz plane squared"
  (local-vars (v0-0 float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (.lvf vf2 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.add.z.vf.x vf1 vf1 vf1)
    (.mov v0-0 vf1)
    v0-0
    )
  )

(defun vector-normalize! ((arg0 vector) (arg1 float))
  "Modify arg0 in place to have length arg1 for its xyz components. The w part is not changed."
  (let ((f0-0 (vector-length arg0)))
    (let ((v1-1 (/ arg1 f0-0)))
      (set! (-> arg0 data 0) (* (-> arg0 data 0) v1-1))
      (set! (-> arg0 data 1) (* (-> arg0 data 1) v1-1))
      (set! (-> arg0 data 2) (* (-> arg0 data 2) v1-1))
      )
    )
  arg0
  ;; og:preserve-this
  ; (rlet ((acc :class vf)
  ;        (Q :class vf)
  ;        (vf0 :class vf)
  ;        (vf1 :class vf)
  ;        (vf2 :class vf)
  ;        (vf3 :class vf)
  ;        )
  ;  (init-vf0-vector)
  ;  (.lvf vf1 (&-> arg0 quad))
  ;  (.mul.vf.xyz vf2 vf1 vf1)
  ;  (let ((v1-0 arg1))
  ;   (.mov vf3 v1-0)
  ;   )
  ;  (.mul.x.vf.w acc vf0 vf2)
  ;  (.add.mul.y.vf.w acc vf0 vf2 acc)
  ;  (.add.mul.z.vf.w vf2 vf0 vf2 acc)
  ;  (.isqrt.vf Q vf3 vf2 :fsf #b0 :ftf #b11)
  ;  (.wait.vf)
  ;  (.mul.vf.xyz vf1 vf1 Q)
  ;  (.nop.vf)
  ;  (.nop.vf)
  ;  (.nop.vf)
  ;  (.svf (&-> arg0 quad) vf1)
  ;  arg0
  ;  )
  )

(defun vector-normalize-ret-len! ((arg0 vector) (arg1 float))
  "Modify arg0 in place to have length arg1 for its xyz components.
   The w part isn't changed and the _original_ length is returned."
     (let ((f0-0 (vector-length arg0)))
    (let ((v1-1 (/ arg1 f0-0)))
      (set! (-> arg0 data 0) (* (-> arg0 data 0) v1-1))
      (set! (-> arg0 data 1) (* (-> arg0 data 1) v1-1))
      (set! (-> arg0 data 2) (* (-> arg0 data 2) v1-1))
      )
    f0-0
    )

  ;; og:preserve-this
  ; (local-vars (v1-1 float))
  ; (rlet ((acc :class vf)
  ;        (Q :class vf)
  ;        (vf0 :class vf)
  ;        (vf1 :class vf)
  ;        (vf2 :class vf)
  ;        (vf3 :class vf)
  ;        )
  ;  (init-vf0-vector)
  ;  (.lvf vf1 (&-> arg0 quad))
  ;  (.mul.vf.xyz vf2 vf1 vf1)
  ;  (let ((v1-0 arg1))
  ;   (.mov vf3 v1-0)
  ;   )
  ;  (.mul.x.vf.w acc vf0 vf2)
  ;  (.add.mul.y.vf.w acc vf0 vf2 acc)
  ;  (.add.mul.z.vf.w vf2 vf0 vf2 acc)
  ;  (.isqrt.vf Q vf3 vf2 :fsf #b0 :ftf #b11)
  ;  (.add.w.vf.x vf2 vf0 vf2)
  ;  (.mov v1-1 vf2)
  ;  (let ((v0-0 (sqrtf v1-1)))
  ;   (.wait.vf)
  ;   (.mul.vf.xyz vf1 vf1 Q)
  ;   (.nop.vf)
  ;   (.nop.vf)
  ;   (.nop.vf)
  ;   (.svf (&-> arg0 quad) vf1)
  ;   v0-0
  ;   )
  ;  )
  )

(defun vector-normalize-copy! ((arg0 vector) (arg1 vector) (arg2 float))
  "Normalize, but not in place.
   This implementation is very good compared to the vector-normalize! one.
   The w component is set to 1."
  (let ((f0-0 (vector-length arg1)))
    (cond
      ((= f0-0 0.0)
       (set! (-> arg0 quad) (-> arg1 quad))
       )
      (else
        (let ((v1-3 (/ arg2 f0-0)))
          (set! (-> arg0 x) (* (-> arg1 x) v1-3))
          (set! (-> arg0 y) (* (-> arg1 y) v1-3))
          (set! (-> arg0 z) (* (-> arg1 z) v1-3))
          )
        )
      )
    )
  (set! (-> arg0 w) 1.0)
  arg0
  )

(defun vector-xz-normalize! ((arg0 vector) (arg1 float))
  "Normalize, xz components only"
  (let* ((v1-0 arg0)
         (f0-4 (sqrtf (+ (* (-> v1-0 x) (-> v1-0 x)) (* (-> v1-0 z) (-> v1-0 z)))))
         )
    (when (!= f0-4 0.0)
      (let ((v1-3 (/ arg1 f0-4)))
        (set! (-> arg0 x) (* (-> arg0 x) v1-3))
        (set! (-> arg0 z) (* (-> arg0 z) v1-3))
        )
      )
    )
  arg0
  )

(defun vector-xz-normalize-copy! ((arg0 vector) (arg1 vector) (arg2 float))
  "Normalize, xz components only"
  (let* ((v1-0 arg1)
         (f0-4 (sqrtf (+ (* (-> v1-0 x) (-> v1-0 x)) (* (-> v1-0 z) (-> v1-0 z)))))
         )
    (cond
      ((= f0-4 0.0)
       (set! (-> arg0 quad) (-> arg1 quad))
       )
      (else
        (let ((v1-3 (/ arg2 f0-4)))
          (set! (-> arg0 x) (* (-> arg1 x) v1-3))
          (set! (-> arg0 y) 0.0)
          (set! (-> arg0 z) (* (-> arg1 z) v1-3))
          )
        )
      )
    )
  (set! (-> arg0 w) 1.0)
  arg0
  )

(defun vector-length-max! ((arg0 vector) (arg1 float))
  "Make vector at most arg1 length (xyz only).
   If it is larger, project onto sphere.
   Doesn't touch w"
   ;; og:preserve-this converted to asm in jak 2
  (let ((f0-0 (vector-length arg0)))
    (when (not (or (= f0-0 0.0) (< f0-0 arg1)))
      (let ((f0-1 (/ f0-0 arg1)))
        (when (!= f0-1 0.0)
          (set! (-> arg0 x) (/ (-> arg0 x) f0-1))
          (set! (-> arg0 y) (/ (-> arg0 y) f0-1))
          (set! (-> arg0 z) (/ (-> arg0 z) f0-1))
          )
        )
      )
    )
  arg0
  )

(defun vector-xz-length-max! ((arg0 vector) (arg1 float))
  "Make vector at most arg1 length (xz only).
   It it is larger, project onto circle.
   Doesn't touch w or y"
  (let* ((v1-0 arg0)
         (f0-4 (sqrtf (+ (* (-> v1-0 x) (-> v1-0 x)) (* (-> v1-0 z) (-> v1-0 z)))))
         )
    (when (not (or (= f0-4 0.0) (< f0-4 arg1)))
      (let ((f0-5 (/ f0-4 arg1)))
        (when (!= f0-5 0.0)
          (set! (-> arg0 x) (/ (-> arg0 x) f0-5))
          (set! (-> arg0 z) (/ (-> arg0 z) f0-5))
          )
        )
      )
    )
  arg0
  )

(defun vector-rotate-around-x! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate a vector around the x axis"
  (let* ((f28-0 (- arg2))
         (f30-0 (cos f28-0))
         (f1-0 (sin f28-0))
         (f2-0 (-> arg1 z))
         (f0-1 (-> arg1 y))
         )
    (set! (-> arg0 quad) (-> arg1 quad))
    (set! (-> arg0 z) (- (* f2-0 f30-0) (* f0-1 f1-0)))
    (set! (-> arg0 y) (+ (* f2-0 f1-0) (* f0-1 f30-0)))
    )
  arg0
  )

(defun vector-rotate-around-y! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate a vector around the y axis"
  (let ((f26-0 (-> arg1 z))
        (f30-0 (-> arg1 x))
        (f28-0 (cos arg2))
        (f0-0 (sin arg2))
        )
    (set! (-> arg0 quad) (-> arg1 quad))
    (set! (-> arg0 z) (- (* f26-0 f28-0) (* f30-0 f0-0)))
    (set! (-> arg0 x) (+ (* f26-0 f0-0) (* f30-0 f28-0)))
    )
  arg0
  )

(defun vector-rotate90-around-y! ((arg0 vector) (arg1 vector))
  "Rotate a vector 90 degrees around y."
  (set! (-> arg0 quad) (-> arg1 quad))
  (let ((f0-0 (-> arg0 x)))
    (set! (-> arg0 x) (- (-> arg1 z)))
    (set! (-> arg0 z) f0-0)
    )
  arg0
  )

(defun vector-rotate-around-z! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate a vector around the z axis"
  (let ((f26-0 (-> arg1 x))
        (f30-0 (-> arg1 y))
        (f28-0 (cos arg2))
        (f0-0 (sin arg2))
        )
    (set! (-> arg0 quad) (-> arg1 quad))
    (set! (-> arg0 x) (- (* f26-0 f28-0) (* f30-0 f0-0)))
    (set! (-> arg0 y) (+ (* f26-0 f0-0) (* f30-0 f28-0)))
    )
  arg0
  )

(defun rotate-y<-vector+vector ((arg0 vector) (arg1 vector))
  "Get the y rotation between vectors. These should have the same length."
  (atan (- (-> arg1 x) (-> arg0 x)) (- (-> arg1 z) (-> arg0 z)))
  )

(defun rotate-x<-vector+vector ((arg0 vector) (arg1 vector))
  "Get the x rotation between vectors. These should have the same length."
  (atan (- (-> arg1 y) (-> arg0 y)) (- (-> arg1 z) (-> arg0 z)))
  )

(defun rotate-z<-vector+vector ((arg0 vector) (arg1 vector))
  "Get the z rotation between vectors. These should have the same length."
  (atan (- (-> arg1 x) (-> arg0 x)) (- (-> arg1 y) (-> arg0 y)))
  )

(defun vector-cvt.w.s! ((arg0 vector) (arg1 vector))
  "Convert float to int32. Truncate."
  (rlet ((vf1 :class vf))
    (.lvf vf1 (&-> arg1 quad))
    (.ftoi.vf vf1 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun vector-cvt.s.w! ((arg0 vector) (arg1 vector))
  "Convert float to int32."
  (rlet ((vf1 :class vf))
    (.lvf vf1 (&-> arg1 quad))
    (.itof.vf vf1 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun rot-zxy-from-vector! ((arg0 vector) (arg1 vector))
  "I think this gives you a vector of euler angles to rotate some unit vector
   to arg1."
  (let* ((f28-0 (-> arg1 z))
         (f30-0 (-> arg1 x))
         (f0-0 (atan f30-0 f28-0))
         )
    (set! (-> arg0 y) f0-0)
    (let* ((f26-0 (- f0-0))
           (f0-4 (- (* f28-0 (cos f26-0)) (* f30-0 (sin f26-0))))
           )
      (set! (-> arg0 x) (atan (- (-> arg1 y)) f0-4))
      )
    )
  (set! (-> arg0 z) 0.0)
  arg0
  )

(defun rot-zyx-from-vector! ((arg0 vector) (arg1 vector))
  "I think this gives you a vector of euler angles to rotate some unit vector
   to arg1."
  (let* ((f28-0 (-> arg1 z))
         (f30-0 (- (-> arg1 y)))
         (f0-1 (atan f30-0 f28-0))
         )
    (set! (-> arg0 x) f0-1)
    (let* ((f26-0 (- f0-1))
           (f0-5 (- (* f28-0 (cos f26-0)) (* f30-0 (sin f26-0))))
           )
      (set! (-> arg0 y) (atan (-> arg1 x) f0-5))
      )
    )
  (set! (-> arg0 z) 0.0)
  arg0
  )

(defun vector-lerp! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Linearly interpolate between two vectors. Alpha isn't clamped.
   w will be set to 1."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.mov vf4 arg3)
    (.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 (&-> arg0 quad) vf3)
    arg0
    )
  )

(defun vector-lerp-clamp! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Linearly interpolate between two vectors, clamping alpha to 0, 1
   w will be set to 1."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (init-vf0-vector)
    (cond
      ((>= 0.0 arg3)
       (set! (-> arg0 quad) (-> arg1 quad))
       )
      ((>= arg3 1.0)
       (set! (-> arg0 quad) (-> arg2 quad))
       )
      (else
        (let ((v1-3 arg0))
          (let ((f0-2 arg3))
            (.lvf vf1 (&-> arg1 quad))
            (.lvf vf2 (&-> arg2 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-3 quad) vf3)
          )
        )
      )
    arg0
    )
  )

(defun vector4-lerp! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Interpolate all 4 elements of a vector. Alpha is not clamped"
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.mov vf4 arg3)
    (.sub.vf vf2 vf2 vf1)
    (.mul.x.vf vf2 vf2 vf4)
    (.add.vf vf3 vf1 vf2)
    (.svf (&-> arg0 quad) vf3)
    arg0
    )
  )

(defun vector4-lerp-clamp! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Interpolate all 4 elements of a vector. Alpha is clamped to [0, 1]"
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (cond
      ((>= 0.0 arg3)
       (set! (-> arg0 quad) (-> arg1 quad))
       )
      ((>= arg3 1.0)
       (set! (-> arg0 quad) (-> arg2 quad))
       )
      (else
        (let ((v1-3 arg0))
          (let ((f0-2 arg3))
            (.lvf vf1 (&-> arg1 quad))
            (.lvf vf2 (&-> arg2 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-3 quad) vf3)
          )
        )
      )
    arg0
    )
  )

(defun vector-degi ((arg0 vector) (arg1 vector))
  "Convert a vector (in _rotations_) to degrees units, stored in an int.
   Truncates to the nearest _rotation_.
   Neither the input or output is a commonly used form.
   Unsurprisingly, this strange function is never used."
  (local-vars (v1-0 uint128) (v1-1 uint128))
  (rlet ((vf1 :class vf))
    (.lvf vf1 (&-> arg1 quad))
    (.ftoi.vf vf1 vf1)
    (.mov v1-0 vf1)
    (.pw.sll v1-1 v1-0 16)
    (set! (-> arg0 quad) v1-1)
    arg0
    )
  )

(defun vector-degf ((arg0 vector) (arg1 vector))
  "Convert a vector (in integer degree units) to floating point rotations.
   Truncates to the nearest _rotation_.
   Like the previous function, this is stupid and unused"
  (local-vars (v1-1 uint128))
  (rlet ((vf1 :class vf))
    (let ((v1-0 (-> arg1 quad)))
      (.pw.sra v1-1 v1-0 16)
      )
    (.mov vf1 v1-1)
    (.itof.vf vf1 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun vector-degmod ((arg0 vector) (arg1 vector))
  "This one is actually right. Wraps degrees units (in floats, like they should be)
  to +/- half a rotation."
  (local-vars (v1-0 uint128) (v1-1 uint128) (v1-2 uint128))
  (rlet ((vf1 :class vf))
    (.lvf vf1 (&-> arg1 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 (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun vector-deg-diff ((arg0 vector) (arg1 vector) (arg2 vector))
  "Wrapped difference, degrees units. Will have the usual 16-bit accuracy issue"
  (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 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 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 (&-> arg0 quad) vf1)
    (.mov v0-0 vf1)
    (none)
    )
  )

(defun vector-deg-lerp-clamp! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Apply deg-lerp-clamp to the xyz components of a vector. Sets w = 1."
  (cond
    ((>= 0.0 arg3)
     (set! (-> arg0 quad) (-> arg1 quad))
     )
    ((>= arg3 1.0)
     (set! (-> arg0 quad) (-> arg2 quad))
     )
    (else
      (set! (-> arg0 x) (deg-lerp-clamp (-> arg1 x) (-> arg2 x) arg3))
      (set! (-> arg0 y) (deg-lerp-clamp (-> arg1 y) (-> arg2 y) arg3))
      (set! (-> arg0 z) (deg-lerp-clamp (-> arg1 z) (-> arg2 z) arg3))
      (set! (-> arg0 w) 1.0)
      )
    )
  arg0
  )

;; The 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! ((arg0 vector) (arg1 vector))
  "Copy a vector3s"
  (set! (-> arg0 x) (-> arg1 x))
  (set! (-> arg0 y) (-> arg1 y))
  (set! (-> arg0 z) (-> arg1 z))
  arg0
  )

(defun vector3s+! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Add 2 vectors3."
  (set! (-> arg0 x) (+ (-> arg1 x) (-> arg2 x)))
  (set! (-> arg0 y) (+ (-> arg1 y) (-> arg2 y)))
  (set! (-> arg0 z) (+ (-> arg1 z) (-> arg2 z)))
  arg0
  )

(defun vector3s*float! ((arg0 vector) (arg1 vector) (arg2 float))
  "mult vectors3 by float"
  (set! (-> arg0 x) (* (-> arg1 x) arg2))
  (set! (-> arg0 y) (* (-> arg1 y) arg2))
  (set! (-> arg0 z) (* (-> arg1 z) arg2))
  arg0
  )

(defun vector3s-! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Subtract 2 vectors3: c = (a - b)."
  (set! (-> arg0 x) (- (-> arg1 x) (-> arg2 x)))
  (set! (-> arg0 y) (- (-> arg1 y) (-> arg2 y)))
  (set! (-> arg0 z) (- (-> arg1 z) (-> arg2 z)))
  arg0
  )

(defun vector4-add! ((arg0 vector4) (arg1 vector4) (arg2 vector4))
  "Add 2 vector4s"
  (local-vars (v0-0 float))
  (rlet ((vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (.lvf vf5 (&-> arg1 quad))
    (.lvf vf6 (&-> arg2 quad))
    (.add.vf vf4 vf5 vf6)
    (.svf (&-> arg0 quad) vf4)
    (.mov v0-0 vf4)
    (none)
    )
  )

(defun vector4-sub! ((arg0 vector4) (arg1 vector4) (arg2 vector4))
  "Subtract 2 vector4s"
  (local-vars (v0-0 float))
  (rlet ((vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (.lvf vf5 (&-> arg1 quad))
    (.lvf vf6 (&-> arg2 quad))
    (.sub.vf vf4 vf5 vf6)
    (.svf (&-> arg0 quad) vf4)
    (.mov v0-0 vf4)
    (none)
    )
  )

(defun vector4-mul! ((arg0 vector4) (arg1 vector4) (arg2 vector4))
  "Multiple 2 vector4s"
  (local-vars (v0-0 float))
  (rlet ((vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (.lvf vf5 (&-> arg1 quad))
    (.lvf vf6 (&-> arg2 quad))
    (.mul.vf vf4 vf5 vf6)
    (.svf (&-> arg0 quad) vf4)
    (.mov v0-0 vf4)
    (none)
    )
  )

(defun vector4-scale! ((arg0 vector4) (arg1 vector4) (arg2 float))
  "arg0 = arg1 * arg2"
  (local-vars (v0-0 float))
  (rlet ((vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (.mov vf6 arg2)
    (.lvf vf5 (&-> arg1 quad))
    (.mul.x.vf vf4 vf5 vf6)
    (.svf (&-> arg0 quad) vf4)
    (.mov v0-0 vf4)
    (none)
    )
  )

(defun vector4-madd! ((arg0 vector4) (arg1 vector4) (arg2 vector4) (arg3 float))
  "arg0 = arg1 + arg2 * arg3"
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         )
    (init-vf0-vector)
    (.mov vf7 arg3)
    (.lvf vf5 (&-> arg1 quad))
    (.lvf vf6 (&-> arg2 quad))
    (.mul.w.vf acc vf5 vf0)
    (.add.mul.x.vf vf4 vf6 vf7 acc)
    (.svf (&-> arg0 quad) vf4)
    (.mov v0-0 vf4)
    (none)
    )
  )

(defun vector4-msub! ((arg0 vector4) (arg1 vector4) (arg2 vector4) (arg3 float))
  "arg0 = arg1 - arg2 * arg3"
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         )
    (init-vf0-vector)
    (.mov vf7 arg3)
    (.lvf vf5 (&-> arg1 quad))
    (.lvf vf6 (&-> arg2 quad))
    (.mul.w.vf acc vf5 vf0)
    (.sub.mul.x.vf vf4 vf6 vf7 acc)
    (.svf (&-> arg0 quad) vf4)
    (.mov v0-0 vf4)
    (none)
    )
  )

(defun vector4-array-add! ((arg0 (inline-array vector4)) (arg1 (inline-array vector4)) (arg2 (inline-array vector4)) (arg3 int))
  "Apply vector4-add! to all arrays."
  (dotimes (s2-0 arg3)
    (vector4-add! (the-as vector4 arg0) (the-as vector4 arg1) (the-as vector4 arg2))
    (set! arg1 (the-as (inline-array vector4) (-> arg1 1)))
    (set! arg2 (the-as (inline-array vector4) (-> arg2 1)))
    (set! arg0 (the-as (inline-array vector4) (-> arg0 1)))
    )
  (none)
  )

(defun vector4-array-sub! ((arg0 (inline-array vector4)) (arg1 (inline-array vector4)) (arg2 (inline-array vector4)) (arg3 int))
  "Apply vector4-sub! to all arrays."
  (dotimes (s2-0 arg3)
    (vector4-sub! (the-as vector4 arg0) (the-as vector4 arg1) (the-as vector4 arg2))
    (set! arg1 (the-as (inline-array vector4) (-> arg1 1)))
    (set! arg2 (the-as (inline-array vector4) (-> arg2 1)))
    (set! arg0 (the-as (inline-array vector4) (-> arg0 1)))
    )
  (none)
  )

(defun vector4-array-mul! ((arg0 (inline-array vector4)) (arg1 (inline-array vector4)) (arg2 (inline-array vector4)) (arg3 int))
  "Apply vector4-mul! to all arrays."
  (dotimes (s2-0 arg3)
    (vector4-mul! (the-as vector4 arg0) (the-as vector4 arg1) (the-as vector4 arg2))
    (set! arg1 (the-as (inline-array vector4) (-> arg1 1)))
    (set! arg2 (the-as (inline-array vector4) (-> arg2 1)))
    (set! arg0 (the-as (inline-array vector4) (-> arg0 1)))
    )
  (none)
  )

(defun vector4-array-scale! ((arg0 (inline-array vector4)) (arg1 (inline-array vector4)) (arg2 float) (arg3 int))
  "Apply vector4-scale! to all arrays."
  (dotimes (s2-0 arg3)
    (vector4-scale! (the-as vector4 arg0) (the-as vector4 arg1) arg2)
    (set! arg1 (the-as (inline-array vector4) (-> arg1 1)))
    (set! arg0 (the-as (inline-array vector4) (-> arg0 1)))
    )
  (none)
  )

(defun vector4-array-madd! ((arg0 (inline-array vector4))
                   (arg1 (inline-array vector4))
                   (arg2 (inline-array vector4))
                   (arg3 float)
                   (arg4 int)
                   )
  "Apply vector4-madd! to all arrays."
  (dotimes (s1-0 arg4)
    (vector4-madd! (the-as vector4 arg0) (the-as vector4 arg1) (the-as vector4 arg2) arg3)
    (set! arg1 (the-as (inline-array vector4) (-> arg1 1)))
    (set! arg2 (the-as (inline-array vector4) (-> arg2 1)))
    (set! arg0 (the-as (inline-array vector4) (-> arg0 1)))
    )
  (none)
  )

(defun vector4-array-msub! ((arg0 (inline-array vector4))
                   (arg1 (inline-array vector4))
                   (arg2 (inline-array vector4))
                   (arg3 float)
                   (arg4 int)
                   )
  "Apply vector4-msub! to all arrays."
  (dotimes (s1-0 arg4)
    (vector4-msub! (the-as vector4 arg0) (the-as vector4 arg1) (the-as vector4 arg2) arg3)
    (set! arg1 (the-as (inline-array vector4) (-> arg1 1)))
    (set! arg2 (the-as (inline-array vector4) (-> arg2 1)))
    (set! arg0 (the-as (inline-array vector4) (-> arg0 1)))
    )
  (none)
  )

(defun vector4-array-lerp! ((arg0 (inline-array vector4))
                   (arg1 (inline-array vector4))
                   (arg2 (inline-array vector4))
                   (arg3 float)
                   (arg4 int)
                   )
  "Apply vector4-lerp! to all arrays."
  (dotimes (s1-0 arg4)
    (vector4-lerp! (the-as vector arg0) (the-as vector arg1) (the-as vector arg2) arg3)
    (set! arg1 (the-as (inline-array vector4) (-> arg1 1)))
    (set! arg2 (the-as (inline-array vector4) (-> arg2 1)))
    (set! arg0 (the-as (inline-array vector4) (-> arg0 1)))
    )
  #f
  )

(defun spheres-overlap? ((arg0 sphere) (arg1 sphere))
  "Do the spheres overlap?"
  (local-vars (v1-0 float) (a0-1 float))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.lvf vf2 (&-> arg1 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 a0-1 vf4)
    (.mov v1-0 vf3)
    (>= a0-1 v1-0)
    )
  )

(defun sphere<-vector! ((arg0 sphere) (arg1 vector))
  "Set the position of the sphere to arg1. Does not change the radius"
  (let ((f0-0 (-> arg0 r)))
    (set! (-> arg0 quad) (-> arg1 quad))
    (set! (-> arg0 r) f0-0)
    )
  arg0
  )

(defun sphere<-vector+r! ((arg0 sphere) (arg1 vector) (arg2 float))
  "Set the position of the sphere from arg1 and the radius from arg2"
  (set! (-> arg0 quad) (-> arg1 quad))
  (set! (-> arg0 r) arg2)
  arg0
  )

(defun rand-vu-sphere-point! ((arg0 vector) (arg1 float))
  "Get a random point on the sphere at the origin with radius arg1.
   The point is on the surface of the sphere."
  (set-vector!
    arg0
    (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! arg0 (rand-vu-float-range 0.0 arg1))
  )

(defmethod print ((obj vector2))
  "Print a vector2."
  (format #t "#<vector ~F ~F @ #x~X>" (-> obj x) (-> obj y) obj)
  obj
  )

(defun vector-vector-angle-safe ((arg0 vector) (arg1 vector))
  "Get the angle between two vectors, with some 'safety' applied..."
  (let ((f0-1 (vector-dot
                (vector-normalize-copy! (new 'stack-no-clear 'vector) arg0 1.0)
                (vector-normalize-copy! (new 'stack-no-clear 'vector) arg1 1.0)
                )
              )
        )
    (cond
      ((>= f0-1 1.0)
       0.0
       )
      ((>= -1.0 f0-1)
       ;; og:preserve-this what is this value?? this should be 180 degrees.
       0.017257283
       )
      (else
        (acos f0-1)
        )
      )
    )
  )