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

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

;; In general, the euler angle stuff is really inefficient. I don't think it's really used outside of
;; a few camera debugging functions.

(defun set-eul! ((arg0 euler-angles) (arg1 float) (arg2 float) (arg3 float) (arg4 int))
  "Set the euler angles. The 4th argument is for flags."
  (set! (-> arg0 data 0) arg1)
  (set! (-> arg0 data 1) arg2)
  (set! (-> arg0 data 2) arg3)
  (set! (-> arg0 data 3) (the float arg4))
  arg0
  )

(defun eul->matrix ((arg0 matrix) (arg1 euler-angles))
  "Convert euler angles to rotation matrix."
  (matrix-identity! arg0)
  (let ((s5-0 (new 'stack-no-clear 'vector)))
    ;; copy to temp storage.
    (set! (-> s5-0 quad) (-> arg1 quad))
    (if (= (logand (the int (-> s5-0 data 3)) 1) 1)
        (let ((f0-2 (-> s5-0 data 0)))
          (set! (-> s5-0 data 0) (-> s5-0 data 2))
          (set! (-> s5-0 data 2) f0-2)
          )
        )
    (when (= (logand (sar (the int (-> s5-0 data 3)) 2) 1) 1)
      (set! (-> s5-0 data 0) (- (-> s5-0 data 0)))
      (set! (-> s5-0 data 1) (- (-> s5-0 data 1)))
      (set! (-> s5-0 data 2) (- (-> s5-0 data 2)))
      )
    (let* ((f26-0 (cos (-> s5-0 data 0)))
           (f30-0 (cos (-> s5-0 data 1)))
           (f22-0 (cos (-> s5-0 data 2)))
           (f24-0 (sin (-> s5-0 data 0)))
           (f28-0 (sin (-> s5-0 data 1)))
           (f4-0 (sin (-> s5-0 data 2)))
           (f0-17 (* f26-0 f22-0))
           (f1-1 (* f26-0 f4-0))
           (f2-0 (* f24-0 f22-0))
           (f3-0 (* f24-0 f4-0))
           )
      (let* ((v1-12 (logand (sar (the int (-> s5-0 data 3)) 2) 1))
             (a1-2 (-> EulSafe (logand (sar (the int (-> s5-0 data 3)) 3) 3)))
             (a0-21 (-> EulNext (+ a1-2 v1-12)))
             (v1-17 (-> EulNext (+ (- 1 v1-12) a1-2)))
             )
        (cond
          ((= (logand (sar (the int (-> s5-0 data 3)) 1) 1) 1)
           (set! (-> (the-as (pointer float) (+ (+ (shl a1-2 4) (shl a1-2 2)) (the-as int arg0))))
                 f30-0
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a1-2 4) (shl a0-21 2)) (the-as int arg0))))
                 (* f28-0 f24-0)
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a1-2 4) (shl v1-17 2)) (the-as int arg0))))
                 (* f28-0 f26-0)
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a0-21 4) (shl a1-2 2)) (the-as int arg0))))
                 (* f28-0 f4-0)
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a0-21 4) (shl a0-21 2)) (the-as int arg0))))
                 (- f0-17 (* f30-0 f3-0))
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a0-21 4) (shl v1-17 2)) (the-as int arg0))))
                 (- (- f2-0) (* f30-0 f1-1))
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl v1-17 4) (shl a1-2 2)) (the-as int arg0))))
                 (- (* f28-0 f22-0))
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl v1-17 4) (shl a0-21 2)) (the-as int arg0))))
                 (+ f1-1 (* f30-0 f2-0))
                 )
           (let ((f0-19 (+ (- f3-0) (* f30-0 f0-17))))
             (set! (-> (the-as (pointer float) (+ (+ (shl v1-17 4) (shl v1-17 2)) (the-as int arg0))))
                   f0-19
                   )
             )
           )
          (else
           (set! (-> (the-as (pointer float) (+ (+ (shl a1-2 4) (shl a1-2 2)) (the-as int arg0))))
                 (* f30-0 f22-0)
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a1-2 4) (shl a0-21 2)) (the-as int arg0))))
                 (+ (- f1-1) (* f28-0 f2-0))
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a1-2 4) (shl v1-17 2)) (the-as int arg0))))
                 (+ f3-0 (* f28-0 f0-17))
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a0-21 4) (shl a1-2 2)) (the-as int arg0))))
                 (* f30-0 f4-0)
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a0-21 4) (shl a0-21 2)) (the-as int arg0))))
                 (+ f0-17 (* f28-0 f3-0))
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl a0-21 4) (shl v1-17 2)) (the-as int arg0))))
                 (+ (- f2-0) (* f28-0 f1-1))
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl v1-17 4) (shl a1-2 2)) (the-as int arg0))))
                 (- f28-0)
                 )
           (set! (-> (the-as (pointer float) (+ (+ (shl v1-17 4) (shl a0-21 2)) (the-as int arg0))))
                 (* f30-0 f24-0)
                 )
           (let ((f0-25 (* f30-0 f26-0)))
             (set! (-> (the-as (pointer float) (+ (+ (shl v1-17 4) (shl v1-17 2)) (the-as int arg0))))
                   f0-25
                   )
             )
           )
          )
        )
      )
    )
  arg0
  )

