mirror of
https://github.com/open-goal/jak-project
synced 2026-08-06 18:03:30 -04:00
water111
153 lines
9.6 KiB
Common Lisp
153 lines
9.6 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! (-> dst data 0) angle0)
|
|
(set! (-> dst data 1) angle1)
|
|
(set! (-> dst data 2) angle2)
|
|
(set! (-> dst data 3) (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."
|
|
(let* ((parity (logand (sar order 2) 1))
|
|
(axis0 (-> EulSafe (logand (sar order 3) 3)))
|
|
(axis1 (-> EulNext (+ axis0 parity)))
|
|
(axis2 (-> EulNext (+ (- 1 parity) axis0))))
|
|
(if (= (logand (sar order 1) 1) 1)
|
|
(let* ((f0-0 (-> (the-as (pointer float) (+ (+ (shl axis1 2) (shl axis0 4)) (the-as int src-mat)))))
|
|
(f0-2 (* f0-0 f0-0))
|
|
(f1-0 (-> (the-as (pointer float) (+ (+ (shl axis2 2) (shl axis0 4)) (the-as int src-mat)))))
|
|
(middle-sine-magnitude (sqrtf (+ f0-2 (* f1-0 f1-0)))))
|
|
(cond
|
|
((< 0.00001 middle-sine-magnitude)
|
|
(set! (-> dst data 0)
|
|
(atan (-> (the-as (pointer float) (+ (+ (shl axis1 2) (shl axis0 4)) (the-as int src-mat))))
|
|
(-> (the-as (pointer float) (+ (+ (shl axis2 2) (shl axis0 4)) (the-as int src-mat))))))
|
|
(set! (-> dst data 1)
|
|
(atan middle-sine-magnitude (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis0 4)) (the-as int src-mat))))))
|
|
(let ((f0-13 (atan (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis1 4)) (the-as int src-mat))))
|
|
(- (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis2 4)) (the-as int src-mat))))))))
|
|
(set! (-> dst data 2) f0-13)))
|
|
(else
|
|
(set! (-> dst data 0)
|
|
(atan (- (-> (the-as (pointer float) (+ (+ (shl axis2 2) (shl axis1 4)) (the-as int src-mat)))))
|
|
(-> (the-as (pointer float) (+ (+ (shl axis1 2) (shl axis1 4)) (the-as int src-mat))))))
|
|
(set! (-> dst data 1)
|
|
(atan middle-sine-magnitude (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis0 4)) (the-as int src-mat))))))
|
|
(let ((f0-20 0.0)) (set! (-> dst data 2) f0-20)))))
|
|
(let* ((f0-21 (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis0 4)) (the-as int src-mat)))))
|
|
(f0-23 (* f0-21 f0-21))
|
|
(f1-3 (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis1 4)) (the-as int src-mat)))))
|
|
(middle-cosine-magnitude (sqrtf (+ f0-23 (* f1-3 f1-3)))))
|
|
(cond
|
|
((< 0.00001 middle-cosine-magnitude)
|
|
(set! (-> dst data 0)
|
|
(atan (-> (the-as (pointer float) (+ (+ (shl axis1 2) (shl axis2 4)) (the-as int src-mat))))
|
|
(-> (the-as (pointer float) (+ (+ (shl axis2 2) (shl axis2 4)) (the-as int src-mat))))))
|
|
(set! (-> dst data 1)
|
|
(atan (- (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis2 4)) (the-as int src-mat)))))
|
|
middle-cosine-magnitude))
|
|
(let ((f0-34 (atan (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis1 4)) (the-as int src-mat))))
|
|
(-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis0 4)) (the-as int src-mat)))))))
|
|
(set! (-> dst data 2) f0-34)))
|
|
(else
|
|
(set! (-> dst data 0)
|
|
(atan (- (-> (the-as (pointer float) (+ (+ (shl axis2 2) (shl axis1 4)) (the-as int src-mat)))))
|
|
(-> (the-as (pointer float) (+ (+ (shl axis1 2) (shl axis1 4)) (the-as int src-mat))))))
|
|
(set! (-> dst data 1)
|
|
(atan (- (-> (the-as (pointer float) (+ (+ (shl axis0 2) (shl axis2 4)) (the-as int src-mat)))))
|
|
middle-cosine-magnitude))
|
|
(let ((f0-42 0.0)) (set! (-> dst data 2) f0-42)))))))
|
|
(when (= (logand (sar order 2) 1) 1)
|
|
(set! (-> dst data 0) (- (-> dst data 0)))
|
|
(set! (-> dst data 1) (- (-> dst data 1)))
|
|
(let ((f0-48 (- (-> dst data 2)))) (set! (-> dst data 2) f0-48)))
|
|
(if (= (logand order 1) 1)
|
|
(let ((f0-49 (-> dst data 0))) (set! (-> dst data 0) (-> dst data 2)) (set! (-> dst data 2) f0-49)))
|
|
(set! (-> dst data 3) (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)
|