jak-project/goal_src/jak2/engine/math/trigonometry.gc

;;-*-Lisp-*-
(in-package goal)

;; name: trigonometry.gc
;; name in dgo: trigonometry
;; dgos: ENGINE, GAME

(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))

;; There is a bug in some of the cosine functions that can be fixed by toggling this flag.
(defglobalconstant FIX_COSINE_BUG #f)


(defun radmod ((arg0 float))
  (let ((f0-1 (+ PI arg0)))
    (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- ((arg0 float) (arg1 float))
  (the float (sar (- (shl (the int arg0) 48) (shl (the int arg1) 48)) 48))
  )

(defun deg-diff ((arg0 float) (arg1 float))
  (the float (sar (- (shl (the int arg1) 48) (shl (the int arg0) 48)) 48))
  )

(defun deg-seek ((in float) (target float) (max-diff float))
  (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)
                          (empty)
                          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))
  (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))
  (cond
    ((>= 0.0 in)
     min-val
     )
    ((>= in 1.0)
     max-val
     )
    (else
      (the float (sar (shl (the int (+ min-val (* in (deg-diff min-val max-val)))) 48) 48))
      )
    )
  )

(define binary-table (the-as (array float) (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 (the-as (array float) (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))
  (let ((f2-0 (* 0.000095873795 (the float (sar (shl (the int arg0) 48) 48)))))
    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 0.999998)
  (set! f8-0 -0.16666014)
  (set! f2-0 (* arg0 f1-0))
  (set! f3-0 (* f1-0 f1-0))
  (set! f9-0 0.008326521)
  (set! f4-0 (* f2-0 f1-0))
  (set! f5-0 (* f3-0 f2-0))
  (set! f10-0 -0.0001956241)
  (set! f6-0 (* f4-0 f3-0))
  (set! f11-0 0.0000023042373)
  ;;(.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))
  (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))
  (local-vars (acc float))
  (let* ((f1-0 (* arg0 arg0))
         (f7-0 1.0)
         (f8-0 -0.49998003)
         (f3-0 (* f1-0 f1-0))
         (f9-0 0.041620404)
         (f10-0 -0.0013636408)
         (f4-0 (* f3-0 f1-0))
         (f5-0 (* f3-0 f3-0))
         (f11-0 0.000020170546)
         )
  ;;(.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)
    )
  acc
  )

(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))
  (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))
  (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 0) f21)
     ;; sll r0, r0, 0
     ;; swc1 f22, 4(a0)
     (set! (-> ,out 1) f22)
     ;; or v0, r0, r0
     0
     )
  )

(defun sincos-rad! ((out (pointer float)) (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 (pointer float)) (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 (the float (sar (shl (the int x) 48) 48))))
  )

