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

;; definition for function set-eul!
(defun
  set-eul!
  ((arg0 euler-angles) (arg1 float) (arg2 float) (arg3 float) (arg4 int))
  (set! (-> arg0 x) arg1)
  (set! (-> arg0 y) arg2)
  (set! (-> arg0 z) arg3)
  (set! (-> arg0 w) (the float arg4))
  arg0
  )

;; definition for function eul->matrix
;; Used lq/sq
(defun eul->matrix ((arg0 matrix) (arg1 euler-angles))
  (matrix-identity! arg0)
  (let ((s5-0 (new 'stack-no-clear 'vector)))
   (set! (-> s5-0 quad) (-> arg1 quad))
   (when (= (logand (the int (-> s5-0 w)) 1) 1)
    (let ((f0-2 (-> s5-0 x)))
     (set! (-> s5-0 x) (-> s5-0 z))
     (set! (-> s5-0 z) f0-2)
     )
    )
   (when (= (logand (/ (the int (-> s5-0 w)) 4) 1) 1)
    (set! (-> s5-0 x) (- (-> s5-0 x)))
    (set! (-> s5-0 y) (- (-> s5-0 y)))
    (set! (-> s5-0 z) (- (-> s5-0 z)))
    )
   (let* ((f26-0 (cos (-> s5-0 x)))
          (f30-0 (cos (-> s5-0 y)))
          (f22-0 (cos (-> s5-0 z)))
          (f24-0 (sin (-> s5-0 x)))
          (f28-0 (sin (-> s5-0 y)))
          (f4-0 (sin (-> s5-0 z)))
          (f0-17 (* f26-0 f22-0))
          (f1-1 (* f26-0 f4-0))
          (f2-0 (* f24-0 f22-0))
          (f3-0 (* f24-0 f4-0))
          )
    0
    0
    0
    (let* ((v1-12 (logand (/ (the int (-> s5-0 w)) 4) 1))
           (a1-2 (-> EulSafe (logand (/ (the int (-> s5-0 w)) 8) 3)))
           (a0-21 (-> EulNext (+ a1-2 v1-12)))
           (v1-17 (-> EulNext (+ (- 1 v1-12) a1-2)))
           )
     (cond
      ((= (logand (/ (the int (-> s5-0 w)) 2) 1) 1)
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a1-2 16) (* a1-2 4)) (the-as int arg0))
          )
         )
        f30-0
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a1-2 16) (* a0-21 4)) (the-as int arg0))
          )
         )
        (* f28-0 f24-0)
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a1-2 16) (* v1-17 4)) (the-as int arg0))
          )
         )
        (* f28-0 f26-0)
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a0-21 16) (* a1-2 4)) (the-as int arg0))
          )
         )
        (* f28-0 f4-0)
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a0-21 16) (* a0-21 4)) (the-as int arg0))
          )
         )
        (- f0-17 (* f30-0 f3-0))
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a0-21 16) (* v1-17 4)) (the-as int arg0))
          )
         )
        (- (- f2-0) (* f30-0 f1-1))
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* v1-17 16) (* a1-2 4)) (the-as int arg0))
          )
         )
        (- (* f28-0 f22-0))
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* v1-17 16) (* a0-21 4)) (the-as int arg0))
          )
         )
        (+ f1-1 (* f30-0 f2-0))
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* v1-17 16) (* v1-17 4)) (the-as int arg0))
          )
         )
        (+ (- f3-0) (* f30-0 f0-17))
        )
       )
      (else
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a1-2 16) (* a1-2 4)) (the-as int arg0))
          )
         )
        (* f30-0 f22-0)
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a1-2 16) (* a0-21 4)) (the-as int arg0))
          )
         )
        (+ (- f1-1) (* f28-0 f2-0))
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a1-2 16) (* v1-17 4)) (the-as int arg0))
          )
         )
        (+ f3-0 (* f28-0 f0-17))
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a0-21 16) (* a1-2 4)) (the-as int arg0))
          )
         )
        (* f30-0 f4-0)
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a0-21 16) (* a0-21 4)) (the-as int arg0))
          )
         )
        (+ f0-17 (* f28-0 f3-0))
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* a0-21 16) (* v1-17 4)) (the-as int arg0))
          )
         )
        (+ (- f2-0) (* f28-0 f1-1))
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* v1-17 16) (* a1-2 4)) (the-as int arg0))
          )
         )
        (- f28-0)
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* v1-17 16) (* a0-21 4)) (the-as int arg0))
          )
         )
        (* f30-0 f24-0)
        )
       (set!
        (->
         (the-as
          (pointer float)
          (+ (+ (* v1-17 16) (* v1-17 4)) (the-as int arg0))
          )
         )
        (* f30-0 f26-0)
        )
       )
      )
     )
    )
   )
  arg0
  )

