mirror of
https://github.com/open-goal/jak-project.git
synced 2024-10-20 21:27:52 -04:00
1078 lines
29 KiB
Common Lisp
1078 lines
29 KiB
Common Lisp
;;-*-Lisp-*-
|
|
(in-package goal)
|
|
|
|
;; name: trigonometry.gc
|
|
;; name in dgo: trigonometry
|
|
;; dgos: ENGINE, GAME
|
|
|
|
#|@file
|
|
The "rotation" unit stores an angle in a float, where 1.0 = 1/65,536 (1/2^16) of a rotation.
|
|
Use the ~r format specifier to print rotations as degrees.
|
|
In general, functions which use these units will only be accurate to within 1/65,536th of a rotation,
|
|
as they often internally convert the float to an integer. These function also handle wrapping
|
|
correctly, and will output angles in the range -32768 to 32768 (+/- one half of a rotation)
|
|
Functions with these units have deg or nothing special in the name.
|
|
|
|
Some functions use radians. These typically have rad in the name, and they don't handle wrapping.
|
|
The input must be in the range -pi to pi
|
|
|
|
General note on floating point constants: to avoid ambiguity/rounding issues related to printing/parsing, weird
|
|
constants are stored as hex. Commonly used constants that are exactly represented (1, 0.5, etc) will appear
|
|
normally.
|
|
|#
|
|
|
|
;; There is a bug in some of the cosine functions that can be fixed by toggling this flag.
|
|
(defglobalconstant FIX_COSINE_BUG #f)
|
|
|
|
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
|
|
;; Floating Point Constants
|
|
;;;;;;;;;;;;;;;;;;;;;;;;;;;;
|
|
|
|
(defconstant ROT_TO_RAD (the-as float #x38c90fda))
|
|
(defconstant PI (the-as float #x40490fda))
|
|
(defconstant MINUS_PI (the-as float #xc0490fda))
|
|
(defconstant PI_OVER_2 (the-as float #x3fc90fda))
|
|
(defconstant TWO_PI (the-as float #x40c90fda))
|
|
|
|
(defmacro unwrap-angle (angle)
|
|
`(the float (sar (shl (the int ,angle) 48) 48))
|
|
)
|
|
|
|
(defmacro unwrap-angle-small-angle-fix (angle)
|
|
"Same as unwrap angle, but small angles are not rounded to zero."
|
|
`(if (< (fabs ,angle) 1.1)
|
|
,angle
|
|
(the float (sar (shl (the int ,angle) 48) 48))
|
|
)
|
|
)
|
|
|
|
;; DECOMP BEGINS
|
|
|
|
(defun radmod ((arg0 float))
|
|
"Wrap arg0 to be within (-pi, pi)."
|
|
(let ((f0-1 (+ 3.1415925 arg0)))
|
|
(if (< 0.0 f0-1)
|
|
(+ -3.1415925 (- f0-1 (* (the float (the int (/ f0-1 6.283185))) 6.283185)))
|
|
(+ 3.1415925 (- f0-1 (* (the float (the int (/ f0-1 6.283185))) 6.283185)))
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun deg- ((arg0 float) (arg1 float))
|
|
"Compute arg0-arg1, unwrapped, using rotation units.
|
|
Result should be in the range (-180, 180)"
|
|
(the float (sar (- (shl (the int arg0) 48) (shl (the int arg1) 48)) 48))
|
|
)
|
|
|
|
(defun deg-diff ((arg0 float) (arg1 float))
|
|
"Very similar to the function above, but computes arg1 - arg0 instead."
|
|
(the float (sar (- (shl (the int arg1) 48) (shl (the int arg0) 48)) 48))
|
|
)
|
|
|
|
(defun deg-seek ((arg0 float) (arg1 float) (arg2 float))
|
|
"Move in toward target by at most max-diff, using rotation units"
|
|
(let ((v1-1 (shl (the int arg0) 48))
|
|
(a0-2 (shl (the int arg1) 48))
|
|
)
|
|
(let* ((a1-2 (shl (the int arg2) 48))
|
|
(a2-1 (- a0-2 v1-1)) (a3-0 (abs a2-1)))
|
|
(set! a0-2 (cond
|
|
((< a3-0 0)
|
|
(+ v1-1 a1-2)
|
|
)
|
|
((>= a1-2 a3-0)
|
|
a0-2
|
|
)
|
|
((>= a2-1 0)
|
|
(+ v1-1 a1-2)
|
|
)
|
|
(else
|
|
(- v1-1 a1-2)
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(the float (sar a0-2 48))
|
|
)
|
|
)
|
|
|
|
(defun deg-seek-smooth ((arg0 float) (arg1 float) (arg2 float) (arg3 float))
|
|
"Step amount of the way from in to target, by at most max-diff, using rotation units"
|
|
(let ((f0-1 (* (deg- arg1 arg0) arg3)))
|
|
(if (< arg2 (fabs f0-1))
|
|
(set! f0-1 (if (>= f0-1 0.0)
|
|
arg2
|
|
(- arg2)
|
|
)
|
|
)
|
|
)
|
|
(+ arg0 f0-1)
|
|
)
|
|
)
|
|
|
|
(defun deg-lerp-clamp ((arg0 float) (arg1 float) (arg2 float))
|
|
"Map [0, 1] to min-val, max-val, handling wrapping and saturating, using rotation units."
|
|
(cond
|
|
((>= 0.0 arg2)
|
|
arg0
|
|
)
|
|
((>= arg2 1.0)
|
|
arg1
|
|
)
|
|
(else
|
|
(the float (sar (shl (the int (+ arg0 (* arg2 (deg-diff arg0 arg1)))) 48) 48))
|
|
)
|
|
)
|
|
)
|
|
|
|
(define binary-table (new 'static 'boxed-array :type float
|
|
1.0
|
|
0.5
|
|
0.25
|
|
0.125
|
|
0.0625
|
|
0.03125
|
|
0.015625
|
|
0.0078125
|
|
0.00390625
|
|
0.001953125
|
|
0.0009765625
|
|
0.00048828125
|
|
0.00024414062
|
|
0.00012207031
|
|
0.000061035156
|
|
0.000030517578
|
|
0.000015258789
|
|
0.0000076293945
|
|
0.0000038146973
|
|
0.0000019073486
|
|
0.0000009536743
|
|
0.00000047683716
|
|
0.00000023841858
|
|
0.00000011920929
|
|
0.000000059604645
|
|
0.000000029802322
|
|
0.000000014901161
|
|
0.000000007450581
|
|
0.0000000037252903
|
|
0.0000000018626451
|
|
0.0000000009313226
|
|
0.0000000004656613
|
|
)
|
|
|
|
)
|
|
|
|
(define sincos-table (new 'static 'boxed-array :type float
|
|
0.7853982
|
|
0.4636476
|
|
0.24497867
|
|
0.124354996
|
|
0.06241881
|
|
0.031239834
|
|
0.015623729
|
|
0.007812341
|
|
0.0039062302
|
|
0.0019531226
|
|
0.0009765622
|
|
0.0004882812
|
|
0.00024414062
|
|
0.00012207031
|
|
0.000061035156
|
|
0.000030517578
|
|
0.000015258789
|
|
0.0000076293945
|
|
0.0000038146973
|
|
0.0000019073486
|
|
0.0000009536743
|
|
0.00000047683716
|
|
0.00000023841858
|
|
0.00000011920929
|
|
0.000000059604645
|
|
0.000000029802322
|
|
0.000000014901161
|
|
0.000000007450581
|
|
0.0000000037252903
|
|
0.0000000018626451
|
|
0.0000000009313226
|
|
0.0000000004656613
|
|
)
|
|
|
|
)
|
|
|
|
(defun sin ((arg0 float))
|
|
"Compute the sine of an angle in rotation units. Unwraps it."
|
|
(let ((f2-0 (* 0.000095873795 (unwrap-angle arg0))))
|
|
f2-0
|
|
(let* ((f1-4 (* 0.999998 f2-0))
|
|
(f0-3 (* f2-0 f2-0))
|
|
(f2-1 (* f2-0 f0-3))
|
|
(f1-5 (+ f1-4 (* -0.16666014 f2-1)))
|
|
(f2-2 (* f2-1 f0-3))
|
|
(f1-6 (+ f1-5 (* 0.008326521 f2-2)))
|
|
(f2-3 (* f2-2 f0-3))
|
|
(f1-7 (+ f1-6 (* -0.0001956241 f2-3)))
|
|
(f0-4 (* f2-3 f0-3))
|
|
)
|
|
(+ f1-7 (* 0.0000023042373 f0-4))
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun sin-rad ((arg0 float))
|
|
"Compute the sine of an angle in radians.
|
|
No unwrap is done, should be in -pi, pi"
|
|
(local-vars
|
|
(f1-0 float)
|
|
(f2-0 float)
|
|
(f3-0 float)
|
|
(f4-0 float)
|
|
(f5-0 float)
|
|
(f6-0 float)
|
|
(f7-0 float)
|
|
(f8-0 float)
|
|
(f9-0 float)
|
|
(f10-0 float)
|
|
(f11-0 float)
|
|
(acc float)
|
|
)
|
|
(set! f1-0 (* arg0 arg0))
|
|
(set! f7-0 (the-as float #x3f7fffde))
|
|
(set! f8-0 (the-as float #xbe2aa8f5))
|
|
(set! f2-0 (* arg0 f1-0))
|
|
(set! f3-0 (* f1-0 f1-0))
|
|
(set! f9-0 (the-as float #x3c086bf6))
|
|
(set! f4-0 (* f2-0 f1-0))
|
|
(set! f5-0 (* f3-0 f2-0))
|
|
(set! f10-0 (the-as float #xb94d2072))
|
|
(set! f6-0 (* f4-0 f3-0))
|
|
(set! f11-0 (the-as float #x361aa27f))
|
|
;;(.mula.s arg0 f7-0)
|
|
(set! acc (* arg0 f7-0))
|
|
;;(.madda.s f2-0 f8-0)
|
|
(set! acc (+ acc (* f2-0 f8-0)))
|
|
;;(.madda.s f4-0 f9-0)
|
|
(set! acc (+ acc (* f4-0 f9-0)))
|
|
;;(.madda.s f5-0 f10-0)
|
|
(set! acc (+ acc (* f5-0 f10-0)))
|
|
;;(.madd.s f12-0 f6-0 f11-0)
|
|
(+ acc (* f6-0 f11-0))
|
|
)
|
|
|
|
(define *sin-poly-vec*
|
|
(new 'static 'vector :x -0.16666014 :y 0.008326521 :z -0.0001956241 :w 0.0000023042373)
|
|
)
|
|
|
|
(define *sin-poly-vec2* (new 'static 'vector :x 0.999998))
|
|
|
|
(defun vector-sin-rad! ((arg0 vector) (arg1 vector))
|
|
"Taylor series approximation of sine on all 4 elements in a vector.
|
|
Inputs should be in radians, in -pi to pi.
|
|
Somehow their coefficients are a little bit off.
|
|
Like the first coefficient, which should obviously be 1, is not quite 1."
|
|
(rlet ((acc :class vf)
|
|
(vf1 :class vf)
|
|
(vf10 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
(vf6 :class vf)
|
|
(vf7 :class vf)
|
|
(vf8 :class vf)
|
|
(vf9 :class vf)
|
|
)
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
(.mul.vf vf3 vf1 vf1)
|
|
(.lvf vf10 (&-> *sin-poly-vec2* quad))
|
|
(.lvf vf9 (&-> *sin-poly-vec* quad))
|
|
(.mul.vf vf4 vf3 vf1)
|
|
(.mul.vf vf5 vf3 vf3)
|
|
(.mul.x.vf acc vf1 vf10)
|
|
(.mul.vf vf6 vf4 vf3)
|
|
(.mul.vf vf7 vf5 vf4)
|
|
(.add.mul.x.vf acc vf4 vf9 acc)
|
|
(.mul.vf vf8 vf6 vf5)
|
|
(.add.mul.y.vf acc vf6 vf9 acc)
|
|
(.add.mul.z.vf acc vf7 vf9 acc)
|
|
(.add.mul.w.vf vf2 vf8 vf9 acc)
|
|
(.svf (&-> arg0 quad) vf2)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
(defun cos-rad ((arg0 float))
|
|
"Cosine with taylor series. Input is in radians, in -pi, pi.
|
|
- TODO constants"
|
|
(local-vars
|
|
(f1-0 float)
|
|
(f3-0 float)
|
|
(f4-0 float)
|
|
(f5-0 float)
|
|
(f7-0 float)
|
|
(f8-0 float)
|
|
(f9-0 float)
|
|
(f10-0 float)
|
|
(f11-0 float)
|
|
(acc float)
|
|
)
|
|
(set! f1-0 (* arg0 arg0))
|
|
(set! f7-0 1.000000)
|
|
(set! f8-0 (the-as float #xbefffd62))
|
|
(set! f3-0 (* f1-0 f1-0))
|
|
(set! f9-0 (the-as float #x3d2a7a28))
|
|
(set! f10-0 (the-as float #xbab2bc31))
|
|
(set! f4-0 (* f3-0 f1-0))
|
|
(set! f5-0 (* f3-0 f3-0))
|
|
(set! f11-0 (the-as float #x37a933eb))
|
|
;;(.mula.s f7-0 f7-0)
|
|
(set! acc (* f7-0 f7-0))
|
|
;;(.madda.s f8-0 f1-0)
|
|
(set! acc (+ acc (* f8-0 f1-0)))
|
|
;;(.madda.s f9-0 f3-0)
|
|
(set! acc (+ acc (* f9-0 f3-0)))
|
|
;;(.madda.s f10-0 f4-0)
|
|
(set! acc (+ acc (* f10-0 f4-0)))
|
|
;;(.madd.s f12-0 f11-0 f5-0)
|
|
(+ acc (* f11-0 f5-0))
|
|
;;(the-as float f12-0)
|
|
)
|
|
|
|
|
|
(define *cos-poly-vec* (new 'static 'vector :x -0.49998003 :y 0.041620404 :z -0.0013636408 :w 0.000020170546))
|
|
|
|
(defun vector-cos-rad! ((arg0 vector) (arg1 vector))
|
|
"Compute the cosine of all 4 vector elements.
|
|
Radians, with no wrapping. Uses taylor series with 4 coefficients."
|
|
(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)
|
|
(vf9 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf1 (&-> arg1 quad))
|
|
;; (.sub.vf vf2 vf2 vf2)
|
|
(.xor.vf vf2 vf2 vf2)
|
|
(.lvf vf9 (&-> *cos-poly-vec* quad))
|
|
(.mul.vf vf3 vf1 vf1)
|
|
(.add.w.vf acc vf2 vf0)
|
|
(.mul.vf vf4 vf3 vf3)
|
|
(.add.mul.x.vf acc vf3 vf9 acc)
|
|
(.mul.vf vf5 vf4 vf3)
|
|
(.add.mul.y.vf acc vf4 vf9 acc)
|
|
(.mul.vf vf6 vf4 vf4)
|
|
(.add.mul.z.vf acc vf5 vf9 acc)
|
|
(.add.mul.w.vf vf2 vf6 vf9 acc)
|
|
(.svf (&-> arg0 quad) vf2)
|
|
arg0
|
|
)
|
|
)
|
|
|
|
(defun vector-sincos-rad! ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Compute the sine and cosine of each element of src, storing it in dst-sin and dst-cos.
|
|
This is more efficient than separate calls to sin and cos.
|
|
Inputs should be radians in -pi to pi."
|
|
(rlet ((acc :class vf)
|
|
(vf0 :class vf)
|
|
(vf1 :class vf)
|
|
(vf10 :class vf)
|
|
(vf11 :class vf)
|
|
(vf12 :class vf)
|
|
(vf13 :class vf)
|
|
(vf14 :class vf)
|
|
(vf2 :class vf)
|
|
(vf3 :class vf)
|
|
(vf4 :class vf)
|
|
(vf5 :class vf)
|
|
(vf6 :class vf)
|
|
(vf7 :class vf)
|
|
(vf8 :class vf)
|
|
(vf9 :class vf)
|
|
)
|
|
(init-vf0-vector)
|
|
(.lvf vf1 (&-> arg2 quad))
|
|
;; (.sub.vf vf14 vf14 vf14)
|
|
(.xor.vf vf14 vf14 vf14)
|
|
|
|
(.lvf vf11 (&-> *sin-poly-vec2* quad))
|
|
(.mul.vf vf2 vf1 vf1)
|
|
(.lvf vf10 (&-> *sin-poly-vec* quad))
|
|
(.lvf vf13 (&-> *cos-poly-vec* quad))
|
|
(.mul.x.vf acc vf1 vf11)
|
|
(.mul.vf vf3 vf2 vf1)
|
|
(.mul.vf vf4 vf2 vf2)
|
|
(.mul.vf vf5 vf3 vf2)
|
|
(.mul.vf vf6 vf3 vf3)
|
|
(.mul.vf vf7 vf4 vf3)
|
|
(.mul.vf vf8 vf4 vf4)
|
|
(.mul.vf vf9 vf5 vf4)
|
|
(.add.mul.x.vf acc vf3 vf10 acc)
|
|
(.add.mul.y.vf acc vf5 vf10 acc)
|
|
(.add.mul.z.vf acc vf7 vf10 acc)
|
|
(.add.mul.w.vf vf12 vf9 vf10 acc)
|
|
(.add.w.vf acc vf14 vf0)
|
|
(.add.mul.x.vf acc vf2 vf13 acc)
|
|
(.add.mul.y.vf acc vf4 vf13 acc)
|
|
(.add.mul.z.vf acc vf6 vf13 acc)
|
|
(.add.mul.w.vf vf14 vf8 vf13 acc)
|
|
(.svf (&-> arg0 quad) vf12)
|
|
(.svf (&-> arg1 quad) vf14)
|
|
0
|
|
)
|
|
)
|
|
|
|
(defmacro sincos-rad-asm (out x)
|
|
;; Compute the sine and cosine of x, store it in the output array
|
|
;; this assembly is shared in two functions.
|
|
`(rlet ((f10 :class fpr :type float) ;; coeff 1.0
|
|
(f11 :class fpr :type float) ;; coeff -1/3!
|
|
(f12 :class fpr :type float) ;; coeff 1/5!
|
|
(f14 :class fpr :type float) ;; coeff -1/7!
|
|
(f15 :class fpr :type float) ;; coeff 1/9!
|
|
(f1 :class fpr :type float) ;; x
|
|
(f2 :class fpr :type float) ;; x^2
|
|
(f3 :class fpr :type float) ;; x^3
|
|
(f4 :class fpr :type float) ;; x^4
|
|
(f5 :class fpr :type float) ;; x^5
|
|
(f6 :class fpr :type float) ;; x^6
|
|
(f7 :class fpr :type float) ;; x^7
|
|
(f8 :class fpr :type float) ;; x^8
|
|
(f9 :class fpr :type float) ;; x^9
|
|
(f21 :class fpr :type float)
|
|
(f22 :class fpr :type float) ;; 0 ?
|
|
(acc :class fpr :type float) ;; temp
|
|
(f16 :class fpr :type float) ;; 1.0
|
|
(f17 :class fpr :type float) ;; cos coeff 1
|
|
(f18 :class fpr :type float) ;; cos coeff 2
|
|
(f19 :class fpr :type float) ;; cos coeff 3
|
|
(f20 :class fpr :type float) ;; cos coeff 4
|
|
)
|
|
;; lui v1, 16255
|
|
;; lui a2, -16854
|
|
;; ori v1, v1, 65502
|
|
;; mtc1 f1, a1
|
|
(set! f1 ,x)
|
|
;; ori a1, a2, 43253
|
|
;; sub.s f22, f22, f22
|
|
(set! f22 (the-as float 0))
|
|
;; lui a2, 15368
|
|
;; mtc1 f10, v1
|
|
(set! f10 (the-as float #x3F7FFFDE)) ;; almost 1.0
|
|
;; ori v1, a2, 27638
|
|
;; mtc1 f11, a1
|
|
(set! f11 (the-as float #xBE2AA8F5)) ;; -0.166, 1/3!
|
|
;; lui a1, -18099
|
|
;; mul.s f2, f1, f1
|
|
(set! f2 (* f1 f1))
|
|
;; ori a1, a1, 8306
|
|
;; mtc1 f12, v1
|
|
(set! f12 (the-as float #x3C086BF6)) ;; 1/5!
|
|
;; lui v1, 13850
|
|
;; mtc1 f14, a1
|
|
(set! f14 (the-as float #xB94D2072)) ;; 1/7!
|
|
;; ori a1, v1, 41599
|
|
;; mula.s f1, f10
|
|
(set! acc (* f1 f10)) ;; x * c_1
|
|
;; lui v1, 16256
|
|
;; mul.s f3, f2, f1
|
|
(set! f3 (* f2 f1)) ;; x^3
|
|
;; or v1, v1, r0
|
|
;; mul.s f4, f2, f2
|
|
(set! f4 (* f2 f2)) ;; x^4
|
|
;; lui a2, -16641
|
|
;; mtc1 f15, a1
|
|
(set! f15 (the-as float #x361AA27F)) ;; 1/9!
|
|
;; lui a1, -16641 ;; I think this is a typo...
|
|
;; or a1, a2, a1 ;; this should set the lower 16 bits.
|
|
;; mtc1 f16, v1
|
|
(set! f16 (the-as float #x3f800000)) ;; 1.0
|
|
;; sll r0, r0, 0
|
|
;; mtc1 f17, a1
|
|
|
|
;; it looks like they set the lower 16-bits of the x^2
|
|
;; coefficient for cosine incorrectly
|
|
(#cond
|
|
(FIX_COSINE_BUG
|
|
;; the constant used in *cos-poly-vec*
|
|
(set! f17 (the-as float #xbefffd62))
|
|
)
|
|
(#t
|
|
;; missing the lower 16 bits.
|
|
(set! f17 (the-as float #xBEFF0000))
|
|
)
|
|
)
|
|
|
|
;; sll r0, r0, 0
|
|
;; mul.s f5, f3, f2
|
|
(set! f5 (* f3 f2))
|
|
;; sll r0, r0, 0
|
|
;; mul.s f6, f3, f3
|
|
(set! f6 (* f3 f3))
|
|
;; sll r0, r0, 0
|
|
;; mul.s f7, f4, f3
|
|
(set! f7 (* f4 f3))
|
|
;; sll r0, r0, 0
|
|
;; mul.s f8, f4, f4
|
|
(set! f8 (* f4 f4))
|
|
;; sll r0, r0, 0
|
|
;; mul.s f9, f5, f4
|
|
(set! f9 (* f5 f4))
|
|
;; lui v1, 15658
|
|
;; madda.s f3, f11
|
|
(set! acc (+ acc (* f3 f11))) ;; add x^3 sine term
|
|
;; ori v1, v1, 31272
|
|
;; madda.s f5, f12
|
|
(set! acc (+ acc (* f5 f12))) ;; add x^5 sine term
|
|
;; lui a1, -17742
|
|
;; madda.s f7, f14
|
|
(set! acc (+ acc (* f7 f14))) ;; add x^7 sine term
|
|
;; ori a1, a1, 48177
|
|
;; madd.s f21, f9, f15
|
|
(set! f21 (+ acc (* f9 f15))) ;; add x^9 sine term
|
|
;; lui a2, 14249
|
|
;; mtc1 f18, v1
|
|
(set! f18 (the-as float #x3D2A7A28)) ;; cos coeff
|
|
;; ori v1, a2, 13291
|
|
;; mtc1 f19, a1
|
|
(set! f19 (the-as float #xBAB2BC31)) ;; cos coeff
|
|
;; sll r0, r0, 0
|
|
;; mtc1 f20, v1
|
|
(set! f20 (the-as float #x37A933EB))
|
|
;; sll r0, r0, 0
|
|
;; mula.s f16, f16
|
|
(set! acc (* f16 f16)) ;; acc = 1, constant cos term.
|
|
;; sll r0, r0, 0
|
|
;; madda.s f2, f17
|
|
(set! acc (+ acc (* f2 f17)))
|
|
;; sll r0, r0, 0
|
|
;; madda.s f4, f18
|
|
(set! acc (+ acc (* f4 f18)))
|
|
;; sll r0, r0, 0
|
|
;; madda.s f6, f19
|
|
(set! acc (+ acc (* f6 f19)))
|
|
;; sll r0, r0, 0
|
|
;; madd.s f22, f8, f20
|
|
(set! f22 (+ acc (* f8 f20)))
|
|
;; sll r0, r0, 0
|
|
;; swc1 f21, 0(a0)
|
|
(set! (-> ,out x) f21)
|
|
;; sll r0, r0, 0
|
|
;; swc1 f22, 4(a0)
|
|
(set! (-> ,out y) f22)
|
|
;; or v0, r0, r0
|
|
0
|
|
)
|
|
)
|
|
|
|
(defun sincos-rad! ((out vector) (x float))
|
|
"Compute the sine and cosine of x, store it in the output array.
|
|
Has the cosine bug."
|
|
(sincos-rad-asm out x)
|
|
)
|
|
|
|
(defun sincos! ((out vector) (x float))
|
|
"Compute the sine and cosine of x, store it in the output array.
|
|
The input is in rotation units, and is unwrapped properly.
|
|
Also has the cosine bug"
|
|
(sincos-rad-asm out (* ROT_TO_RAD (unwrap-angle x)))
|
|
)
|
|
|
|
(defun vector-rad<-vector-deg! ((out vector) (in vector))
|
|
"Convert a vector in rotation units to radians, and unwrap.
|
|
Input can be anything, output will be -2pi to pi."
|
|
(rlet ((rot-to-rad :class vf)
|
|
(vf1 :class vf))
|
|
(.mov rot-to-rad (the-as float ROT_TO_RAD))
|
|
(.lvf vf1 in)
|
|
(.ftoi.vf vf1 vf1) ;; to int
|
|
(.pw.sll vf1 vf1 16) ;; shifts
|
|
(.pw.sra vf1 vf1 16)
|
|
(.itof.vf vf1 vf1) ;; to float
|
|
(.mul.x.vf vf1 vf1 rot-to-rad)
|
|
(.svf out vf1)
|
|
)
|
|
)
|
|
|
|
(defun vector-rad<-vector-deg/2! ((out vector) (in vector))
|
|
"Divide the input by two, and then convert from rotation units to radians, unwrapping.
|
|
Not sure why this really needs to be separate the from previous function..."
|
|
(rlet ((temp :class vf)
|
|
(vf1 :class vf))
|
|
;;(set! v1-0 952700890)
|
|
;;(set! a2-0 1056964608)
|
|
;;(.lqc2 vf1 0 a1-0)
|
|
(.lvf vf1 in)
|
|
|
|
;; multiply by 0.5.
|
|
;;(.qmtc2.i vf2 a2-0)
|
|
(.mov temp (the-as float #x3f000000)) ;; 0.5
|
|
;;(.vmulx.xyzw vf1 vf1 vf2)
|
|
(.mul.x.vf vf1 vf1 temp)
|
|
|
|
;;(.vftoi0.xyzw vf1 vf1)
|
|
(.ftoi.vf vf1 vf1)
|
|
;;(.qmtc2.i vf2 v1-0)
|
|
;;(.qmfc2.i v1-1 vf1)
|
|
;;(.psllw v1-2 v1-1 16)
|
|
(.pw.sll vf1 vf1 16)
|
|
;;(.psraw v1-3 v1-2 16)
|
|
(.pw.sra vf1 vf1 16)
|
|
;;(.qmtc2.i vf1 v1-3)
|
|
;;(.vitof0.xyzw vf1 vf1)
|
|
(.itof.vf vf1 vf1)
|
|
;;(.vmulx.xyzw vf1 vf1 vf2)
|
|
(.mov temp (the-as float ROT_TO_RAD))
|
|
(.mul.x.vf vf1 vf1 temp)
|
|
;;(.sqc2 vf1 0 arg0)
|
|
;;(.qmfc2.i v0-0 vf1)
|
|
(.svf out vf1)
|
|
0
|
|
)
|
|
)
|
|
|
|
(defun vector-sincos! ((arg0 vector) (arg1 vector) (arg2 vector))
|
|
"Compute sine and cosine of each element in a vector, in rotation units"
|
|
(let ((s4-0 (new 'stack-no-clear 'vector)))
|
|
(vector-rad<-vector-deg! s4-0 arg2)
|
|
(vector-sincos-rad! arg0 arg1 s4-0)
|
|
)
|
|
)
|
|
|
|
(defun tan-rad ((arg0 float))
|
|
"This function appears to be named wrong and actually operates on rotation units."
|
|
(/ (sin arg0) (cos arg0))
|
|
)
|
|
|
|
(defun cos ((arg0 float))
|
|
"Cosine of rotation units"
|
|
(sin (+ 16384.0 arg0))
|
|
)
|
|
|
|
(defun tan ((arg0 float))
|
|
"Correctly named tangent of rotation units"
|
|
(/ (sin arg0) (cos arg0))
|
|
)
|
|
|
|
(defun atan0 ((arg0 float) (arg1 float))
|
|
"inverse tangent, to rotation units. y,x order. Does not handle signs correctly.
|
|
Do not use this function directly, instead use atan2"
|
|
(rlet ((f20 :class fpr :type float)
|
|
(f21 :class fpr :type float)
|
|
(f1 :class fpr :type float)
|
|
(f2 :class fpr :type float)
|
|
(f3 :class fpr :type float)
|
|
(f4 :class fpr :type float)
|
|
(f5 :class fpr :type float)
|
|
(f6 :class fpr :type float)
|
|
(f7 :class fpr :type float)
|
|
(f8 :class fpr :type float)
|
|
(f9 :class fpr :type float)
|
|
(f10 :class fpr :type float)
|
|
(f19 :class fpr :type float)
|
|
(f11 :class fpr :type float)
|
|
(f12 :class fpr :type float)
|
|
(f13 :class fpr :type float)
|
|
(f14 :class fpr :type float)
|
|
(f15 :class fpr :type float)
|
|
(f16 :class fpr :type float)
|
|
(f17 :class fpr :type float)
|
|
(f18 :class fpr :type float)
|
|
(acc :class fpr :type float)
|
|
)
|
|
|
|
;;mtc1 f20, a1
|
|
(set! f20 arg1)
|
|
;;mtc1 f21, a0
|
|
(set! f21 arg0)
|
|
;;sub.s f1, f21, f20
|
|
(set! f1 (- f21 f20))
|
|
;;add.s f2, f21, f20
|
|
(set! f2 (+ f21 f20))
|
|
;;div.s f1, f1, f2
|
|
(set! f1 (/ f1 f2))
|
|
;;lwc1 f19, L132(fp)
|
|
(set! f19 (the-as float #x46000000))
|
|
;;lwc1 f11, L140(fp)
|
|
(set! f11 (the-as float #x4622f97c))
|
|
;;lwc1 f12, L151(fp)
|
|
(set! f12 (the-as float #xc55946e1))
|
|
;;lwc1 f13, L120(fp)
|
|
(set! f13 (the-as float #x450207fd))
|
|
;;lwc1 f14, L126(fp)
|
|
(set! f14 (the-as float #xc4b556ce))
|
|
;;lwc1 f15, L113(fp)
|
|
(set! f15 (the-as float #x447b6ca4))
|
|
;;mul.s f2, f1, f1
|
|
(set! f2 (* f1 f1))
|
|
;;lwc1 f16, L142(fp)
|
|
(set! f16 (the-as float #xc411ca52))
|
|
;;lwc1 f17, L118(fp)
|
|
(set! f17 (the-as float #x43640558))
|
|
;;mul.s f3, f1, f2
|
|
(set! f3 (* f1 f2))
|
|
;;mul.s f1, f1, f11
|
|
(set! f1 (* f1 f11))
|
|
;;mul.s f4, f2, f2
|
|
(set! f4 (* f2 f2))
|
|
;;lwc1 f18, L141(fp)
|
|
(set! f18 (the-as float #xc2292434))
|
|
;;mul.s f5, f3, f2
|
|
(set! f5 (* f3 f2))
|
|
;;mul.s f6, f4, f3
|
|
(set! f6 (* f4 f3))
|
|
;;mul.s f7, f5, f4
|
|
(set! f7 (* f5 f4))
|
|
;;mul.s f8, f6, f4
|
|
(set! f8 (* f6 f4))
|
|
;;mul.s f9, f7, f4
|
|
(set! f9 (* f7 f4))
|
|
;;mul.s f10, f8, f4
|
|
(set! f10 (* f8 f4))
|
|
;;adda.s f1, f19
|
|
(set! acc (+ f1 f19))
|
|
;;madda.s f3, f12
|
|
(set! acc (+ acc (* f3 f12)))
|
|
|
|
;;madda.s f5, f13
|
|
(set! acc (+ acc (* f5 f13)))
|
|
|
|
;;madda.s f6, f14
|
|
(set! acc (+ acc (* f6 f14)))
|
|
|
|
;;madda.s f7, f15
|
|
(set! acc (+ acc (* f7 f15)))
|
|
|
|
;;madda.s f8, f16
|
|
(set! acc (+ acc (* f8 f16)))
|
|
|
|
;;madda.s f9, f17
|
|
(set! acc (+ acc (* f9 f17)))
|
|
|
|
;;madd.s f19, f10, f18
|
|
;;mfc1 v0, f19
|
|
(+ acc (* f10 f18))
|
|
)
|
|
)
|
|
|
|
|
|
(defmacro .adda.s (a b)
|
|
`(set! acc (+ ,a ,b))
|
|
)
|
|
|
|
(defmacro .madda.s (a b)
|
|
`(set! acc (+ acc (* ,a ,b)))
|
|
)
|
|
|
|
(defmacro .madd.s (a b c)
|
|
`(set! ,a (+ acc (* ,b ,c)))
|
|
)
|
|
|
|
(defun atan-series-rad ((arg0 float))
|
|
"A helper function for atan"
|
|
(local-vars
|
|
(f0-1 float)
|
|
(f1-0 float)
|
|
(f2-0 float)
|
|
(f3-0 float)
|
|
(f4-0 float)
|
|
(f5-0 float)
|
|
(f6-0 float)
|
|
(f7-0 float)
|
|
(f8-0 float)
|
|
(f9-0 float)
|
|
(f10-0 float)
|
|
(f11-0 float)
|
|
(f12-0 float)
|
|
(f13-0 float)
|
|
(f14-0 float)
|
|
(f15-0 float)
|
|
(f16-0 float)
|
|
(f17-0 float)
|
|
(f18-0 float)
|
|
(acc float)
|
|
(f18-1 float)
|
|
)
|
|
(set! f1-0 (* arg0 arg0))
|
|
(set! f10-0 (the-as float #x3f7ffff5))
|
|
(set! f11-0 (the-as float #xbeaaa61c))
|
|
(set! f2-0 (* arg0 f1-0))
|
|
(set! f3-0 (* f1-0 f1-0))
|
|
(set! f12-0 (the-as float #x3e4c40a6))
|
|
(set! f4-0 (* f2-0 f1-0))
|
|
(set! f5-0 (* f3-0 f2-0))
|
|
(set! f13-0 (the-as float #xbe0e6c63))
|
|
(set! f6-0 (* f4-0 f3-0))
|
|
(set! f7-0 (* f5-0 f3-0))
|
|
(set! f14-0 (the-as float #x3dc577df))
|
|
(set! f8-0 (* f6-0 f3-0))
|
|
(set! f9-0 (* f7-0 f3-0))
|
|
(set! f15-0 (the-as float #xbd6501c4))
|
|
(set! f18-0 (the-as float #x3f490fdb))
|
|
(set! f0-1 (* arg0 f10-0))
|
|
(set! f16-0 (the-as float #x3cb31652))
|
|
(set! f17-0 (the-as float #xbb84d7e7))
|
|
(.adda.s f0-1 f18-0)
|
|
(.madda.s f2-0 f11-0)
|
|
(.madda.s f4-0 f12-0)
|
|
(.madda.s f5-0 f13-0)
|
|
(.madda.s f6-0 f14-0)
|
|
(.madda.s f7-0 f15-0)
|
|
(.madda.s f8-0 f16-0)
|
|
(.madd.s f18-1 f9-0 f17-0)
|
|
;;(the-as float f18-1)
|
|
f18-1
|
|
)
|
|
|
|
(defun atan-rad ((arg0 float))
|
|
"inverse tangent in radians"
|
|
(atan-series-rad (/ (+ -1.0 arg0) (+ 1.0 arg0)))
|
|
)
|
|
|
|
(defun sign-bit ((arg0 int))
|
|
"Return 1 if bit 31 is set, otherwise 0."
|
|
;; originally used 32-bit shift, doesn't really matter.
|
|
(logand (shr arg0 31) 1)
|
|
|
|
;(local-vars (v1-1 int))
|
|
; (let ((v1-0 arg0))
|
|
; (shift-arith-right-32 v1-1 v1-0 31)
|
|
; )
|
|
; (logand v1-1 1)
|
|
)
|
|
|
|
(defun sign-float ((arg0 float))
|
|
"Return 1 if arg0 is positive or zero, -1 otherwise.
|
|
Fast (no branching)"
|
|
(let ((a1-0 #xffffffff80000000)
|
|
(v1-0 #x3f800000)
|
|
)
|
|
(the-as float (logior (logand (the-as uint arg0) (the-as uint a1-0)) v1-0))
|
|
)
|
|
)
|
|
|
|
(defun sign ((arg0 float))
|
|
"Similar to above, but returns 0 if input is 0.
|
|
But is more complicated."
|
|
(cond
|
|
((< 0.0 arg0)
|
|
1.0
|
|
)
|
|
((< arg0 0.0)
|
|
-1.0
|
|
)
|
|
(else
|
|
0.0
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun atan2-rad ((arg0 float) (arg1 float))
|
|
"Atan for radians"
|
|
(cond
|
|
((= arg1 0.0)
|
|
(* 1.5707963 (sign arg0))
|
|
)
|
|
((and (< arg0 0.0) (< arg1 0.0))
|
|
(let ((f30-1 -3.1415925)
|
|
(f0-6 (/ arg0 arg1))
|
|
)
|
|
(+ f30-1 (atan-series-rad (/ (+ -1.0 f0-6) (+ 1.0 f0-6))))
|
|
)
|
|
)
|
|
((< arg0 0.0)
|
|
(let ((f0-14 (- (/ arg0 arg1))))
|
|
(- (atan-series-rad (/ (+ -1.0 f0-14) (+ 1.0 f0-14))))
|
|
)
|
|
)
|
|
((< arg1 0.0)
|
|
(let ((f30-2 3.1415925)
|
|
(f0-22 (- (/ arg0 arg1)))
|
|
)
|
|
(- f30-2 (atan-series-rad (/ (+ -1.0 f0-22) (+ 1.0 f0-22))))
|
|
)
|
|
)
|
|
(else
|
|
(let ((f0-28 (/ arg0 arg1)))
|
|
(atan-series-rad (/ (+ -1.0 f0-28) (+ 1.0 f0-28)))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun atan ((arg0 float) (arg1 float))
|
|
"Atan for rotation units. Signs behave like atan.."
|
|
(cond
|
|
((and (= arg1 0.0) (= arg0 0.0))
|
|
0.0
|
|
)
|
|
((and (< arg1 0.0) (< arg0 0.0))
|
|
(+ -32768.0 (atan0 (- arg0) (- arg1)))
|
|
)
|
|
((< arg0 0.0)
|
|
(- (atan0 (- arg0) arg1))
|
|
)
|
|
((< arg1 0.0)
|
|
(- 32768.0 (atan0 arg0 (- arg1)))
|
|
)
|
|
(else
|
|
(atan0 arg0 arg1)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun asin ((arg0 float))
|
|
"Inverse sine, rotation units."
|
|
(let ((gp-0 #f))
|
|
0.0
|
|
(when (< arg0 0.0)
|
|
(set! arg0 (- arg0))
|
|
(set! gp-0 #t)
|
|
)
|
|
(let ((f0-5 (cond
|
|
((< 1.0 arg0)
|
|
16383.996
|
|
)
|
|
(else
|
|
(let* ((f0-6 1.0) (f1-2 arg0) (f0-8 (sqrtf (- f0-6 (* f1-2 f1-2))))) (atan0 arg0 f0-8))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
(if gp-0
|
|
(- f0-5)
|
|
f0-5
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun acos ((arg0 float))
|
|
"Inverse cosine. Returns rotation units"
|
|
(let ((result (- 16384.000000 (asin arg0))))
|
|
(#when PC_PORT
|
|
;; to avoid punch glitch:
|
|
;; (note: it might be a better fix to change the global rounding mode,
|
|
;; but it's not super clear to me that the mode picked by PCSX2 is
|
|
;; more accurate than normal in all cases. So, we'll do this for now.)
|
|
(when (= result 0.0)
|
|
(set! result 0.00000000001)
|
|
)
|
|
)
|
|
result
|
|
)
|
|
)
|
|
|
|
|
|
(defun acos-rad ((arg0 float))
|
|
"Inverse cosine, radians."
|
|
(cond
|
|
((>= arg0 0.0)
|
|
(let* ((f0-1 1.0)
|
|
(f1-1 arg0)
|
|
(f0-3 (sqrtf (- f0-1 (* f1-1 f1-1))))
|
|
(f0-5 (/ (- f0-3 arg0) (+ f0-3 arg0)))
|
|
)
|
|
(atan-series-rad f0-5)
|
|
)
|
|
)
|
|
(else
|
|
(let* ((f0-6 1.0)
|
|
(f1-6 arg0)
|
|
(f0-8 (sqrtf (- f0-6 (* f1-6 f1-6))))
|
|
(f0-10 (/ (+ f0-8 arg0) (- f0-8 arg0)))
|
|
)
|
|
(- 3.1415925 (atan-series-rad f0-10))
|
|
)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun sinerp ((arg0 float) (arg1 float) (arg2 float))
|
|
"map amount to min,max using sine. Kinda weird, usually people use cosine."
|
|
(lerp arg0 arg1 (sin (* 16384.0 arg2)))
|
|
)
|
|
|
|
(defun sinerp-clamp ((arg0 float) (arg1 float) (arg2 float))
|
|
"Like sinerp, but clamp to min,max"
|
|
(cond
|
|
((>= 0.0 arg2)
|
|
arg0
|
|
)
|
|
((>= arg2 1.0)
|
|
arg1
|
|
)
|
|
(else
|
|
(sinerp arg0 arg1 arg2)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun coserp ((arg0 float) (arg1 float) (arg2 float))
|
|
"Weird lerp with cosine (over 90 degrees?)"
|
|
(lerp arg0 arg1 (- 1.0 (cos (* 16384.0 arg2))))
|
|
)
|
|
|
|
(defun coserp-clamp ((arg0 float) (arg1 float) (arg2 float))
|
|
"Weird 90 degree lerp with cosine, clamped to min,max"
|
|
(cond
|
|
((>= 0.0 arg2)
|
|
arg0
|
|
)
|
|
((>= arg2 1.0)
|
|
arg1
|
|
)
|
|
(else
|
|
(coserp arg0 arg1 arg2)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun coserp180 ((arg0 float) (arg1 float) (arg2 float))
|
|
"Classic lerp with cosine"
|
|
(lerp arg0 arg1 (* 0.5 (- 1.0 (cos (* 32768.0 arg2)))))
|
|
)
|
|
|
|
(defun coserp180-clamp ((arg0 float) (arg1 float) (arg2 float))
|
|
"Classic coserp with saturation"
|
|
(cond
|
|
((>= 0.0 arg2)
|
|
arg0
|
|
)
|
|
((>= arg2 1.0)
|
|
arg1
|
|
)
|
|
(else
|
|
(coserp180 arg0 arg1 arg2)
|
|
)
|
|
)
|
|
)
|
|
|
|
(defun ease-in-out ((arg0 int) (arg1 int))
|
|
"Weird coserp like mapping from 0 to 1 as progress goes from 0 to total"
|
|
(local-vars (v1-0 int))
|
|
(cond
|
|
((>= arg1 arg0)
|
|
1.0
|
|
)
|
|
((<= arg1 0)
|
|
0.0
|
|
)
|
|
((begin (set! v1-0 (/ arg0 2)) (< v1-0 arg1))
|
|
(let ((a0-1 (- arg1 arg0)))
|
|
(+ 0.5 (* 0.5 (sin (- 16384.0 (/ (* 16384.0 (the float a0-1)) (the float v1-0))))))
|
|
)
|
|
)
|
|
(else
|
|
(- 0.5 (* 0.5 (cos (/ (* 16384.0 (the float arg1)) (the float v1-0)))))
|
|
)
|
|
)
|
|
)
|
|
|
|
|
|
|
|
|