jak-project/test/decompiler/reference/jak2/engine/math/quaternion_REF.gc

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

;; definition for method 3 of type quaternion
(defmethod inspect ((this quaternion))
  (format #t "[~8x] quaternion~%" this)
  (format #t "~T[~F] [~F] [~F] [~F]~%" (-> this x) (-> this y) (-> this z) (-> this w))
  (let ((f0-5 (/ 1.0 (sqrtf (+ (* (-> this x) (-> this x)) (* (-> this y) (-> this y)) (* (-> this z) (-> this z))))))
        )
    (format #t "~Taxis: ~F ~F ~F" (* f0-5 (-> this x)) (* f0-5 (-> this y)) (* f0-5 (-> this z)))
    )
  (let ((f0-9 (* 2.0 (acos (-> this w)))))
    (format #t "~T~Tangle: (deg ~R)~%" f0-9)
    )
  this
  )

;; definition for function quaternion-axis-angle!
(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
  )

;; definition for function quaternion-vector-angle!
(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
  )

;; definition for function vector-angle<-quaternion!
(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
  )

;; definition for function quaternion-look-at!
;; INFO: Used lq/sq
(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! (the-as vector (-> s5-0 vector)) arg2 arg1)
    (vector-cross! (-> s5-0 vector 1) arg1 (the-as vector (-> s5-0 vector)))
    (set! (-> (the-as vector (-> s5-0 vector 2)) quad) (-> arg1 quad))
    (quaternion-normalize! (matrix->quaternion arg0 s5-0))
    )
  )

;; definition for function quaternion-zero!
;; INFO: Used lq/sq
(defun quaternion-zero! ((arg0 quaternion))
  "Set quaternion to all 0's."
  (set! (-> arg0 quad) (the-as uint128 0))
  arg0
  )

;; definition for function quaternion-identity!
;; INFO: Used lq/sq
(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
  )

;; definition for function quaternion-i!
;; INFO: Used lq/sq
(defun quaternion-i! ((arg0 quaternion))
  "Create unit i quaternion."
  (set! (-> arg0 quad) (the-as uint128 0))
  (set! (-> arg0 x) 1.0)
  arg0
  )

;; definition for function quaternion-j!
;; INFO: Used lq/sq
(defun quaternion-j! ((arg0 quaternion))
  "Create unit j quaternion."
  (set! (-> arg0 quad) (the-as uint128 0))
  (set! (-> arg0 y) 1.0)
  arg0
  )

;; definition for function quaternion-k!
;; INFO: Used lq/sq
(defun quaternion-k! ((arg0 quaternion))
  "Create unit k quaternion."
  (set! (-> arg0 quad) (the-as uint128 0))
  (set! (-> arg0 z) 1.0)
  arg0
  )

;; definition for function quaternion-copy!
;; INFO: Used lq/sq
(defun quaternion-copy! ((arg0 quaternion) (arg1 quaternion))
  "Set arg0 = arg1."
  (set! (-> arg0 quad) (-> arg1 quad))
  arg0
  )

;; definition for function quaternion-set!
(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
  )

;; definition for function quaternion+!
(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
    )
  )

;; definition for function quaternion-!
(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
    )
  )

;; definition for function quaternion-negate!
;; 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))
    (.sub.vf vf2 vf2 vf2)
    (.sub.vf vf1 vf2 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0
    )
  )

;; definition for function quaternion-conjugate!
;; 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))
    (.sub.vf vf2 vf2 vf2)
    (.sub.vf vf2 vf2 vf1 :mask #b111)
    (.add.vf vf2 vf2 vf1 :mask #b1000)
    (.svf (&-> arg0 quad) vf2)
    arg0
    )
  )

;; definition for function quaternion-float*!
(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
    )
  )

;; definition for function quaternion-float/!
(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
  )

;; definition for function quaternion-norm2
(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
    )
  )

;; definition for function quaternion-norm
(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)
    )
  )

;; definition for function quaternion-normalize!
(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)
    (.wait.vf)
    (.mul.vf vf2 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> arg0 quad) vf2)
    arg0
    )
  )

;; definition for function quaternion-inverse!
;; 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)
    (.sub.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
    )
  )

