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

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

(defmethod print transform ((obj transform))
  (format #t "#<transform @ #x~X~%" obj)
  (format #t "~T~Ttrans:~F ~F ~F ~F ~%"
   (-> obj trans data 0)
   (-> obj trans data 1)
   (-> obj trans data 2)
   (-> obj trans data 3)
   )
  (format #t "~T~Trot:  ~F ~F ~F ~F ~%"
   (-> obj rot data 0)
   (-> obj rot data 1)
   (-> obj rot data 2)
   (-> obj rot data 3)
   )
  (format #t "~T~Tscale:~F ~F ~F ~F>"
   (-> obj scale data 0)
   (-> obj scale data 1)
   (-> obj scale data 2)
   (-> obj scale data 3)
   )
  obj
  )

(defmethod new trs ((allocation symbol) (type-to-make type))
  "Create a new trs and set it equal to identity."
  (let ((obj (object-new allocation type-to-make (the-as int (-> type-to-make size)))))
   (set! (-> obj trans data 3) 1.0)
   (set! (-> obj rot data 3) 1.0)
   (vector-identity! (-> obj scale))
   obj
   )
  )

(defun transform-matrix-calc! ((tf transform) (dst-mat matrix))
  "Convert a transform to a matrix. This is not particularly efficient."
  (let ((s4-0 (new-stack-matrix0))
        (s3-0 (new-stack-matrix0))
        )
    ;; start with identity
    (matrix-identity! dst-mat)
    ;; set translation (which also sets identity...)
    (matrix-translate! dst-mat (-> tf trans))
    ;; rotate y axis (this is first, so yaw is "world aligned"
    (matrix-rotate-y! s4-0 (-> tf rot data 1))
    (matrix*! s3-0 s4-0 dst-mat)
    ;; rotate x axis
    (matrix-rotate-x! s4-0 (-> tf rot data 0))
    (matrix*! dst-mat s4-0 s3-0)
    ;; rotate z axis
    (matrix-rotate-z! s4-0 (-> tf rot data 2))
    (matrix*! s3-0 s4-0 dst-mat)
    ;; apply scale
    (matrix-scale! s4-0 (-> tf scale))
   )
  )

(defun transform-matrix-parent-calc! ((tf transform) (dst-mat matrix) (inv-scale vector))
  "Convert a transform to a matrix, applying an inverse scaling."
  (let ((s4-0 (new-stack-matrix0))
        (s3-0 (new-stack-matrix0))
        )
    (matrix-identity! s3-0)
    (matrix-translate! s3-0 (-> tf trans))
    (matrix-inv-scale! s4-0 inv-scale)
    (matrix*! dst-mat s4-0 s3-0)
    (matrix-rotate-y! s4-0 (-> tf rot data 1))
    (matrix*! s3-0 s4-0 dst-mat)
    (matrix-rotate-x! s4-0 (-> tf rot data 0))
    (matrix*! dst-mat s4-0 s3-0)
    (matrix-rotate-z! s4-0 (-> tf rot data 2))
    (matrix*! s3-0 s4-0 dst-mat)
    (matrix-scale! s4-0 (-> tf scale))
    (matrix*! dst-mat s4-0 s3-0)
    )
  )

(defun trs-matrix-calc! ((tf trs) (dst-mat matrix))
  "Convert a trs to a matrix"
  ;; this relies on the fact that trs and transform both have the same memory layout.
  (transform-matrix-calc! (the-as transform (-> tf trans)) dst-mat)
  )