mirror of
https://github.com/open-goal/jak-project
synced 2026-05-31 09:22:14 -04:00
e5f0fecf17
* get to vif * support basic bitfield access * make bitfields in dma work * clean up dma * fix merge conflict
517 lines
11 KiB
Common Lisp
517 lines
11 KiB
Common Lisp
;;-*-Lisp-*-
|
|
(in-package goal)
|
|
|
|
;; definition for function set-eul!
|
|
(defun
|
|
set-eul!
|
|
((arg0 euler-angles) (arg1 float) (arg2 float) (arg3 float) (arg4 int))
|
|
(set! (-> arg0 data 0) arg1)
|
|
(set! (-> arg0 data 1) arg2)
|
|
(set! (-> arg0 data 2) arg3)
|
|
(set! (-> arg0 data 3) (the float arg4))
|
|
arg0
|
|
)
|
|
|
|
;; definition for function eul->matrix
|
|
;; Used lq/sq
|
|
(defun eul->matrix ((arg0 matrix) (arg1 euler-angles))
|
|
(matrix-identity! arg0)
|
|
(let ((s5-0 (new 'stack 'vector)))
|
|
(set! (-> s5-0 quad) (-> arg1 quad))
|
|
(if (= (logand (the int (-> s5-0 data 3)) 1) 1)
|
|
(let ((f0-2 (-> s5-0 data 0)))
|
|
(set! (-> s5-0 data 0) (-> s5-0 data 2))
|
|
(set! (-> s5-0 data 2) f0-2)
|
|
)
|
|
)
|
|
(when (= (logand (sar (the int (-> s5-0 data 3)) 2) 1) 1)
|
|
(set! (-> s5-0 data 0) (- (-> s5-0 data 0)))
|
|
(set! (-> s5-0 data 1) (- (-> s5-0 data 1)))
|
|
(let ((f0-10 (- (-> s5-0 data 2))))
|
|
(set! (-> s5-0 data 2) f0-10)
|
|
)
|
|
)
|
|
(let* ((f26-0 (cos (-> s5-0 data 0)))
|
|
(f30-0 (cos (-> s5-0 data 1)))
|
|
(f22-0 (cos (-> s5-0 data 2)))
|
|
(f24-0 (sin (-> s5-0 data 0)))
|
|
(f28-0 (sin (-> s5-0 data 1)))
|
|
(f4-0 (sin (-> s5-0 data 2)))
|
|
(f0-17 (* f26-0 f22-0))
|
|
(f1-1 (* f26-0 f4-0))
|
|
(f2-0 (* f24-0 f22-0))
|
|
(f3-0 (* f24-0 f4-0))
|
|
)
|
|
(let ((v1-7 0))
|
|
)
|
|
(let ((v1-8 0))
|
|
)
|
|
(let ((v1-9 0))
|
|
)
|
|
(let* ((v1-12 (logand (sar (the int (-> s5-0 data 3)) 2) 1))
|
|
(a1-2 (-> EulSafe (logand (sar (the int (-> s5-0 data 3)) 3) 3)))
|
|
(a0-21 (-> EulNext (+ a1-2 v1-12)))
|
|
(v1-17 (-> EulNext (+ (- 1 v1-12) a1-2)))
|
|
)
|
|
(cond
|
|
((= (logand (sar (the int (-> s5-0 data 3)) 1) 1) 1)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a1-2 4) (shl a1-2 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
f30-0
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a1-2 4) (shl a0-21 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(* f28-0 f24-0)
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a1-2 4) (shl v1-17 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(* f28-0 f26-0)
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a0-21 4) (shl a1-2 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(* f28-0 f4-0)
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a0-21 4) (shl a0-21 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(- f0-17 (* f30-0 f3-0))
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a0-21 4) (shl v1-17 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(- (- f2-0) (* f30-0 f1-1))
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-17 4) (shl a1-2 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(- (* f28-0 f22-0))
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-17 4) (shl a0-21 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(+ f1-1 (* f30-0 f2-0))
|
|
)
|
|
(let ((f0-19 (+ (- f3-0) (* f30-0 f0-17))))
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-17 4) (shl v1-17 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
f0-19
|
|
)
|
|
)
|
|
)
|
|
(else
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a1-2 4) (shl a1-2 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(* f30-0 f22-0)
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a1-2 4) (shl a0-21 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(+ (- f1-1) (* f28-0 f2-0))
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a1-2 4) (shl v1-17 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(+ f3-0 (* f28-0 f0-17))
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a0-21 4) (shl a1-2 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(* f30-0 f4-0)
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a0-21 4) (shl a0-21 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(+ f0-17 (* f28-0 f3-0))
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl a0-21 4) (shl v1-17 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(+ (- f2-0) (* f28-0 f1-1))
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-17 4) (shl a1-2 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(- f28-0)
|
|
)
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-17 4) (shl a0-21 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
(* f30-0 f24-0)
|
|
)
|
|
(let ((f0-25 (* f30-0 f26-0)))
|
|
(set!
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl v1-17 4) (shl v1-17 2)) (the-as int arg0))
|
|
)
|
|
)
|
|
f0-25
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
arg0
|
|
)
|
|
|
|
;; definition for function matrix->eul
|
|
(defun matrix->eul ((arg0 euler-angles) (arg1 matrix) (arg2 int))
|
|
(let ((v1-0 0))
|
|
)
|
|
(let ((v1-1 0))
|
|
)
|
|
(let ((v1-2 0))
|
|
)
|
|
(let* ((v1-4 (logand (sar arg2 2) 1))
|
|
(s3-0 (-> EulSafe (logand (sar arg2 3) 3)))
|
|
(s2-0 (-> EulNext (+ s3-0 v1-4)))
|
|
(s1-0 (-> EulNext (+ (- 1 v1-4) s3-0)))
|
|
)
|
|
(if (= (logand (sar arg2 1) 1) 1)
|
|
(let*
|
|
((f0-0
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s2-0 2) (shl s3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
(f0-2 (* f0-0 f0-0))
|
|
(f1-0
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s1-0 2) (shl s3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
(f30-0 (sqrtf (+ f0-2 (* f1-0 f1-0))))
|
|
)
|
|
(cond
|
|
((< 0.00001 f30-0)
|
|
(set!
|
|
(-> arg0 data 0)
|
|
(atan
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s2-0 2) (shl s3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s1-0 2) (shl s3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(set!
|
|
(-> arg0 data 1)
|
|
(atan
|
|
f30-0
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(let
|
|
((f0-13
|
|
(atan
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(-
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s1-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(set! (-> arg0 data 2) f0-13)
|
|
)
|
|
)
|
|
(else
|
|
(set!
|
|
(-> arg0 data 0)
|
|
(atan
|
|
(-
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s1-0 2) (shl s2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s2-0 2) (shl s2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(set!
|
|
(-> arg0 data 1)
|
|
(atan
|
|
f30-0
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(let ((f0-20 0.0))
|
|
(set! (-> arg0 data 2) f0-20)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(let*
|
|
((f0-21
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
(f0-23 (* f0-21 f0-21))
|
|
(f1-3
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
(f30-1 (sqrtf (+ f0-23 (* f1-3 f1-3))))
|
|
)
|
|
(cond
|
|
((< 0.00001 f30-1)
|
|
(set!
|
|
(-> arg0 data 0)
|
|
(atan
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s2-0 2) (shl s1-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s1-0 2) (shl s1-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(set!
|
|
(-> arg0 data 1)
|
|
(atan
|
|
(-
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s1-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
f30-1
|
|
)
|
|
)
|
|
(let
|
|
((f0-34
|
|
(atan
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s3-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(set! (-> arg0 data 2) f0-34)
|
|
)
|
|
)
|
|
(else
|
|
(set!
|
|
(-> arg0 data 0)
|
|
(atan
|
|
(-
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s1-0 2) (shl s2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s2-0 2) (shl s2-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(set!
|
|
(-> arg0 data 1)
|
|
(atan
|
|
(-
|
|
(->
|
|
(the-as
|
|
(pointer float)
|
|
(+ (+ (shl s3-0 2) (shl s1-0 4)) (the-as int arg1))
|
|
)
|
|
)
|
|
)
|
|
f30-1
|
|
)
|
|
)
|
|
(let ((f0-42 0.0))
|
|
(set! (-> arg0 data 2) f0-42)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(when (= (logand (sar arg2 2) 1) 1)
|
|
(set! (-> arg0 data 0) (- (-> arg0 data 0)))
|
|
(set! (-> arg0 data 1) (- (-> arg0 data 1)))
|
|
(let ((f0-48 (- (-> arg0 data 2))))
|
|
(set! (-> arg0 data 2) f0-48)
|
|
)
|
|
)
|
|
(if (= (logand arg2 1) 1)
|
|
(let ((f0-49 (-> arg0 data 0)))
|
|
(set! (-> arg0 data 0) (-> arg0 data 2))
|
|
(set! (-> arg0 data 2) f0-49)
|
|
)
|
|
)
|
|
(set! (-> arg0 data 3) (the float arg2))
|
|
arg0
|
|
)
|
|
|
|
;; definition for function eul->quat
|
|
(defun eul->quat ((arg0 quaternion) (arg1 euler-angles))
|
|
(let ((s5-0 (new 'stack 'matrix)))
|
|
(eul->matrix s5-0 arg1)
|
|
(matrix->quaternion arg0 s5-0)
|
|
)
|
|
arg0
|
|
)
|
|
|
|
;; definition for function quat->eul
|
|
(defun quat->eul ((arg0 euler-angles) (arg1 quaternion) (arg2 int))
|
|
(let ((s5-0 (new 'stack 'matrix)))
|
|
(quaternion->matrix s5-0 arg1)
|
|
(matrix->eul arg0 s5-0 arg2)
|
|
)
|
|
arg0
|
|
)
|
|
|
|
;; failed to figure out what this is:
|
|
(none)
|
|
|
|
|
|
|
|
|