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

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

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

(defmethod inspect quaternion ((obj quaternion))
  "Print a quaternion. Prints the values and axis-angle"
  (format #t "[~8x] quaternion~%" obj)
  (format #t "~T[~F] [~F] [~F] [~F]~%"
          (-> obj x)
          (-> obj y)
          (-> obj z)
          (-> obj w)
          )
  (let ((f0-5 (/ 1.0 (sqrtf
                      (+ (* (-> obj x) (-> obj x))
                         (* (-> obj y) (-> obj y))
                         (* (-> obj z) (-> obj z))
                         )
                      )
                 )
          )
        )
    (format #t "~Taxis: ~F ~F ~F"
            (* f0-5 (-> obj x))
            (* f0-5 (-> obj y))
            (* f0-5 (-> obj z))
            )
    )
  (let ((f0-9 (* 2.0 (acos (-> obj w)))))
    (format #t "~T~Tangle: (deg ~R)~%" f0-9)
    )
  obj
  )

(defun quaternion-axis-angle! ((quat quaternion) (x float) (y float) (z float) (angle float))
  "Construct a quaternion from an axis and angle. The axis should be normalized."
  (let* ((f28-0 (* 0.5 angle))
         (f30-0 (sin f28-0))
         (f0-1 (cos f28-0))
         )
    (set! (-> quat x) (* x f30-0))
    (set! (-> quat y) (* y f30-0))
    (set! (-> quat z) (* z f30-0))
    (set! (-> quat w) f0-1)
    )
  quat
  )

(defun quaternion-vector-angle! ((quat quaternion) (axis vector) (angle float))
  "Construct a quaternion from an axis and angle. The axis should be normalized."
  (let* ((f28-0 (* 0.5 angle))
         (f30-0 (sin f28-0))
         (f0-1 (cos f28-0))
         )
    (set! (-> quat x) (* (-> axis data 0) f30-0))
    (set! (-> quat y) (* (-> axis data 1) f30-0))
    (set! (-> quat z) (* (-> axis data 2) f30-0))
    (set! (-> quat w) f0-1)
    )
  quat
  )

(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 data 0) (* (-> arg1 x) f30-0))
    (set! (-> arg0 data 1) (* (-> arg1 y) f30-0))
    (set! (-> arg0 data 2) (* (-> arg1 z) f30-0))
    (set! (-> arg0 data 3) f0-3)
    )
  arg0
  )

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

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

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

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

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

(defun quaternion-copy! ((arg0 quaternion) (arg1 quaternion))
  "Set arg0 = arg1"
  (set! (-> arg0 vec quad) (-> arg1 vec 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 vec quad))
    (.lvf vf2 (&-> arg2 vec quad))
    (.add.vf vf1 vf1 vf2)
    (.svf (&-> arg0 vec 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 vec quad))
    (.lvf vf2 (&-> arg2 vec quad))
    (.sub.vf vf1 vf1 vf2)
    (.svf (&-> arg0 vec quad) vf1)
    arg0
    )
  )

(defun quaternion-negate! ((arg0 quaternion) (arg1 quaternion))
  "Set arg0 = -arg1."
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (.lvf vf1 (&-> arg1 vec quad))
    ;;(.sub.vf vf2 vf2 vf2)
    (.xor.vf vf2 vf2 vf2)
    (.sub.vf vf1 vf2 vf1)
    (.svf (&-> arg0 vec quad) vf1)
    arg0
    )
  )

(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 vec quad))
    ;;(.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 vec 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 vec quad))
    (.mov vf2 arg2)
    (.mul.x.vf vf1 vf1 vf2)
    (.svf (&-> arg0 vec 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 vec 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 vec 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 vec 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 vec quad) vf2)
    arg0
    )
  )

