jak-project/goal_src/jak3/engine/math/quaternion.gc

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

;; name: quaternion.gc
;; name in dgo: quaternion
;; dgos: GAME

;; DECOMP BEGINS


(defun quaternion-axis-angle! ((arg0 quaternion) (arg1 float) (arg2 float) (arg3 float) (arg4 float))
  "Construct a quaternion from an axis and angle. The axis should be normalized."
  (let ((s5-0 (new 'stack-no-clear 'vector)))
    (sincos! s5-0 (* 0.5 arg4))
    (let ((f0-2 (-> s5-0 x)))
      (set! (-> arg0 x) (* arg1 f0-2))
      (set! (-> arg0 y) (* arg2 f0-2))
      (set! (-> arg0 z) (* arg3 f0-2))
      )
    (set! (-> arg0 w) (-> s5-0 y))
    )
  arg0
  )

(defun quaternion-vector-angle! ((arg0 quaternion) (arg1 vector) (arg2 float))
  "Construct a quaternion from an axis and angle. The axis should be normalized."
  (let ((s5-0 (new 'stack-no-clear 'vector)))
    (sincos! s5-0 (* 0.5 arg2))
    (let ((f0-2 (-> s5-0 x)))
      (set! (-> arg0 x) (* (-> arg1 x) f0-2))
      (set! (-> arg0 y) (* (-> arg1 y) f0-2))
      (set! (-> arg0 z) (* (-> arg1 z) f0-2))
      )
    (set! (-> arg0 w) (-> s5-0 y))
    )
  arg0
  )

(defun vector-angle<-quaternion! ((arg0 vector) (arg1 quaternion))
  "Convert the quaternion arg1 to axis-angle form and store in arg0 (angle goes in w)"
  (let* ((f0-0 1.0)
         (f1-0 1.0)
         (f2-0 (-> arg1 w))
         (f30-0 (/ f0-0 (sqrtf (- f1-0 (* f2-0 f2-0)))))
         (f0-3 (* 2.0 (acos-rad (-> arg1 w))))
         )
    (set! (-> arg0 x) (* (-> arg1 x) f30-0))
    (set! (-> arg0 y) (* (-> arg1 y) f30-0))
    (set! (-> arg0 z) (* (-> arg1 z) f30-0))
    (set! (-> arg0 w) f0-3)
    )
  arg0
  )

(defun quaternion-look-at! ((arg0 quaternion) (arg1 vector) (arg2 vector))
  "Look along arg1, with arg2 as up."
  (let ((s5-0 (new 'stack-no-clear 'matrix)))
    (vector-cross! (-> s5-0 rvec) arg2 arg1)
    (vector-cross! (-> s5-0 uvec) arg1 (-> s5-0 rvec))
    (set! (-> s5-0 fvec quad) (-> arg1 quad))
    (quaternion-normalize! (matrix->quaternion arg0 s5-0))
    )
  )

(defun quaternion-zero! ((arg0 quaternion))
  "Set quaternion to all 0's."
  (set! (-> arg0 quad) (the-as uint128 0))
  arg0
  )

(defun quaternion-identity! ((arg0 quaternion))
  "Set quaternion to 0,0,0,1 (identity)."
  (set! (-> arg0 quad) (the-as uint128 0))
  (set! (-> arg0 w) 1.0)
  arg0
  )

(defun quaternion-i! ((arg0 quaternion))
  "Create unit i quaternion."
  (set! (-> arg0 quad) (the-as uint128 0))
  (set! (-> arg0 x) 1.0)
  arg0
  )

(defun quaternion-j! ((arg0 quaternion))
  "Create unit j quaternion."
  (set! (-> arg0 quad) (the-as uint128 0))
  (set! (-> arg0 y) 1.0)
  arg0
  )

(defun quaternion-k! ((arg0 quaternion))
  "Create unit k quaternion."
  (set! (-> arg0 quad) (the-as uint128 0))
  (set! (-> arg0 z) 1.0)
  arg0
  )

(defun quaternion-copy! ((arg0 quaternion) (arg1 quaternion))
  "Set arg0 = arg1."
  (set! (-> arg0 quad) (-> arg1 quad))
  arg0
  )

(defun quaternion-set! ((arg0 quaternion) (arg1 float) (arg2 float) (arg3 float) (arg4 float))
  "Set arg0 = [arg1, arg2, arg3, arg4]."
  (set! (-> arg0 x) arg1)
  (set! (-> arg0 y) arg2)
  (set! (-> arg0 z) arg3)
  (set! (-> arg0 w) arg4)
  arg0
  )

(defun quaternion+! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion))
  "Add quaternions as vectors."
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.add.vf vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun quaternion-! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion))
  "Subtract quaternions as vectors."
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.sub.vf vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