;; definition for function quaternion-dot
(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
    )
  )

;; definition for function quaternion*!
(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
    )
  )

;; definition for function quaternion-right-mult-matrix!
(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 vector 0 x) f0-0)
    (set! (-> arg0 vector 0 y) f1-0)
    (set! (-> arg0 vector 0 z) (- f2-0))
    (set! (-> arg0 vector 0 w) f3-0)
    (set! (-> arg0 vector 1 x) (- f1-0))
    (set! (-> arg0 vector 1 y) f0-0)
    (set! (-> arg0 vector 1 z) f3-0)
    (set! (-> arg0 vector 1 w) f2-0)
    (set! (-> arg0 vector 2 x) f2-0)
    (set! (-> arg0 vector 2 y) (- f3-0))
    (set! (-> arg0 vector 2 z) f0-0)
    (set! (-> arg0 vector 2 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
  )

;; definition for function quaternion-left-mult-matrix!
(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 vector 0 x) f2-0)
      (set! (-> arg0 vector 0 y) f3-0)
      (set! (-> arg0 vector 0 z) (- f0-0))
      (set! (-> arg0 vector 0 w) f1-0)
      (set! (-> arg0 vector 1 x) f1-0)
      (set! (-> arg0 vector 1 y) f0-0)
      (set! (-> arg0 vector 1 z) f3-0)
      (set! (-> arg0 vector 1 w) (- f3-0))
      (set! (-> arg0 vector 2 x) f0-0)
      (set! (-> arg0 vector 2 y) (- f1-0))
      (set! (-> arg0 vector 2 z) f2-0)
      (set! (-> arg0 vector 2 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
  )

;; definition for function quaternion->matrix
(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 quad 0) vf2)
    (.svf (&-> arg0 quad 1) vf3)
    (.svf (&-> arg0 quad 2) vf4)
    arg0
    )
  )

;; definition for function quaternion->matrix-2
;; ERROR: function was not converted to expressions. Cannot decompile.

;; definition for function matrix->quaternion
(defun matrix->quaternion ((arg0 quaternion) (arg1 matrix))
  "Convert a rotation matrix to a quaternion."
  (let ((f0-2 (+ (-> arg1 vector 0 x) (-> arg1 vector 1 y) (-> arg1 vector 2 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 vector 1 z) (-> arg1 vector 2 y))))
           (set! (-> arg0 y) (* f0-5 (- (-> arg1 vector 2 x) (-> arg1 vector 0 z))))
           (set! (-> arg0 z) (* f0-5 (- (-> arg1 vector 0 y) (-> arg1 vector 1 x))))
           )
         )
       )
      (else
        (let ((a2-0 0)
              (a3-0 1)
              (v1-4 2)
              )
          (when (< (-> arg1 vector 0 x) (-> arg1 vector 1 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 vector 2 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
  )

;; definition for function matrix-with-scale->quaternion
;; INFO: Used lq/sq
(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-matrix0)))
      (let* ((f0-1 (vector-dot (the-as vector (-> arg1 vector)) (the-as vector (-> arg1 vector))))
             (f1-1 (vector-dot (-> arg1 vector 1) (-> arg1 vector 1)))
             (f2-1 (vector-dot (-> arg1 vector 2) (-> arg1 vector 2)))
             (f0-3 (/ 1.0 (sqrtf f0-1)))
             (f1-3 (/ 1.0 (sqrtf f1-1)))
             (f2-3 (/ 1.0 (sqrtf f2-1)))
             )
        (.lvf vf1 (&-> arg1 quad 0))
        (.lvf vf2 (&-> arg1 quad 1))
        (.lvf vf3 (&-> arg1 quad 2))
        (.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 quad 0) vf1)
      (.svf (&-> v1-0 quad 1) vf2)
      (.svf (&-> v1-0 quad 2) vf3)
      (.svf (&-> v1-0 trans quad) vf4)
      (.mov a1-4 vf4)
      (matrix->quaternion arg0 v1-0)
      )
    )
  )

;; definition for function quaternion-vector-len
(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)))
    )
  )

;; definition for function quaternion-log!
(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
  )

;; definition for function quaternion-exp!
(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
  )

;; definition for function quaternion-slerp!
(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
    )
  )