(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 vec 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 vec 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 vec quad))
    (.lvf vf2 (&-> arg1 vec 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 vec quad))
    (.lvf vf2 (&-> arg2 vec 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 vec 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 data 0) f0-0)
    (set! (-> arg0 data 1) f1-0)
    (set! (-> arg0 data 2) (- f2-0))
    (set! (-> arg0 data 3) f3-0)
    (set! (-> arg0 data 4) (- f1-0))
    (set! (-> arg0 data 5) f0-0)
    (set! (-> arg0 data 6) f3-0)
    (set! (-> arg0 data 7) f2-0)
    (set! (-> arg0 data 8) f2-0)
    (set! (-> arg0 data 9) (- f3-0))
    (set! (-> arg0 data 10) f0-0)
    (set! (-> arg0 data 11) f1-0)
    (set! (-> arg0 data 12) (- f3-0))
    (set! (-> arg0 data 13) (- f2-0))
    (set! (-> arg0 data 14) (- f1-0))
    (set! (-> arg0 data 15) 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 data 0) f2-0)
      (set! (-> arg0 data 1) f3-0)
      (set! (-> arg0 data 2) (- f0-0))
      (set! (-> arg0 data 3) f1-0)
      (set! (-> arg0 data 4) f1-0)
      (set! (-> arg0 data 5) f0-0)
      (set! (-> arg0 data 6) f3-0)
      (set! (-> arg0 data 7) (- f3-0))
      (set! (-> arg0 data 8) f0-0)
      (set! (-> arg0 data 9) (- f1-0))
      (set! (-> arg0 data 10) f2-0)
      (set! (-> arg0 data 11) f3-0)
      (set! (-> arg0 data 12) f3-0)
      )
    (set! (-> arg0 data 13) (- f2-0))
    (set! (-> arg0 data 14) (- f1-0))
    (set! (-> arg0 data 15) (- 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 vec 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 vector 3 quad) vf0)
    (.svf (&-> arg0 vector 0 quad) vf2)
    (.svf (&-> arg0 vector 1 quad) vf3)
    (.svf (&-> arg0 vector 2 quad) vf4)
    arg0
    )
  )

(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-1 2)
              )
          (when (< (-> arg1 vector 0 x) (-> arg1 vector 1 y))
            (set! a2-0 1)
            (set! a3-0 2)
            (set! v1-1 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-1 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-1 4) (* v1-1 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-1 4) (* a3-0 16)) (the-as int arg1))))
                        (-> (the-as (pointer float) (+ (+ (* a3-0 4) (* v1-1 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-1)
                  (* (+ (-> (the-as (pointer float) (+ (+ (* v1-1 4) (* a2-0 16)) (the-as int arg1))))
                        (-> (the-as (pointer float) (+ (+ (* a2-0 4) (* v1-1 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)"
  (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)))
      ;; compute norm of rows of rotation matrix (vector-dot does only xyz)
      (let ((f0-2 (vector-dot (-> arg1 vector 0) (-> arg1 vector 0))))
        (let ((f1-3 (vector-dot (-> arg1 vector 1) (-> arg1 vector 1))))
          (let* ((f2-4 (vector-dot (-> arg1 vector 2) (-> arg1 vector 2)))
                 ;; compute scaling factors.
                 (f0-4 (/ 1.0 (sqrtf f0-2)))
                 (f1-5 (/ 1.0 (sqrtf f1-3)))
                 (f2-6 (/ 1.0 (sqrtf f2-4)))
                 )
            ;; load the origin matrix.
            (.lvf vf1 (&-> arg1 vector 0 quad))
            (.lvf vf2 (&-> arg1 vector 1 quad))
            (.lvf vf3 (&-> arg1 vector 2 quad))
            (.lvf vf4 (&-> arg1 vector 3 quad))
            ;; move scaling factors into vector regs
            (let ((a1-1 f0-4))
              (.mov vf5 a1-1)
              )
            (let ((a1-2 f1-5))
              (.mov vf6 a1-2)
              )
            (let ((a1-3 f2-6))
              (.mov vf7 a1-3)
              )
            )
          )
        )
      ;; scale!
      (.mul.x.vf vf1 vf1 vf5)
      (.mul.x.vf vf2 vf2 vf6)
      (.mul.x.vf vf3 vf3 vf7)
      ;; store in temp matrix
      (.svf (&-> v1-0 vector 0 quad) vf1)
      (.svf (&-> v1-0 vector 1 quad) vf2)
      (.svf (&-> v1-0 vector 2 quad) vf3)
      (.svf (&-> v1-0 vector 3 quad) vf4)
      ;; and convert!
      (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))
        )
    ;; sqrt(1 - w^2) = sqrt(x^2 + y^2 + z^2) = length
    (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))
       (let ((f0-10 (* (-> arg1 z) f0-9)))
         (set! (-> arg0 z) f0-10)
         (let ((v1-0 f0-10))
           )
         )
       )
     )
    )
  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! (the-as (pointer float) s5-0) f30-0)
         (let ((f0-6 (/ (-> s5-0 data 0) 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 data 1))
           )
         )
       )
      )
    )
  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-7 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)
        (let ((v1-1 f30-0))
          )
        )
      (cond
        ((< (- 1.0 f0-0) 0.0001)
         (let ((v1-2 (- 1.0 arg3)))
           (.mov vf1 v1-2)
           )
         (let ((v1-3 (* arg3 f30-0)))
           (.mov vf2 v1-3)
           )
         (.lvf vf3 (&-> arg1 vec quad))
         (.lvf vf4 (&-> arg2 vec quad))
         (.mul.x.vf acc vf3 vf1)
         (.add.mul.x.vf vf3 vf4 vf2 acc)
         (.svf (&-> arg0 vec 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 data 0) (* (- 1.0 arg3) f0-7))
             (set! (-> s2-0 data 1) (* (* arg3 f0-7) f30-0))
             (vector-sin-rad! s2-0 s2-0)
             (.lvf vf1 (&-> s2-0 quad))
             )
           (let ((v1-6 f28-0))
             (.mov vf2 v1-6)
             )
           )
         (.mul.x.vf vf1 vf1 vf2)
         (.lvf vf3 (&-> arg1 vec quad))
         (.lvf vf4 (&-> arg2 vec quad))
         (.mul.x.vf acc vf3 vf1)
         (.add.mul.y.vf vf3 vf4 vf1 acc)
         (.svf (&-> arg0 vec quad) vf3)
         (.mov v1-7 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)
        (let ((f0-1 (- f1-0)))
          )
        (set! f0-0 -1.0)
        (let ((v1-1 f0-0))
          )
        )
      (let ((v1-2 (- 1.0 arg3)))
        (.mov vf1 v1-2)
        )
      (let ((v1-3 (* arg3 f0-0)))
        (.mov vf2 v1-3)
        )
      )
    (.lvf vf3 (&-> arg1 vec quad))
    (.lvf vf4 (&-> arg2 vec quad))
    (.mul.x.vf acc vf3 vf1)
    (.add.mul.x.vf vf3 vf4 vf2 acc)
    (.svf (&-> arg0 vec quad) vf3)
    (quaternion-normalize! arg0)
    arg0
    )
  )

