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

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

;; 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 vf4 vf4 vf4 :mask #b1)
    (let ((result (the-as float 0)))
      (.add.x.vf vf6 vf6 vf6 :mask #b10)
      (.sub.x.vf vf6 vf6 vf3 :mask #b100)
      (.add.z.vf vf4 vf4 vf4 :mask #b1)
      (.add.x.vf vf5 vf5 vf5 :mask #b10)
      (let ((v1-0 (the-as float 0)))
        (.add.z.vf vf6 vf6 vf6 :mask #b10)
        (.div.vf Q vf0 vf4 :fsf #b11 :ftf #b0)
        (.add.z.vf vf5 vf5 vf5 :mask #b10)
        (.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 vf4 vf0 Q :mask #b1000)
        (.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 vf9 vf0 Q :mask #b1000)
      (.sub.vf vf6 vf9 vf6)
      (.add.w.vf vf9 vf5 vf9 :mask #b10)
      (.mov a0-3 vf6)
      (.mul.w.vf vf9 vf9 vf4 :mask #b10)
      (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 vf4 vf4 vf4 :mask #b1)
    (let ((result (the-as float 0)))
      (.add.x.vf vf6 vf6 vf6 :mask #b10)
      (.sub.x.vf vf6 vf6 vf3 :mask #b100)
      (.add.z.vf vf4 vf4 vf4 :mask #b1)
      (.add.x.vf vf5 vf5 vf5 :mask #b10)
      (let ((v1-0 (the-as float 0)))
        (.add.z.vf vf6 vf6 vf6 :mask #b10)
        (.div.vf Q vf0 vf4 :fsf #b11 :ftf #b0)
        (.add.z.vf vf5 vf5 vf5 :mask #b10)
        (.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 vf4 vf0 Q :mask #b1000)
        (.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 vf9 vf0 Q :mask #b1000)
      (.sub.vf vf6 vf9 vf6)
      (.add.w.vf vf9 vf5 vf9 :mask #b10)
      (.mov a0-3 vf6)
      (.mul.w.vf vf9 vf9 vf4 :mask #b10)

      ;; 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 vf1 vf0 :mask #b10)
   (.lvf vf2 (&-> circle-origin quad))
   (.mov.vf vf2 vf0 :mask #b10)
   (.sub.vf vf1 vf1 vf2)
   (.lvf vf2 (&-> ray-dir quad))
   (.mov.vf vf2 vf0 :mask #b10)
   ;;(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 vf16 vf16 vf16 :mask #b10)
    (.add.x.vf vf17 vf17 vf17 :mask #b10)
    (.add.z.vf vf16 vf16 vf16 :mask #b10)
    (.add.z.vf vf17 vf17 vf17 :mask #b10)
    (.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 vf12 vf12 vf13 :mask #b111)
       )
      (.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.vf vf4 vf2 vf1)
    (.mul.vf vf8 vf8 vf4)
    (.mul.vf vf9 vf7 vf4)
    (.mul.vf vf5 vf4 vf4)
    (.add.y.vf vf8 vf8 vf8 :mask #b1)
    (.add.y.vf vf9 vf9 vf9 :mask #b1)
    (.add.y.vf vf5 vf5 vf5 :mask #b1)
    (.add.z.vf vf8 vf8 vf8 :mask #b1)
    (.add.z.vf vf9 vf9 vf9 :mask #b1)
    (.add.z.vf vf5 vf5 vf5 :mask #b1)
    (.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 vf4 vf0 :mask #b1000)
           (.wait.vf)
           (.mul.vf vf4 vf4 Q :mask #b111)
           (let ((v0-0 f1-1))
             (.mov vf8 v0-0)
             (.svf (&-> arg1 quad) vf4)
             (.mul.x.vf acc vf7 vf8)
             (.add.mul.w.vf vf7 vf6 vf0 acc :mask #b111)
             (.svf (&-> arg0 quad) vf7)
             (.mov v1-1 vf7)
             v0-0
             )
           )
         )
        (else
         -100000000.0
         )
        )
      )
    )
  )