;; definition for function quaternion-pseudo-slerp!
(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
    )
  )

;; definition for function quaternion-pseudo-seek
(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)
    )
  )

;; definition for function quaternion-smooth-seek!
(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))
    )
  )

;; definition for function quaternion-zxy!
(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
    )
  )

;; definition for function vector-x-quaternion!
;; INFO: Used lq/sq
(defun vector-x-quaternion! ((arg0 vector) (arg1 quaternion))
  "Get the first row of the rotation matrix for this quaternion."
  (let ((s5-0 (new-stack-matrix0)))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> (the-as (pointer uint128) (-> s5-0 vector)) 0))
    )
  arg0
  )

;; definition for function vector-y-quaternion!
;; INFO: Used lq/sq
(defun vector-y-quaternion! ((arg0 vector) (arg1 quaternion))
  "Get the second row of the rotation matrix for this quaternion."
  (let ((s5-0 (new-stack-matrix0)))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> (the-as (pointer uint128) (-> s5-0 vector 1)) 0))
    )
  arg0
  )

;; definition for function vector-z-quaternion!
;; INFO: Used lq/sq
(defun vector-z-quaternion! ((arg0 vector) (arg1 quaternion))
  "Get the third row of the rotation matrix for this quaternion."
  (let ((s5-0 (new-stack-matrix0)))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> (the-as (pointer uint128) (-> s5-0 vector 2)) 0))
    )
  arg0
  )

;; definition for function quaternion-x-angle
(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))
    )
  )

;; definition for function quaternion-y-angle
(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))
    )
  )

;; definition for function quaternion-z-angle
(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))
    )
  )

;; definition for function quaternion-vector-y-angle
(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)
    )
  )

;; definition for function quaternion-rotate-local-x!
;; INFO: Used lq/sq
(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))
    )
  )

;; definition for function quaternion-rotate-local-y!
;; INFO: Used lq/sq
(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))
    )
  )

;; definition for function quaternion-rotate-local-z!
;; INFO: Used lq/sq
(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))
    )
  )

;; definition for function quaternion-rotate-y!
;; INFO: Used lq/sq
(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))
    )
  )

;; definition for function quaternion-rotate-x!
;; INFO: Used lq/sq
(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))
    )
  )

;; definition for function quaternion-rotate-z!
;; INFO: Used lq/sq
(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))
    )
  )

;; definition for function quaternion-delta-y
(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)
          )
        )
  )

;; definition for function quaternion-rotate-y-to-vector!
(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))
    )
  )

;; definition for function vector-rotate-x!
(defun vector-rotate-x! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate vector along x axis."
  (let ((a1-2 (quaternion-vector-angle! (new 'stack-no-clear 'quaternion) (new 'static 'vector :x 1.0 :w 1.0) arg2))
        (s4-0 (new 'stack-no-clear 'matrix))
        )
    (quaternion->matrix s4-0 a1-2)
    (vector-matrix*! arg0 arg1 s4-0)
    )
  )

;; definition for function vector-rotate-y!
(defun vector-rotate-y! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate vector along y axis."
  (let ((a1-2 (quaternion-vector-angle! (new 'stack-no-clear 'quaternion) (new 'static 'vector :y 1.0 :w 1.0) arg2))
        (s4-0 (new 'stack-no-clear 'matrix))
        )
    (quaternion->matrix s4-0 a1-2)
    (vector-matrix*! arg0 arg1 s4-0)
    )
  )

;; definition for function vector-rotate-z!
(defun vector-rotate-z! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate vector along z axis."
  (let ((a1-2 (quaternion-vector-angle! (new 'stack-no-clear 'quaternion) (new 'static 'vector :z 1.0 :w 1.0) arg2))
        (s4-0 (new 'stack-no-clear 'matrix))
        )
    (quaternion->matrix s4-0 a1-2)
    (vector-matrix*! arg0 arg1 s4-0)
    )
  )

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

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

;; definition for function quaternion<-rotate-y-vector
(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))
  )

;; definition (debug) for function quaternion-validate
;; WARN: Return type mismatch int vs none.
(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)
    )
  )

;; definition for function quaternion-xz-angle
(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)
    )
  )