(defun vector-rad<-vector-deg! ((arg0 vector) (arg1 vector))
  (local-vars (v0-0 float) (v1-1 uint128) (v1-2 uint128) (v1-3 uint128))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (let ((v1-0 #x38c90fda))
      (.lvf vf1 (&-> arg1 quad))
      (.ftoi.vf vf1 vf1)
      (.mov vf2 v1-0)
      )
    (.mov v1-1 vf1)
    (.pw.sll v1-2 v1-1 16)
    (.pw.sra v1-3 v1-2 16)
    (.mov vf1 v1-3)
    (.itof.vf vf1 vf1)
    (.mul.x.vf vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    (.mov v0-0 vf1)
    (none)
    )
  )

(defun vector-rad<-vector-deg/2! ((arg0 vector) (arg1 vector))
  (local-vars (v0-0 float) (v1-1 uint128) (v1-2 uint128) (v1-3 uint128))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (let ((v1-0 #x38c90fda))
      (let ((a2-0 #x3f000000))
        (.lvf vf1 (&-> arg1 quad))
        (.mov vf2 a2-0)
        )
      (.mul.x.vf vf1 vf1 vf2)
      (.ftoi.vf vf1 vf1)
      (.mov vf2 v1-0)
      )
    (.mov v1-1 vf1)
    (.pw.sll v1-2 v1-1 16)
    (.pw.sra v1-3 v1-2 16)
    (.mov vf1 v1-3)
    (.itof.vf vf1 vf1)
    (.mul.x.vf vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    (.mov v0-0 vf1)
    (the-as int v0-0)
    )
  )

(defun vector-sincos! ((arg0 vector) (arg1 vector) (arg2 vector))
  (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))
  (/ (sin arg0) (cos arg0))
  )

(defun cos ((arg0 float))
  (sin (+ 16384.0 arg0))
  )

(defun tan ((arg0 float))
  (/ (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))
  (atan-series-rad (/ (+ -1.0 arg0) (+ 1.0 arg0)))
  )

(defun sign ((arg0 float))
  (cond
    ((< 0.0 arg0)
     1.0
     )
    ((< arg0 0.0)
     -1.0
     )
    (else
      0.0
      )
    )
  )

(defun atan2-rad ((arg0 float) (arg1 float))
  (cond
    ((= arg1 0.0)
     (* 1.5707963 (sign arg0))
     )
    ((and (< arg0 0.0) (< arg1 0.0))
     (let ((f30-1 MINUS_PI)
           (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 PI)
           (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)))
        )
      )
    )
  )

(deftype float-type (uint32)
  ()
  :method-count-assert 9
  :size-assert         #x4
  :flag-assert         #x900000004
  )

(define exp-slead (new 'static 'array float 32
                    1.0
                    1.0218964
                    1.0442734
                    1.0671387
                    1.0905075
                    1.1143799
                    1.1387863
                    1.1637192
                    1.1892014
                    1.2152405
                    1.2418518
                    1.2690506
                    1.2968369
                    1.3252335
                    1.354248
                    1.3839035
                    1.4142075
                    1.4451752
                    1.4768219
                    1.5091629
                    1.5422058
                    1.5759735
                    1.6104889
                    1.645752
                    1.6817856
                    1.7186127
                    1.7562485
                    1.7947083
                    1.8340073
                    1.8741608
                    1.9151993
                    1.9571381
                    )
        )

(define exp-strail (new 'static 'array float 32
                     0.0
                     0.0000007863494
                     0.00000040596257
                     0.0000017288019
                     0.00000022534104
                     0.0000068597833
                     0.0000023188388
                     0.0000056815315
                     0.0000057600223
                     0.0000068814647
                     0.000006005433
                     0.0000003590472
                     0.0000027016238
                     0.000003183687
                     0.000007500062
                     0.000006378546
                     0.000006103877
                     0.0000056360786
                     0.0000042465254
                     0.0000015247614
                     0.000005014861
                     0.000007334366
                     0.0000014403477
                     0.000003525029
                     0.000007247011
                     0.000006627224
                     0.0000036862523
                     0.00000082304996
                     0.0000008232258
                     0.0000068675085
                     0.000007281612
                     0.000006062652
                     )
        )

(defun exp ((arg float))
  (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 arg)
  (set! f0 (fabs f0))
  (set! f1 (the-as float #x435c6bba))
  ;;(b! (>=.s f1 f0) L44 (nop!))
  (when-goto (>= f1 f0) L44)

  (set! f0 0.0)
  (set! f1 arg)
  ;;(b! (>=.s f0 f1) L42 (nop!))
  (when-goto (>= f0 f1) L42)

  (set! v0 (the-as float #x7f7fffff))
  ;;(b! #t L43 (nop!))
  (goto L43)

  (label L42)
  (set! v0 (the-as float #x0))

  (label L43)
  ;;(b! #t L49 (nop!))
  (goto L49)


  (label L44)
  (set! f1 (the-as float #x33000000))
  ;;(b! (>=.s f0 f1) L45 (nop!))
  (when-goto (>= f0 f1) L45)

  (set! f0 (the-as float #x3f800000))
  (set! f1 arg)
  (set! f0 (+ f0 f1))
  (set! v0 f0)
  ;;(b! #t L49 (nop!))
  (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 arg)
  (set! f0 (* f0 f16))
  ;;(set! f0 (f2i f0))
  ;;(set! a2 (fpr->gpr f0))
  (set! a2 (the int f0))
  (set! v1 (logand a2 31))
  (set! a1 (- a2 v1))
  (set! a3 512)
  (set! t0 a2)
  ;;(bl! (<0.si t0) L46 (no-delay!))
  ;;(set! t0 (- t0))
  (set! t0 (abs t0))

  ;;(label L46)
  ;;(b! (>=.si a3 t0) L47 (nop!))
  (when-goto (>= a3 t0) L47)
  ;;(set! f17 a1)
  ;;(set! f18 v1)
  ;;(set! f17 (i2f f17))
  ;;(set! f18 (i2f f18))
  (set! f17 (the float a1))
  (set! f18 (the float v1))
  (set! f17 (* f17 f12))
  (set! f18 (* f18 f12))
  (set! f0 arg)
  (set! f17 (- f0 f17))
  ;;(b! #t L48 (set! f2 (-.s f17 f18)))
  (set! f2 (- f17 f18))
  (goto L48)

  (label L47)
  ;;(set! f17 (gpr->fpr a2))
  ;;(set! f17 (i2f f17))
  (set! f17 (the float a2))
  (set! f17 (* f17 f12))
  (set! f0 arg)
  (set! f2 (- f0 f17))

  (label L48)
  (set! a0-2 (- a2))
  ;;(set! f17 (gpr->fpr a0))
  ;;(set! f17 (i2f f17))
  (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! a1 exp-slead)
  ;;(set! a2 (sll v1 2))
  ;;(set! a1 (+ a1 a2))
  ;;(set! f10 (l.f a1))
  (set! f10 (-> exp-slead v1))
  ;;(set! a1 exp-strail)
  ;;(set! v1 (sll v1 2))
  ;;(set! v1 (+ a1 v1))
  ;;(set! f11 (l.f 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 ((arg0 float) (arg1 float))
  (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))
  (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))
  (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)))
             )
        (- PI (atan-series-rad f0-10))
        )
      )
    )
  )

(defun sinerp ((arg0 float) (arg1 float) (arg2 float))
  (lerp arg0 arg1 (sin (* 16384.0 arg2)))
  )

(defun sinerp-clamp ((minimum float) (maximum float) (amount float))
  (cond
    ((>= 0.0 amount)
     minimum
     )
    ((>= amount 1.0)
     maximum
     )
    (else
      (sinerp minimum maximum amount)
      )
    )
  )

(defun coserp ((minimum float) (maximum float) (amount float))
  (lerp minimum maximum (- 1.0 (cos (* 16384.0 amount))))
  )

(defun coserp-clamp ((minimum float) (maximum float) (amount float))
  (cond
    ((>= 0.0 amount)
     minimum
     )
    ((>= amount 1.0)
     maximum
     )
    (else
      (coserp minimum maximum amount)
      )
    )
  )

(defun coserp180 ((minimum float) (maximum float) (amount float))
  (lerp minimum maximum (* 0.5 (- 1.0 (cos (* 32768.0 amount)))))
  )

(defun coserp180-clamp ((minimum float) (maximum float) (amount float))
  (cond
    ((>= 0.0 amount)
     minimum
     )
    ((>= amount 1.0)
     maximum
     )
    (else
      (coserp180 minimum maximum amount)
      )
    )
  )

(defun ease-in-out ((total int) (progress int))
  (local-vars (v1-0 int))
  (cond
    ((>= progress total)
     1.0
     )
    ((<= progress 0)
     0.0
     )
    ((begin (set! v1-0 (/ total 2)) (< v1-0 progress))
     (let ((a0-1 (- progress total)))
       (+ 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 progress)) (the float v1-0)))))
      )
    )
  )