mirror/jak-project
1
0
Fork 0
mirror of https://github.com/open-goal/jak-project synced 2026-08-06 18:03:30 -04:00
Code Issues Packages Projects Releases Wiki Activity
Files
dfb75fc594b19bdfd0f38437e96e3c3c21e34bec
jak-project/goal_src/jak1/engine/math/trigonometry.gc
T
water111 a11c6de73e file updates
2026-07-26 14:43:53 -04:00

1178 lines
40 KiB
Common Lisp
Raw Blame History

;;-*-Lisp-*-
(in-package goal)
(bundles "ENGINE.CGO" "GAME.CGO")
(require "engine/math/vector-h.gc")
(require "engine/math/math.gc")
;; 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.
;; The shared scalar sine/cosine routine constructs its x^2 cosine coefficient with only
;; `lui #xbeff`, producing -0.498046875 instead of the intended #xbefffd62 value used by
;; cos-rad. The standalone cosine routines are unaffected.
(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))
;; DECOMP BEGINS
(defun radmod ((angle float))
"Wrap a radian angle into the signed pi interval."
(let ((f0-1 (+ PI angle)))
(if (< 0.0 f0-1)
(+ MINUS_PI (- f0-1 (* (the float (the int (/ f0-1 6.283185))) 6.283185)))
(+ PI (- f0-1 (* (the float (the int (/ f0-1 6.283185))) 6.283185))))))
(defun deg- ((angle float) (reference float))
"Return the shortest signed rotation-unit difference angle minus reference."
(the float (sar (- (shl (the int angle) 48) (shl (the int reference) 48)) 48)))
(defun deg-diff ((from float) (to float))
"Return the shortest signed rotation-unit difference from the first angle to the second."
(the float (sar (- (shl (the int to) 48) (shl (the int from) 48)) 48)))
(defun deg-seek ((in float) (target float) (max-diff float))
"Move in toward target by at most max-diff along the shortest wrapped direction."
(let ((in-int (shl (the int in) 48))
(target-int (shl (the int target) 48)))
(let* ((max-diff-int (shl (the int max-diff) 48))
(diff (- target-int in-int))
(abs-diff (abs diff)))
(set! target-int
(cond
((< abs-diff 0) (+ in-int max-diff-int))
((>= max-diff-int abs-diff) target-int)
((>= diff 0) (+ in-int max-diff-int))
(else (- in-int max-diff-int)))))
(the float (sar target-int 48))))
(defun deg-seek-smooth ((in float) (target float) (max-diff float) (amount float))
"Move amount of the shortest wrapped difference toward target, capped by max-diff."
(let ((step (* (deg- target in) amount)))
(if (< max-diff (fabs step)) (set! step (if (>= step 0.0) max-diff (- max-diff))))
(+ in step)))
(defun deg-lerp-clamp ((min-val float) (max-val float) (in float))
"Interpolate along the shortest wrapped rotation-unit arc, clamping in to the endpoints."
(cond
((>= 0.00000000 in) min-val)
((>= in 1.00000000) max-val)
(else (the float (sar (shl (the int (+ min-val (* in (deg-diff min-val max-val)))) 48) 48)))))
;; create a static array with the given values, interpreted as floats.
(defmacro make-float-table (name size vals)
`(define ,name (new 'static 'boxed-array :type float :length ,size ,@(apply (lambda (x) `(the-as float ,x)) vals))))
;; table[x] = 1/(2^x)
;; unused?
(make-float-table binary-table
32
(#x3f800000 ;; 1.0
#x3f000000 ;; 0.5
#x3e800000 ;; 0.25
#x3e000000 ;; ...
#x3d800000
#x3d000000
#x3c800000
#x3c000000
#x3b800000
#x3b000000
#x3a800000
#x3a000000
#x39800000
#x39000000
#x38800000
#x38000000
#x37800000
#x37000000
#x36800000
#x36000000
#x35800000
#x35000000
#x34800000
#x34000000
#x33800000
#x33000000
#x32800000
#x32000000
#x31800000
#x31000000
#x30800000
#x30000000))
;; CORDIC angle table: sincos-table[i] = atan(2^-i) radians. From entry 12 onward,
;; atan(x) rounds to x in single precision, so the tail matches binary-table. The shipping
;; trigonometric routines use polynomials instead; both tables are retained dead data from
;; an earlier shift-and-add implementation.
(make-float-table sincos-table
32
(#x3f490fdb
#x3eed6338
#x3e7adbb0
#x3dfeadd5
#x3d7faade
#x3cffeaae
#x3c7ffaab
#x3bfffeab
#x3b7fffab
#x3affffeb
#x3a7ffffb
#x39fffffe
#x39800000
#x39000000
#x38800000
#x38000000
#x37800000
#x37000000
#x36800000
#x36000000
#x35800000
#x35000000
#x34800000
#x34000000
#x33800000
#x33000000
#x32800000
#x32000000
#x31800000
#x31000000
#x30800000
#x30000000))
(defun sin ((angle float))
"Return sine for an angle in rotation units after wrapping it to a signed half-turn."
;; Sign-extend the low 16 bits, then convert rotation units to radians before evaluating
;; the degree-nine odd minimax polynomial.
(let ((f2-0 (* 0.000095873795 (the float (sar (shl (the int angle) 48) 48)))))
(let ((f0-1 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 ((angle float))
"Return sine for a radian angle in the supported -pi to pi interval with a degree-nine odd minimax polynomial."
(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)
(result float)
(acc float))
(set! f1-0 (* angle angle))
(set! f7-0 (the-as float #x3f7fffde))
(set! f8-0 (the-as float #xbe2aa8f5))
(set! f2-0 (* angle 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))
(#unless PC_PORT
(mula.s angle f7-0)
(madda.s f2-0 f8-0)
(madda.s f4-0 f9-0)
(madda.s f5-0 f10-0)
(madd.s result f6-0 f11-0))
(#when PC_PORT
(set! acc (* angle f7-0))
(set! acc (+ acc (* f2-0 f8-0)))
(set! acc (+ acc (* f4-0 f9-0)))
(set! acc (+ acc (* f5-0 f10-0)))
(set! result (+ acc (* f6-0 f11-0))))
result)
;; Minimax coefficients for the odd sine approximation.
(define *sin-poly-vec*
(new 'static
'vector
:x
(the-as float #xbe2aa8f5) ;; -1/3!
:y
(the-as float #x3c086bf6) ;; 1/5!
:z
(the-as float #xb94d2072) ;; -1/7!
:w
(the-as float #x361aa27f) ;; approximately 1/9!
))
;;
(define *sin-poly-vec2* (new 'static 'vector :x (the-as float #x3f7fffde) :y 0.0 :z 0.0 :w 0.0))
(#unless PC_PORT
(defun vector-sin-rad! ((dst vector) (src vector))
"Compute per-lane sine for radian inputs in the supported -pi to pi interval.
The degree-nine odd polynomial uses minimax-adjusted coefficients near the Taylor values."
(rlet ((input :class vf)
(result :class vf)
(input-squared :class vf)
(input-cubed :class vf)
(input-fourth :class vf)
(input-fifth :class vf)
(input-seventh :class vf)
(input-ninth :class vf)
(coefficients :class vf)
(linear-coefficient :class vf)
(coefficient-address)
(acc :class vf))
(.lvf input src)
(.mul.vf input-squared input input)
(m! coefficient-address *sin-poly-vec2*)
(.lvf linear-coefficient coefficient-address)
(m! coefficient-address *sin-poly-vec*)
(.lvf coefficients coefficient-address)
(.mul.vf input-cubed input-squared input)
(.mul.vf input-fourth input-squared input-squared)
(.mul.x.vf acc input linear-coefficient)
(.mul.vf input-fifth input-cubed input-squared)
(.mul.vf input-seventh input-fourth input-cubed)
(.add.mul.x.vf acc input-cubed coefficients acc)
(.mul.vf input-ninth input-fifth input-fourth)
(.add.mul.y.vf acc input-fifth coefficients acc)
(.add.mul.z.vf acc input-seventh coefficients acc)
(.add.mul.w.vf result input-ninth coefficients acc)
(.svf dst result)
dst)))
(#when PC_PORT
(defun vector-sin-rad! ((dst vector) (src vector))
"Compute per-lane sine for radian inputs in the supported -pi to pi interval.
The degree-nine odd polynomial uses minimax-adjusted coefficients near the Taylor values."
(rlet ((input :class vf)
(result :class vf)
(input-squared :class vf)
(input-cubed :class vf)
(input-fourth :class vf)
(input-fifth :class vf)
(input-seventh :class vf)
(input-ninth :class vf)
(coefficients :class vf)
(linear-coefficient :class vf)
(acc :class vf))
(.lvf input src)
(.mul.vf input-squared input input)
(.lvf linear-coefficient *sin-poly-vec2*)
(.lvf coefficients *sin-poly-vec*)
(.mul.vf input-cubed input-squared input)
(.mul.vf input-fourth input-squared input-squared)
(.mul.x.vf acc input linear-coefficient)
(.mul.vf input-fifth input-cubed input-squared)
(.mul.vf input-seventh input-fourth input-cubed)
(.add.mul.x.vf acc input-cubed coefficients acc)
(.mul.vf input-ninth input-fifth input-fourth)
(.add.mul.y.vf acc input-fifth coefficients acc)
(.add.mul.z.vf acc input-seventh coefficients acc)
(.add.mul.w.vf result input-ninth coefficients acc)
(.svf dst result)
dst)))
(defun cos-rad ((angle float))
"Return cosine for a radian angle in the supported -pi to pi interval with a degree-eight even minimax polynomial."
(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)
(result float)
(acc float))
(set! f1-0 (* angle angle))
(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))
(#unless PC_PORT
(mula.s f7-0 f7-0)
(madda.s f8-0 f1-0)
(madda.s f9-0 f3-0)
(madda.s f10-0 f4-0)
(madd.s result f11-0 f5-0))
(#when PC_PORT
(set! acc (* f7-0 f7-0))
(set! acc (+ acc (* f8-0 f1-0)))
(set! acc (+ acc (* f9-0 f3-0)))
(set! acc (+ acc (* f10-0 f4-0)))
(set! result (+ acc (* f11-0 f5-0))))
result)
(define *cos-poly-vec*
(new 'static
'vector
:x
(the-as float #xbefffd62)
:y
(the-as float #x3d2a7a28)
:z
(the-as float #xbab2bc31)
:w
(the-as float #x37a933eb)))
(#unless PC_PORT
(defun vector-cos-rad! ((dst vector) (src vector))
"Compute per-lane cosine for radian inputs in the supported -pi to pi interval."
(rlet ((vf0 :reg vf0)
(input :class vf)
(result :class vf)
(input-squared :class vf)
(input-fourth :class vf)
(input-sixth :class vf)
(input-eighth :class vf)
(coefficients :class vf)
(coefficient-address)
(acc :class vf))
(.lvf input src)
;; The original VU register is deliberately cleared by subtracting it from itself.
(.sub.vf result result result)
(m! coefficient-address *cos-poly-vec*)
(.lvf coefficients coefficient-address)
(.mul.vf input-squared input input)
(.add.w.vf acc result vf0)
(.mul.vf input-fourth input-squared input-squared)
(.add.mul.x.vf acc input-squared coefficients acc)
(.mul.vf input-sixth input-fourth input-squared)
(.add.mul.y.vf acc input-fourth coefficients acc)
(.mul.vf input-eighth input-fourth input-fourth)
(.add.mul.z.vf acc input-sixth coefficients acc)
(.add.mul.w.vf result input-eighth coefficients acc)
(.svf dst result)
dst)))
(#when PC_PORT
(defun vector-cos-rad! ((dst vector) (src vector))
"Compute per-lane cosine for radian inputs in the supported -pi to pi interval."
(rlet ((vf0 :class vf)
(input :class vf)
(result :class vf)
(input-squared :class vf)
(input-fourth :class vf)
(input-sixth :class vf)
(input-eighth :class vf)
(coefficients :class vf)
(acc :class vf))
(init-vf0-vector)
(.lvf input src)
;; Subtracting an uninitialized host SIMD register from itself can propagate NaNs.
(.xor.vf result result result) ;; og:preserve
(.lvf coefficients *cos-poly-vec*)
(.mul.vf input-squared input input)
(.add.w.vf acc result vf0)
(.mul.vf input-fourth input-squared input-squared)
(.add.mul.x.vf acc input-squared coefficients acc)
(.mul.vf input-sixth input-fourth input-squared)
(.add.mul.y.vf acc input-fourth coefficients acc)
(.mul.vf input-eighth input-fourth input-fourth)
(.add.mul.z.vf acc input-sixth coefficients acc)
(.add.mul.w.vf result input-eighth coefficients acc)
(.svf dst result)
dst)))
(#unless PC_PORT
(defun vector-sincos-rad! ((dst-sin vector) (dst-cos vector) (src vector))
"Compute per-lane sine and cosine for radian inputs in the supported -pi to pi interval."
(rlet ((vf0 :reg vf0)
(input :class vf)
(input-squared :class vf)
(input-cubed :class vf)
(input-fourth :class vf)
(input-fifth :class vf)
(input-sixth :class vf)
(input-seventh :class vf)
(input-eighth :class vf)
(input-ninth :class vf)
(sin-coefficients :class vf)
(sin-linear-coefficient :class vf)
(sin-result :class vf)
(cos-coefficients :class vf)
(cos-result :class vf)
(coefficient-address)
(acc :class vf))
(.lvf input src)
;; The original VU register is deliberately cleared by subtracting it from itself.
(.sub.vf cos-result cos-result cos-result)
(m! coefficient-address *sin-poly-vec2*)
(.lvf sin-linear-coefficient coefficient-address)
(.mul.vf input-squared input input)
(m! coefficient-address *sin-poly-vec*)
(.lvf sin-coefficients coefficient-address)
(m! coefficient-address *cos-poly-vec*)
(.lvf cos-coefficients coefficient-address)
(.mul.x.vf acc input sin-linear-coefficient)
(.mul.vf input-cubed input-squared input)
(.mul.vf input-fourth input-squared input-squared)
(.mul.vf input-fifth input-cubed input-squared)
(.mul.vf input-sixth input-cubed input-cubed)
(.mul.vf input-seventh input-fourth input-cubed)
(.mul.vf input-eighth input-fourth input-fourth)
(.mul.vf input-ninth input-fifth input-fourth)
(.add.mul.x.vf acc input-cubed sin-coefficients acc)
(.add.mul.y.vf acc input-fifth sin-coefficients acc)
(.add.mul.z.vf acc input-seventh sin-coefficients acc)
(.add.mul.w.vf sin-result input-ninth sin-coefficients acc)
(.add.w.vf acc cos-result vf0)
(.add.mul.x.vf acc input-squared cos-coefficients acc)
(.add.mul.y.vf acc input-fourth cos-coefficients acc)
(.add.mul.z.vf acc input-sixth cos-coefficients acc)
(.add.mul.w.vf cos-result input-eighth cos-coefficients acc)
(.svf dst-sin sin-result)
(.svf dst-cos cos-result)
0)))
(#when PC_PORT
(defun vector-sincos-rad! ((dst-sin vector) (dst-cos vector) (src vector))
"Compute per-lane sine and cosine for radian inputs in the supported -pi to pi interval."
(rlet ((vf0 :class vf)
(input :class vf)
(input-squared :class vf)
(input-cubed :class vf)
(input-fourth :class vf)
(input-fifth :class vf)
(input-sixth :class vf)
(input-seventh :class vf)
(input-eighth :class vf)
(input-ninth :class vf)
(sin-coefficients :class vf)
(sin-linear-coefficient :class vf)
(sin-result :class vf)
(cos-coefficients :class vf)
(cos-result :class vf)
(acc :class vf))
(init-vf0-vector)
(.lvf input src)
;; Subtracting an uninitialized host SIMD register from itself can propagate NaNs.
(.xor.vf cos-result cos-result cos-result) ;; og:preserve
(.lvf sin-linear-coefficient *sin-poly-vec2*)
(.mul.vf input-squared input input)
(.lvf sin-coefficients *sin-poly-vec*)
(.lvf cos-coefficients *cos-poly-vec*)
(.mul.x.vf acc input sin-linear-coefficient)
(.mul.vf input-cubed input-squared input)
(.mul.vf input-fourth input-squared input-squared)
(.mul.vf input-fifth input-cubed input-squared)
(.mul.vf input-sixth input-cubed input-cubed)
(.mul.vf input-seventh input-fourth input-cubed)
(.mul.vf input-eighth input-fourth input-fourth)
(.mul.vf input-ninth input-fifth input-fourth)
(.add.mul.x.vf acc input-cubed sin-coefficients acc)
(.add.mul.y.vf acc input-fifth sin-coefficients acc)
(.add.mul.z.vf acc input-seventh sin-coefficients acc)
(.add.mul.w.vf sin-result input-ninth sin-coefficients acc)
(.add.w.vf acc cos-result vf0)
(.add.mul.x.vf acc input-squared cos-coefficients acc)
(.add.mul.y.vf acc input-fourth cos-coefficients acc)
(.add.mul.z.vf acc input-sixth cos-coefficients acc)
(.add.mul.w.vf cos-result input-eighth cos-coefficients acc)
(.svf dst-sin sin-result)
(.svf dst-cos cos-result)
0)))
(defmacro sincos-rad-asm (out x)
"Emit the shared scalar sine/cosine polynomial and store sine followed by cosine."
`(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
)
(set! f1 ,x)
(#unless PC_PORT
(sub.s f22 f22 f22))
(#when PC_PORT
;; Do not derive zero from an uninitialized host register.
(set! f22 0.0))
(set! f10 (the-as float #x3F7FFFDE)) ;; almost 1.0
(set! f11 (the-as float #xBE2AA8F5)) ;; -0.166, 1/3!
(set! f2 (* f1 f1))
(set! f12 (the-as float #x3C086BF6)) ;; 1/5!
(set! f14 (the-as float #xB94D2072)) ;; 1/7!
(#unless PC_PORT
(mula.s f1 f10))
(#when PC_PORT
(set! acc (* f1 f10))) ;; x * c_1
(set! f3 (* f2 f1)) ;; x^3
(set! f4 (* f2 f2)) ;; x^4
(set! f15 (the-as float #x361AA27F)) ;; 1/9!
(set! f16 (the-as float #x3f800000)) ;; 1.0
;; The original constant construction omits the low half of the x^2 coefficient.
(#cond
(FIX_COSINE_BUG
;; Match the coefficient used by the standalone and packed cosine routines.
(set! f17 (the-as float #xbefffd62)))
(#t
;; Retain the shipped fused-sincos result.
(set! f17 (the-as float #xBEFF0000))))
(set! f5 (* f3 f2))
(set! f6 (* f3 f3))
(set! f7 (* f4 f3))
(set! f8 (* f4 f4))
(set! f9 (* f5 f4))
(#unless PC_PORT
(madda.s f3 f11))
(#when PC_PORT
(set! acc (+ acc (* f3 f11)))) ;; add x^3 sine term
(#unless PC_PORT
(madda.s f5 f12))
(#when PC_PORT
(set! acc (+ acc (* f5 f12)))) ;; add x^5 sine term
(#unless PC_PORT
(madda.s f7 f14))
(#when PC_PORT
(set! acc (+ acc (* f7 f14)))) ;; add x^7 sine term
(#unless PC_PORT
(madd.s f21 f9 f15))
(#when PC_PORT
(set! f21 (+ acc (* f9 f15)))) ;; add x^9 sine term
(set! f18 (the-as float #x3D2A7A28)) ;; cos coeff
(set! f19 (the-as float #xBAB2BC31)) ;; cos coeff
(set! f20 (the-as float #x37A933EB))
(#unless PC_PORT
(mula.s f16 f16))
(#when PC_PORT
(set! acc (* f16 f16))) ;; acc = 1, constant cos term.
(#unless PC_PORT
(madda.s f2 f17))
(#when PC_PORT
(set! acc (+ acc (* f2 f17))))
(#unless PC_PORT
(madda.s f4 f18))
(#when PC_PORT
(set! acc (+ acc (* f4 f18))))
(#unless PC_PORT
(madda.s f6 f19))
(#when PC_PORT
(set! acc (+ acc (* f6 f19))))
(#unless PC_PORT
(madd.s f22 f8 f20))
(#when PC_PORT
(set! f22 (+ acc (* f8 f20))))
(set! (-> ,out 0) f21)
(set! (-> ,out 1) f22)
0))
(defun sincos-rad! ((out (pointer float)) (angle float))
"Write sine and cosine for a radian angle with the shared scalar polynomial.
The original fused cosine coefficient retains its small construction error."
(sincos-rad-asm out angle))
(defun sincos! ((out (pointer float)) (angle float))
"Write sine and cosine for a wrapped rotation-unit angle.
The original fused cosine coefficient retains its small construction error."
(sincos-rad-asm out (* ROT_TO_RAD (the float (sar (shl (the int angle) 48) 48)))))
(#unless PC_PORT
(defun vector-rad<-vector-deg! ((out vector) (in vector))
"Wrap four rotation-unit angles to a signed half-turn and convert them to radians."
(rlet ((rot-to-rad-bits)
(packed-angles :class i128)
(angles :class vf)
(scale :class vf)
(result :reg v0 :class i128))
(lui rot-to-rad-bits #x38c9)
(ori rot-to-rad-bits rot-to-rad-bits #x0fda)
(.lvf angles in)
(.ftoi.vf angles angles)
(m scale rot-to-rad-bits)
(m packed-angles angles)
(.pw.sll packed-angles packed-angles 16)
(.pw.sra packed-angles packed-angles 16)
(m angles packed-angles)
(.itof.vf angles angles)
(.mul.x.vf angles angles scale)
(.svf out angles)
(m result angles))))
(#when PC_PORT
(defun vector-rad<-vector-deg! ((out vector) (in vector))
"Wrap four rotation-unit angles to a signed half-turn and convert them to radians."
(rlet ((rot-to-rad :class vf)
(angles :class vf))
(.mov rot-to-rad (the-as float ROT_TO_RAD))
(.lvf angles in)
(.ftoi.vf angles angles)
(.pw.sll angles angles 16)
(.pw.sra angles angles 16)
(.itof.vf angles angles)
(.mul.x.vf angles angles rot-to-rad)
(.svf out angles))))
(#unless PC_PORT
(defun vector-rad<-vector-deg/2! ((out vector) (in vector))
"Halve and wrap four rotation-unit angles, then convert them to radians."
(rlet ((rot-to-rad-bits)
(half-bits)
(packed-angles :class i128)
(angles :class vf)
(scale :class vf)
(result :reg v0 :class i128))
(lui rot-to-rad-bits #x38c9)
(ori rot-to-rad-bits rot-to-rad-bits #x0fda)
(lui half-bits #x3f00)
(.lvf angles in)
(m scale half-bits)
(.mul.x.vf angles angles scale)
(.ftoi.vf angles angles)
(m scale rot-to-rad-bits)
(m packed-angles angles)
(.pw.sll packed-angles packed-angles 16)
(.pw.sra packed-angles packed-angles 16)
(m angles packed-angles)
(.itof.vf angles angles)
(.mul.x.vf angles angles scale)
(.svf out angles)
(m result angles))))
(#when PC_PORT
(defun vector-rad<-vector-deg/2! ((out vector) (in vector))
"Halve and wrap four rotation-unit angles, then convert them to radians."
(rlet ((scale :class vf)
(angles :class vf))
(.lvf angles in)
(.mov scale (the-as float #x3f000000))
(.mul.x.vf angles angles scale)
(.ftoi.vf angles angles)
(.pw.sll angles angles 16)
(.pw.sra angles angles 16)
(.itof.vf angles angles)
(.mov scale (the-as float ROT_TO_RAD))
(.mul.x.vf angles angles scale)
(.svf out angles)
0)))
(defun vector-sincos! ((out-sin vector) (out-cos vector) (in vector))
"Compute per-lane sine and cosine for angles in rotation units."
(let ((temp (new 'stack-no-clear 'vector))) (vector-rad<-vector-deg! temp in) (vector-sincos-rad! out-sin out-cos temp)))
(defun-extern cos float float)
(defun tan-rad ((angle float))
"Return sine divided by cosine using rotation units despite the historical name."
(/ (sin angle) (cos angle)))
(defun cos ((angle float))
"Return cosine for an angle in rotation units."
(sin (+ 16384.000000 angle)))
(defun tan ((angle float))
"Return tangent for an angle in rotation units."
(/ (sin angle) (cos angle)))
(defun atan0 ((y float) (x float))
"Approximate atan(y/x) in rotation units after reducing about 45 degrees with (y-x)/(y+x).
The caller handles signs and quadrants."
(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))
(set! f20 x)
(set! f21 y)
;; Center the approximation at 45 degrees so the polynomial sees a small signed input.
(set! f1 (- f21 f20))
(set! f2 (+ f21 f20))
(set! f1 (/ f1 f2))
(set! f19 (the-as float #x46000000))
(set! f11 (the-as float #x4622f97c))
(set! f12 (the-as float #xc55946e1))
(set! f13 (the-as float #x450207fd))
(set! f14 (the-as float #xc4b556ce))
(set! f15 (the-as float #x447b6ca4))
(set! f2 (* f1 f1))
(set! f16 (the-as float #xc411ca52))
(set! f17 (the-as float #x43640558))
(set! f3 (* f1 f2))
(set! f1 (* f1 f11))
(set! f4 (* f2 f2))
(set! f18 (the-as float #xc2292434))
(set! f5 (* f3 f2))
(set! f6 (* f4 f3))
(set! f7 (* f5 f4))
(set! f8 (* f6 f4))
(set! f9 (* f7 f4))
(set! f10 (* f8 f4))
(#unless PC_PORT
(adda.s f1 f19)
(madda.s f3 f12)
(madda.s f5 f13)
(madda.s f6 f14)
(madda.s f7 f15)
(madda.s f8 f16)
(madda.s f9 f17)
(madd.s f19 f10 f18))
(#when PC_PORT
(set! acc (+ f1 f19))
(set! acc (+ acc (* f3 f12)))
(set! acc (+ acc (* f5 f13)))
(set! acc (+ acc (* f6 f14)))
(set! acc (+ acc (* f7 f15)))
(set! acc (+ acc (* f8 f16)))
(set! acc (+ acc (* f9 f17)))
(set! f19 (+ acc (* f10 f18))))
f19))
(defun atan-series-rad ((reduced float))
"Evaluate atan in radians from reduced = (x-1)/(x+1).
The polynomial's constant term supplies the pi/4 removed by the range reduction."
(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)
(result float))
(set! f1-0 (* reduced reduced))
(set! f10-0 (the-as float #x3f7ffff5))
(set! f11-0 (the-as float #xbeaaa61c))
(set! f2-0 (* reduced 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 (* reduced f10-0))
(set! f16-0 (the-as float #x3cb31652))
(set! f17-0 (the-as float #xbb84d7e7))
(#unless PC_PORT
(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 result f9-0 f17-0))
(#when PC_PORT
(set! acc (+ f0-1 f18-0))
(set! acc (+ acc (* f2-0 f11-0)))
(set! acc (+ acc (* f4-0 f12-0)))
(set! acc (+ acc (* f5-0 f13-0)))
(set! acc (+ acc (* f6-0 f14-0)))
(set! acc (+ acc (* f7-0 f15-0)))
(set! acc (+ acc (* f8-0 f16-0)))
(set! result (+ acc (* f9-0 f17-0))))
result)
(defun atan-rad ((value float))
"Return inverse tangent in radians."
(atan-series-rad (/ (+ -1.0 value) (+ 1.0 value))))
(defun sign ((value float))
"Return -1, 0, or 1 according to value's sign."
(cond
((< 0.000000 value) 1.000000)
((< value 0.000000) -1.000000)
(else 0.000000)))
(defun atan2-rad ((y float) (x float))
"Return atan2(y, x) in radians."
(cond
((= x 0.0) (* 1.5707963 (sign y)))
(else
(cond
((and (< y 0.0) (< x 0.0))
(let ((f30-1 MINUS_PI)
(f0-6 (/ y x)))
(+ f30-1 (atan-series-rad (/ (+ -1.0 f0-6) (+ 1.0 f0-6))))))
((< y 0.0) (let ((f0-14 (- (/ y x)))) (- (atan-series-rad (/ (+ -1.0 f0-14) (+ 1.0 f0-14))))))
((< x 0.0)
(let ((f30-2 PI)
(f0-22 (- (/ y x))))
(- f30-2 (atan-series-rad (/ (+ -1.0 f0-22) (+ 1.0 f0-22))))))
(else (let ((f0-28 (/ y x))) (atan-series-rad (/ (+ -1.0 f0-28) (+ 1.0 f0-28)))))))))
;; Raw 32-bit storage used for float exponent and mantissa construction in exp.
(deftype float-type (uint32) ())
;; Tang-style exp table. Range reduction writes x = k*ln(2)/32 + r and splits k into
;; an exponent contribution plus j = k & 31. For each j, slead[j] + strail[j] gives
;; 2^(j/32); the split high and low parts retain more accuracy than one rounded float.
(define exp-slead
(new 'static
'array
float
32
(the-as float #x3f800000)
(the-as float #x3f82cd80)
(the-as float #x3f85aac0)
(the-as float #x3f889800)
(the-as float #x3f8b95c0)
(the-as float #x3f8ea400)
(the-as float #x3f91c3c0)
(the-as float #x3f94f4c0)
(the-as float #x3f9837c0)
(the-as float #x3f9b8d00)
(the-as float #x3f9ef500)
(the-as float #x3fa27040)
(the-as float #x3fa5fec0)
(the-as float #x3fa9a140)
(the-as float #x3fad5800)
(the-as float #x3fb123c0)
(the-as float #x3fb504c0)
(the-as float #x3fb8fb80)
(the-as float #x3fbd0880)
(the-as float #x3fc12c40)
(the-as float #x3fc56700)
(the-as float #x3fc9b980)
(the-as float #x3fce2480)
(the-as float #x3fd2a800)
(the-as float #x3fd744c0)
(the-as float #x3fdbfb80)
(the-as float #x3fe0ccc0)
(the-as float #x3fe5b900)
(the-as float #x3feac0c0)
(the-as float #x3fefe480)
(the-as float #x3ff52540)
(the-as float #x3ffa8380)))
(define exp-strail
(new 'static
'array
float
32
(the-as float #x0)
(the-as float #x35531585)
(the-as float #x34d9f312)
(the-as float #x35e8092e)
(the-as float #x3471f546)
(the-as float #x36e62d17)
(the-as float #x361b9d59)
(the-as float #x36bea3fc)
(the-as float #x36c14637)
(the-as float #x36e6e755)
(the-as float #x36c98247)
(the-as float #x34c0c312)
(the-as float #x36354d8b)
(the-as float #x3655a754)
(the-as float #x36fba90b)
(the-as float #x36d6074b)
(the-as float #x36cccfe7)
(the-as float #x36bd1d8c)
(the-as float #x368e7d60)
(the-as float #x35cca667)
(the-as float #x36a84554)
(the-as float #x36f619b9)
(the-as float #x35c151f8)
(the-as float #x366c8f89)
(the-as float #x36f32b5a)
(the-as float #x36de5f6c)
(the-as float #x36776155)
(the-as float #x355cef90)
(the-as float #x355cfba5)
(the-as float #x36e66f73)
(the-as float #x36f45492)
(the-as float #x36cb6dc9)))
(defun exp ((value float))
"Approximate e^value with Tang-style range reduction.
Magnitudes above 220.42 saturate, tiny inputs use 1+value, and the general path writes
value = k*ln(2)/32+r. It combines split high/low values for 2^((k mod 32)/32) with a cubic
approximation of e^r, then constructs the remaining power-of-two scale in the float exponent."
(local-vars
(f0 float)
(f1 float)
(f2 float)
(f3 float)
(f4 float)
(f5 float)
(f6 float)
(f7 float)
(f8 float)
(f10 float)
(f11 float)
(f12 float)
(f13 float)
(f14 float)
(f15 float)
(f16 float)
(f17 float)
(f18 float)
(a2 int)
(v0 float)
(v1 int)
(a1 int)
(a3 int)
(t0 int)
(a0-2 int))
(set! f0 value)
(set! f0 (fabs f0))
(set! f1 (the-as float #x435c6bba))
(when-goto (>= f1 f0) L44)
;; Saturate inputs beyond the supported range.
(set! f0 0.0)
(set! f1 value)
(when-goto (>= f0 f1) L42)
(set! v0 (the-as float #x7f7fffff))
(goto L43)
(label L42)
(set! v0 (the-as float #x0))
(label L43)
(goto L49)
(label L44)
(set! f1 (the-as float #x33000000))
(when-goto (>= f0 f1) L45)
;; First-order expansion is exact enough at this scale.
(set! f0 (the-as float #x3f800000))
(set! f1 value)
(set! f0 (+ f0 f1))
(set! v0 f0)
(goto L49)
(label L45)
(set! f16 (the-as float #x4238aa3b))
(set! f12 (the-as float #x3cb17200))
(set! f13 (the-as float #x333fbe8e))
(set! f14 (the-as float #x3f000044))
(set! f15 (the-as float #x3e2aaaec))
(set! f0 value)
(set! f0 (* f0 f16))
(set! a2 (the int f0))
(set! v1 (logand a2 31))
(set! a1 (- a2 v1))
(set! a3 512)
(set! t0 a2)
(set! t0 (abs t0))
(when-goto (>= a3 t0) L47)
;; Split a large k into two products so range reduction loses fewer low bits.
(set! f17 (the float a1))
(set! f18 (the float v1))
(set! f17 (* f17 f12))
(set! f18 (* f18 f12))
(set! f0 value)
(set! f17 (- f0 f17))
(set! f2 (- f17 f18))
(goto L48)
(label L47)
(set! f17 (the float a2))
(set! f17 (* f17 f12))
(set! f0 value)
(set! f2 (- f0 f17))
(label L48)
(set! a0-2 (- a2))
(set! f17 (the float a0-2))
(set! f3 (* f17 f13))
(set! a0-2 (sar a1 5))
(set! f4 (+ f2 f3))
(set! f6 (* f4 f15))
(set! f6 (+ f14 f6))
(set! f6 (* f4 f6))
(set! f6 (* f4 f6))
(set! f5 (+ f3 f6))
(set! f5 (+ f2 f5))
(set! f10 (-> exp-slead v1))
(set! f11 (-> exp-strail v1))
(set! f7 (+ f10 f11))
(set! f8 (* f7 f5))
(set! f8 (+ f11 f8))
(set! f8 (+ f8 f10))
(set! v1 (the-as int f8))
(set! a0-2 (logand a0-2 511))
(set! a0-2 (shl a0-2 23))
(set! v0 (the-as float (+ v1 a0-2)))
(label L49)
v0)
(defun atan ((y float) (x float))
"Return atan2(y, x) in signed rotation units."
(if (and (= x 0.0) (= y 0.0))
0.000000
(cond
((and (< x 0.0) (< y 0.0)) (+ -32768.0 (atan0 (- y) (- x))))
((< y 0.) (- (atan0 (- y) x)))
((< x 0.) (- 32768.0 (atan0 y (- x))))
(else (atan0 y x)))))
(defun asin ((value float))
"Return inverse sine in rotation units, saturating magnitudes above one to a quarter-turn."
(let ((gp-0 #f))
(let ((f0-0 0.0)))
(when (< value 0.0)
(set! value (- value))
(set! gp-0 #t))
(let ((f0-5 (cond
((< 1.0 value) 16383.996)
(else
(let* ((f0-6 1.0)
(f1-2 value)
(f0-8 (sqrtf (- f0-6 (* f1-2 f1-2)))))
(atan0 value f0-8))))))
(if gp-0 (- f0-5) f0-5))))
(defun acos ((value float))
"Return inverse cosine in rotation units."
(let ((result (- 16384.000000 (asin value))))
(#when PC_PORT
;; Host rounding can produce exact zero here and trigger a punch-animation discontinuity.
;; Keep the PC result positive without changing the global floating-point rounding mode.
(when (= result 0.0)
(set! result 0.00000000001))) ;; og:preserve
result))
(defun acos-rad ((value float))
"Return inverse cosine in radians."
(cond
((>= value 0.0)
(let* ((f0-1 1.0)
(f1-1 value)
(f0-3 (sqrtf (- f0-1 (* f1-1 f1-1))))
(f0-5 (/ (- f0-3 value) (+ f0-3 value))))
(atan-series-rad f0-5)))
(else
(let* ((f0-6 1.0)
(f1-6 value)
(f0-8 (sqrtf (- f0-6 (* f1-6 f1-6))))
(f0-10 (/ (+ f0-8 value) (- f0-8 value))))
(- PI (atan-series-rad f0-10))))))
(defun sinerp ((minimum float) (maximum float) (amount float))
"Interpolate from minimum to maximum with a quarter-sine ease-out curve."
(lerp minimum maximum (sin (* 16384.000000 amount))))
(defun sinerp-clamp ((minimum float) (maximum float) (amount float))
"Apply quarter-sine interpolation with amount clamped to the endpoints."
(cond
((>= 0.000000 amount) minimum)
((>= amount 1.000000) maximum)
(else (sinerp minimum maximum amount))))
(defun coserp ((minimum float) (maximum float) (amount float))
"Interpolate from minimum to maximum with a quarter-cosine ease-in curve."
(lerp minimum maximum (- 1.000000 (cos (* 16384.000000 amount)))))
(defun coserp-clamp ((minimum float) (maximum float) (amount float))
"Apply quarter-cosine interpolation with amount clamped to the endpoints."
(cond
((>= 0.000000 amount) minimum)
((>= amount 1.000000) maximum)
(else (coserp minimum maximum amount))))
(defun coserp180 ((minimum float) (maximum float) (amount float))
"Interpolate with a half-cosine ease-in/ease-out curve."
(lerp minimum maximum (* 0.5 (- 1.000000 (cos (* 32768.000000 amount))))))
(defun coserp180-clamp ((minimum float) (maximum float) (amount float))
"Apply half-cosine interpolation with amount clamped to the endpoints."
(cond
((>= 0.000000 amount) minimum)
((>= amount 1.000000) maximum)
(else (coserp180 minimum maximum amount))))
(defun ease-in-out ((total int) (progress int))
"Map integer progress to a smooth zero-to-one sine/cosine ease over total steps."
(local-vars (v1-0 int) (a0-1 int))
(cond
((>= progress total)
;; past the end
1.000000)
((<= progress 0)
;; negative progress
0.000000)
((begin
(set! v1-0 (sar total 1))
(< v1-0 progress))
;; more than half way there!
(set! a0-1 (- progress total))
(+ 0.500000 (* 0.500000 (sin (- 16384.000000 (/ (* 16384.000000 (the float a0-1)) (the float v1-0)))))))
(else
;; less than half way there.
(- 0.500000 (* 0.500000 (cos (/ (* 16384.000000 (the float progress)) (the float v1-0))))))))
Reference in New Issue View Git Blame Copy Permalink