mirror of
https://github.com/open-goal/jak-project.git
synced 2024-10-20 21:27:52 -04:00
3d8013633a
* top level in rlet * detect matrix and vector inline 0 * pretty print the symbol map
1085 lines
31 KiB
Common Lisp
1085 lines
31 KiB
Common Lisp
;;-*-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.vf vf4 vf1 vf2)
|
|
(.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.vf vf2 vf5 vf2)
|
|
(.outer.product.vf vf3 vf5 vf3)
|
|
(.outer.product.vf vf4 vf5 vf4)
|
|
(.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))
|
|
(let ((f0-2 (+ (+ (-> arg1 data 0) (-> arg1 data 5)) (-> arg1 data 10))))
|
|
(if (< 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))))
|
|
(let ((f0-6 (* f0-5 (- (-> arg1 data 1) (-> arg1 data 4)))))
|
|
(set! (-> arg0 z) f0-6)
|
|
(let ((v1-0 f0-6))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(let ((a2-0 0)
|
|
(a3-0 1)
|
|
(v1-1 2)
|
|
)
|
|
(when (< (-> arg1 data 0) (-> arg1 data 5))
|
|
(set! a2-0 1)
|
|
(set! a3-0 2)
|
|
(set! v1-1 0)
|
|
(let ((t0-1 v1-1))
|
|
)
|
|
)
|
|
(when
|
|
(<
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a2-0 2) (shl a2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(-> arg1 data 10)
|
|
)
|
|
(set! a2-0 2)
|
|
(set! a3-0 0)
|
|
(set! v1-1 1)
|
|
(let ((t0-6 v1-1))
|
|
)
|
|
)
|
|
(let
|
|
((f0-12
|
|
(sqrtf
|
|
(+
|
|
(-
|
|
1.0
|
|
(+
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a3-0 2) (shl a3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-1 2) (shl v1-1 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a2-0 2) (shl a2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(set! (-> arg0 data a2-0) (* 0.5 f0-12))
|
|
(when (!= f0-12 0.0)
|
|
(set! f0-12 (/ 0.5 f0-12))
|
|
(let ((t0-19 f0-12))
|
|
)
|
|
)
|
|
(set!
|
|
(-> arg0 w)
|
|
(*
|
|
(-
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-1 2) (shl a3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a3-0 2) (shl v1-1 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
f0-12
|
|
)
|
|
)
|
|
(set!
|
|
(-> arg0 data a3-0)
|
|
(*
|
|
(+
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a3-0 2) (shl a2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a2-0 2) (shl a3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
f0-12
|
|
)
|
|
)
|
|
(let
|
|
((f0-13
|
|
(*
|
|
(+
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-1 2) (shl a2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a2-0 2) (shl v1-1 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
f0-12
|
|
)
|
|
)
|
|
)
|
|
(set! (-> arg0 data v1-1) f0-13)
|
|
(let ((v1-4 f0-13))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
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))
|
|
(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
|
|
)
|
|
)
|
|
|
|
(defun quaternion-zxy! ((arg0 quaternion) (arg1 vector))
|
|
(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.vf vf6 vf0 vf0)
|
|
(.outer.product.vf vf5 vf0 vf0)
|
|
(.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))
|
|
"Not 100% sure, but get the y rotation angle?"
|
|
(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 ((t9-0 quaternion-vector-angle!)
|
|
(a0-1 (new 'stack-no-clear 'quaternion))
|
|
)
|
|
(set! (-> a0-1 vec quad) (the-as uint128 0))
|
|
(let ((a2-1 (t9-0 a0-1 (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 ((t9-0 quaternion-vector-angle!)
|
|
(a0-1 (new 'stack-no-clear 'quaternion))
|
|
)
|
|
(set! (-> a0-1 vec quad) (the-as uint128 0))
|
|
(let ((a2-1 (t9-0 a0-1 (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 ((t9-0 quaternion-vector-angle!)
|
|
(a0-1 (new 'stack-no-clear 'quaternion))
|
|
)
|
|
(set! (-> a0-1 vec quad) (the-as uint128 0))
|
|
(let ((a2-1 (t9-0 a0-1 (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 ((t9-0 quaternion-vector-angle!)
|
|
(a0-1 (new 'stack-no-clear 'quaternion))
|
|
)
|
|
(set! (-> a0-1 vec quad) (the-as uint128 0))
|
|
(let ((a1-2 (t9-0 a0-1 (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 ((s4-0 quaternion-vector-angle!)
|
|
(s3-0 (new 'stack-no-clear 'quaternion))
|
|
)
|
|
(set! (-> s3-0 vec quad) (the-as uint128 0))
|
|
(let ((t9-0 vector-x-quaternion!)
|
|
(a0-1 (new 'stack-no-clear 'vector))
|
|
)
|
|
(set! (-> a0-1 quad) (the-as uint128 0))
|
|
(let ((a1-3 (s4-0 s3-0 (t9-0 a0-1 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 ((s4-0 quaternion-vector-angle!)
|
|
(s3-0 (new 'stack-no-clear 'quaternion))
|
|
)
|
|
(set! (-> s3-0 vec quad) (the-as uint128 0))
|
|
(let ((t9-0 vector-z-quaternion!)
|
|
(a0-1 (new 'stack-no-clear 'vector))
|
|
)
|
|
(set! (-> a0-1 quad) (the-as uint128 0))
|
|
(let ((a1-3 (s4-0 s3-0 (t9-0 a0-1 arg1) arg2)))
|
|
(quaternion-normalize! (quaternion*! arg0 a1-3 arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun quaternion-delta-y ((arg0 quaternion) (arg1 quaternion))
|
|
(acos (vector-dot (vector-z-quaternion! (new 'stack-no-clear 'vector) arg0)
|
|
(vector-z-quaternion! (new 'stack-no-clear 'vector) arg1)
|
|
)
|
|
)
|
|
)
|
|
|
|
;; (local-vars (f0-1 float))
|
|
;; (let ((gp-0 acos))
|
|
;; (let* ((s5-0 (vector-z-quaternion! (new 'stack 'vector) arg0))
|
|
;; (v1-1 (vector-z-quaternion! (new 'stack 'vector) arg1))
|
|
;; (f0-0 (-> s5-0 data 0))
|
|
;; (f1-0 (-> s5-0 data 1))
|
|
;; (f2-0 (-> s5-0 data 2))
|
|
;; (f3-0 (-> v1-1 data 0))
|
|
;; (f4-0 (-> v1-1 data 1))
|
|
;; (f5-0 (-> v1-1 data 2))
|
|
;; )
|
|
;; (.mula.s f0-0 f3-0)
|
|
;; (.madda.s f1-0 f4-0)
|
|
;; (.madd.s f0-1 f2-0 f5-0)
|
|
;; )
|
|
;; (gp-0 f0-1)
|
|
;; )
|
|
|
|
|
|
(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 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))
|
|
(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-y-angle ((arg0 vector))
|
|
(atan (-> arg0 data 0) (-> arg0 data 2))
|
|
)
|
|
|
|
(defun vector-x-angle ((arg0 vector))
|
|
(atan (-> arg0 data 1) (vector-xz-length arg0))
|
|
)
|
|
|
|
(defun quaterion<-rotate-y-vector ((arg0 quaternion) (arg1 vector))
|
|
(quaternion-vector-angle!
|
|
arg0
|
|
(new 'static 'vector :y 1.0 :w 1.0)
|
|
(vector-y-angle arg1)
|
|
)
|
|
)
|
|
|
|
(defun quaternion-xz-angle ((arg0 quaternion))
|
|
(let ((gp-0 (new 'stack-no-clear 'matrix))
|
|
(s5-0 (new 'stack-no-clear 'vector))
|
|
)
|
|
(quaternion->matrix gp-0 arg0)
|
|
(let ((v1-1 s5-0))
|
|
(set! (-> v1-1 data 0) 0.0)
|
|
(set! (-> v1-1 data 1) 0.0)
|
|
(set! (-> v1-1 data 2) 1.0)
|
|
(set! (-> v1-1 data 3) 1.0)
|
|
)
|
|
(vector-matrix*! s5-0 s5-0 gp-0)
|
|
(vector-y-angle s5-0)
|
|
)
|
|
)
|
|
|
|
(defun-debug quaternion-validate ((arg0 quaternion))
|
|
(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)
|
|
)
|
|
)
|