(defun matrix->eul ((arg0 euler-angles) (arg1 matrix) (arg2 int))
  "Conver matrix to euler angles. Takes some weird flag for what kind of euler angles"
  (let* ((v1-4 (logand (sar arg2 2) 1))
         (s3-0 (-> EulSafe (logand (sar arg2 3) 3)))
         (s2-0 (-> EulNext (+ s3-0 v1-4)))
         (s1-0 (-> EulNext (+ (- 1 v1-4) s3-0)))
         )
    (if (= (logand (sar arg2 1) 1) 1)
        (let* ((f0-0 (-> (the-as (pointer float) (+ (+ (shl s2-0 2) (shl s3-0 4)) (the-as int arg1)))))
               (f0-2 (* f0-0 f0-0))
               (f1-0 (-> (the-as (pointer float) (+ (+ (shl s1-0 2) (shl s3-0 4)) (the-as int arg1)))))
               (f30-0 (sqrtf (+ f0-2 (* f1-0 f1-0))))
               )
          (cond
            ((< 0.00001 f30-0)
             (set! (-> arg0 data 0)
                   (atan (-> (the-as (pointer float) (+ (+ (shl s2-0 2) (shl s3-0 4)) (the-as int arg1))))
                         (-> (the-as (pointer float) (+ (+ (shl s1-0 2) (shl s3-0 4)) (the-as int arg1))))
                         )
                   )
             (set! (-> arg0 data 1)
                   (atan f30-0 (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s3-0 4)) (the-as int arg1)))))
                   )
             (let ((f0-13 (atan
                           (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s2-0 4)) (the-as int arg1))))
                           (- (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s1-0 4)) (the-as int arg1)))))
                           )
                     )
                   )
               (set! (-> arg0 data 2) f0-13)
               )
             )
            (else
             (set! (-> arg0 data 0)
                   (atan (- (-> (the-as (pointer float) (+ (+ (shl s1-0 2) (shl s2-0 4)) (the-as int arg1)))))
                         (-> (the-as (pointer float) (+ (+ (shl s2-0 2) (shl s2-0 4)) (the-as int arg1))))
                         )
                   )
             (set! (-> arg0 data 1)
                   (atan
                    f30-0
                    (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s3-0 4)) (the-as int arg1))))
                    )
                   )
             (let ((f0-20 0.0))
               (set! (-> arg0 data 2) f0-20)
               )
             )
            )
          )
        (let* ((f0-21 (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s3-0 4)) (the-as int arg1)))))
               (f0-23 (* f0-21 f0-21))
               (f1-3 (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s2-0 4)) (the-as int arg1)))))
               (f30-1 (sqrtf (+ f0-23 (* f1-3 f1-3))))
               )
          (cond
            ((< 0.00001 f30-1)
             (set! (-> arg0 data 0)
                   (atan
                    (-> (the-as (pointer float) (+ (+ (shl s2-0 2) (shl s1-0 4)) (the-as int arg1))))
                    (-> (the-as (pointer float) (+ (+ (shl s1-0 2) (shl s1-0 4)) (the-as int arg1))))
                    )
                   )
             (set! (-> arg0 data 1)
                   (atan
                    (- (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s1-0 4)) (the-as int arg1)))))
                    f30-1
                    )
                   )
             (let ((f0-34 (atan
                           (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s2-0 4)) (the-as int arg1))))
                           (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s3-0 4)) (the-as int arg1))))
                           )
                     )
                   )
               (set! (-> arg0 data 2) f0-34)
               )
             )
            (else
             (set! (-> arg0 data 0)
                   (atan
                    (- (-> (the-as (pointer float) (+ (+ (shl s1-0 2) (shl s2-0 4)) (the-as int arg1)))))
                    (-> (the-as (pointer float) (+ (+ (shl s2-0 2) (shl s2-0 4)) (the-as int arg1))))
                    )
                   )
             (set! (-> arg0 data 1)
                   (atan
                    (- (-> (the-as (pointer float) (+ (+ (shl s3-0 2) (shl s1-0 4)) (the-as int arg1)))))
                    f30-1
                    )
                   )
             (let ((f0-42 0.0))
               (set! (-> arg0 data 2) f0-42)
               )
             )
            )
          )
        )
    )
  (when (= (logand (sar arg2 2) 1) 1)
    (set! (-> arg0 data 0) (- (-> arg0 data 0)))
    (set! (-> arg0 data 1) (- (-> arg0 data 1)))
    (let ((f0-48 (- (-> arg0 data 2))))
      (set! (-> arg0 data 2) f0-48)
      )
    )
  (if (= (logand arg2 1) 1)
      (let ((f0-49 (-> arg0 data 0)))
        (set! (-> arg0 data 0) (-> arg0 data 2))
        (set! (-> arg0 data 2) f0-49)
        )
      )
  (set! (-> arg0 data 3) (the float arg2))
  arg0
  )

(defun eul->quat ((arg0 quaternion) (arg1 euler-angles))
  "Convert euler angles to quaternion"
  (let ((s5-0 (new 'stack-no-clear 'matrix)))
   (eul->matrix s5-0 arg1)
   (matrix->quaternion arg0 s5-0)
   )
  arg0
  )

(defun quat->eul ((arg0 euler-angles) (arg1 quaternion) (arg2 int))
  "Convert quaternion to euler angles. The last argument is euler flags"
  (let ((s5-0 (new 'stack-no-clear 'matrix)))
   (quaternion->matrix s5-0 arg1)
   (matrix->eul arg0 s5-0 arg2)
   )
  arg0
  )