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

;; definition for method 2 of type transformq
(defmethod print transformq ((obj transformq))
  (format #t "#<transformq @ #x~X~%" obj)
  (format #t "~T~Ttrans:~F ~F ~F ~F ~%" (-> obj trans x) (-> obj trans y) (-> obj trans z) (-> obj trans w))
  (format #t "~T~Tquat:  ~F ~F ~F ~F ~%" (-> obj quat x) (-> obj quat y) (-> obj quat z) (-> obj quat w))
  (format #t "~T~Tscale:~F ~F ~F ~F>" (-> obj scale x) (-> obj scale y) (-> obj scale z) (-> obj scale w))
  obj
  )

;; definition for method 27 of type trsqv
(defmethod get-quaternion trsqv ((obj trsqv))
  (-> obj quat)
  )

;; definition for method 18 of type trsqv
(defmethod set-quaternion! trsqv ((obj trsqv) (arg0 quaternion))
  (quaternion-copy! (get-quaternion obj) arg0)
  )

;; definition for method 21 of type trsqv
(defmethod rot->dir-targ! trsqv ((obj trsqv))
  (quaternion-copy! (-> obj dir-targ) (get-quaternion obj))
  )

;; definition for method 22 of type trsqv
(defmethod y-angle trsqv ((obj trsqv))
  (quaternion-y-angle (get-quaternion obj))
  )

;; definition for method 9 of type trsqv
(defmethod seek-toward-heading-vec! trsqv ((obj trsqv) (dir vector) (vel float) (frame-count time-frame))
  (let* ((yaw-error (deg-diff (quaternion-y-angle (-> obj quat)) (vector-y-angle dir)))
         (yaw-limit
           (fmin (* vel (-> *display* seconds-per-frame)) (/ (* 5.0 (fabs yaw-error)) (the float frame-count)))
           )
         (saturated-yaw (fmax (fmin yaw-error yaw-limit) (- yaw-limit)))
         )
    (let ((old-diff (-> obj old-y-angle-diff)))
      (set! saturated-yaw
            (cond
              ((or (= old-diff 0.0)
                   (and (< 0.0 saturated-yaw) (< 0.0 old-diff))
                   (or (and (< saturated-yaw 0.0) (< old-diff 0.0))
                       (>= (- (-> *display* base-frame-counter) (-> obj angle-change-time)) (seconds 0.2))
                       )
                   )
               (set! (-> obj angle-change-time) (-> *display* base-frame-counter))
               saturated-yaw
               )
              (else
                (* 0.000000001 saturated-yaw)
                )
              )
            )
      )
    (set! (-> obj old-y-angle-diff) saturated-yaw)
    (let ((quat (get-quaternion obj)))
      (quaternion-rotate-y! quat quat saturated-yaw)
      )
    )
  )

;; definition for method 10 of type trsqv
(defmethod set-heading-vec! trsqv ((obj trsqv) (arg0 vector))
  (let ((s3-0 (get-quaternion obj)))
    (forward-up-nopitch->quaternion
      s3-0
      (vector-normalize-copy! (new 'stack-no-clear 'vector) arg0 1.0)
      (vector-y-quaternion! (new 'stack-no-clear 'vector) s3-0)
      )
    )
  )

;; definition for method 11 of type trsqv
(defmethod seek-to-point-toward-point! trsqv ((obj trsqv) (arg0 vector) (arg1 float) (arg2 time-frame))
  (seek-toward-heading-vec! obj (vector-! (new 'stack-no-clear 'vector) arg0 (-> obj trans)) arg1 arg2)
  )

;; definition for method 12 of type trsqv
(defmethod point-toward-point! trsqv ((obj trsqv) (arg0 vector))
  (let ((s3-0 (get-quaternion obj)))
    (forward-up-nopitch->quaternion
      s3-0
      (vector-normalize! (vector-! (new 'stack-no-clear 'vector) arg0 (-> obj trans)) 1.0)
      (vector-y-quaternion! (new 'stack-no-clear 'vector) s3-0)
      )
    )
  )

;; definition for method 13 of type trsqv
(defmethod seek-toward-yaw-angle! trsqv ((obj trsqv) (yaw float) (vel float) (frame-count time-frame))
  (let ((s3-0 (method-of-object obj seek-toward-heading-vec!))
        (s2-0 (new 'stack-no-clear 'vector))
        )
    (set! (-> s2-0 x) (sin yaw))
    (set! (-> s2-0 y) 0.0)
    (set! (-> s2-0 z) (cos yaw))
    (set! (-> s2-0 w) 1.0)
    (s3-0 obj s2-0 vel frame-count)
    )
  )

;; definition for method 14 of type trsqv
(defmethod set-yaw-angle-clear-roll-pitch! trsqv ((obj trsqv) (arg0 float))
  (let ((s5-0 (method-of-object obj set-heading-vec-clear-roll-pitch!))
        (s4-0 (new 'stack-no-clear 'vector))
        )
    (set! (-> s4-0 x) (sin arg0))
    (set! (-> s4-0 y) 0.0)
    (set! (-> s4-0 z) (cos arg0))
    (set! (-> s4-0 w) 1.0)
    (s5-0 obj s4-0)
    )
  )

;; definition for method 15 of type trsqv
(defmethod set-roll-to-grav! trsqv ((obj trsqv) (arg0 float))
  (set-roll-to-grav-2! obj arg0)
  )

;; definition for method 16 of type trsqv
(defmethod set-roll-to-grav-2! trsqv ((obj trsqv) (arg0 float))
  (let* ((quat (get-quaternion obj))
         (grav (-> *standard-dynamics* gravity-normal))
         (rot-mat (quaternion->matrix (new 'stack-no-clear 'matrix) quat))
         )
    (let ((dir-z (-> rot-mat vector 2)))
      (vector-normalize! (vector-flatten! (-> rot-mat vector 1) grav dir-z) 1.0)
      (vector-cross! (the-as vector (-> rot-mat vector)) (-> rot-mat vector 1) dir-z)
      )
    (let ((a1-5 (matrix-rotate-z! (new 'stack-no-clear 'matrix) arg0)))
      (matrix*! rot-mat a1-5 rot-mat)
      )
    (matrix->quaternion quat rot-mat)
    )
  )

;; definition for method 25 of type trsqv
(defmethod roll-relative-to-gravity trsqv ((obj trsqv))
  (let* ((quat (get-quaternion obj))
         (dir-z (vector-z-quaternion! (new 'stack-no-clear 'vector) quat))
         (dir-y (vector-y-quaternion! (new 'stack-no-clear 'vector) quat))
         (dir-grav (-> *standard-dynamics* gravity-normal))
         (grav-z-plane (vector-normalize! (vector-flatten! (new 'stack-no-clear 'vector) dir-grav dir-z) 1.0))
         (grav-dot (vector-dot grav-z-plane dir-y))
         )
    (if (< (vector-dot (vector-cross! (new 'stack-no-clear 'vector) grav-z-plane dir-y) dir-z) 0.0)
        (- (acos grav-dot))
        (acos grav-dot)
        )
    )
  )

;; definition for method 17 of type trsqv
;; Used lq/sq
(defmethod rotate-toward-orientation! trsqv ((obj trsqv) (target quaternion) (y-rate float) (z-rate float))
  (local-vars (sv-96 vector))
  (let ((quat (get-quaternion obj)))
    (let ((temp-quat (new 'stack-no-clear 'quaternion)))
      (when (< 0.0 z-rate)
        (let ((s1-0 quaternion-from-two-vectors-max-angle!)
              (s0-0 temp-quat)
              )
          (set! sv-96 (vector-y-quaternion! (new 'stack-no-clear 'vector) quat))
          (let ((a2-1 (vector-y-quaternion! (new 'stack-no-clear 'vector) target))
                (a3-1 (* z-rate (-> *display* seconds-per-frame)))
                )
            (s1-0 s0-0 sv-96 a2-1 a3-1)
            )
          )
        (quaternion-normalize! (quaternion*! quat temp-quat quat))
        )
      (when (< 0.0 y-rate)
        (quaternion-from-two-vectors-max-angle!
          temp-quat
          (vector-z-quaternion! (new 'stack-no-clear 'vector) quat)
          (vector-z-quaternion! (new 'stack-no-clear 'vector) target)
          (* y-rate (-> *display* seconds-per-frame))
          )
        (quaternion-normalize! (quaternion*! quat temp-quat quat))
        )
      )
    quat
    )
  )

;; definition for method 19 of type trsqv
(defmethod set-heading-vec-clear-roll-pitch! trsqv ((obj trsqv) (arg0 vector))
  (forward-up->quaternion
    (get-quaternion obj)
    (vector-normalize-copy! (new 'stack-no-clear 'vector) arg0 1.0)
    (new 'static 'vector :y 1.0 :w 1.0)
    )
  )

;; definition for method 20 of type trsqv
(defmethod point-toward-point-clear-roll-pitch! trsqv ((obj trsqv) (arg0 vector))
  (forward-up->quaternion
    (get-quaternion obj)
    (vector-normalize! (vector-! (new 'stack-no-clear 'vector) arg0 (-> obj trans)) 1.0)
    (new 'static 'vector :y 1.0 :w 1.0)
    )
  )

;; definition for function transformq-copy!
;; Used lq/sq
(defun transformq-copy! ((arg0 transformq) (arg1 transformq))
  (set! (-> arg0 trans quad) (-> arg1 trans quad))
  (set! (-> arg0 quat vec quad) (-> arg1 quat vec quad))
  (set! (-> arg0 scale quad) (-> arg1 scale quad))
  arg0
  )

;; definition for function matrix<-transformq!
;; Used lq/sq
(defun matrix<-transformq! ((arg0 matrix) (arg1 transformq))
  (local-vars (v1-1 float))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         )
    (init-vf0-vector)
    (quaternion->matrix arg0 (-> arg1 quat))
    (cond
      (#f
        (set! (-> arg0 vector 3 quad) (-> arg1 trans quad))
        )
      (else
        (.lvf vf1 (&-> arg1 scale quad))
        (.lvf vf2 (&-> arg1 trans quad))
        (.lvf vf3 (&-> arg0 vector 0 quad))
        (.lvf vf4 (&-> arg0 vector 1 quad))
        (.lvf vf5 (&-> arg0 vector 2 quad))
        (.mov.vf vf2 vf0 :mask #b1000)
        (.mul.x.vf vf3 vf3 vf1)
        (.mul.y.vf vf4 vf4 vf1)
        (.mul.z.vf vf5 vf5 vf1)
        (.svf (&-> arg0 vector 3 quad) vf2)
        (.svf (&-> arg0 vector 0 quad) vf3)
        (.svf (&-> arg0 vector 1 quad) vf4)
        (.svf (&-> arg0 vector 2 quad) vf5)
        (.mov v1-1 vf5)
        )
      )
    arg0
    )
  )

;; definition for function matrix<-no-trans-transformq!
(defun matrix<-no-trans-transformq! ((arg0 matrix) (arg1 transformq))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         )
    (init-vf0-vector)
    (quaternion->matrix arg0 (-> arg1 quat))
    (.lvf vf1 (&-> arg1 scale quad))
    (.lvf vf3 (&-> arg0 vector 0 quad))
    (.lvf vf4 (&-> arg0 vector 1 quad))
    (.lvf vf5 (&-> arg0 vector 2 quad))
    (.mov.vf vf2 vf0)
    (.mul.x.vf vf3 vf3 vf1)
    (.mul.y.vf vf4 vf4 vf1)
    (.mul.z.vf vf5 vf5 vf1)
    (.svf (&-> arg0 vector 3 quad) vf2)
    (.svf (&-> arg0 vector 0 quad) vf3)
    (.svf (&-> arg0 vector 1 quad) vf4)
    (.svf (&-> arg0 vector 2 quad) vf5)
    arg0
    )
  )

;; definition for function matrix<-transformq+trans!
(defun matrix<-transformq+trans! ((arg0 matrix) (arg1 transformq) (arg2 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)
         )
    (init-vf0-vector)
    (quaternion->matrix arg0 (-> arg1 quat))
    (.lvf vf1 (&-> arg1 scale quad))
    (.lvf vf2 (&-> arg1 trans quad))
    (.lvf vf6 (&-> arg2 quad))
    (.lvf vf3 (&-> arg0 vector 0 quad))
    (.lvf vf4 (&-> arg0 vector 1 quad))
    (.lvf vf5 (&-> arg0 vector 2 quad))
    (.mov.vf vf2 vf0 :mask #b1000)
    (.mul.x.vf vf3 vf3 vf1)
    (.mul.y.vf vf4 vf4 vf1)
    (.mul.z.vf vf5 vf5 vf1)
    (.mul.x.vf acc vf3 vf6)
    (.add.mul.y.vf acc vf4 vf6 acc)
    (.add.mul.z.vf acc vf5 vf6 acc)
    (.add.mul.w.vf vf2 vf2 vf0 acc :mask #b111)
    (.svf (&-> arg0 vector 3 quad) vf2)
    (.svf (&-> arg0 vector 0 quad) vf3)
    (.svf (&-> arg0 vector 1 quad) vf4)
    (.svf (&-> arg0 vector 2 quad) vf5)
    arg0
    )
  )

;; definition for function matrix<-transformq+world-trans!
(defun matrix<-transformq+world-trans! ((arg0 matrix) (arg1 transformq) (arg2 vector))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (init-vf0-vector)
    (quaternion->matrix arg0 (-> arg1 quat))
    (.lvf vf1 (&-> arg1 scale quad))
    (.lvf vf2 (&-> arg1 trans quad))
    (.lvf vf6 (&-> arg2 quad))
    (.lvf vf3 (&-> arg0 vector 0 quad))
    (.lvf vf4 (&-> arg0 vector 1 quad))
    (.lvf vf5 (&-> arg0 vector 2 quad))
    (.mov.vf vf2 vf0 :mask #b1000)
    (.mul.x.vf vf3 vf3 vf1)
    (.mul.y.vf vf4 vf4 vf1)
    (.mul.z.vf vf5 vf5 vf1)
    (.add.vf vf2 vf2 vf6 :mask #b111)
    (.svf (&-> arg0 vector 3 quad) vf2)
    (.svf (&-> arg0 vector 0 quad) vf3)
    (.svf (&-> arg0 vector 1 quad) vf4)
    (.svf (&-> arg0 vector 2 quad) vf5)
    arg0
    )
  )

;; definition for function matrix<-parented-transformq!
(defun matrix<-parented-transformq! ((arg0 matrix) (arg1 transformq) (arg2 vector))
  (local-vars (v1-1 float))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (init-vf0-vector)
    (quaternion->matrix arg0 (-> arg1 quat))
    (let ((v1-0 (new 'stack-no-clear 'vector)))
      (set! (-> v1-0 x) (/ 1.0 (-> arg2 x)))
      (set! (-> v1-0 y) (/ 1.0 (-> arg2 y)))
      (set! (-> v1-0 z) (/ 1.0 (-> arg2 z)))
      (.lvf vf1 (&-> arg1 scale quad))
      (.lvf vf2 (&-> arg1 trans quad))
      (.mov.vf vf2 vf0 :mask #b1000)
      (.lvf vf4 (&-> arg0 vector 0 quad))
      (.lvf vf5 (&-> arg0 vector 1 quad))
      (.lvf vf6 (&-> arg0 vector 2 quad))
      (.mul.x.vf vf4 vf4 vf1)
      (.mul.y.vf vf5 vf5 vf1)
      (.mul.z.vf vf6 vf6 vf1)
      (.lvf vf3 (&-> v1-0 quad))
      )
    (.mul.vf vf4 vf4 vf3)
    (.mul.vf vf5 vf5 vf3)
    (.mul.vf vf6 vf6 vf3)
    (.svf (&-> arg0 vector 3 quad) vf2)
    (.svf (&-> arg0 vector 0 quad) vf4)
    (.svf (&-> arg0 vector 1 quad) vf5)
    (.svf (&-> arg0 vector 2 quad) vf6)
    (.mov v1-1 vf6)
    arg0
    )
  )