jak-project/goal_src/jak2/engine/physics/chain-physics.gc

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

;; name: chain-physics.gc
;; name in dgo: chain-physics
;; dgos: ENGINE, GAME

;; DECOMP BEGINS

(defmethod gravity-update chain-physics ((obj chain-physics) (arg0 process-drawable))
  (vector-seek-3d-smooth! (-> obj gravity) (-> obj gravity-target) 0.05 0.1)
  0
  (none)
  )

(defmethod apply-gravity chain-physics ((obj chain-physics) (arg0 vector) (arg1 int) (arg2 process-drawable))
  (vector-float*!
    arg0
    (-> obj gravity)
    (* 4096.0 (-> self clock time-adjust-ratio) (fmax 0.2 (lerp-scale 0.38 0.18 (the float arg1) 0.0 5.0)))
    )
  0
  (none)
  )

(defmethod chain-physics-method-14 chain-physics ((obj chain-physics) (arg0 vector) (arg1 int))
  (vector-float*!
    arg0
    (the-as vector (+ (the-as uint (-> obj chain-joints 0 velocity)) (* (+ arg1 1) 64)))
    (fmin 0.9 (lerp-scale 0.2 1.0 (the float arg1) 0.0 4.0))
    )
  0
  (none)
  )

(defmethod clamp-length chain-physics ((obj chain-physics) (arg0 vector) (arg1 vector) (arg2 object) (arg3 process-drawable))
  (let ((gp-0 (new 'stack-no-clear 'vector)))
    (vector-! gp-0 arg0 arg1)
    (vector-normalize! gp-0 (-> obj joint-length))
    (vector+! arg0 gp-0 arg1)
    )
  )

(defmethod chain-physics-method-16 chain-physics ((obj chain-physics) (arg0 int))
  (lerp-scale 5461.3335 10922.667 (the float arg0) 0.0 8.0)
  )

(defmethod chain-physics-method-17 chain-physics ((obj chain-physics) (arg0 vector) (arg1 int))
  0
  (none)
  )

(defmethod initialize-chain-joints chain-physics ((obj chain-physics))
  (set! (-> obj turn-off-start) 0)
  (dotimes (s5-0 (the-as int (-> obj num-joints)))
    (let ((s4-0 (-> obj chain-joints s5-0)))
      (vector<-cspace! (-> s4-0 position) (-> s4-0 joint-mod joint))
      (vector-reset! (-> s4-0 velocity))
      (mode-set! (-> s4-0 joint-mod) (joint-mod-mode joint-set-world))
      )
    )
  #f
  )

(defmethod turn-off chain-physics ((obj chain-physics) (arg0 time-frame))
  (set! (-> obj turn-off-start) (current-time))
  (set! (-> obj turn-off-duration) arg0)
  0
  (none)
  )