(defmacro quaternion-vompula-hack (vf-out vf-in)
  `(let ((in-vec (new 'stack-no-clear 'vector))
        (out-vec (new 'stack-no-clear 'vector)))
    (.svf (&-> in-vec quad) ,vf-in)
    (set! (-> out-vec x) (* (-> in-vec y) (-> in-vec z)))
    (set! (-> out-vec y) (* (-> in-vec z) (-> in-vec x)))
    (set! (-> out-vec z) (* (-> in-vec x) (-> in-vec y)))
    ;;(set! (-> out-vec w) (-> in-vec w))
    (.lvf ,vf-out (&-> out-vec quad))
    )
  )

(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 vec 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-matrix0)))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> (the-as (pointer uint128) (-> s5-0 data)) 0))
    )
  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-matrix0)))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> (the-as (pointer uint128) (&-> s5-0 data 4)) 0))
    )
  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-matrix0)))
    (quaternion->matrix s5-0 arg1)
    (set! (-> arg0 quad) (-> (the-as (pointer uint128) (&-> s5-0 data 8)) 0))
    )
  arg0
  )

(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 data 0) (-> v1-1 data 2))
    )
  )

(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 data 0) (-> arg1 data 2)))
        )
    (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. This has a different implementation
   from the others for some reason."
  (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. Has the weird implementation too."
  (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))
  "Rotate along y so z-axis points to match another. Use arg3 as the max rotation amount."
  (let ((s5-0 (new 'stack-no-clear 'quaternion)))
    (let ((t9-0 vector-xz-normalize!)
          (a0-1 (new 'stack-no-clear 'vector))
          )
      (set! (-> a0-1 data 0) (-> arg2 x))
      (set! (-> a0-1 data 1) 0.0)
      (set! (-> a0-1 data 2) (-> arg2 z))
      (set! (-> a0-1 data 3) 1.0)
      (let ((s0-0 (t9-0 a0-1 1.0)))
        (quaternion-from-two-vectors-max-angle!
         s5-0
         (vector-z-quaternion! (the-as vector (new 'stack-no-clear 'quaternion)) arg1)
         s0-0
         arg3
         )
        )
      )
    (quaternion-normalize! (quaternion*! arg0 s5-0 arg1))
    )
  )


(defun vector-rotate-y! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate vector along y axis. Not very efficient."
  (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)
    )
  )


;; note that these kind of assume a rotation ordering where you can yaw as much as you want,
;; but if you pitch 180 degrees everything is bad.

(defun vector-y-angle ((arg0 vector))
  "Get the yaw angle of a vector."
  (atan (-> arg0 data 0) (-> arg0 data 2))
  )

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

(defun quaterion<-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 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)
    )
  )

(defun-debug quaternion-validate ((arg0 quaternion))
  "Verify that a quaternion is valid, print an error if not."
  (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-type? (-> pp type) process-tree))
            (format #t "~A~%" (-> pp name))
            (format #t "#f~%")
            )
        )
      )
    (none)
    )
  )