mirror of
https://github.com/open-goal/jak-project
synced 2026-08-20 22:35:07 -04:00
water111
153 lines
9.2 KiB
Common Lisp
153 lines
9.2 KiB
Common Lisp
;;-*-Lisp-*-
|
|
(in-package goal)
|
|
(bundles "ENGINE.CGO" "GAME.CGO")
|
|
(require "engine/math/quaternion.gc")
|
|
(require "engine/math/euler-h.gc")
|
|
|
|
;; In general, the euler angle stuff is really inefficient. I don't think it's really used outside of
|
|
;; a few camera debugging functions.
|
|
;;
|
|
;; The order code follows Shoemake's convention: bit 0 selects a rotating frame, bit 1 repeats the
|
|
;; first axis, bit 2 selects odd parity, and bits 3-4 select the initial axis through EulSafe. This
|
|
;; compactly describes all 24 Euler conventions without separate conversion routines.
|
|
|
|
(defun set-eul! ((dst euler-angles) (angle0 float) (angle1 float) (angle2 float) (order int))
|
|
"Store three ordered Euler angles and their packed Shoemake order code."
|
|
(set-vector! dst angle0 angle1 angle2 (the float order))
|
|
dst)
|
|
|
|
(defun eul->matrix ((dst-mat matrix) (src euler-angles))
|
|
"Convert src to a rotation matrix using its packed Shoemake Euler order. The order selects the initial axis, parity, whether the first axis repeats, and static- or rotating-frame interpretation."
|
|
(matrix-identity! dst-mat)
|
|
(let ((working-angles (new 'stack-no-clear 'vector)))
|
|
;; copy to temp storage.
|
|
(vector-copy! working-angles src)
|
|
(if (= (logand (the int (-> working-angles data 3)) 1) 1)
|
|
(let ((angle-swap (-> working-angles data 0)))
|
|
(set! (-> working-angles data 0) (-> working-angles data 2))
|
|
(set! (-> working-angles data 2) angle-swap)))
|
|
(when (= (logand (sar (the int (-> working-angles data 3)) 2) 1) 1)
|
|
(set! (-> working-angles data 0) (- (-> working-angles data 0)))
|
|
(set! (-> working-angles data 1) (- (-> working-angles data 1)))
|
|
(set! (-> working-angles data 2) (- (-> working-angles data 2))))
|
|
(let* ((cos0 (cos (-> working-angles data 0)))
|
|
(cos1 (cos (-> working-angles data 1)))
|
|
(cos2 (cos (-> working-angles data 2)))
|
|
(sin0 (sin (-> working-angles data 0)))
|
|
(sin1 (sin (-> working-angles data 1)))
|
|
(sin2 (sin (-> working-angles data 2)))
|
|
(cos0-cos2 (* cos0 cos2))
|
|
(cos0-sin2 (* cos0 sin2))
|
|
(sin0-cos2 (* sin0 cos2))
|
|
(sin0-sin2 (* sin0 sin2)))
|
|
(let* ((parity (logand (sar (the int (-> working-angles data 3)) 2) 1))
|
|
(axis0 (-> EulSafe (logand (sar (the int (-> working-angles data 3)) 3) 3)))
|
|
(axis1 (-> EulNext (+ axis0 parity)))
|
|
(axis2 (-> EulNext (+ (- 1 parity) axis0))))
|
|
(cond
|
|
((= (logand (sar (the int (-> working-angles data 3)) 1) 1) 1)
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis0 4) (shl axis0 2)) (the-as int dst-mat)))) cos1)
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis0 4) (shl axis1 2)) (the-as int dst-mat)))) (* sin1 sin0))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis0 4) (shl axis2 2)) (the-as int dst-mat)))) (* sin1 cos0))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis1 4) (shl axis0 2)) (the-as int dst-mat)))) (* sin1 sin2))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis1 4) (shl axis1 2)) (the-as int dst-mat))))
|
|
(- cos0-cos2 (* cos1 sin0-sin2)))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis1 4) (shl axis2 2)) (the-as int dst-mat))))
|
|
(- (- sin0-cos2) (* cos1 cos0-sin2)))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis2 4) (shl axis0 2)) (the-as int dst-mat)))) (- (* sin1 cos2)))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis2 4) (shl axis1 2)) (the-as int dst-mat))))
|
|
(+ cos0-sin2 (* cos1 sin0-cos2)))
|
|
(let ((f0-19 (+ (- sin0-sin2) (* cos1 cos0-cos2))))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis2 4) (shl axis2 2)) (the-as int dst-mat)))) f0-19)))
|
|
(else
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis0 4) (shl axis0 2)) (the-as int dst-mat)))) (* cos1 cos2))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis0 4) (shl axis1 2)) (the-as int dst-mat))))
|
|
(+ (- cos0-sin2) (* sin1 sin0-cos2)))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis0 4) (shl axis2 2)) (the-as int dst-mat))))
|
|
(+ sin0-sin2 (* sin1 cos0-cos2)))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis1 4) (shl axis0 2)) (the-as int dst-mat)))) (* cos1 sin2))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis1 4) (shl axis1 2)) (the-as int dst-mat))))
|
|
(+ cos0-cos2 (* sin1 sin0-sin2)))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis1 4) (shl axis2 2)) (the-as int dst-mat))))
|
|
(+ (- sin0-cos2) (* sin1 cos0-sin2)))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis2 4) (shl axis0 2)) (the-as int dst-mat)))) (- sin1))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis2 4) (shl axis1 2)) (the-as int dst-mat)))) (* cos1 sin0))
|
|
(let ((f0-25 (* cos1 cos0)))
|
|
(set! (-> (the-as (pointer float) (+ (+ (shl axis2 4) (shl axis2 2)) (the-as int dst-mat)))) f0-25)))))))
|
|
dst-mat)
|
|
|
|
(defun matrix->eul ((dst euler-angles) (src-mat matrix) (order int))
|
|
"Convert src-mat to the convention selected by packed Shoemake order. Repeated-axis and three-distinct-axis orders use separate formulas; near a coordinate singularity the underdetermined final angle is set to zero."
|
|
0
|
|
0
|
|
0
|
|
(let* ((parity (logand (/ order 4) 1))
|
|
(axis0 (-> EulSafe (logand (/ order 8) 3)))
|
|
(axis1 (-> EulNext (+ axis0 parity)))
|
|
(axis2 (-> EulNext (+ (- 1 parity) axis0))))
|
|
(cond
|
|
((= (logand (/ order 2) 1) 1)
|
|
(let ((middle-sine-magnitude (sqrtf (+ (square (-> (the-as (pointer float) (+ (+ (* axis1 4) (* axis0 16)) (the-as int src-mat)))))
|
|
(square (-> (the-as (pointer float) (+ (+ (* axis2 4) (* axis0 16)) (the-as int src-mat)))))))))
|
|
(cond
|
|
((< 0.00001 middle-sine-magnitude)
|
|
(set! (-> dst x)
|
|
(atan (-> (the-as (pointer float) (+ (+ (* axis1 4) (* axis0 16)) (the-as int src-mat))))
|
|
(-> (the-as (pointer float) (+ (+ (* axis2 4) (* axis0 16)) (the-as int src-mat))))))
|
|
(set! (-> dst y)
|
|
(atan middle-sine-magnitude (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis0 16)) (the-as int src-mat))))))
|
|
(set! (-> dst z)
|
|
(atan (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis1 16)) (the-as int src-mat))))
|
|
(- (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis2 16)) (the-as int src-mat))))))))
|
|
(else
|
|
(set! (-> dst x)
|
|
(atan (- (-> (the-as (pointer float) (+ (+ (* axis2 4) (* axis1 16)) (the-as int src-mat)))))
|
|
(-> (the-as (pointer float) (+ (+ (* axis1 4) (* axis1 16)) (the-as int src-mat))))))
|
|
(set! (-> dst y)
|
|
(atan middle-sine-magnitude (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis0 16)) (the-as int src-mat))))))
|
|
(set! (-> dst z) 0.0)))))
|
|
(else
|
|
(let ((middle-cosine-magnitude (sqrtf (+ (square (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis0 16)) (the-as int src-mat)))))
|
|
(square (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis1 16)) (the-as int src-mat)))))))))
|
|
(cond
|
|
((< 0.00001 middle-cosine-magnitude)
|
|
(set! (-> dst x)
|
|
(atan (-> (the-as (pointer float) (+ (+ (* axis1 4) (* axis2 16)) (the-as int src-mat))))
|
|
(-> (the-as (pointer float) (+ (+ (* axis2 4) (* axis2 16)) (the-as int src-mat))))))
|
|
(set! (-> dst y)
|
|
(atan (- (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis2 16)) (the-as int src-mat))))) middle-cosine-magnitude))
|
|
(set! (-> dst z)
|
|
(atan (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis1 16)) (the-as int src-mat))))
|
|
(-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis0 16)) (the-as int src-mat)))))))
|
|
(else
|
|
(set! (-> dst x)
|
|
(atan (- (-> (the-as (pointer float) (+ (+ (* axis2 4) (* axis1 16)) (the-as int src-mat)))))
|
|
(-> (the-as (pointer float) (+ (+ (* axis1 4) (* axis1 16)) (the-as int src-mat))))))
|
|
(set! (-> dst y)
|
|
(atan (- (-> (the-as (pointer float) (+ (+ (* axis0 4) (* axis2 16)) (the-as int src-mat))))) middle-cosine-magnitude))
|
|
(set! (-> dst z) 0.0)))))))
|
|
(when (= (logand (/ order 4) 1) 1)
|
|
(set! (-> dst x) (- (-> dst x)))
|
|
(set! (-> dst y) (- (-> dst y)))
|
|
(set! (-> dst z) (- (-> dst z))))
|
|
(when (= (logand order 1) 1)
|
|
(let ((f0-49 (-> dst x)))
|
|
(set! (-> dst x) (-> dst z))
|
|
(set! (-> dst z) f0-49)))
|
|
(set! (-> dst w) (the float order))
|
|
dst)
|
|
|
|
(defun eul->quat ((dst quaternion) (src euler-angles))
|
|
"Convert src from its Euler convention to a quaternion."
|
|
(let ((rotation-mat (new 'stack-no-clear 'matrix)))
|
|
(eul->matrix rotation-mat src)
|
|
(matrix->quaternion dst rotation-mat))
|
|
dst)
|
|
|
|
(defun quat->eul ((dst euler-angles) (src quaternion) (order int))
|
|
"Convert src to the Euler convention selected by order."
|
|
(let ((rotation-mat (new 'stack-no-clear 'matrix)))
|
|
(quaternion->matrix rotation-mat src)
|
|
(matrix->eul dst rotation-mat order))
|
|
dst)
|