(defmethod update chain-physics ((obj chain-physics) (arg0 process-drawable))
  (local-vars
    (v1-78 float)
    (f0-11 float)
    (sv-272 chain-physics-joint)
    (sv-288 vector)
    (sv-304 vector)
    (sv-320 (function vector float vector))
    (sv-336 vector)
    (sv-352 vector)
    (sv-368 vector)
    (sv-384 vector)
    (sv-400 vector)
    (sv-416 vector)
    (sv-432 vector)
    (sv-448 (inline-array vector))
    (sv-464 (inline-array vector))
    (sv-480 (inline-array vector))
    (sv-496 vector)
    (sv-512 (inline-array vector))
    (sv-528 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)
    (gravity-update obj arg0)
    (let ((s4-0 (new 'stack-no-clear 'matrix))
          (s3-0 (vector<-cspace! (new 'stack-no-clear 'vector) (-> arg0 node-list data (-> obj root-joint-index))))
          (s2-0 (vector-normalize-copy!
                  (new 'stack-no-clear 'vector)
                  (-> arg0 node-list data (-> obj root-joint-index) bone transform vector 2)
                  1.0
                  )
                )
          (s1-0 (vector-normalize-copy!
                  (new 'stack-no-clear 'vector)
                  (-> arg0 node-list data (-> obj root-joint-index) bone transform vector 1)
                  1.0
                  )
                )
          )
      (new 'stack-no-clear 'vector)
      (let ((f30-0 1.0))
        (if (nonzero? (-> obj turn-off-start))
            (set! f30-0
                  (fmax
                    0.0
                    (fmin
                      1.0
                      (parameter-ease-sin-clamp
                        (- 1.0 (/ (the float (- (current-time) (-> obj turn-off-start))) (the float (-> obj turn-off-duration))))
                        )
                      )
                    )
                  )
            )
        (dotimes (s0-0 (the-as int (-> obj num-joints)))
          (set! sv-272 (-> obj chain-joints s0-0))
          (set! sv-528 (new 'stack-no-clear 'vector))
          (let ((v1-27 (-> sv-272 position quad)))
            (set! (-> sv-528 quad) v1-27)
            )
          (set! (-> sv-272 joint-mod flex-blend) f30-0)
          (if (-> obj negate-y)
              (vector-negate! s1-0 s1-0)
              )
          (set! sv-288 (new 'stack-no-clear 'vector))
          (apply-gravity obj sv-288 s0-0 arg0)
          (let ((v1-34 sv-528))
            (let ((a0-10 sv-528))
              (.mov.vf vf6 vf0 :mask #b1000)
              (.lvf vf4 (&-> a0-10 quad))
              )
            (.lvf vf5 (&-> sv-288 quad))
            (.add.vf vf6 vf4 vf5 :mask #b111)
            (.svf (&-> v1-34 quad) vf6)
            )
          (when (< s0-0 (the-as int (+ (-> obj root-joint-index) -1)))
            (set! sv-304 (new 'stack-no-clear 'vector))
            (chain-physics-method-14 obj sv-304 s0-0)
            (let ((v1-40 sv-528))
              (let ((a0-13 sv-528))
                (.mov.vf vf6 vf0 :mask #b1000)
                (.lvf vf4 (&-> a0-13 quad))
                )
              (.lvf vf5 (&-> sv-304 quad))
              (.add.vf vf6 vf4 vf5 :mask #b111)
              (.svf (&-> v1-40 quad) vf6)
              )
            )
          (clamp-length obj sv-528 s3-0 s0-0 arg0)
          (set! sv-400 (new 'stack-no-clear 'vector))
          (let ((v1-43 sv-528)
                (a0-16 s3-0)
                )
            (.lvf vf4 (&-> v1-43 quad))
            (.lvf vf5 (&-> a0-16 quad))
            )
          (.mov.vf vf6 vf0 :mask #b1000)
          (.sub.vf vf6 vf4 vf5 :mask #b111)
          (.svf (&-> sv-400 quad) vf6)
          (let ((f28-1 (vector-normalize-ret-len! sv-400 1.0))
                (f24-0 (vector-dot sv-400 s1-0))
                (f26-0 (chain-physics-method-16 obj s0-0))
                )
            (when (< f24-0 (cos f26-0))
              (vector--float*! sv-400 sv-400 s1-0 f24-0)
              (set! sv-320 vector-normalize!)
              (set! sv-336 sv-400)
              (let ((a1-17 (sin f26-0)))
                (sv-320 sv-336 a1-17)
                )
              (set! sv-384 sv-400)
              (set! sv-352 sv-400)
              (set! sv-368 s1-0)
              (let ((f0-6 (cos f26-0)))
                (.lvf vf2 (&-> sv-368 quad))
                (.lvf vf1 (&-> sv-352 quad))
                (let ((v1-55 f0-6))
                  (.mov vf3 v1-55)
                  )
                )
              (.add.x.vf vf4 vf0 vf0 :mask #b1000)
              (.mul.x.vf acc vf2 vf3)
              (.add.mul.w.vf vf4 vf1 vf0 acc :mask #b111)
              (.svf (&-> sv-384 quad) vf4)
              (vector+float*! sv-528 s3-0 sv-400 f28-1)
              )
            )
          (chain-physics-method-17 obj sv-528 s0-0)
          (set! sv-432 (new 'stack-no-clear 'vector))
          (let ((v1-61 s3-0)
                (a0-27 sv-528)
                )
            (.lvf vf4 (&-> v1-61 quad))
            (.lvf vf5 (&-> a0-27 quad))
            )
          (.mov.vf vf6 vf0 :mask #b1000)
          (.sub.vf vf6 vf4 vf5 :mask #b111)
          (.svf (&-> sv-432 quad) vf6)
          (vector-normalize-ret-len! sv-432 1.0)
          (set! sv-416 (new 'stack-no-clear 'vector))
          (let ((v1-64 sv-528)
                (a0-30 (-> sv-272 position))
                )
            (.lvf vf4 (&-> v1-64 quad))
            (.lvf vf5 (&-> a0-30 quad))
            )
          (.mov.vf vf6 vf0 :mask #b1000)
          (.sub.vf vf6 vf4 vf5 :mask #b111)
          (.svf (&-> sv-416 quad) vf6)
          (let ((f28-2 (vector-dot sv-432 sv-416)))
            (vector--float*! sv-416 sv-416 sv-432 f28-2)
            (cond
              ((< f28-2 0.0)
               (set! f0-11 (* f28-2 (-> obj compress-vel-parallel)))
               (vector-float*! sv-416 sv-416 (-> obj compress-vel))
               )
              (else
                (set! f0-11 (* f28-2 (-> obj stretch-vel-parallel)))
                (vector-float*! sv-416 sv-416 (-> obj stretch-vel))
                )
              )
            )
          (let ((v1-73 (-> sv-272 velocity)))
            (.lvf vf2 (&-> sv-432 quad))
            (.lvf vf1 (&-> sv-416 quad))
            (let ((a0-37 f0-11))
              (.mov vf3 a0-37)
              )
            (.add.x.vf vf4 vf0 vf0 :mask #b1000)
            (.mul.x.vf acc vf2 vf3)
            (.add.mul.w.vf vf4 vf1 vf0 acc :mask #b111)
            (.svf (&-> v1-73 quad) vf4)
            )
          (vector+! sv-528 (-> sv-272 position) (-> sv-272 velocity))
          (let ((a2-9 (vector-! (new 'stack-no-clear 'vector) s3-0 sv-528)))
            (.lvf vf1 (&-> a2-9 quad))
            (.add.w.vf vf2 vf0 vf0 :mask #b1)
            (.mul.vf vf1 vf1 vf1)
            (.mul.x.vf acc vf2 vf1 :mask #b1)
            (.add.mul.y.vf acc vf2 vf1 acc :mask #b1)
            (.add.mul.z.vf vf1 vf2 vf1 acc :mask #b1)
            (.mov v1-78 vf1)
            (let* ((f0-13 v1-78)
                   (f1-6 (* (-> obj maximum-stretch) (-> obj joint-length)))
                   (f2-3 f1-6)
                   )
              (if (< (* f2-3 f2-3) f0-13)
                  (vector--float*! sv-528 s3-0 a2-9 (/ f1-6 (sqrtf f0-13)))
                  )
              )
            )
          (set! (-> sv-272 position quad) (-> sv-528 quad))
          (if (-> sv-272 joint-mod)
              (set! (-> sv-272 joint-mod trans quad) (-> sv-528 quad))
              )
          (when (< s0-0 (the-as int (-> obj num-joints)))
            (vector-! (-> s4-0 vector 1) sv-528 s3-0)
            (vector-normalize! (-> s4-0 vector 1) 1.0)
            (if (-> obj negate-y)
                (vector-negate! (-> s4-0 vector 1) (-> s4-0 vector 1))
                )
            (vector-cross! (the-as vector (-> s4-0 vector)) (-> s4-0 vector 1) s2-0)
            (vector-normalize! (the-as vector (-> s4-0 vector)) 1.0)
            (set! sv-496 (new 'stack-no-clear 'vector))
            (let ((v1-97 (-> sv-272 old-x))
                  (a0-51 (-> s4-0 vector))
                  )
              (.lvf vf1 (&-> v1-97 quad))
              (.lvf vf2 (&-> a0-51 0 quad))
              )
            (.outer.product.a.vf acc vf1 vf2)
            (.outer.product.b.vf vf3 vf2 vf1 acc)
            (.svf (&-> sv-496 quad) vf3)
            (vector-flatten! sv-496 (-> sv-272 old-x) (-> s4-0 vector 1))
            (vector-normalize! sv-496 1.0)
            (cond
              ((< (vector-dot (the-as vector (-> s4-0 vector)) sv-496) (cos (-> obj axial-slop)))
               (vector-cross! sv-496 sv-496 (the-as vector (-> s4-0 vector)))
               (vector-cross! sv-496 (the-as vector (-> s4-0 vector)) sv-496)
               (vector-normalize! sv-496 1.0)
               (set! sv-464 (-> s4-0 vector))
               (set! sv-448 (-> s4-0 vector))
               (let ((f0-20 (cos (-> obj axial-slop))))
                 (.lvf vf1 (&-> sv-448 0 quad))
                 (let ((v1-107 f0-20))
                   (.mov vf2 v1-107)
                   )
                 )
               (.add.x.vf vf1 vf0 vf0 :mask #b1000)
               (.mul.x.vf vf1 vf1 vf2 :mask #b111)
               (.svf (&-> sv-464 0 quad) vf1)
               (set! sv-512 (-> s4-0 vector))
               (set! sv-480 (-> s4-0 vector))
               (let ((f0-22 (sin (-> obj axial-slop))))
                 (.lvf vf2 (&-> sv-496 quad))
                 (.lvf vf1 (&-> sv-480 0 quad))
                 (let ((v1-113 f0-22))
                   (.mov vf3 v1-113)
                   )
                 )
               (.add.x.vf vf4 vf0 vf0 :mask #b1000)
               (.mul.x.vf acc vf2 vf3)
               (.add.mul.w.vf vf4 vf1 vf0 acc :mask #b111)
               (.svf (&-> sv-512 0 quad) vf4)
               (set! (-> sv-272 old-x quad) (-> s4-0 vector 0 quad))
               )
              (else
                (set! (-> s4-0 vector 0 quad) (-> sv-496 quad))
                (set! (-> sv-272 old-x quad) (-> s4-0 vector 0 quad))
                )
              )
            (vector-cross! (-> s4-0 vector 2) (the-as vector (-> s4-0 vector)) (-> s4-0 vector 1))
            (matrix->quaternion (-> sv-272 joint-mod quat) s4-0)
            (set! (-> s1-0 quad) (-> s4-0 vector 1 quad))
            (set! (-> s2-0 quad) (-> s4-0 vector 2 quad))
            0
            )
          (set! (-> s3-0 quad) (-> sv-528 quad))
          0
          )
        )
      )
    0
    (none)
    )
  )

(defmethod relocate chain-physics ((obj chain-physics) (arg0 int))
  (dotimes (v1-0 (the-as int (-> obj num-joints)))
    (if (nonzero? (-> obj chain-joints v1-0 joint-mod))
        (&+! (-> obj chain-joints v1-0 joint-mod) arg0)
        )
    )
  obj
  )

(defun chain-physics-initialize ((arg0 process-drawable) (arg1 chain-physics) (arg2 int) (arg3 float) (arg4 (array chain-physics-setup)))
  (set! (-> arg1 num-joints) (the-as uint (min 20 (-> arg4 length))))
  (dotimes (s1-0 (the-as int (-> arg1 num-joints)))
    (set! (-> arg1 chain-joints s1-0 joint-mod)
          (new 'process 'joint-mod (joint-mod-mode flex-blend) arg0 (-> arg4 s1-0 joint-index))
          )
    (set! (-> arg1 chain-joints s1-0 joint-mod track-mode) (track-mode no-scale))
    )
  (set! (-> arg1 root-joint-index) (the-as uint arg2))
  (set! (-> arg1 joint-length) arg3)
  (set-vector! (-> arg1 gravity) 0.0 -1.0 0.0 1.0)
  (set! (-> arg1 gravity-target quad) (-> arg1 gravity quad))
  (set! (-> arg1 stretch-vel) 0.7)
  (set! (-> arg1 stretch-vel-parallel) 0.8)
  (set! (-> arg1 compress-vel) 0.85)
  (set! (-> arg1 compress-vel-parallel) 0.75)
  (set! (-> arg1 negate-y) #f)
  (set! (-> arg1 axial-slop) 3640.889)
  (set! (-> arg1 maximum-stretch) 1.5)
  (set! (-> arg1 turn-off-start) 0)
  0
  )