mirror of
https://github.com/open-goal/jak-project.git
synced 2024-10-20 11:26:18 -04:00
c162c66118
This PR does two main things: 1. Work through the main low-hanging fruit issues in the formatter keeping it from feeling mature and usable 2. Iterate and prove that point by formatting all of the Jak 1 code base. **This has removed around 100K lines in total.** - The decompiler will now format it's results for jak 1 to keep things from drifting back to where they were. This is controlled by a new config flag `format_code`. How am I confident this hasn't broken anything?: - I compiled the entire project and stored it's `out/jak1/obj` files separately - I then recompiled the project after formatting and wrote a script that md5's each file and compares it (`compare-compilation-outputs.py` - The results (eventually) were the same:  > This proves that the only difference before and after is non-critical whitespace for all code/macros that is actually in use. I'm still aware of improvements that could be made to the formatter, as well as general optimization of it's performance. But in general these are for rare or non-critical situations in my opinion and I'll work through them before doing Jak 2. The vast majority looks great and is working properly at this point. Those known issues are the following if you are curious: 
725 lines
27 KiB
Common Lisp
725 lines
27 KiB
Common Lisp
;;-*-Lisp-*-
|
|
(in-package goal)
|
|
(bundles "ENGINE.CGO" "GAME.CGO")
|
|
(require "engine/math/quaternion-h.gc")
|
|
(require "engine/math/matrix.gc")
|
|
|
|
;; DECOMP BEGINS
|
|
|
|
(defmethod inspect ((this quaternion))
|
|
"Print a quaternion. Prints the values and axis-angle"
|
|
(format #t "[~8x] quaternion~%" this)
|
|
(format #t "~T[~F] [~F] [~F] [~F]~%" (-> this x) (-> this y) (-> this z) (-> this w))
|
|
(let ((f0-5 (/ 1.0 (sqrtf (+ (* (-> this x) (-> this x)) (* (-> this y) (-> this y)) (* (-> this z) (-> this z)))))))
|
|
(format #t "~Taxis: ~F ~F ~F" (* f0-5 (-> this x)) (* f0-5 (-> this y)) (* f0-5 (-> this z))))
|
|
(let ((f0-9 (* 2.0 (acos (-> this w))))) (format #t "~T~Tangle: (deg ~R)~%" f0-9))
|
|
this)
|
|
|
|
(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 x) f30-0))
|
|
(set! (-> quat y) (* (-> axis y) f30-0))
|
|
(set! (-> quat z) (* (-> axis z) 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 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-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 vector 0 x) f0-0)
|
|
(set! (-> arg0 vector 0 y) f1-0)
|
|
(set! (-> arg0 vector 0 z) (- f2-0))
|
|
(set! (-> arg0 vector 0 w) f3-0)
|
|
(set! (-> arg0 vector 1 x) (- f1-0))
|
|
(set! (-> arg0 vector 1 y) f0-0)
|
|
(set! (-> arg0 vector 1 z) f3-0)
|
|
(set! (-> arg0 vector 1 w) f2-0)
|
|
(set! (-> arg0 vector 2 x) f2-0)
|
|
(set! (-> arg0 vector 2 y) (- f3-0))
|
|
(set! (-> arg0 vector 2 z) f0-0)
|
|
(set! (-> arg0 vector 2 w) f1-0)
|
|
(set! (-> arg0 vector 3 x) (- f3-0))
|
|
(set! (-> arg0 vector 3 y) (- f2-0))
|
|
(set! (-> arg0 vector 3 z) (- f1-0))
|
|
(set! (-> arg0 vector 3 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 vector 0 x) f2-0)
|
|
(set! (-> arg0 vector 0 y) f3-0)
|
|
(set! (-> arg0 vector 0 z) (- f0-0))
|
|
(set! (-> arg0 vector 0 w) f1-0)
|
|
(set! (-> arg0 vector 1 x) f1-0)
|
|
(set! (-> arg0 vector 1 y) f0-0)
|
|
(set! (-> arg0 vector 1 z) f3-0)
|
|
(set! (-> arg0 vector 1 w) (- f3-0))
|
|
(set! (-> arg0 vector 2 x) f0-0)
|
|
(set! (-> arg0 vector 2 y) (- f1-0))
|
|
(set! (-> arg0 vector 2 z) f2-0)
|
|
(set! (-> arg0 vector 2 w) f3-0)
|
|
(set! (-> arg0 vector 3 x) f3-0))
|
|
(set! (-> arg0 vector 3 y) (- f2-0))
|
|
(set! (-> arg0 vector 3 z) (- f1-0))
|
|
(set! (-> arg0 vector 3 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 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))
|
|
(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-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))
|
|
(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 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-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)
|
|
(- f1-0)
|
|
(set! f0-0 -1.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 vector)) 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 vector 1)) 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 vector 2)) 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 x) (-> v1-1 z))))
|
|
|
|
(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. 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 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! (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 x) (-> arg0 z)))
|
|
|
|
(defun vector-x-angle ((arg0 vector))
|
|
"Get the pitch angle of a vector."
|
|
(atan (-> arg0 y) (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-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~%"))))
|
|
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)))
|