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

;; name: collide-func.gc
;; name in dgo: collide-func
;; dgos: GAME, ENGINE

#|@file
This file contains the primitive intersection functions used for collision.
Most take a description of primitive and a "probe"
The probe has an origin and a direction.  The length of the direction vector is the length
of the probe.

Generally, collision functions will return the fraction of the probe to reach the primitive.
For example, if the probe is 5.0 long, and hits the primitive 2.0 away from the probe origin,
the return value (u) would be 0.4.

If (u) would be > 1.0, then it counts as "not intersecting" (object too far away)
If (u) would be < 0.0, then it counts as "not intersecting" (object behind probe)
If there's a miss, return COLLISION_MISS, a large negative number.
If we are inside of the primitive, return 0.0
|#

(defconstant COLLISION_MISS -100000000.0)


(defun raw-ray-sphere-intersect ((arg0 float))
  "DANGER: this function takes two arguments by vf registers.
   As a result, it doesn't work properly in OpenGOAL.  See the functions below."
  (local-vars
   (v1-1 float)
   (v1-2 float)
   (v1-4 number)
   (a0-1 float)
   (a0-2 float)
   (a0-3 int)
   (a1-0 float)
   )
  (crash!)
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         (vf8 :class vf)
         (vf9 :class vf)
         )
    (init-vf0-vector)
    (.mov vf3 arg0) ;; vf3 = radius
    ;; sphere is at the origin, vf1 is source of the ray (o)
    ;; vf2 is the ray's unit vector (u)
    (.mul.vf vf4 vf2 vf2) ;; vf4 = u.^2
    (.mul.vf vf3 vf3 vf3) ;; vf3 = r^2
    (.mul.vf vf6 vf1 vf1) ;; vf6 = o.^2
    (.mul.vf vf5 vf2 vf1) ;; vf5 = u . o
    (.add.y.vf.x vf4 vf4 vf4)
    (let ((result (the-as float 0)))
      (.add.x.vf.y vf6 vf6 vf6)
      (.sub.x.vf.z vf6 vf6 vf3)
      (.add.z.vf.x vf4 vf4 vf4)
      (.add.x.vf.y vf5 vf5 vf5)
      (let ((v1-0 (the-as float 0)))
        (.add.z.vf.y vf6 vf6 vf6)
        (.div.vf Q vf0 vf4 :fsf #b11 :ftf #b0)
        (.add.z.vf.y vf5 vf5 vf5)
        (.mov a0-1 vf4)
        (.mul.x.vf vf7 vf6 vf4)
        (.mov a1-0 vf6)
        (.mul.vf vf8 vf5 vf5)
        (b! (< (the-as int a1-0) 0) cfg-7 :delay (set! a0-2 a0-1))
        (.mul.vf.w vf4 vf0 Q)
        (.sub.vf vf9 vf8 vf7)
        (b! (= a0-2 v1-0) cfg-6 :delay (.mov v1-1 vf5))
        )
      (.sqrt.vf Q vf9 :ftf #b1)
      (b! (>= (the-as int v1-1) 0) cfg-6 :delay (.mov v1-2 vf9))
      (b! (< (the-as int v1-2) 0) cfg-6 :delay 1.0)
      (.add.x.vf vf6 vf5 vf4)
      (.mov v1-4 vf6)
      (.mul.vf vf6 vf6 vf6)
      (.mul.vf.w vf9 vf0 Q)
      (.sub.vf vf6 vf9 vf6)
      (.add.w.vf.y vf9 vf5 vf9)
      (.mov a0-3 vf6)
      (.mul.w.vf.y vf9 vf9 vf4)
      (b!
       (< (logand (the-as uint v1-4) (the-as uint a0-3)) 0)
       cfg-6
       :delay
       (.sub.y.vf vf4 vf0 vf9)
       )
      (b! #t cfg-7 :delay (.mov result vf4))
      (label cfg-6)
      (set! result -100000000.0)
      (label cfg-7)
      (the-as float result)
      )
    )
  )

(defmacro pc-port-do-raw-ray-sphere-intersect (rad vf1-val vf2-val)
  "Calls to raw-ray-sphere-intersect should be replaced with this macro,
   and this should be given vf1, vf2, which contain the origin and direction of the probe."
  `(let ((vf1-storage (new 'stack-no-clear 'vector))
         (vf2-storage (new 'stack-no-clear 'vector))
         )
     (.svf (&-> vf1-storage quad) ,vf1-val)
     (.svf (&-> vf2-storage quad) ,vf2-val)
     (pc-port-raw-ray-sphere-implementation ,rad vf1-storage vf2-storage)
     )
  )


(defun pc-port-raw-ray-sphere-implementation ((rad float) (vf1-val vector) (vf2-val vector))
  "This is one of the main primitives for collision.
   Assumes a sphere of radius rad is at the origin.
   Handles:
     - miss (return MISS)
     - behind (return MISS)
     - too far away (return MISS)
     - inside (return 0)
   "
  (local-vars
   (v1-1 int)
   (v1-2 int)
   (v1-4 int)
   (a0-1 float)
   (a0-2 float)
   (a0-3 int)
   (a1-0 int)
   )
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         (vf8 :class vf)
         (vf9 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> vf1-val quad))
    (.lvf vf2 (&-> vf2-val quad))
    (.mov vf3 rad)
    (.mul.vf vf4 vf2 vf2)
    (.mul.vf vf3 vf3 vf3)
    (.mul.vf vf6 vf1 vf1)
    (.mul.vf vf5 vf2 vf1)
    (.add.y.vf.x vf4 vf4 vf4)
    (let ((result (the-as float 0)))
      (.add.x.vf.y vf6 vf6 vf6)
      (.sub.x.vf.z vf6 vf6 vf3)
      (.add.z.vf.x vf4 vf4 vf4)
      (.add.x.vf.y vf5 vf5 vf5)
      (let ((v1-0 (the-as float 0)))
        (.add.z.vf.y vf6 vf6 vf6)
        (.div.vf Q vf0 vf4 :fsf #b11 :ftf #b0)
        (.add.z.vf.y vf5 vf5 vf5)
        (.mov a0-1 vf4)
        (.mul.x.vf vf7 vf6 vf4)
        (.mov a1-0 vf6)
        (.mul.vf vf8 vf5 vf5)
        (b! (< (the-as int a1-0) 0) cfg-7 :delay (set! a0-2 a0-1)) ;; in the sphere
        (.mul.vf.w vf4 vf0 Q)
        (.sub.vf vf9 vf8 vf7)
        (b! (= a0-2 v1-0) cfg-6 :delay (.mov v1-1 vf5)) ;; bad denominator in division
        )
      (.sqrt.vf Q vf9 :ftf #b1)
      (b! (>= (the-as int v1-1) 0) cfg-6 :delay (.mov v1-2 vf9)) ;; wrong dir
      (b! (< (the-as int v1-2) 0) cfg-6 :delay 1.0) ;; bad sqrt
      (.add.x.vf vf6 vf5 vf4)
      (.mov v1-4 vf6)
      (.mul.vf vf6 vf6 vf6)
      (.mul.vf.w vf9 vf0 Q)
      (.sub.vf vf6 vf9 vf6)
      (.add.w.vf.y vf9 vf5 vf9)
      (.mov a0-3 vf6)
      (.mul.w.vf.y vf9 vf9 vf4)

      ;; too far.
      (b! (< (logand (the-as int v1-4) (the-as int a0-3)) 0)
       cfg-6
       :delay (.sub.y.vf vf4 vf0 vf9)
       )
      (b! #t cfg-7 :delay (.mov result vf4))
      (label cfg-6)
      (set! result -100000000.0)
      (label cfg-7)
      (the-as float result)
      )
    )
  )


(defun ray-sphere-intersect ((ray-origin vector) (ray-dir vector) (sph-origin vector) (radius float))
  "Intersect a ray and sphere. Will return 0 if you are in the sphere, -huge number if you don't hit it.
   Returns the length of the ray to the first intersection."

  ;; offset stuff as if the sphere is at the origin.
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         )
    (.lvf vf1 (&-> ray-origin quad))
    (.lvf vf2 (&-> sph-origin quad))
    (.sub.vf vf1 vf1 vf2) ;; the sphere is at the origin in the actual intersection.
    (.lvf vf2 (&-> ray-dir quad))
    ;;(raw-ray-sphere-intersect radius)
    (pc-port-do-raw-ray-sphere-intersect radius vf1 vf2)
    )
  )

(defun ray-circle-intersect ((ray-origin vector) (ray-dir vector) (circle-origin vector) (radius float))
  "Intersect ray with circle. Circle is on the y plane and this throws out the y components
   of ray-origin, circle-origin, and ray-dir"
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         )
   (init-vf0-vector)
   (.lvf vf1 (&-> ray-origin quad))
   (.mov.vf.y vf1 vf0)
   (.lvf vf2 (&-> circle-origin quad))
   (.mov.vf.y vf2 vf0)
   (.sub.vf vf1 vf1 vf2)
   (.lvf vf2 (&-> ray-dir quad))
   (.mov.vf.y vf2 vf0)
   ;;(raw-ray-sphere-intersect radius)
   (pc-port-do-raw-ray-sphere-intersect radius vf1 vf2)
   )
  )

(defun ray-cylinder-intersect ((ray-origin vector) (ray-dir vector) (cyl-origin vector) (cyl-axis vector) (cyl-rad float) (cyl-len float) (pt-out vector))
  "Intersect with a cylinder.
   Currently this is untested."
  (local-vars
   (v0-1 float)
   (v1-0 int)
   (v1-2 int)
   (a0-1 int)
   (a0-2 int)
   (a0-4 int)
   (a0-5 int)
   )
  (rlet ((vf1 :class vf)
         (vf10 :class vf)
         (vf11 :class vf)
         (vf12 :class vf)
         (vf13 :class vf)
         (vf14 :class vf)
         (vf15 :class vf)
         (vf16 :class vf)
         (vf17 :class vf)
         (vf18 :class vf)
         (vf19 :class vf)
         (vf2 :class vf)
         (vf20 :class vf)
         (vf21 :class vf)
         )
    (.lvf vf10 (&-> ray-origin quad))
    (.lvf vf12 (&-> cyl-origin quad))
    (.sub.vf vf15 vf10 vf12)
    (.lvf vf11 (&-> ray-dir quad))
    (.lvf vf13 (&-> cyl-axis quad))
    (.mov vf14 cyl-len)
    (.mul.vf vf16 vf15 vf13)
    (.mul.vf vf17 vf11 vf13)
    (.add.x.vf.y vf16 vf16 vf16)
    (.add.x.vf.y vf17 vf17 vf17)
    (.add.z.vf.y vf16 vf16 vf16)
    (.add.z.vf.y vf17 vf17 vf17)
    (.mul.y.vf vf1 vf13 vf16)
    (.add.vf vf18 vf17 vf16)
    (.sub.x.vf vf19 vf16 vf14)
    (.mul.y.vf vf2 vf13 vf17)
    (.mov v1-0 vf16)
    (.sub.x.vf vf20 vf18 vf14)
    (.mov a0-1 vf18)
    (let ((v1-1 (logand v1-0 (the-as uint a0-1))))
      (.sub.vf vf1 vf15 vf1)
      (b! (< v1-1 0) cfg-6 :delay (.sub.vf vf2 vf11 vf2))
      )
    (.mov v1-2 vf19)
    (.mov a0-2 vf20)
    (b! (>= (the-as int (logior v1-2 (the-as uint a0-2))) 0) cfg-6 :delay (nop!))
    ;;(let ((v1-4 (raw-ray-sphere-intersect cyl-rad)))
    (let ((v1-4 (pc-port-do-raw-ray-sphere-intersect cyl-rad vf1 vf2)))
      (b! (< (the-as int v1-4) 0) cfg-6 :delay (.mov vf21 v1-4))
      (.mul.x.vf vf17 vf17 vf21)
      (.add.vf vf16 vf16 vf17)
      (.mul.y.vf vf13 vf13 vf16)
      (.sub.x.vf vf19 vf16 vf14)
      (.mov a0-4 vf16)
      (b!
       (< (the-as int a0-4) 0)
       cfg-6
       :delay
       (.add.vf.xyz vf12 vf12 vf13)
       )
      (.mov a0-5 vf19)
      (b! (>= (the-as int a0-5) 0) cfg-6 :delay (.svf (&-> pt-out quad) vf12))
      (b! #t cfg-7 :delay (set! v0-1 v1-4))
      )
    (label cfg-6)
    (set! v0-1 -100000000.0)
    (label cfg-7)
    v0-1
    )
  )

(defun ray-plane-intersect ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 vector) (arg5 vector) (arg6 vector))
  "Unused."
  (local-vars (v1-0 float) (v1-1 float) (a2-1 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         (vf8 :class vf)
         (vf9 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf3 (&-> arg5 quad))
    (.lvf vf1 (&-> arg4 quad))
    (.lvf vf2 (&-> arg6 quad))
    (.sub.vf vf1 vf3 vf1)
    (.sub.vf vf2 vf3 vf2)
    (.lvf vf6 (&-> arg2 quad))
    (.lvf vf7 (&-> arg3 quad))
    (.sub.vf vf8 vf3 vf6)
    (.outer.product.a.vf acc vf2 vf1)
    (.outer.product.b.vf vf4 vf1 vf2 acc)
    (.mul.vf vf8 vf8 vf4)
    (.mul.vf vf9 vf7 vf4)
    (.mul.vf vf5 vf4 vf4)
    (.add.y.vf.x vf8 vf8 vf8)
    (.add.y.vf.x vf9 vf9 vf9)
    (.add.y.vf.x vf5 vf5 vf5)
    (.add.z.vf.x vf8 vf8 vf8)
    (.add.z.vf.x vf9 vf9 vf9)
    (.add.z.vf.x vf5 vf5 vf5)
    (.mov v1-0 vf9)
    (.mov a2-1 vf8)
    (.isqrt.vf Q vf0 vf5 :fsf #b11 :ftf #b0)
    (let ((f1-0 a2-1)
          (f2-0 v1-0)
          )
      (cond
        ((!= f2-0 0.0)
         (let ((f1-1 (/ f1-0 f2-0)))
           (.mov.vf.w vf4 vf0)
           (.wait.vf)
           (.mul.vf.xyz vf4 vf4 Q)
           (let ((v0-0 f1-1))
             (.mov vf8 v0-0)
             (.svf (&-> arg1 quad) vf4)
             (.mul.x.vf acc vf7 vf8)
             (.add.mul.w.vf.xyz vf7 vf6 vf0 acc)
             (.svf (&-> arg0 quad) vf7)
             (.mov v1-1 vf7)
             v0-0
             )
           )
         )
        (else
         -100000000.0
         )
        )
      )
    )
  )

;; ray-triangle-intersect (unused)
;; collide-do-primitives (used in moving-sphere-triangle-intersect)
(def-mips2c collide-do-primitives (function float))
;; moving-sphere-triangle-intersect (used in cam)
(def-mips2c moving-sphere-triangle-intersect (function vector vector float collide-cache-tri vector vector float))
;; moving-sphere-sphere-intersect (used in collide)
(defun moving-sphere-sphere-intersect ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (let ((f30-0 (ray-sphere-intersect arg0 arg1 arg2 (+ (-> arg0 w) (-> arg2 w)))))
    (when (>= f30-0 0.0)
      (let ((s3-1 (vector-normalize! (vector-! (new-stack-vector0) arg2 arg0) (-> arg0 w))))
        (vector+*! arg3 arg0 arg1 f30-0)
        (vector+! arg3 arg3 s3-1)
        )
      )
    f30-0
    )
  )

;; moving-sphere-moving-sphere-intersect (unused)
