mirror of
https://github.com/open-goal/jak-project.git
synced 2024-10-20 21:27:52 -04:00
f7bd0752f8
* wip * getting stuff set up so we can actually run test cases * better handle block entry stuff * types2 working on gstring * comments * math ref working * up to first stack stuff * stack fixes * bounding box * math stuff is working * float fixes * temp debug for (method 9 profile-array) * stupid stupid bug * debugging * everything is broken * some amount of type stuff works * bitfield * texture bitfields not working * temp * types * more stuff * type check * temp * float related fixes for light and res problems * revisit broken files, fix bugs * more types * vector debug * bug fixes for decompiler crashes in harder functions * update goal_src
1068 lines
33 KiB
Common Lisp
1068 lines
33 KiB
Common Lisp
;;-*-Lisp-*-
|
|
(in-package goal)
|
|
|
|
;; name: quaternion.gc
|
|
;; name in dgo: quaternion
|
|
;; dgos: ENGINE, GAME
|
|
|
|
;; DECOMP BEGINS
|
|
|
|
(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! ((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! (the-as (pointer float) 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! (the-as (pointer float) 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! (the-as vector (-> s5-0 data)) arg2 arg1)
|
|
(vector-cross! (the-as vector (&-> s5-0 data 4)) arg1 (the-as vector (-> s5-0 data)))
|
|
(set! (-> (the-as vector (&-> s5-0 data 8)) quad) (-> arg1 quad))
|
|
(quaternion-normalize! (matrix->quaternion arg0 s5-0))
|
|
)
|
|
)
|
|
|
|
(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)
|
|
(.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 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 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 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 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 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 trans quad) vf0)
|
|
(.svf (&-> arg0 quad 0) vf2)
|
|
(.svf (&-> arg0 quad 1) vf3)
|
|
(.svf (&-> arg0 quad 2) 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 data 0) (-> arg1 data 5) (-> arg1 data 10))))
|
|
(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 data 6) (-> arg1 data 9))))
|
|
(set! (-> arg0 y) (* f0-5 (- (-> arg1 data 8) (-> arg1 data 2))))
|
|
(set! (-> arg0 z) (* f0-5 (- (-> arg1 data 1) (-> arg1 data 4))))
|
|
)
|
|
)
|
|
)
|
|
(else
|
|
(let ((a2-0 0)
|
|
(a3-0 1)
|
|
(v1-4 2)
|
|
)
|
|
(when (< (-> arg1 data 0) (-> arg1 data 5))
|
|
(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 data 10))
|
|
(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-matrix0)))
|
|
(let* ((f0-1 (vector-dot (the-as vector (-> arg1 data)) (the-as vector (-> arg1 data))))
|
|
(f1-1 (vector-dot (the-as vector (&-> arg1 data 4)) (the-as vector (&-> arg1 data 4))))
|
|
(f2-1 (vector-dot (the-as vector (&-> arg1 data 8)) (the-as vector (&-> arg1 data 8))))
|
|
(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)
|
|
)
|
|
)
|
|
)
|
|
|
|
(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! (the-as (pointer float) 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 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 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 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-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 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
|
|
)
|
|
)
|
|
|
|
(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 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-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))
|
|
"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 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-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)
|
|
)
|
|
)
|
|
|
|
(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)
|
|
)
|
|
)
|
|
|
|
(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)
|
|
)
|
|
)
|
|
|
|
;; 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 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)
|
|
)
|
|
)
|
|
|
|
|
|
|
|
|