;; definition for function matrix->eul
(defun matrix->eul ((arg0 euler-angles) (arg1 matrix) (arg2 int))
  0
  0
  0
  (let* ((v1-4 (logand (/ arg2 4) 1))
         (s3-0 (-> EulSafe (logand (/ arg2 8) 3)))
         (s2-0 (-> EulNext (+ s3-0 v1-4)))
         (s1-0 (-> EulNext (+ (- 1 v1-4) s3-0)))
         )
   (cond
    ((= (logand (/ arg2 2) 1) 1)
     (let*
      ((f0-0
        (->
         (the-as
          (pointer float)
          (+ (+ (* s2-0 4) (* s3-0 16)) (the-as int arg1))
          )
         )
        )
       (f0-2 (* f0-0 f0-0))
       (f1-0
        (->
         (the-as
          (pointer float)
          (+ (+ (* s1-0 4) (* s3-0 16)) (the-as int arg1))
          )
         )
        )
       (f30-0 (sqrtf (+ f0-2 (* f1-0 f1-0))))
       )
      (cond
       ((< 0.00001 f30-0)
        (set!
         (-> arg0 x)
         (atan
          (->
           (the-as
            (pointer float)
            (+ (+ (* s2-0 4) (* s3-0 16)) (the-as int arg1))
            )
           )
          (->
           (the-as
            (pointer float)
            (+ (+ (* s1-0 4) (* s3-0 16)) (the-as int arg1))
            )
           )
          )
         )
        (set!
         (-> arg0 y)
         (atan
          f30-0
          (->
           (the-as
            (pointer float)
            (+ (+ (* s3-0 4) (* s3-0 16)) (the-as int arg1))
            )
           )
          )
         )
        (set!
         (-> arg0 z)
         (atan
          (->
           (the-as
            (pointer float)
            (+ (+ (* s3-0 4) (* s2-0 16)) (the-as int arg1))
            )
           )
          (-
           (->
            (the-as
             (pointer float)
             (+ (+ (* s3-0 4) (* s1-0 16)) (the-as int arg1))
             )
            )
           )
          )
         )
        )
       (else
        (set!
         (-> arg0 x)
         (atan
          (-
           (->
            (the-as
             (pointer float)
             (+ (+ (* s1-0 4) (* s2-0 16)) (the-as int arg1))
             )
            )
           )
          (->
           (the-as
            (pointer float)
            (+ (+ (* s2-0 4) (* s2-0 16)) (the-as int arg1))
            )
           )
          )
         )
        (set!
         (-> arg0 y)
         (atan
          f30-0
          (->
           (the-as
            (pointer float)
            (+ (+ (* s3-0 4) (* s3-0 16)) (the-as int arg1))
            )
           )
          )
         )
        (set! (-> arg0 z) 0.0)
        )
       )
      )
     )
    (else
     (let*
      ((f0-21
        (->
         (the-as
          (pointer float)
          (+ (+ (* s3-0 4) (* s3-0 16)) (the-as int arg1))
          )
         )
        )
       (f0-23 (* f0-21 f0-21))
       (f1-3
        (->
         (the-as
          (pointer float)
          (+ (+ (* s3-0 4) (* s2-0 16)) (the-as int arg1))
          )
         )
        )
       (f30-1 (sqrtf (+ f0-23 (* f1-3 f1-3))))
       )
      (cond
       ((< 0.00001 f30-1)
        (set!
         (-> arg0 x)
         (atan
          (->
           (the-as
            (pointer float)
            (+ (+ (* s2-0 4) (* s1-0 16)) (the-as int arg1))
            )
           )
          (->
           (the-as
            (pointer float)
            (+ (+ (* s1-0 4) (* s1-0 16)) (the-as int arg1))
            )
           )
          )
         )
        (set!
         (-> arg0 y)
         (atan
          (-
           (->
            (the-as
             (pointer float)
             (+ (+ (* s3-0 4) (* s1-0 16)) (the-as int arg1))
             )
            )
           )
          f30-1
          )
         )
        (set!
         (-> arg0 z)
         (atan
          (->
           (the-as
            (pointer float)
            (+ (+ (* s3-0 4) (* s2-0 16)) (the-as int arg1))
            )
           )
          (->
           (the-as
            (pointer float)
            (+ (+ (* s3-0 4) (* s3-0 16)) (the-as int arg1))
            )
           )
          )
         )
        )
       (else
        (set!
         (-> arg0 x)
         (atan
          (-
           (->
            (the-as
             (pointer float)
             (+ (+ (* s1-0 4) (* s2-0 16)) (the-as int arg1))
             )
            )
           )
          (->
           (the-as
            (pointer float)
            (+ (+ (* s2-0 4) (* s2-0 16)) (the-as int arg1))
            )
           )
          )
         )
        (set!
         (-> arg0 y)
         (atan
          (-
           (->
            (the-as
             (pointer float)
             (+ (+ (* s3-0 4) (* s1-0 16)) (the-as int arg1))
             )
            )
           )
          f30-1
          )
         )
        (set! (-> arg0 z) 0.0)
        )
       )
      )
     )
    )
   )
  (when (= (logand (/ arg2 4) 1) 1)
   (set! (-> arg0 x) (- (-> arg0 x)))
   (set! (-> arg0 y) (- (-> arg0 y)))
   (set! (-> arg0 z) (- (-> arg0 z)))
   )
  (when (= (logand arg2 1) 1)
   (let ((f0-49 (-> arg0 x)))
    (set! (-> arg0 x) (-> arg0 z))
    (set! (-> arg0 z) f0-49)
    )
   )
  (set! (-> arg0 w) (the float arg2))
  arg0
  )

;; definition for function eul->quat
(defun eul->quat ((arg0 quaternion) (arg1 euler-angles))
  (let ((s5-0 (new 'stack-no-clear 'matrix)))
   (eul->matrix s5-0 arg1)
   (matrix->quaternion arg0 s5-0)
   )
  arg0
  )

;; definition for function quat->eul
(defun quat->eul ((arg0 euler-angles) (arg1 quaternion) (arg2 int))
  (let ((s5-0 (new 'stack-no-clear 'matrix)))
   (quaternion->matrix s5-0 arg1)
   (matrix->eul arg0 s5-0 arg2)
   )
  arg0
  )