;; ERROR: Bad vector register dependency: vf2
(defun quaternion-negate! ((arg0 quaternion) (arg1 quaternion))
  "Set arg0 = -arg1."
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (.lvf vf1 (&-> arg1 quad))
    ;; og:preserve-this
    ;; (.sub.vf vf2 vf2 vf2)
    (.xor.vf vf2 vf2 vf2)
    (.sub.vf vf1 vf2 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

;; ERROR: Bad vector register dependency: vf2
(defun quaternion-conjugate! ((arg0 quaternion) (arg1 quaternion))
  "Set arg0 to the conjugate of arg1 (negate only ijk).
   If arg1 is normalized, this is equivalent to the inverse
   NOTE: this gives you the inverse rotation."
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (.lvf vf1 (&-> arg1 quad))
    ;; og:preserve-this
    ;; (.sub.vf vf2 vf2 vf2)
    (.xor.vf vf2 vf2 vf2)
    (.sub.vf vf2 vf2 vf1 :mask #b111)
    (.add.vf vf2 vf2 vf1 :mask #b1000)
    (.svf (&-> arg0 quad) vf2)
    arg0
    )
  )

(defun quaternion-float*! ((arg0 quaternion) (arg1 quaternion) (arg2 float))
  "Multiply each element."
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (.lvf vf1 (&-> arg1 quad))
    (.mov vf2 arg2)
    (.mul.x.vf vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun quaternion-float/! ((arg0 quaternion) (arg1 quaternion) (arg2 float))
  "Divide each element."
  (let ((f0-1 (/ 1.0 arg2)))
    (quaternion-float*! arg0 arg1 f0-1)
    )
  arg0
  )

(defun quaternion-norm2 ((arg0 quaternion))
  "Get the squared norm of a quaternion."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.mul.vf vf1 vf1 vf1)
    (.add.z.vf acc vf1 vf1 :mask #b1000)
    (.add.mul.y.vf acc vf0 vf1 acc :mask #b1000)
    (.add.mul.x.vf vf1 vf0 vf1 acc :mask #b1000)
    (.add.w.vf vf1 vf0 vf1)
    (.mov v0-0 vf1)
    v0-0
    )
  )

(defun quaternion-norm ((arg0 quaternion))
  "Get the norm of a quaternion."
  (local-vars (v1-1 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.mul.vf vf1 vf1 vf1)
    (.add.z.vf acc vf1 vf1 :mask #b1000)
    (.add.mul.y.vf acc vf0 vf1 acc :mask #b1000)
    (.add.mul.x.vf vf1 vf0 vf1 acc :mask #b1000)
    (.add.w.vf vf1 vf0 vf1)
    (.mov v1-1 vf1)
    (sqrtf v1-1)
    )
  )

(defun quaternion-normalize! ((arg0 quaternion))
  "Normalize a quaternion."
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.mul.vf vf2 vf1 vf1)
    (.add.z.vf acc vf2 vf2 :mask #b1000)
    (.add.mul.y.vf acc vf0 vf2 acc :mask #b1000)
    (.add.mul.x.vf vf2 vf0 vf2 acc :mask #b1000)
    (.isqrt.vf Q vf0 vf2 :fsf #b11 :ftf #b11)
    ;; og:preserve-this
    ;; (.wait.vf)
    (.mul.vf vf2 vf1 Q)
    ;; (.nop.vf)
    ;; (.nop.vf)
    (.svf (&-> arg0 quad) vf2)
    arg0
    )
  )

;; ERROR: Bad vector register dependency: vf3
(defun quaternion-inverse! ((arg0 quaternion) (arg1 quaternion))
  "Invert a quaternion. The inverse will satisfy q * q^-1 = identity, even if q is not normalized.
   If your quaternion is normalized, it is faster/more accurate to do quaternion-conjugate!"
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (.mul.vf vf2 vf1 vf1)
    ;; og:preserve-this
    ;; (.sub.vf vf3 vf3 vf3)
    (.xor.vf vf3 vf3 vf3)
    (.add.z.vf acc vf2 vf2 :mask #b1000)
    (.add.mul.y.vf acc vf0 vf2 acc :mask #b1000)
    (.add.mul.x.vf vf2 vf0 vf2 acc :mask #b1000)
    (.sub.vf vf3 vf3 vf1 :mask #b111)
    (.div.vf Q vf0 vf2 :fsf #b11 :ftf #b11)
    (.add.vf vf3 vf3 vf1 :mask #b1000)
    (.wait.vf)
    (.mul.vf vf3 vf3 Q)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> arg0 quad) vf3)
    arg0
    )
  )

(defun quaternion-dot ((arg0 quaternion) (arg1 quaternion))
  "Treat quaternions as vectors and take the dot product."
  (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))
    (.lvf vf2 (&-> arg1 quad))
    (.mul.vf vf1 vf1 vf2)
    (.add.z.vf acc vf1 vf1 :mask #b1000)
    (.add.mul.y.vf acc vf0 vf1 acc :mask #b1000)
    (.add.mul.x.vf vf1 vf0 vf1 acc :mask #b1000)
    (.add.w.vf vf1 vf0 vf1)
    (.mov v0-0 vf1)
    v0-0
    )
  )

(defun quaternion*! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion))
  "Real quaternion multiplication."
  (rlet ((acc :class vf)
         (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))
    (.sub.vf vf4 vf0 vf0 :mask #b1000)
    (.mul.vf vf3 vf1 vf2)
    (.outer.product.a.vf acc vf1 vf2)
    (.outer.product.b.vf vf4 vf2 vf1 acc)
    (.mul.w.vf acc vf1 vf2)
    (.add.mul.w.vf acc vf2 vf1 acc)
    (.sub.mul.w.vf acc vf0 vf3 acc :mask #b1000)
    (.sub.mul.z.vf acc vf0 vf3 acc :mask #b1000)
    (.sub.mul.y.vf acc vf0 vf3 acc :mask #b1000)
    (.sub.mul.x.vf acc vf0 vf3 acc :mask #b1000)
    (.add.mul.w.vf vf1 vf4 vf0 acc)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

(defun quaternion-right-mult-matrix! ((arg0 matrix) (arg1 quaternion))
  "Place quaternion coefficients into a matrix.
   You can convert a quaternion to a matrix by taking the product of this
   right-mult and left-mult matrix, but this method is not used.
   Instead, quaternion->matrix is a more efficient implementation."
  (let ((f3-0 (-> arg1 x))
        (f2-0 (-> arg1 y))
        (f1-0 (-> arg1 z))
        (f0-0 (-> arg1 w))
        )
    (set! (-> arg0 rvec x) f0-0)
    (set! (-> arg0 rvec y) f1-0)
    (set! (-> arg0 rvec z) (- f2-0))
    (set! (-> arg0 rvec w) f3-0)
    (set! (-> arg0 uvec x) (- f1-0))
    (set! (-> arg0 uvec y) f0-0)
    (set! (-> arg0 uvec z) f3-0)
    (set! (-> arg0 uvec w) f2-0)
    (set! (-> arg0 fvec x) f2-0)
    (set! (-> arg0 fvec y) (- f3-0))
    (set! (-> arg0 fvec z) f0-0)
    (set! (-> arg0 fvec w) f1-0)
    (set! (-> arg0 trans x) (- f3-0))
    (set! (-> arg0 trans y) (- f2-0))
    (set! (-> arg0 trans z) (- f1-0))
    (set! (-> arg0 trans w) f0-0)
    )
  arg0
  )

(defun quaternion-left-mult-matrix! ((arg0 matrix) (arg1 quaternion))
  "Place quaternion coefficients into a matrix. Unused."
  (let ((f2-0 (-> arg1 x))
        (f1-0 (-> arg1 y))
        (f0-0 (-> arg1 z))
        )
    (let ((f3-0 (-> arg1 w)))
      (set! (-> arg0 rvec x) f2-0)
      (set! (-> arg0 rvec y) f3-0)
      (set! (-> arg0 rvec z) (- f0-0))
      (set! (-> arg0 rvec w) f1-0)
      (set! (-> arg0 uvec x) f1-0)
      (set! (-> arg0 uvec y) f0-0)
      (set! (-> arg0 uvec z) f3-0)
      (set! (-> arg0 uvec w) (- f3-0))
      (set! (-> arg0 fvec x) f0-0)
      (set! (-> arg0 fvec y) (- f1-0))
      (set! (-> arg0 fvec z) f2-0)
      (set! (-> arg0 fvec w) f3-0)
      (set! (-> arg0 trans x) f3-0)
      )
    (set! (-> arg0 trans y) (- f2-0))
    (set! (-> arg0 trans z) (- f1-0))
    (set! (-> arg0 trans w) (- f0-0))
    )
  arg0
  )

(defun quaternion->matrix ((arg0 matrix) (arg1 quaternion))
  "Convert quaternion to matrix."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (.add.vf vf5 vf1 vf1)
    (.add.w.vf vf2 vf0 vf1 :mask #b1)
    (.add.z.vf vf2 vf0 vf1 :mask #b10)
    (.sub.y.vf vf2 vf0 vf1 :mask #b100)
    (.sub.w.vf vf2 vf0 vf0 :mask #b1000)
    (.sub.z.vf vf3 vf0 vf1 :mask #b1)
    (.add.w.vf vf3 vf0 vf1 :mask #b10)
    (.add.x.vf vf3 vf0 vf1 :mask #b100)
    (.sub.w.vf vf3 vf0 vf0 :mask #b1000)
    (.add.y.vf vf4 vf0 vf1 :mask #b1)
    (.sub.x.vf vf4 vf0 vf1 :mask #b10)
    (.add.w.vf vf4 vf0 vf1 :mask #b100)
    (.sub.w.vf vf4 vf0 vf0 :mask #b1000)
    (.outer.product.a.vf acc vf5 vf2)
    (.outer.product.b.vf vf2 vf2 vf5 acc)
    (.outer.product.a.vf acc vf5 vf3)
    (.outer.product.b.vf vf3 vf3 vf5 acc)
    (.outer.product.a.vf acc vf5 vf4)
    (.outer.product.b.vf vf4 vf4 vf5 acc)
    (.add.w.vf vf2 vf2 vf0 :mask #b1)
    (.add.w.vf vf3 vf3 vf0 :mask #b10)
    (.add.w.vf vf4 vf4 vf0 :mask #b100)
    (.svf (&-> arg0 trans quad) vf0)
    (.svf (&-> arg0 rvec quad) vf2)
    (.svf (&-> arg0 uvec quad) vf3)
    (.svf (&-> arg0 fvec quad) vf4)
    arg0
    )
  )

(defun quaternion->matrix-2 ((arg0 matrix) (arg1 quaternion))
  "Unused alternate quaternion->matrix function."
  (/ 0 0)
  arg0
  ; (local-vars (v1-1 uint128) (a1-1 uint128) (a1-2 uint128) (a2-0 uint128) (a3-0 uint128))
  ; (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)
  ;        (vf7 :class vf)
  ;        (vf8 :class vf)
  ;        )
  ;   (init-vf0-vector)
  ;   (let ((v1-0 (-> arg1 vec quad)))
  ;     (.pexew a1-1 v1-0)
  ;     (.mov vf1 v1-0)
  ;     (.prot3w a2-0 a1-1)
  ;     (.pcpyud a1-2 v1-0 a1-1)
  ;     (.mov vf4 a2-0)
  ;     (let ((a2-1 #x3f800000))
  ;       (.add.vf vf5 vf1 vf1)
  ;       (.sub.y.vf vf4 vf0 vf4 :mask #b10)
  ;       (.pextlw a3-0 v1-0 a1-2)
  ;       (.mov vf3 a1-2)
  ;       (.pextuw v1-1 v1-0 a3-0)
  ;       (.mov vf2 v1-1)
  ;       (set! (-> arg0 trans quad) (the-as uint128 0))
  ;       (.sub.x.vf vf3 vf0 vf3 :mask #b1)
  ;       (set! (-> arg0 trans w) (the-as float a2-1))
  ;       (.sub.z.vf vf2 vf0 vf2 :mask #b100)
  ;       (.outer.product.a.vf acc vf5 vf4)
  ;       (.outer.product.b.vf vf8 vf4 vf5 acc)
  ;       (.outer.product.a.vf acc vf5 vf3)
  ;       (.outer.product.b.vf vf7 vf3 vf5 acc)
  ;       (.outer.product.a.vf acc vf5 vf2)
  ;       (.outer.product.b.vf vf6 vf2 vf5 acc)
  ;       (.add.w.vf vf8 vf8 vf0 :mask #b100)
  ;       (.add.w.vf vf7 vf7 vf0 :mask #b10)
  ;       (.add.w.vf vf6 vf6 vf0 :mask #b1)
  ;       (.svf (&-> arg0 quad 2) vf8)
  ;       (set! (-> arg0 data 11) (the-as float a2-1))
  ;       (.svf (&-> arg0 quad 1) vf7)
  ;       (set! (-> arg0 data 7) (the-as float a2-1))
  ;       (.svf (&-> arg0 quad 0) vf6)
  ;       (set! (-> arg0 data 3) (the-as float a2-1))
  ;       )
  ;     )
  ;   arg0
  ;   )
  )

(defun matrix->quaternion ((arg0 quaternion) (arg1 matrix))
  "Convert a rotation matrix to a quaternion."
  (let ((f0-2 (+ (-> arg1 rvec x) (-> arg1 uvec y) (-> arg1 fvec z))))
    (cond
      ((< 0.0 f0-2)
       (let ((f0-4 (sqrtf (+ 1.0 f0-2))))
         (set! (-> arg0 w) (* 0.5 f0-4))
         (let ((f0-5 (/ 0.5 f0-4)))
           (set! (-> arg0 x) (* f0-5 (- (-> arg1 uvec z) (-> arg1 fvec y))))
           (set! (-> arg0 y) (* f0-5 (- (-> arg1 fvec x) (-> arg1 rvec z))))
           (set! (-> arg0 z) (* f0-5 (- (-> arg1 rvec y) (-> arg1 uvec x))))
           )
         )
       )
      (else
        (let ((a2-0 0)
              (a3-0 1)
              (v1-4 2)
              )
          (when (< (-> arg1 rvec x) (-> arg1 uvec y))
            (set! a2-0 1)
            (set! a3-0 2)
            (set! v1-4 0)
            )
          (when (< (-> (the-as (pointer float) (+ (+ (* a2-0 4) (* a2-0 16)) (the-as int arg1)))) (-> arg1 fvec z))
            (set! a2-0 2)
            (set! a3-0 0)
            (set! v1-4 1)
            )
          (let ((f0-12
                  (sqrtf
                    (+ (- 1.0
                          (+ (-> (the-as (pointer float) (+ (+ (* a3-0 4) (* a3-0 16)) (the-as int arg1))))
                             (-> (the-as (pointer float) (+ (+ (* v1-4 4) (* v1-4 16)) (the-as int arg1))))
                             )
                          )
                       (-> (the-as (pointer float) (+ (+ (* a2-0 4) (* a2-0 16)) (the-as int arg1))))
                       )
                    )
                  )
                )
            (set! (-> arg0 data a2-0) (* 0.5 f0-12))
            (if (!= f0-12 0.0)
                (set! f0-12 (/ 0.5 f0-12))
                )
            (set! (-> arg0 w)
                  (* (- (-> (the-as (pointer float) (+ (+ (* v1-4 4) (* a3-0 16)) (the-as int arg1))))
                        (-> (the-as (pointer float) (+ (+ (* a3-0 4) (* v1-4 16)) (the-as int arg1))))
                        )
                     f0-12
                     )
                  )
            (set! (-> arg0 data a3-0)
                  (* (+ (-> (the-as (pointer float) (+ (+ (* a3-0 4) (* a2-0 16)) (the-as int arg1))))
                        (-> (the-as (pointer float) (+ (+ (* a2-0 4) (* a3-0 16)) (the-as int arg1))))
                        )
                     f0-12
                     )
                  )
            (set! (-> arg0 data v1-4)
                  (* (+ (-> (the-as (pointer float) (+ (+ (* v1-4 4) (* a2-0 16)) (the-as int arg1))))
                        (-> (the-as (pointer float) (+ (+ (* a2-0 4) (* v1-4 16)) (the-as int arg1))))
                        )
                     f0-12
                     )
                  )
            )
          )
        )
      )
    )
  arg0
  )

(defun matrix-with-scale->quaternion ((arg0 quaternion) (arg1 matrix))
  "Convert a matrix with a rotation and scale into a quaternion (just the rotation)."
  (local-vars (a1-4 float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         )
    (let ((v1-0 (new 'stack-no-clear 'matrix)))
      (set! (-> v1-0 rvec quad) (the-as uint128 0))
      (set! (-> v1-0 uvec quad) (the-as uint128 0))
      (set! (-> v1-0 fvec quad) (the-as uint128 0))
      (set! (-> v1-0 trans quad) (the-as uint128 0))
      (let* ((f0-1 (vector-dot (-> arg1 rvec) (-> arg1 rvec)))
             (f1-1 (vector-dot (-> arg1 uvec) (-> arg1 uvec)))
             (f2-1 (vector-dot (-> arg1 fvec) (-> arg1 fvec)))
             (f0-3 (/ 1.0 (sqrtf f0-1)))
             (f1-3 (/ 1.0 (sqrtf f1-1)))
             (f2-3 (/ 1.0 (sqrtf f2-1)))
             )
        (.lvf vf1 (&-> arg1 rvec quad))
        (.lvf vf2 (&-> arg1 uvec quad))
        (.lvf vf3 (&-> arg1 fvec quad))
        (.lvf vf4 (&-> arg1 trans quad))
        (let ((a1-1 f0-3))
          (.mov vf5 a1-1)
          )
        (let ((a1-2 f1-3))
          (.mov vf6 a1-2)
          )
        (let ((a1-3 f2-3))
          (.mov vf7 a1-3)
          )
        )
      (.mul.x.vf vf1 vf1 vf5)
      (.mul.x.vf vf2 vf2 vf6)
      (.mul.x.vf vf3 vf3 vf7)
      (.svf (&-> v1-0 rvec quad) vf1)
      (.svf (&-> v1-0 uvec quad) vf2)
      (.svf (&-> v1-0 fvec quad) vf3)
      (.svf (&-> v1-0 trans quad) vf4)
      (.mov a1-4 vf4)
      (matrix->quaternion arg0 v1-0)
      )
    )
  )

(defun quaternion-vector-len ((arg0 quaternion))
  "Assuming quaternion is normalized, get the length of the xyz part."
  (let ((f0-0 1.0)
        (f1-0 (-> arg0 w))
        )
    (sqrtf (- f0-0 (* f1-0 f1-0)))
    )
  )

(defun quaternion-log! ((arg0 quaternion) (arg1 quaternion))
  "Take the log of a quaternion. Unused."
  (cond
    ((= (-> arg1 w) 0.0)
     (set! (-> arg0 x) (* 1.5707963 (-> arg1 x)))
     (set! (-> arg0 y) (* 1.5707963 (-> arg1 y)))
     (set! (-> arg0 z) (* 1.5707963 (-> arg1 z)))
     )
    (else
      (let* ((f30-0 (quaternion-vector-len arg1))
             (f0-9 (/ (atan2-rad (-> arg1 w) f30-0) f30-0))
             )
        (set! (-> arg0 x) (* (-> arg1 x) f0-9))
        (set! (-> arg0 y) (* (-> arg1 y) f0-9))
        (set! (-> arg0 z) (* (-> arg1 z) f0-9))
        )
      )
    )
  arg0
  )

(defun quaternion-exp! ((arg0 quaternion) (arg1 quaternion))
  "Quaternion exponentiation. Unused."
  (let ((f30-0 (vector-length (the-as vector arg1))))
    (cond
      ((= f30-0 0.0)
       (set! (-> arg0 x) 0.0)
       (set! (-> arg0 y) 0.0)
       (set! (-> arg0 z) 0.0)
       (set! (-> arg0 w) 1.0)
       )
      (else
        (let ((s5-0 (new 'stack-no-clear 'vector)))
          (sincos-rad! s5-0 f30-0)
          (let ((f0-6 (/ (-> s5-0 x) f30-0)))
            (set! (-> arg0 x) (* (-> arg1 x) f0-6))
            (set! (-> arg0 y) (* (-> arg1 y) f0-6))
            (set! (-> arg0 z) (* (-> arg1 z) f0-6))
            )
          (set! (-> arg0 w) (-> s5-0 y))
          )
        )
      )
    )
  arg0
  )

(defun quaternion-slerp! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float))
  "Real quaternion slerp. Spherical-linear interpolation is a nice way to interpolate
   between quaternions."
  (local-vars (v1-15 float))
  (rlet ((acc :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (let ((f0-0 (quaternion-dot arg1 arg2))
          (f30-0 1.0)
          )
      (when (< f0-0 0.0)
        (set! f0-0 (- f0-0))
        (set! f30-0 -1.0)
        )
      (cond
        ((< (- 1.0 f0-0) 0.0001)
         (let ((v1-7 (- 1.0 arg3)))
           (.mov vf1 v1-7)
           )
         (let ((v1-8 (* arg3 f30-0)))
           (.mov vf2 v1-8)
           )
         (.lvf vf3 (&-> arg1 quad))
         (.lvf vf4 (&-> arg2 quad))
         (.mul.x.vf acc vf3 vf1)
         (.add.mul.x.vf vf3 vf4 vf2 acc)
         (.svf (&-> arg0 quad) vf3)
         (quaternion-normalize! arg0)
         )
        (else
          (let* ((f1-4 1.0)
                 (f2-1 f0-0)
                 (f1-6 (sqrtf (- f1-4 (* f2-1 f2-1))))
                 (f0-6 (/ (- f1-6 f0-0) (+ f1-6 f0-0)))
                 (f28-0 (/ 1.0 f1-6))
                 )
            (let ((f0-7 (atan-series-rad f0-6))
                  (s2-0 (new 'stack-no-clear 'vector))
                  )
              (set! (-> s2-0 x) (* (- 1.0 arg3) f0-7))
              (set! (-> s2-0 y) (* arg3 f0-7 f30-0))
              (vector-sin-rad! s2-0 s2-0)
              (.lvf vf1 (&-> s2-0 quad))
              )
            (let ((v1-14 f28-0))
              (.mov vf2 v1-14)
              )
            )
          (.mul.x.vf vf1 vf1 vf2)
          (.lvf vf3 (&-> arg1 quad))
          (.lvf vf4 (&-> arg2 quad))
          (.mul.x.vf acc vf3 vf1)
          (.add.mul.y.vf vf3 vf4 vf1 acc)
          (.svf (&-> arg0 quad) vf3)
          (.mov v1-15 vf3)
          )
        )
      )
    arg0
    )
  )

(defun quaternion-pseudo-slerp! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float))
  "This is a bad interpolation between quaternions. It lerps then normalizes.
   It will behave extremely poorly for 180 rotations.
   It is unused."
  (rlet ((acc :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (let ((f1-0 (quaternion-dot arg1 arg2))
          (f0-0 1.0)
          )
      (when (< f1-0 0.0)
        (- f1-0)
        (set! f0-0 -1.0)
        )
      (let ((v1-5 (- 1.0 arg3)))
        (.mov vf1 v1-5)
        )
      (let ((v1-6 (* arg3 f0-0)))
        (.mov vf2 v1-6)
        )
      )
    (.lvf vf3 (&-> arg1 quad))
    (.lvf vf4 (&-> arg2 quad))
    (.mul.x.vf acc vf3 vf1)
    (.add.mul.x.vf vf3 vf4 vf2 acc)
    (.svf (&-> arg0 quad) vf3)
    (quaternion-normalize! arg0)
    arg0
    )
  )

(defun quaternion-pseudo-seek ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float))
  "Seek one quaternion toward another. Not using real slerp, so this is only good if the quaternions
   are pretty similar."
  (let ((s3-0 (new 'stack-no-clear 'quaternion)))
    (let ((s5-0 (new 'stack-no-clear 'quaternion)))
      (quaternion-copy! s3-0 arg2)
      (if (< (quaternion-dot s3-0 arg1) 0.0)
          (quaternion-negate! s3-0 s3-0)
          )
      (quaternion-! s5-0 s3-0 arg1)
      (let ((f0-1 (quaternion-norm2 s5-0))
            (f1-1 arg3)
            )
        (if (< (* f1-1 f1-1) f0-1)
            (quaternion-float*! s5-0 s5-0 (/ arg3 (sqrtf f0-1)))
            )
        )
      (quaternion+! s3-0 arg1 s5-0)
      )
    (quaternion-normalize! s3-0)
    (quaternion-copy! arg0 s3-0)
    )
  )

(defun quaternion-smooth-seek! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float))
  "Another hacky rotation interpolation."
  (let ((gp-0 (new 'stack-no-clear 'inline-array 'quaternion 2)))
    (quaternion-copy! (-> gp-0 0) arg2)
    (if (< (quaternion-dot (-> gp-0 0) arg1) 0.0)
        (quaternion-negate! (-> gp-0 0) (-> gp-0 0))
        )
    (quaternion-! (-> gp-0 1) (-> gp-0 0) arg1)
    (quaternion-float*! (-> gp-0 1) (-> gp-0 1) (fmin 1.0 arg3))
    (quaternion+! (-> gp-0 0) arg1 (-> gp-0 1))
    (quaternion-normalize! (-> gp-0 0))
    (quaternion-copy! arg0 (-> gp-0 0))
    )
  )

(defun quaternion-zxy! ((arg0 quaternion) (arg1 vector))
  "Make a quaternion from a sequence of z, x, y axis rotations."
  (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)
         (vf7 :class vf)
         )
    (init-vf0-vector)
    (let ((s4-0 (new 'stack-no-clear 'vector))
          (gp-0 (new 'stack-no-clear 'vector))
          (s5-0 (new 'stack-no-clear 'vector))
          )
      (vector-rad<-vector-deg/2! s4-0 arg1)
      (vector-sincos-rad! gp-0 s5-0 s4-0)
      (.lvf vf1 (&-> gp-0 quad))
      (.lvf vf2 (&-> s5-0 quad))
      )
    (.mul.x.vf vf4 vf0 vf1 :mask #b1000)
    (.add.vf vf4 vf0 vf2 :mask #b111)
    (.sub.vf vf4 vf0 vf4 :mask #b110)
    (.add.vf vf3 vf0 vf1 :mask #b111)
    (.mul.x.vf vf3 vf0 vf2 :mask #b1000)
    (.outer.product.a.vf acc vf1 vf1)
    (.outer.product.b.vf vf6 vf0 vf0 acc)
    (.outer.product.a.vf acc vf2 vf2)
    (.outer.product.b.vf vf5 vf0 vf0 acc)
    (.mul.x.vf vf6 vf0 vf6 :mask #b1000)
    (.mul.x.vf vf5 vf0 vf5 :mask #b1000)
    (.mul.vf acc vf6 vf4)
    (.add.mul.vf vf7 vf5 vf3 acc)
    (.svf (&-> arg0 quad) vf7)
    arg0
    )
  )

(defun vector-x-quaternion! ((arg0 vector) (arg1 quaternion))
  "Get the first row of the rotation matrix for this quaternion."
  (let ((s5-0 (new 'stack-no-clear 'matrix)))
    (set! (-> s5-0 rvec quad) (the-as uint128 0))
    (set! (-> s5-0 uvec quad) (the-as uint128 0))
    (set! (-> s5-0 fvec quad) (the-as uint128 0))
    (set! (-> s5-0 trans quad) (the-as uint128 0))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> s5-0 rvec quad))
    )
  arg0
  )

(defun vector-y-quaternion! ((arg0 vector) (arg1 quaternion))
  "Get the second row of the rotation matrix for this quaternion."
  (let ((s5-0 (new 'stack-no-clear 'matrix)))
    (set! (-> s5-0 rvec quad) (the-as uint128 0))
    (set! (-> s5-0 uvec quad) (the-as uint128 0))
    (set! (-> s5-0 fvec quad) (the-as uint128 0))
    (set! (-> s5-0 trans quad) (the-as uint128 0))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> s5-0 uvec quad))
    )
  arg0
  )

(defun vector-z-quaternion! ((arg0 vector) (arg1 quaternion))
  "Get the third row of the rotation matrix for this quaternion."
  (let ((s5-0 (new 'stack-no-clear 'matrix)))
    (set! (-> s5-0 rvec quad) (the-as uint128 0))
    (set! (-> s5-0 uvec quad) (the-as uint128 0))
    (set! (-> s5-0 fvec quad) (the-as uint128 0))
    (set! (-> s5-0 trans quad) (the-as uint128 0))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> s5-0 fvec quad))
    )
  arg0
  )

(defun quaternion-x-angle ((arg0 quaternion))
  "Get the x rotation angle. Not very efficient."
  (let ((v1-1 (vector-y-quaternion! (new 'stack-no-clear 'vector) arg0)))
    (atan (-> v1-1 z) (-> v1-1 y))
    )
  )

(defun quaternion-y-angle ((arg0 quaternion))
  "Get the y rotation angle. Not very efficient."
  (let ((v1-1 (vector-z-quaternion! (new 'stack-no-clear 'vector) arg0)))
    (atan (-> v1-1 x) (-> v1-1 z))
    )
  )

(defun quaternion-z-angle ((arg0 quaternion))
  "Get the z rotation angle. Not very efficient."
  (let ((v1-1 (vector-y-quaternion! (new 'stack-no-clear 'vector) arg0)))
    (atan (-> v1-1 x) (-> v1-1 y))
    )
  )

(defun quaternion-vector-y-angle ((arg0 quaternion) (arg1 vector))
  "Not sure. Angle between quaternion and axis, projected in xz plane?"
  (let ((f30-0 (quaternion-y-angle arg0))
        (f0-2 (atan (-> arg1 x) (-> arg1 z)))
        )
    (deg-diff f30-0 f0-2)
    )
  )

(defun quaternion-rotate-local-x! ((arg0 quaternion) (arg1 quaternion) (arg2 float))
  "Rotate existing quaternion along x axis."
  (let ((a2-1 (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :x 1.0 :w 1.0) arg2)))
    (quaternion-normalize! (quaternion*! arg0 arg1 a2-1))
    )
  )

(defun quaternion-rotate-local-y! ((arg0 quaternion) (arg1 quaternion) (arg2 float))
  "Rotate existing quaternion along y axis."
  (let ((a2-1 (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :y 1.0 :w 1.0) arg2)))
    (quaternion-normalize! (quaternion*! arg0 arg1 a2-1))
    )
  )

(defun quaternion-rotate-local-z! ((arg0 quaternion) (arg1 quaternion) (arg2 float))
  "Rotate existing quaternion along z axis."
  (let ((a2-1 (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :z 1.0 :w 1.0) arg2)))
    (quaternion-normalize! (quaternion*! arg0 arg1 a2-1))
    )
  )

(defun quaternion-rotate-y! ((arg0 quaternion) (arg1 quaternion) (arg2 float))
  "Rotate existing quaternion along y axis (right multiply)."
  (let ((a1-2 (quaternion-vector-angle! (new-stack-quaternion0) (new 'static 'vector :y 1.0 :w 1.0) arg2)))
    (quaternion-normalize! (quaternion*! arg0 a1-2 arg1))
    )
  )

(defun quaternion-rotate-x! ((arg0 quaternion) (arg1 quaternion) (arg2 float))
  "Rotate existing quaternion along x axis (right multiply)."
  (let ((a1-3 (quaternion-vector-angle! (new-stack-quaternion0) (vector-x-quaternion! (new-stack-vector0) arg1) arg2))
        )
    (quaternion-normalize! (quaternion*! arg0 a1-3 arg1))
    )
  )

(defun quaternion-rotate-z! ((arg0 quaternion) (arg1 quaternion) (arg2 float))
  "Rotate existing quaternion along z axis (right multiply)."
  (let ((a1-3 (quaternion-vector-angle! (new-stack-quaternion0) (vector-z-quaternion! (new-stack-vector0) arg1) arg2))
        )
    (quaternion-normalize! (quaternion*! arg0 a1-3 arg1))
    )
  )

(defun quaternion-delta-y ((arg0 quaternion) (arg1 quaternion))
  "Difference in yaw between two quaternions."
  (acos (vector-dot
          (vector-z-quaternion! (new 'stack-no-clear 'vector) arg0)
          (vector-z-quaternion! (new 'stack-no-clear 'vector) arg1)
          )
        )
  )

(defun quaternion-rotate-y-to-vector! ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float))
  (let ((s5-0 (new 'stack-no-clear 'quaternion)))
    (let ((t9-0 vector-xz-normalize!)
          (a0-1 (new 'stack-no-clear 'vector))
          )
      (set! (-> a0-1 x) (-> arg2 x))
      (set! (-> a0-1 y) 0.0)
      (set! (-> a0-1 z) (-> arg2 z))
      (set! (-> a0-1 w) 1.0)
      (let ((s0-0 (t9-0 a0-1 1.0)))
        (quaternion-from-two-vectors-max-angle!
          s5-0
          (vector-z-quaternion! (new 'stack-no-clear 'vector) arg1)
          s0-0
          arg3
          )
        )
      )
    (quaternion-normalize! (quaternion*! arg0 s5-0 arg1))
    )
  )

(defun vector-rotate-around-axis! ((arg0 vector) (arg1 quaternion) (arg2 float) (arg3 vector))
  "Rotate along y so z-axis points to match another. Use arg3 as the max rotation amount."
  (let ((a2-1 (quaternion-vector-angle! (new 'stack-no-clear 'quaternion) arg3 arg2)))
    (vector-orient-by-quat! arg0 (the-as vector arg1) a2-1)
    )
  )

(defun vector-rotate-x! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate vector along x axis."
  (vector-rotate-around-axis! arg0 (the-as quaternion arg1) arg2 *x-vector*)
  )

(defun vector-rotate-y! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate vector along y axis."
  (vector-rotate-around-axis! arg0 (the-as quaternion arg1) arg2 *y-vector*)
  )

(defun vector-rotate-z! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate vector along z axis."
  (vector-rotate-around-axis! arg0 (the-as quaternion arg1) arg2 *z-vector*)
  )

(defun vector-y-angle ((arg0 vector))
  "Get the yaw angle of a vector."
  (atan (-> arg0 x) (-> arg0 z))
  )

(defun vector-x-angle ((arg0 vector))
  "Get the pitch angle of a vector."
  (atan (-> arg0 y) (vector-xz-length arg0))
  )

(defun quaternion<-rotate-y-vector ((arg0 quaternion) (arg1 vector))
  "Create a quaternion representing only the yaw of the given vector."
  (quaternion-vector-angle! arg0 (new 'static 'vector :y 1.0 :w 1.0) (vector-y-angle arg1))
  )

(defun-debug quaternion-validate ((arg0 quaternion))
  "Verify that a quaternion is valid, print an error if the length is off by more than 1%.."
  (with-pp
    (let ((f0-0 (quaternion-norm arg0)))
      (when (or (< 1.01 f0-0) (< f0-0 0.99))
        (format #t "WARNING: bad quaternion (magnitude ~F) process is " f0-0)
        (if (and pp (type? pp process-tree))
            (format #t "~A~%" (-> pp name))
            (format #t "#f~%")
            )
        )
      )
    0
    (none)
    )
  )

(defun quaternion-xz-angle ((arg0 quaternion))
  "Yet another function to compute the yaw of a quaternion. This is a particularly inefficient version."
  (let ((gp-0 (new 'stack-no-clear 'matrix))
        (s5-0 (new 'stack-no-clear 'vector))
        )
    (quaternion->matrix gp-0 arg0)
    (set-vector! s5-0 0.0 0.0 1.0 1.0)
    (vector-matrix*! s5-0 s5-0 gp-0)
    (vector-y-angle s5-0)
    )
  )