;;-*-Lisp-*-
(in-package goal)
(bundles "ENGINE.CGO" "GAME.CGO")
(require "engine/collide/collide-func.gc")
(require "engine/nav/navigate-h.gc")
(require "engine/math/transformq.gc")
(require "engine/debug/debug.gc")

;; og:preserve-this added this to avoid a bug where enemies with NaN positions end up inside the nav mesh.
;; it's not clear yet why the enemies have NaN positions, but nothing else seems to go wrong if we
;; ignore them here. It's possible this happens as enemies are spawned/despawned by the visibility system.
(defmacro is-nan? (flt)
  `(and (< 0.0 ,flt) (< ,flt 0.0)))

;; DECOMP BEGINS

(define *nav-timer* (new 'global 'stopwatch))

(define *travel-timer* (new 'global 'stopwatch))

(define *clip-for-spheres-timer* (new 'global 'stopwatch))

(define *find-poly-timer* (new 'global 'stopwatch))

(define *nav-timer-enable* #t)

(define *nav-patch-route-table* #t)

(define *nav-triangle-test-count* 0)

(define *nav-last-triangle-test-count* 0)

(define *debug-traverse* 0)

(define *debug-tests* 0)

(defun pke-nav-hack ()
  (format *stdcon* "travs: ~d, tests: ~d~%" *debug-traverse* *debug-tests*)
  (set! *debug-traverse* 0)
  (set! *debug-tests* 0)
  0
  (none))

(defun debug-report-nav-stats ()
  0
  (none))

(define *debug-output* #f)

(define *debug-nav* #t)

(define *debug-nav-ray* (the-as nav-ray #f))

(define *debug-ray-offset* (new 'static 'vector))

(define *debug-nav-travel* #f)

(define *color-black* (new 'static 'rgba :a #x80))

(define *color-white* (new 'static 'rgba :r #xff :g #xff :b #xff :a #x80))

(define *color-gray* (new 'static 'rgba :r #x80 :g #x80 :b #x80 :a #x80))

(define *color-red* (new 'static 'rgba :r #xff :a #x80))

(define *color-green* (new 'static 'rgba :g #xff :a #x80))

(define *color-blue* (new 'static 'rgba :b #xff :a #x80))

(define *color-cyan* (new 'static 'rgba :g #xff :b #xff :a #x80))

(define *color-magenta* (new 'static 'rgba :r #xff :b #xff :a #x80))

(define *color-yellow* (new 'static 'rgba :r #xff :g #xff :a #x80))

(define *color-light-red* (new 'static 'rgba :r #xff :g #x80 :b #x80 :a #x80))

(define *color-light-green* (new 'static 'rgba :r #x80 :g #xff :b #x80 :a #x80))

(define *color-light-blue* (new 'static 'rgba :r #x80 :g #x80 :b #xff :a #x80))

(define *color-light-cyan* (new 'static 'rgba :r #x80 :g #xff :b #xff :a #x80))

(define *color-light-magenta* (new 'static 'rgba :r #xff :g #x80 :b #xff :a #x80))

(define *color-light-yellow* (new 'static 'rgba :r #xff :g #xff :b #x80 :a #x80))

(define *color-dark-red* (new 'static 'rgba :r #x80 :a #x80))

(define *color-dark-green* (new 'static 'rgba :g #x80 :a #x80))

(define *color-dark-blue* (new 'static 'rgba :b #x80 :a #x80))

(define *color-dark-cyan* (new 'static 'rgba :g #x80 :b #x80 :a #x80))

(define *color-dark-magenta* (new 'static 'rgba :r #x80 :b #x80 :a #x80))

(define *color-dark-yellow* (new 'static 'rgba :r #x80 :g #x80 :a #x80))

(define *color-orange* (new 'static 'rgba :r #xff :g #x80 :a #x80))

(defun entity-nav-login ((arg0 entity-actor))
  (local-vars (sv-16 res-tag))
  (let ((gp-0 (if (nonzero? (-> arg0 nav-mesh)) (-> arg0 nav-mesh))))
    (when gp-0
      (when (zero? (-> gp-0 user-list))
        (set! (-> gp-0 user-list)
              (new 'loading-level 'engine 'nav-engine (res-lump-value arg0 'nav-max-users int :default (the-as uint128 32))))
        (initialize-mesh! gp-0)
        (update-route-table gp-0))
      (when (zero? (-> gp-0 static-sphere))
        (set! sv-16 (new 'static 'res-tag))
        (let ((v1-11 (res-lump-data arg0 'nav-mesh-sphere pointer :tag-ptr (& sv-16))))
          (when v1-11
            (set! (-> gp-0 static-sphere-count) (-> sv-16 elt-count))
            (set! (-> gp-0 static-sphere) (the-as (inline-array nav-sphere) v1-11)))))))
  0
  (none))

(defmethod length ((this nav-control))
  (-> this num-spheres))

(defmethod asize-of ((this nav-control))
  (the-as int (+ (-> nav-control size) (* (-> this max-spheres) 16))))

(define *default-nav-mesh*
  (new 'static
       'nav-mesh
       :bounds
       (new 'static 'sphere :x 12288.0 :y -200704.0 :z 12288.0 :w 20480.0)
       :origin
       (new 'static 'vector :y -200704.0 :w 1.0)
       :vertex-count 4
       :vertex
       (new 'static
            'inline-array
            nav-vertex
            4
            (new 'static 'nav-vertex :z 16384.0 :w 1.0)
            (new 'static 'nav-vertex :z 8192.0 :w 1.0)
            (new 'static 'nav-vertex :x 8192.0 :z 8192.0 :w 1.0)
            (new 'static 'nav-vertex :x 8192.0 :w 1.0))
       :poly-count 2
       :poly
       (new 'static
            'inline-array
            nav-poly
            2
            (new 'static 'nav-poly :vertex (new 'static 'array uint8 3 #x0 #x2 #x1) :adj-poly (new 'static 'array uint8 3 #xff #x1 #xff))
            (new 'static
                 'nav-poly
                 :id #x1
                 :vertex
                 (new 'static 'array uint8 3 #x1 #x2 #x3)
                 :adj-poly
                 (new 'static 'array uint8 3 #x0 #xff #xff)))
       :route
       (new 'static
            'inline-array
            vector4ub
            4
            (new 'static 'vector4ub :data (new 'static 'array uint8 4 #xcb #x0 #x0 #x0))
            (new 'static 'vector4ub)
            (new 'static 'vector4ub)
            (new 'static 'vector4ub))))

(let ((gp-0 *default-nav-mesh*))
  (if (zero? (-> gp-0 user-list)) (set! (-> gp-0 user-list) (new 'global 'engine 'nav-engine 32))))

(defmethod length ((this nav-mesh))
  (-> this poly-count))

(defmethod debug-draw-poly ((this nav-mesh) (arg0 nav-poly) (arg1 rgba))
  (let ((s5-0 (-> this origin))
        (s2-0 (-> this vertex))
        (gp-0 (new 'stack-no-clear 'vector))
        (s4-0 (new 'stack-no-clear 'vector)))
    (add-debug-line #t
                    (bucket-id debug-no-zbuf)
                    (vector+! gp-0 s5-0 (the-as vector (-> s2-0 (-> arg0 vertex 0))))
                    (vector+! s4-0 s5-0 (the-as vector (-> s2-0 (-> arg0 vertex 1))))
                    arg1
                    #f
                    (the-as rgba -1))
    (add-debug-line #t
                    (bucket-id debug-no-zbuf)
                    (vector+! gp-0 s5-0 (the-as vector (-> s2-0 (-> arg0 vertex 1))))
                    (vector+! s4-0 s5-0 (the-as vector (-> s2-0 (-> arg0 vertex 2))))
                    arg1
                    #f
                    (the-as rgba -1))
    (add-debug-line #t
                    (bucket-id debug-no-zbuf)
                    (vector+! gp-0 s5-0 (the-as vector (-> s2-0 (-> arg0 vertex 2))))
                    (vector+! s4-0 s5-0 (the-as vector (-> s2-0 (-> arg0 vertex 0))))
                    arg1
                    #f
                    (the-as rgba -1)))
  0
  (none))

(define *nav-one-third* (new 'global 'vector))

(set! (-> *nav-one-third* x) 0.33333334)

(defmethod tri-centroid-local ((this nav-mesh) (arg0 nav-poly) (arg1 vector))
  (set! (-> arg1 quad) (-> this vertex (-> arg0 vertex 0) quad))
  (vector+! arg1 arg1 (the-as vector (-> this vertex (-> arg0 vertex 1))))
  (vector+! arg1 arg1 (the-as vector (-> this vertex (-> arg0 vertex 2))))
  (vector-float*! arg1 arg1 0.333333))

(defmethod tri-centroid-world ((this nav-mesh) (arg0 nav-poly) (arg1 vector))
  (tri-centroid-local this arg0 arg1)
  (vector+! arg1 arg1 (-> this origin)))

(define *edge-vert0-table* (new 'static 'boxed-array :type int8 1 2 0))

(define *edge-vert1-table* (new 'static 'boxed-array :type int8 2 0 1))

(define *edge-mask-table* (new 'static 'boxed-array :type int8 1 2 4))

(defun inc-mod3 ((arg0 int))
  (local-vars (v0-1 int) (v1-1 int))
  (let ((v0-0 (+ arg0 1))) (let ((v1-0 2)) (set-on-less-than v1-1 v1-0 v0-0)) (move-if-not-zero v0-1 0 v1-1 v0-0))
  v0-1)

(defun dec-mod3 ((arg0 int))
  (local-vars (v0-1 int) (a0-1 int))
  (let ((v0-0 (+ arg0 -1))
        (v1-0 2))
    (set-on-less-than a0-1 v0-0 0)
    (move-if-not-zero v0-1 v1-0 a0-1 v0-0))
  v0-1)

(defun vu-point-triangle-intersection? ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (local-vars (v1-0 int) (v1-1 int) (v1-3 int) (a0-1 int) (a0-2 int) (a0-4 int) (a1-1 int) (a1-2 int))
  (rlet ((acc :class vf)
         (vf1 :class vf)
         (vf10 :class vf)
         (vf11 :class vf)
         (vf12 :class vf)
         (vf13 :class vf)
         (vf14 :class vf)
         (vf15 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf9 :class vf))
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.sub.vf.yw vf1 vf1 vf1)
    (.lvf vf3 (&-> arg3 quad))
    (.sub.vf.yw vf2 vf2 vf2)
    (.lvf vf12 (&-> arg0 quad))
    (.sub.vf.yw vf3 vf3 vf3)
    (.sub.vf vf4 vf2 vf1)
    (.sub.vf vf9 vf1 vf12)
    (.sub.vf vf5 vf3 vf2)
    (.sub.vf vf10 vf2 vf12)
    (.sub.vf vf6 vf1 vf3)
    (.sub.vf vf11 vf3 vf12)
    (.outer.product.a.vf acc vf9 vf4)
    (.outer.product.b.vf vf13 vf4 vf9 acc)
    (.outer.product.a.vf acc vf10 vf5)
    (.outer.product.b.vf vf14 vf5 vf10 acc)
    (.outer.product.a.vf acc vf11 vf6)
    (.outer.product.b.vf vf15 vf6 vf11 acc)
    (.mov v1-0 vf13)
    (.mov a1-1 vf14)
    (.mov a0-1 vf15)
    (set-on-less-than v1-1 v1-0 0)
    (set-on-less-than a1-2 a1-1 0)
    (set-on-less-than a0-2 a0-1 0)
    (let ((a0-3 (logxor a1-2 a0-2))
          (v1-2 (logxor v1-1 a1-2)))
      (set-on-less-than a0-4 a0-3 1)
      (set-on-less-than v1-3 v1-2 1))
    (logtest? v1-3 a0-4)))

(defmethod point-in-poly? ((this nav-mesh) (arg0 nav-poly) (arg1 vector))
  (vu-point-triangle-intersection? arg1
                                   (-> this vertex (-> arg0 vertex 0))
                                   (-> this vertex (-> arg0 vertex 1))
                                   (-> this vertex (-> arg0 vertex 2))))

(defmethod closest-point-on-boundary ((this nav-mesh) (arg0 nav-poly) (arg1 vector) (arg2 vector))
  (let ((s2-0 (-> this vertex))
        (s1-0 (new 'stack-no-clear 'vector))
        (s5-0 (new 'stack-no-clear 'vector)))
    (let ((f30-0 10000000000000000000000000000000000000.0))
      (dotimes (s0-0 3)
        (let* ((a1-1 (-> s2-0 (-> arg0 vertex s0-0)))
               (f0-0 (vector-segment-distance-point! arg2 a1-1 (-> s2-0 (-> arg0 vertex (mod (+ s0-0 1) 3))) s1-0)))
          (when (< f0-0 f30-0)
            (set! f30-0 f0-0)
            (set! (-> s5-0 quad) (-> s1-0 quad))))))
    (set! (-> arg1 quad) (-> s5-0 quad)))
  arg1)

(defmethod project-point-into-tri-2d ((this nav-mesh) (arg0 nav-poly) (arg1 vector) (arg2 vector))
  (if (point-in-poly? this arg0 arg2) (set! (-> arg1 quad) (-> arg2 quad)) (closest-point-on-boundary this arg0 arg1 arg2))
  arg1)

(defun point-inside-rect? ((arg0 nav-node) (arg1 vector) (arg2 float))
  (and (>= (-> arg1 x) (- (-> arg0 center-x) (-> arg0 radius-x)))
       (>= (+ (-> arg0 center-x) (-> arg0 radius-x)) (-> arg1 x))
       (>= (-> arg1 z) (- (-> arg0 center-z) (-> arg0 radius-z)))
       (>= (+ (-> arg0 center-z) (-> arg0 radius-z)) (-> arg1 z))
       (>= (+ (-> arg1 y) arg2) (- (-> arg0 center-y) (-> arg0 radius-y)))
       (>= (+ (-> arg0 center-y) (-> arg0 radius-y)) (- (-> arg1 y) arg2))))

(defun point-inside-poly? ((arg0 nav-mesh) (arg1 uint) (arg2 vector) (arg3 float))
  (local-vars (v1-6 int) (v1-7 int) (v1-9 int) (a0-3 int) (a0-4 int) (a0-6 int) (a1-6 int) (a1-7 int))
  (rlet ((acc :class vf)
         (vf1 :class vf)
         (vf10 :class vf)
         (vf11 :class vf)
         (vf12 :class vf)
         (vf13 :class vf)
         (vf14 :class vf)
         (vf15 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf9 :class vf))
    (let ((t0-0 (-> arg0 poly arg1)))
      (when (not (logtest? (-> t0-0 pat) 1))
        (let ((v1-5 (-> arg0 vertex (-> t0-0 vertex 0)))
              (a1-5 (-> arg0 vertex (-> t0-0 vertex 1)))
              (a0-2 (-> arg0 vertex (-> t0-0 vertex 2))))
          0.0
          (let ((f0-3 (fabs (- (-> arg2 y) (* 0.333333 (+ (-> v1-5 y) (-> a1-5 y) (-> a0-2 y)))))))
            (when (>= arg3 f0-3)
              (.lvf vf1 (&-> v1-5 quad))
              (.lvf vf2 (&-> a1-5 quad))
              (.sub.vf.yw vf1 vf1 vf1)
              (.lvf vf3 (&-> a0-2 quad))
              (.sub.vf.yw vf2 vf2 vf2)
              (.lvf vf12 (&-> arg2 quad))
              (.sub.vf.yw vf3 vf3 vf3)
              (.sub.vf vf4 vf2 vf1)
              (.sub.vf vf9 vf1 vf12)
              (.sub.vf vf5 vf3 vf2)
              (.sub.vf vf10 vf2 vf12)
              (.sub.vf vf6 vf1 vf3)
              (.sub.vf vf11 vf3 vf12)
              (.outer.product.a.vf acc vf9 vf4)
              (.outer.product.b.vf vf13 vf4 vf9 acc)
              (.outer.product.a.vf acc vf10 vf5)
              (.outer.product.b.vf vf14 vf5 vf10 acc)
              (.outer.product.a.vf acc vf11 vf6)
              (.outer.product.b.vf vf15 vf6 vf11 acc)
              (.mov v1-6 vf13)
              (.mov a1-6 vf14)
              (.mov a0-3 vf15)
              (set-on-less-than v1-7 v1-6 0)
              (set-on-less-than a1-7 a1-6 0)
              (set-on-less-than a0-4 a0-3 0)
              (let ((a0-5 (logxor a1-7 a0-4))
                    (v1-8 (logxor v1-7 a1-7)))
                (set-on-less-than a0-6 a0-5 1)
                (set-on-less-than v1-9 v1-8 1))
              (if (logtest? v1-9 a0-6) (return #t)))))))
    #f))

(defun recursive-inside-poly ((arg0 nav-mesh) (arg1 nav-node) (arg2 vector) (arg3 float))
  (when (point-inside-rect? arg1 arg2 arg3)
    (cond
      ((zero? (-> arg1 type))
       (let ((v1-2 (-> arg1 left-offset))
             (s3-0 (-> arg1 right-offset)))
         (when (>= v1-2 0)
           (let* ((a1-2 (the-as nav-node (&+ (the-as (pointer nav-node) (-> arg0 nodes)) v1-2)))
                  (v1-3 (recursive-inside-poly arg0 a1-2 arg2 arg3)))
             (if (>= v1-3 0) (return v1-3))))
         (when (>= s3-0 0)
           (let* ((a1-3 (&+ (the-as (pointer nav-node) (-> arg0 nodes)) s3-0))
                  (v1-7 (recursive-inside-poly arg0 (the-as nav-node a1-3) arg2 arg3)))
             (if (>= v1-7 0) (return v1-7)))))
       (return -1))
      (else
       (let ((s3-1 (-> arg1 num-tris))
             (s2-1 (-> arg1 first-tris)))
         (dotimes (s1-0 (the-as int s3-1))
           (let ((s0-0 (-> s2-1 0))) (if (point-inside-poly? arg0 s0-0 arg2 arg3) (return (the-as int s0-0))))
           (set! s2-1 (&-> s2-1 1))
           (if (= s1-0 3) (set! s2-1 (&-> s2-1 4)))))
       (return -1)))
    (the-as none 0))
  -1)

(defmethod find-poly-fast ((this nav-mesh) (arg0 vector) (arg1 meters))
  (local-vars (a0-6 symbol) (a2-3 uint128) (a2-4 uint128))
  -1
  (let ((s2-0 -1)
        (s3-0 (current-time))
        (f0-0 (-> this bounds w)))
    (when (>= (* f0-0 f0-0) (vector-length-squared arg0))
      (dotimes (v1-3 4)
        (set! a0-6
              (and (= (-> this cache v1-3 time) s3-0)
                   (= (-> this cache v1-3 vec w) arg1)
                   (begin
                     (let ((a3-0 (-> this cache v1-3))
                           (a2-1 arg0))
                       (set! a0-6 #t)
                       (let* ((a1-3 #xffff)
                              (a3-1 (-> a3-0 vec quad))
                              (a1-4 (shl a1-3 48)))
                         (let ((a2-2 (-> a2-1 quad))) (.pceqw a2-3 a3-1 a2-2))
                         (.ppach a2-4 (the-as uint128 0) a2-3)
                         (b! (= (logior (the-as int a2-4) a1-4) -1) cfg-10 :delay (nop!))))
                     #f)))
        (label cfg-10)
        (if a0-6 (return (-> this poly (-> this cache v1-3 poly-ind))))
        (if (and (< s2-0 0) (!= (-> this cache v1-3 time) s3-0)) (set! s2-0 v1-3)))
      (let ((v1-12 (recursive-inside-poly this (-> this nodes 0) arg0 arg1)))
        (when (>= v1-12 0)
          (when (>= s2-0 0)
            (let ((a0-24 (-> this cache s2-0)))
              (set! (-> a0-24 vec x) (-> arg0 x))
              (set! (-> a0-24 vec y) (-> arg0 y))
              (set! (-> a0-24 vec z) (-> arg0 z))
              (set! (-> a0-24 vec w) arg1)
              (set! (-> a0-24 time) (the-as uint s3-0))
              (set! (-> a0-24 poly-ind) (the-as uint v1-12))))
          (return (-> this poly v1-12))))))
  (the-as nav-poly #f))

(defun circle-triangle-intersection-proc? ((arg0 vector) (arg1 float) (arg2 (inline-array nav-vertex)))
  (let ((v1-0 0))
    (dotimes (a3-0 3)
      (let* ((t0-4 (-> arg2 (-> *edge-vert0-table* a3-0)))
             (t1-4 (-> arg2 (-> *edge-vert1-table* a3-0)))
             (f0-1 (- (-> t0-4 z) (-> t1-4 z)))
             (f1-2 (- (-> t1-4 x) (-> t0-4 x)))
             (f2-2 (- (-> arg0 x) (-> t0-4 x)))
             (f3-2 (- (-> arg0 z) (-> t0-4 z)))
             (f4-1 f0-1)
             (f4-3 (* f4-1 f4-1))
             (f5-0 f1-2)
             (f4-5 (sqrtf (+ f4-3 (* f5-0 f5-0))))
             (f4-7 (/ 1.0 f4-5))
             (f0-2 (* f0-1 f4-7))
             (f1-3 (* f1-2 f4-7))
             (f0-4 (+ (* f2-2 f0-2) (* f3-2 f1-3))))
        (cond
          ((< arg1 f0-4) (return #f))
          ((< f0-4 0.0) (+! v1-0 (-> *edge-mask-table* a3-0))))))
    (if (= v1-0 7) (return #t))
    (let ((t0-15 #f)
          (a3-5 0))
      (cond
        ((= v1-0 1) (set! t0-15 #t) (set! a3-5 0))
        ((= v1-0 2) (set! t0-15 #t) (set! a3-5 1))
        ((= v1-0 4) (set! t0-15 #t) (set! a3-5 2))
        ((= v1-0 6) (set! a3-5 0))
        ((= v1-0 5) (set! a3-5 1))
        ((= v1-0 3) (set! a3-5 2)))
      (when (not t0-15)
        (let* ((t0-16 (-> arg2 (-> *edge-vert0-table* a3-5)))
               (v1-18 (the-as vector (-> arg2 (-> *edge-vert1-table* a3-5))))
               (f1-8 (- (-> v1-18 x) (-> t0-16 x)))
               (f0-7 (- (-> v1-18 z) (-> t0-16 z)))
               (f2-7 (+ (* f1-8 (- (-> arg0 x) (-> t0-16 x))) (* f0-7 (- (-> arg0 z) (-> t0-16 z)))))
               (f0-9 (+ (* f1-8 (- (-> arg0 x) (-> v1-18 x))) (* f0-7 (- (-> arg0 z) (-> v1-18 z))))))
          (cond
            ((< f2-7 0.0) (set! t0-15 #t) (set! a3-5 (-> *edge-vert0-table* a3-5)))
            ((< 0.0 f0-9) (set! t0-15 #t) (set! a3-5 (-> *edge-vert1-table* a3-5)))
            (else (return #t)))))
      (when t0-15
        (let* ((v1-28 (-> arg2 a3-5))
               (f0-11 (- (-> v1-28 x) (-> arg0 x)))
               (f0-13 (* f0-11 f0-11))
               (f1-14 (- (-> v1-28 z) (-> arg0 z)))
               (f0-14 (+ f0-13 (* f1-14 f1-14)))
               (f1-17 arg1))
          (if (>= (* f1-17 f1-17) f0-14) #t))))))

(defun circle-triangle-intersection? ((arg0 vector) (arg1 float) (arg2 (inline-array nav-vertex)))
  (let ((v1-0 arg0)
        (f0-0 arg1)
        (a0-1 arg2)
        (a1-1 0))
    (dotimes (a2-1 3)
      (let* ((a3-4 (-> a0-1 (-> *edge-vert0-table* a2-1)))
             (t0-4 (-> a0-1 (-> *edge-vert1-table* a2-1)))
             (f1-1 (- (-> a3-4 z) (-> t0-4 z)))
             (f2-2 (- (-> t0-4 x) (-> a3-4 x)))
             (f3-2 (- (-> v1-0 x) (-> a3-4 x)))
             (f4-2 (- (-> v1-0 z) (-> a3-4 z)))
             (f5-1 f1-1)
             (f5-3 (* f5-1 f5-1))
             (f6-0 f2-2)
             (f5-5 (sqrtf (+ f5-3 (* f6-0 f6-0))))
             (f5-7 (/ 1.0 f5-5))
             (f1-2 (* f1-1 f5-7))
             (f2-3 (* f2-2 f5-7))
             (f1-4 (+ (* f3-2 f1-2) (* f4-2 f2-3))))
        (cond
          ((< f0-0 f1-4) (return #f))
          ((< f1-4 0.0) (+! a1-1 (-> *edge-mask-table* a2-1))))))
    (if (= a1-1 7) (return #t))
    (let ((a3-15 #f)
          (a2-6 0))
      (cond
        ((= a1-1 1) (set! a3-15 #t) (set! a2-6 0))
        ((= a1-1 2) (set! a3-15 #t) (set! a2-6 1))
        ((= a1-1 4) (set! a3-15 #t) (set! a2-6 2))
        ((= a1-1 6) (set! a2-6 0))
        ((= a1-1 5) (set! a2-6 1))
        ((= a1-1 3) (set! a2-6 2)))
      (when (not a3-15)
        (let* ((a3-16 (-> a0-1 (-> *edge-vert0-table* a2-6)))
               (a1-18 (-> a0-1 (-> *edge-vert1-table* a2-6)))
               (f2-7 (- (-> a1-18 x) (-> a3-16 x)))
               (f1-7 (- (-> a1-18 z) (-> a3-16 z)))
               (f3-7 (+ (* f2-7 (- (-> v1-0 x) (-> a3-16 x))) (* f1-7 (- (-> v1-0 z) (-> a3-16 z)))))
               (f1-9 (+ (* f2-7 (- (-> v1-0 x) (-> a1-18 x))) (* f1-7 (- (-> v1-0 z) (-> a1-18 z))))))
          (cond
            ((< f3-7 0.0) (set! a3-15 #t) (set! a2-6 (-> *edge-vert0-table* a2-6)))
            ((< 0.0 f1-9) (set! a3-15 #t) (set! a2-6 (-> *edge-vert1-table* a2-6)))
            (else (return #t)))))
      (when a3-15
        (let* ((a0-2 (-> a0-1 a2-6))
               (f1-11 (- (-> a0-2 x) (-> v1-0 x)))
               (f1-13 (* f1-11 f1-11))
               (f2-13 (- (-> a0-2 z) (-> v1-0 z)))
               (f1-14 (+ f1-13 (* f2-13 f2-13))))
          (if (>= (* f0-0 f0-0) f1-14) #t))))))

;; ERROR: Failed load: (set! vf3 (l.vf a0-4)) at op 42
(defmethod is-in-mesh? ((this nav-mesh) (arg0 vector) (arg1 float) (arg2 meters))
  (local-vars (v1-10 int))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf))
    (let ((f0-1 (+ arg1 (-> this bounds w))))
      (when (>= (* f0-1 f0-1) (vector-length-squared arg0))
        (let ((s2-0 (new 'stack-no-clear 'inline-array 'nav-vertex 6))
              (f30-1 0.33333334))
          (countdown (s1-0 (-> this poly-count))
            (set! *debug-traverse* (+ *debug-traverse* 1))
            (let ((a0-3 (-> this poly s1-0)))
              (when (not (logtest? (-> a0-3 pat) 1))
                (nop!)
                (let ((v1-8 (the-as object (-> this vertex))))
                  (nop!)
                  (let ((a2-1 (-> a0-3 vertex 0)))
                    (nop!)
                    (let* ((a1-1 (-> a0-3 vertex 1))
                           (a2-2 (* a2-1 16))
                           (a3-1 (-> a0-3 vertex 2))
                           (a1-2 (* a1-1 16))
                           (a0-4 (+ a2-2 (the-as int v1-8)))
                           (a2-3 (* a3-1 16))
                           (a1-3 (+ a1-2 (the-as int v1-8)))
                           (v1-9 (+ a2-3 (the-as int v1-8))))
                      (.lvf vf3 a0-4)
                      (let ((a0-5 f30-1)) (.mov vf7 a0-5))
                      (.lvf vf4 a1-3)
                      (.lvf vf5 v1-9))))
                (.min.vf.xyz vf1 vf3 vf4)
                (.lvf vf6 (&-> arg0 quad))
                (.add.y.vf.y vf7 vf3 vf4)
                (.svf (&-> s2-0 0 quad) vf3)
                (.max.vf.xyz vf2 vf3 vf4)
                (.svf (&-> s2-0 1 quad) vf4)
                (.min.vf.xyz vf1 vf1 vf5)
                (.svf (&-> s2-0 2 quad) vf5)
                (.max.vf.xyz vf2 vf2 vf5)
                (.mul.x.vf.y vf7 vf7 vf7)
                (.svf (&-> s2-0 3 quad) vf1)
                (.svf (&-> s2-0 4 quad) vf2)
                (.sub.y.vf.y vf7 vf6 vf7)
                (.mov v1-10 vf7)
                (let ((v1-11 (the-as number (sar (the-as int v1-10) 32))))
                  (when (and (>= arg2 (fabs (the-as float v1-11)))
                             (>= (+ (-> arg0 x) arg1) (-> s2-0 3 x))
                             (>= (-> s2-0 4 x) (- (-> arg0 x) arg1))
                             (>= (+ (-> arg0 z) arg1) (-> s2-0 3 z))
                             (>= (-> s2-0 4 z) (- (-> arg0 z) arg1)))
                    (set! *debug-tests* (+ *debug-tests* 1))
                    (if (circle-triangle-intersection? arg0 arg1 s2-0) (return #t))))))))))
    #f))

(defmethod move-along-nav-ray! ((this nav-mesh) (arg0 nav-ray))
  (local-vars (a2-7 int) (a3-3 int))
  (let ((v1-0 -1)
        (f0-1 (- (-> arg0 dest-pos x) (-> arg0 current-pos x)))
        (f1-2 (- (-> arg0 dest-pos z) (-> arg0 current-pos z))))
    (dotimes (a2-0 3)
      (let* ((a3-1 (-> this vertex (-> arg0 current-poly vertex (-> *edge-vert0-table* a2-0))))
             (t0-7 (-> this vertex (-> arg0 current-poly vertex (-> *edge-vert1-table* a2-0))))
             (f4-0 (- (-> a3-1 z) (-> t0-7 z)))
             (f3-2 (- (-> t0-7 x) (-> a3-1 x)))
             (f2-4 (+ (* f0-1 f4-0) (* f1-2 f3-2))))
        (when (< 0.0 f2-4)
          (let ((f3-4 (+ (* f4-0 (- (-> a3-1 x) (-> arg0 current-pos x))) (* f3-2 (- (-> a3-1 z) (-> arg0 current-pos z))))))
            (when (< f3-4 f2-4)
              (set! v1-0 a2-0)
              (let ((f2-6 (fmax 0.0 (/ f3-4 f2-4)))) (set! f0-1 (* f0-1 f2-6)) (set! f1-2 (* f1-2 f2-6))))))))
    (let ((f2-9 (+ (* f0-1 (-> arg0 dir x)) (* f1-2 (-> arg0 dir z))))) (+! (-> arg0 len) f2-9))
    0
    (set! (-> arg0 next-poly) #f)
    (cond
      ((= v1-0 -1)
       (set! (-> arg0 current-pos quad) (-> arg0 dest-pos quad))
       (set! (-> arg0 reached-dest) #t)
       (set! (-> arg0 terminated) #t))
      (else
       (+! (-> arg0 current-pos x) f0-1)
       (+! (-> arg0 current-pos z) f1-2)
       (let ((a2-6 (+ v1-0 1))) (let ((a3-2 2)) (set-on-less-than a3-3 a3-2 a2-6)) (move-if-not-zero a2-7 0 a3-3 a2-6))
       (let ((a2-9 (-> arg0 current-poly adj-poly a2-7))) (if (!= a2-9 255) (set! (-> arg0 next-poly) (-> this poly a2-9))))
       (cond
         ((and (-> arg0 next-poly) (not (logtest? (-> arg0 next-poly pat) 1))) (set! (-> arg0 current-poly) (-> arg0 next-poly)))
         (else
          (set! (-> arg0 last-edge) v1-0)
          (if (-> arg0 next-poly) (set! (-> arg0 hit-gap) #t) (set! (-> arg0 hit-boundary) #t))
          (set! (-> arg0 terminated) #t))))))
  0
  (none))

(defun init-ray ((arg0 nav-ray))
  (vector-! (-> arg0 dir) (-> arg0 dest-pos) (-> arg0 current-pos))
  (set! (-> arg0 dir y) 0.0)
  (vector-xz-normalize! (-> arg0 dir) 1.0)
  (set! (-> arg0 next-poly) #f)
  (set! (-> arg0 len) 0.0)
  (set! (-> arg0 last-edge) -1)
  (set! (-> arg0 terminated) #f)
  (set! (-> arg0 reached-dest) #f)
  (set! (-> arg0 hit-boundary) #f)
  (set! (-> arg0 hit-gap) #f)
  #f)

(defun init-ray-local ((arg0 nav-ray) (arg1 nav-poly) (arg2 vector) (arg3 vector))
  (set! (-> arg0 current-pos quad) (-> arg2 quad))
  (set! (-> arg0 dest-pos quad) (-> arg3 quad))
  (set! (-> arg0 current-poly) arg1)
  (init-ray arg0))

(defun init-ray-dir-local ((arg0 nav-ray) (arg1 nav-poly) (arg2 vector) (arg3 vector) (arg4 float))
  (set! (-> arg0 current-poly) arg1)
  (set! (-> arg0 current-pos quad) (-> arg2 quad))
  (vector+*! (-> arg0 dest-pos) (-> arg0 current-pos) arg3 arg4)
  (init-ray arg0))

(defmethod try-move-along-ray ((this nav-mesh) (arg0 nav-poly) (arg1 vector) (arg2 vector) (arg3 float))
  (local-vars (v1-2 symbol))
  (let ((gp-0 (new 'stack-no-clear 'nav-ray)))
    (let ((s4-0 0))
      (init-ray-dir-local gp-0 arg0 arg1 arg2 arg3)
      (until v1-2
        (+! s4-0 1)
        (move-along-nav-ray! this gp-0)
        (set! v1-2 (or (>= s4-0 15) (-> gp-0 terminated)))))
    (-> gp-0 len)))

(defun nav-ray-test ((arg0 nav-mesh) (arg1 nav-poly) (arg2 vector) (arg3 vector))
  (let ((s4-1 (vector-! (new 'stack-no-clear 'vector) arg2 (-> arg0 origin)))
        (s3-1 (vector-! (new 'stack-no-clear 'vector) arg3 arg2)))
    0.0
    (set! (-> s3-1 y) 0.0)
    (let ((f30-0 (vector-vector-xz-distance arg2 arg3)))
      (vector-xz-normalize! s3-1 1.0)
      (try-move-along-ray arg0 arg1 s4-1 s3-1 f30-0))))

(defun nav-ray-test-local? ((arg0 nav-mesh) (arg1 nav-poly) (arg2 vector) (arg3 vector))
  (local-vars (v1-2 symbol))
  (let ((gp-0 (new 'stack-no-clear 'nav-ray)))
    (let ((s4-0 0))
      (init-ray-local gp-0 arg1 arg2 arg3)
      (until v1-2
        (+! s4-0 1)
        (move-along-nav-ray! arg0 gp-0)
        (set! v1-2 (or (>= s4-0 15) (-> gp-0 terminated)))))
    (-> gp-0 reached-dest)))

(defun nav-mesh-update-route-table ((arg0 nav-mesh) (arg1 int) (arg2 int) (arg3 uint))
  (let* ((a1-1 (* (+ arg2 (* arg1 (-> arg0 poly-count))) 2))
         (v1-3 (/ a1-1 8))
         (a2-2 (- a1-1 (* v1-3 8)))
         (a1-4 (lognot (ash 3 a2-2)))
         (v0-0 (logior (ash (logand arg3 3) a2-2) (logand a1-4 (-> (the-as (pointer uint8) (-> arg0 route)) v1-3)))))
    (set! (-> (the-as (pointer uint8) (-> arg0 route)) v1-3) (the-as uint v0-0))
    (the-as uint v0-0)))

(defun nav-mesh-lookup-route ((arg0 nav-mesh) (arg1 int) (arg2 int))
  (let ((v1-3 (* (+ arg1 (* arg2 (-> arg0 poly-count))) 2)))
    (logand (ash (-> (the-as (pointer uint8) (-> arg0 route)) (/ v1-3 8)) (- (logand v1-3 7))) 3)))

(define *nav-update-route-table-ray-count* 0)

(define *nav-update-route-table-route-count* 0)

(defmethod nav-mesh-method-18 ((this nav-mesh) (arg0 int) (arg1 vector) (arg2 int) (arg3 (pointer int8)) (arg4 int))
  (local-vars (sv-32 int) (sv-48 uint))
  (set! (-> arg3 arg2) 1)
  (nav-mesh-update-route-table this arg0 arg2 (the-as uint 3))
  (nav-mesh-update-route-table this arg2 arg0 (the-as uint 3))
  (set! *nav-update-route-table-route-count* (+ *nav-update-route-table-route-count* 1))
  (when (> arg4 0)
    (let ((s1-1 (-> this poly arg2))
          (s0-0 (new 'stack-no-clear 'vector)))
      (set! sv-32 0)
      (while (< sv-32 3)
        (set! sv-48 (-> (the-as nav-poly (+ sv-32 (the-as int s1-1))) adj-poly 0))
        (when (and (!= sv-48 255) (!= 1 (-> (the-as (pointer uint8) (&+ arg3 sv-48)))))
          (set! (-> arg3 sv-48) 1)
          (when (not (logtest? (-> this poly sv-48 pat) 1))
            (let ((v0-3 (= (nav-mesh-lookup-route this arg0 (the-as int sv-48)) 3)))
              (when (not v0-3)
                (tri-centroid-local this s1-1 s0-0)
                (set! *nav-update-route-table-ray-count* (+ *nav-update-route-table-ray-count* 1))
                (set! v0-3 (nav-ray-test-local? this (-> this poly arg0) arg1 s0-0)))
              (when v0-3
                (let* ((a0-16 this)
                       (t9-5 (method-of-object a0-16 nav-mesh-method-18))
                       (a1-7 arg0)
                       (a2-6 arg1)
                       (t0-1 arg3)
                       (t1-1 (+ arg4 -1)))
                  (t9-5 a0-16 a1-7 a2-6 (the-as int sv-48) t0-1 t1-1))))))
        (set! sv-32 (+ sv-32 1)))))
  0
  (none))

(defmethod update-route-table ((this nav-mesh))
  (when *nav-patch-route-table*
    (stopwatch-init *nav-timer*)
    (stopwatch-begin *nav-timer*)
    (let ((s5-0 (new 'stack-no-clear 'vector)))
      (new 'stack-no-clear 'vector)
      (let ((s4-0 (new 'stack-no-clear 'array 'int8 256)))
        (set! *nav-update-route-table-ray-count* 0)
        (set! *nav-update-route-table-route-count* 0)
        (countdown (s3-0 (-> this poly-count))
          (let ((s2-0 (-> this poly s3-0)))
            (when (not (logtest? (-> s2-0 pat) 1))
              (tri-centroid-local this s2-0 s5-0)
              (mem-set32! s4-0 64 0)
              (set! (-> s4-0 s3-0) 1)
              (dotimes (s1-0 3)
                (let ((a3-0 (-> s2-0 adj-poly s1-0)))
                  (when (!= a3-0 255)
                    (if (not (logtest? (-> this poly a3-0 pat) 1)) (nav-mesh-method-18 this s3-0 s5-0 (the-as int a3-0) s4-0 0))))))))))
    (stopwatch-end *nav-timer*))
  0
  (none))

(defun point-triangle-distance-min ((arg0 vector) (arg1 float) (arg2 (inline-array nav-vertex)))
  (let ((v1-0 0)
        (f0-0 arg1))
    (dotimes (a3-0 3)
      (let* ((t0-4 (-> arg2 (-> *edge-vert0-table* a3-0)))
             (t1-4 (-> arg2 (-> *edge-vert1-table* a3-0)))
             (f1-1 (- (-> t0-4 z) (-> t1-4 z)))
             (f2-2 (- (-> t1-4 x) (-> t0-4 x)))
             (f3-2 (- (-> arg0 x) (-> t0-4 x)))
             (f4-2 (- (-> arg0 z) (-> t0-4 z)))
             (f5-1 f1-1)
             (f5-3 (* f5-1 f5-1))
             (f6-0 f2-2)
             (f5-5 (sqrtf (+ f5-3 (* f6-0 f6-0))))
             (f5-7 (/ 1.0 f5-5))
             (f1-2 (* f1-1 f5-7))
             (f2-3 (* f2-2 f5-7))
             (f1-4 (+ (* f1-2 f3-2) (* f2-3 f4-2))))
        (cond
          ((< arg1 f1-4) (return arg1))
          ((>= 0.0 f1-4) (+! v1-0 (-> *edge-mask-table* a3-0)))
          (else (set! f0-0 f1-4)))))
    (if (= v1-0 7) (return 0.0))
    (let ((t0-14 #f)
          (a3-5 0))
      (cond
        ((= v1-0 1) (set! t0-14 #t) (set! a3-5 0))
        ((= v1-0 2) (set! t0-14 #t) (set! a3-5 1))
        ((= v1-0 4) (set! t0-14 #t) (set! a3-5 2))
        ((= v1-0 6) (set! a3-5 0))
        ((= v1-0 5) (set! a3-5 1))
        ((= v1-0 3) (set! a3-5 2)))
      (when (not t0-14)
        (let* ((t0-15 (-> arg2 (-> *edge-vert0-table* a3-5)))
               (v1-17 (-> arg2 (-> *edge-vert1-table* a3-5)))
               (f2-8 (- (-> v1-17 x) (-> t0-15 x)))
               (f1-7 (- (-> v1-17 z) (-> t0-15 z)))
               (f3-7 (+ (* f2-8 (- (-> arg0 x) (-> t0-15 x))) (* f1-7 (- (-> arg0 z) (-> t0-15 z)))))
               (f1-9 (+ (* f2-8 (- (-> arg0 x) (-> v1-17 x))) (* f1-7 (- (-> arg0 z) (-> v1-17 z))))))
          (cond
            ((< f3-7 0.0) (set! t0-14 #t) (set! a3-5 (-> *edge-vert0-table* a3-5)))
            ((< 0.0 f1-9) (set! t0-14 #t) (set! a3-5 (-> *edge-vert1-table* a3-5)))
            (else (return f0-0)))))
      (when t0-14
        (let* ((v1-27 (-> arg2 a3-5))
               (f0-2 (- (-> v1-27 x) (-> arg0 x)))
               (f0-4 (* f0-2 f0-2))
               (f1-12 (- (-> v1-27 z) (-> arg0 z))))
          (set! f0-0 (fmin (sqrtf (+ f0-4 (* f1-12 f1-12))) arg1)))))
    f0-0))

(defmethod find-poly ((this nav-mesh) (arg0 vector) (arg1 meters) (arg2 (pointer nav-control-flags)))
  (local-vars (s3-1 nav-poly))
  (let ((v1-1 (find-poly-fast this arg0 arg1)))
    (when v1-1
      (if arg2 (logior! (-> arg2 0) (nav-control-flags navcf20)))
      (set! s3-1 v1-1)
      (goto cfg-14)))
  (if arg2 (logclear! (-> arg2 0) (nav-control-flags navcf20)))
  (let ((s2-0 (new 'stack-no-clear 'inline-array 'nav-vertex 3)))
    (set! s3-1 (the-as nav-poly #f))
    (let ((f30-0 10000000000000000000000000000000000000.0))
      (countdown (s1-0 (-> this poly-count))
        (let ((s0-0 (-> this poly s1-0)))
          (when (not (logtest? (-> s0-0 pat) 1))
            (set! (-> s2-0 0 quad) (-> this vertex (-> s0-0 vertex 0) quad))
            (set! (-> s2-0 1 quad) (-> this vertex (-> s0-0 vertex 1) quad))
            (set! (-> s2-0 2 quad) (-> this vertex (-> s0-0 vertex 2) quad))
            (let ((f1-2 (- (-> arg0 y) (* 0.333333 (+ (-> s2-0 0 y) (-> s2-0 1 y) (-> s2-0 2 y))))))
              (when (>= arg1 (fabs f1-2))
                (let ((f0-2 (point-triangle-distance-min arg0 f30-0 s2-0))) (when (< f0-2 f30-0) (set! f30-0 f0-2) (set! s3-1 s0-0))))))))))
  (label cfg-14)
  s3-1)

(defmethod setup-portal ((this nav-mesh) (arg0 nav-poly) (arg1 nav-poly) (arg2 nav-route-portal))
  (local-vars (t0-6 int) (t1-1 int))
  (set! (-> arg2 next-poly) #f)
  (cond
    ((and arg0 arg1)
     (let* ((v1-3 (* (+ (-> arg1 id) (* (the-as int (-> arg0 id)) (-> this poly-count))) 2))
            (v1-8 (logand (ash (-> (the-as (pointer uint8) (-> this route)) (/ (the-as int v1-3) 8)) (- (the-as int (logand v1-3 7)))) 3)))
       (set! (-> arg2 edge-index) -1)
       (when (!= v1-8 3)
         (let ((a2-9 (-> arg0 adj-poly v1-8)))
           (when (!= a2-9 255)
             (set! (-> arg2 edge-index) (the-as int v1-8))
             (set! (-> arg2 next-poly) (-> this poly a2-9))
             (let ((a2-12 (-> this vertex)))
               (let ((t0-5 (+ v1-8 1))) (let ((t1-0 2)) (set-on-less-than t1-1 t1-0 t0-5)) (move-if-not-zero t0-6 0 t1-1 t0-5))
               (set! (-> arg2 vertex 0) (-> a2-12 (-> arg0 vertex t0-6))))
             (set! (-> arg2 vertex 1) (-> this vertex (-> arg0 vertex v1-8)))
             #t)))))
    (else 0)))

(defmethod get-adj-poly ((this nav-mesh) (arg0 nav-poly) (arg1 nav-poly) (arg2 symbol))
  (local-vars (v1-12 uint) (t0-6 int) (t1-1 int))
  (cond
    ((and arg0 arg1)
     (let* ((v1-3 (* (+ (-> arg1 id) (* (the-as int (-> arg0 id)) (-> this poly-count))) 2))
            (a2-6 (logand (ash (-> (the-as (pointer uint8) (-> this route)) (/ (the-as int v1-3) 8)) (- (the-as int (logand v1-3 7)))) 3)))
       (if arg2 (set! (-> arg2 value) (the-as uint #xffffffff)))
       (cond
         ((= a2-6 3) (the-as nav-poly #f))
         ((begin (set! v1-12 (-> arg0 adj-poly a2-6)) (= v1-12 255)) (the-as nav-poly #f))
         (else
          (when arg2
            (let ((t0-5 (+ a2-6 1))) (let ((t1-0 2)) (set-on-less-than t1-1 t1-0 t0-5)) (move-if-not-zero t0-6 0 t1-1 t0-5))
            (set! (-> arg2 value) (logior (-> arg0 vertex t0-6) (shl (-> arg0 vertex a2-6) 16))))
          (-> this poly v1-12)))))
    (else (the-as nav-poly #f))))

(defmethod compute-bounding-box ((this nav-mesh) (arg0 vector) (arg1 vector))
  (let ((f0-0 10000000000000000000000000000000000000.0)
        (f1-0 -10000000000000000000000000000000000000.0))
    (set! (-> arg0 x) f0-0)
    (set! (-> arg0 y) f0-0)
    (set! (-> arg0 z) f0-0)
    (set! (-> arg1 x) f1-0)
    (set! (-> arg1 y) f1-0)
    (set! (-> arg1 z) f1-0))
  (dotimes (v1-1 (-> this vertex-count))
    (let ((a3-1 (-> this vertex v1-1)))
      (set! (-> arg0 x) (fmin (-> arg0 x) (-> a3-1 x)))
      (set! (-> arg0 y) (fmin (-> arg0 y) (-> a3-1 y)))
      (set! (-> arg0 z) (fmin (-> arg0 z) (-> a3-1 z)))
      (set! (-> arg1 x) (fmax (-> arg1 x) (-> a3-1 x)))
      (set! (-> arg1 y) (fmax (-> arg1 y) (-> a3-1 y)))
      (set! (-> arg1 z) (fmax (-> arg1 z) (-> a3-1 z)))))
  (vector+! arg0 arg0 (-> this origin))
  (vector+! arg1 arg1 (-> this origin))
  0
  (none))

(defmethod initialize-mesh! ((this nav-mesh))
  (local-vars (sv-32 vector) (sv-48 int))
  (with-pp
    (set! sv-32 (new 'stack-no-clear 'vector))
    (let ((s5-0 (-> this poly-count))
          (s3-0 (-> this vertex-count))
          (s1-0 0)
          (s2-0 0)
          (s4-0 0)
          (gp-0 #f))
      (set! sv-48 s5-0)
      (while (nonzero? sv-48)
        (set! sv-48 (+ sv-48 -1))
        (let ((v1-4 (-> this poly sv-48)))
          (if (logtest? (-> v1-4 pat) 1) (+! s4-0 1))
          (let ((a1-2 (-> this vertex (-> v1-4 vertex 0)))
                (a2-2 (-> this vertex (-> v1-4 vertex 1)))
                (a3-0 (-> this vertex (-> v1-4 vertex 2))))
            (vector-3pt-cross! sv-32 a1-2 a2-2 a3-0)))
        (cond
          ((= (vector-length sv-32) 0.0) (+! s1-0 1))
          (else (vector-normalize! sv-32 1.0) (if (>= 0.0 (-> sv-32 y)) (+! s2-0 1)))))
      (when (zero? s5-0)
        (format #t "WARNING:  nav-mesh has no triangles. ")
        (set! gp-0 #t))
      (when (> s1-0 0)
        (format #t "WARNING:  nav-mesh has ~D triangles with zero area (out of ~D triangles). " s1-0 s5-0)
        (set! gp-0 #t))
      (when (> s2-0 0)
        (format #t "WARNING:  nav-mesh has ~D triangles with inverted normals (out of ~D triangles). " s2-0 s5-0)
        (set! gp-0 #t))
      (when (< 255 s5-0)
        (format #t "WARNING:  nav-mesh has ~D triangles (only up to ~D are allowed). " s5-0 255)
        (set! gp-0 #t))
      (when (< 255 s3-0)
        (format #t "WARNING:  nav-mesh has ~D vertices (only up to ~D are allowed). " s3-0 255)
        (set! gp-0 #t))
      (when (= s4-0 s5-0)
        (format #t "WARNING:  nav-mesh only contains gap triangles (~D triangles total). " s5-0)
        (set! gp-0 #t))
      (when gp-0
        (if pp (format #t "current process is ~A~%" (-> pp name)) (format #t "(no current process).~%"))))
    0
    (none)))

(defmethod find-opposite-vertices ((this nav-mesh) (arg0 nav-poly) (arg1 nav-poly))
  (when (!= arg0 arg1)
    (dotimes (v1-1 3)
      (dotimes (a0-1 3)
        (when (= (-> arg0 vertex v1-1) (-> arg1 vertex a0-1))
          (let ((a3-6 (-> arg0 vertex (mod (+ v1-1 1) 3))))
            (dotimes (t0-3 3)
              (if (and (!= t0-3 a0-1) (= a3-6 (-> arg1 vertex t0-3))) (return (logior a3-6 (shl (-> arg0 vertex v1-1) 16))))))))))
  (the-as uint #xffffffff))

(defun ray-ccw-line-segment-intersection? ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (let ((f0-2 (- (* (-> arg2 x) (-> arg1 z)) (* (-> arg2 z) (-> arg1 x))))
        (f2-4 (- (* (-> arg3 x) (-> arg1 z)) (* (-> arg3 z) (-> arg1 x))))
        (f3-4 (- (* (-> arg0 x) (-> arg1 z)) (* (-> arg0 z) (-> arg1 x))))
        (v0-0 #f))
    (let ((f1-7 (- f2-4 f0-2))
          (f2-5 (- f2-4 f3-4))
          (f3-5 (- f3-4 f0-2)))
      (when (and (>= (fabs f1-7) (fmax (fabs f3-5) (fabs f2-5))) (!= f1-7 0.0))
        (let ((f0-7 (+ (* (-> arg1 x) (- (-> arg3 z) (-> arg2 z))) (* (-> arg1 z) (- (-> arg2 x) (-> arg3 x))))))
          (set! v0-0 (< 0.0 f0-7)))))
    v0-0))

(defun ray-line-segment-intersection? ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (let ((f1-3 (- (* (-> arg2 x) (-> arg1 z)) (* (-> arg2 z) (-> arg1 x))))
        (f0-4 (- (* (-> arg3 x) (-> arg1 z)) (* (-> arg3 z) (-> arg1 x))))
        (f2-6 (- (* (-> arg0 x) (-> arg1 z)) (* (-> arg0 z) (-> arg1 x))))
        (gp-0 #f))
    (let ((f30-0 (- f0-4 f1-3))
          (f0-5 (- f0-4 f2-6))
          (f1-4 (- f2-6 f1-3)))
      (when (and (>= (fabs f30-0) (fmax (fabs f1-4) (fabs f0-5))) (!= f30-0 0.0))
        (let ((f2-11 (+ (* (-> arg2 x) (-> arg1 x)) (* (-> arg2 z) (-> arg1 z))))
              (f3-11 (+ (* (-> arg3 x) (-> arg1 x)) (* (-> arg3 z) (-> arg1 z))))
              (f28-0 (+ (* (-> arg0 x) (-> arg1 x)) (* (-> arg0 z) (-> arg1 z)))))
          (if (>= (* (+ (* f2-11 f0-5) (* f3-11 f1-4)) (sign f30-0)) (* f28-0 (fabs f30-0))) (set! gp-0 #t)))))
    gp-0))

(define *debug-offset* (new 'static 'vector :y 4096.0 :w 1.0))

(defun clip-vector-to-halfspace! ((arg0 vector) (arg1 float) (arg2 float) (arg3 float))
  (let ((f0-2 (+ (* arg1 (-> arg0 x)) (* arg2 (-> arg0 z)))))
    (when (< arg3 f0-2)
      (let ((f0-3 (/ arg3 f0-2))) (set! (-> arg0 x) (* (-> arg0 x) f0-3)) (set! (-> arg0 z) (* (-> arg0 z) f0-3))))))

(defmethod nav-mesh-method-23 ((this nav-mesh) (arg0 nav-poly) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 nav-route-portal))
  (local-vars (v1-32 int) (a0-14 int) (a0-17 int) (a1-10 int) (sv-16 nav-vertex))
  (let ((s1-0 (-> this vertex))
        (s0-0 -1))
    (set! (-> arg2 quad) (-> arg3 quad))
    (dotimes (v1-1 3)
      (let* ((a0-8 (-> s1-0 (-> arg0 vertex (-> *edge-vert0-table* v1-1))))
             (a1-7 (-> s1-0 (-> arg0 vertex (-> *edge-vert1-table* v1-1))))
             (f0-1 (- (-> a0-8 z) (-> a1-7 z)))
             (f1-2 (- (-> a1-7 x) (-> a0-8 x)))
             (f2-4 (+ (* f0-1 (- (-> a0-8 x) (-> arg1 x))) (* f1-2 (- (-> a0-8 z) (-> arg1 z)))))
             (f0-3 (+ (* (-> arg2 x) f0-1) (* (-> arg2 z) f1-2))))
        (when (< f2-4 f0-3)
          (set! s0-0 v1-1)
          (let ((f0-4 (/ f2-4 f0-3))) (set! (-> arg2 x) (* (-> arg2 x) f0-4)) (set! (-> arg2 z) (* (-> arg2 z) f0-4))))))
    (when arg4
      (cond
        ((= s0-0 -1) (set! (-> arg4 next-poly) #f))
        (else
         (while (!= s0-0 -1)
           (let ((v1-11 (-> arg0 adj-poly (-> *edge-vert0-table* s0-0))))
             (cond
               ((!= v1-11 255) (set! (-> arg4 next-poly) (-> this poly v1-11)) (set! s0-0 -1))
               ((let ((a1-8 (-> s1-0 (-> arg0 vertex (-> *edge-vert0-table* s0-0)))))
                  (set! sv-16 (-> s1-0 (-> arg0 vertex (-> *edge-vert1-table* s0-0))))
                  (< (vector-vector-xz-distance arg1 a1-8) 204.8))
                (let ((a1-9 (+ s0-0 -1))
                      (v1-28 2))
                  (set-on-less-than a0-14 a1-9 0)
                  (move-if-not-zero a1-10 v1-28 a0-14 a1-9))
                (set! s0-0 a1-10))
               ((< (vector-vector-xz-distance arg1 sv-16) 204.8)
                (let ((v1-31 (+ s0-0 1))) (let ((a0-16 2)) (set-on-less-than a0-17 a0-16 v1-31)) (move-if-not-zero v1-32 0 a0-17 v1-31))
                (set! s0-0 v1-32))
               (else (set! (-> arg4 next-poly) #f) (set! s0-0 -1)))))))))
  (set! (-> arg2 y) (-> arg1 y))
  arg2)

(defmethod project-point-into-tri-3d ((this nav-mesh) (arg0 nav-poly) (arg1 vector) (arg2 vector))
  (let ((s3-0 (new 'stack-no-clear 'vector))
        (s4-0 (new 'stack-no-clear 'matrix)))
    (dotimes (v1-0 3)
      (set! (-> s4-0 vector v1-0 quad) (the-as uint128 0)))
    (set! (-> s4-0 vector 0 quad) (-> this vertex (-> arg0 vertex 0) quad))
    (set! (-> s4-0 vector 1 quad) (-> this vertex (-> arg0 vertex 1) quad))
    (set! (-> s4-0 vector 2 quad) (-> this vertex (-> arg0 vertex 2) quad))
    (normal-of-plane s3-0 (the-as vector (-> s4-0 vector)) (-> s4-0 vector 1) (-> s4-0 vector 2))
    (closest-pt-in-triangle arg1 arg2 s4-0 s3-0))
  (none))

(defmethod project-onto-nav-mesh ((this nav-control) (arg0 vector) (arg1 vector))
  (local-vars (sv-32 int))
  (let ((s5-0 (-> this mesh))
        (s3-1 (vector-! (new 'stack-no-clear 'vector) arg1 (-> this mesh origin))))
    (set! sv-32 0)
    (let ((a1-5 (find-poly s5-0 s3-1 (-> this nearest-y-threshold) (the-as (pointer nav-control-flags) (& sv-32)))))
      (cond
        ((logtest? #x100000 sv-32) (set! (-> arg0 quad) (-> arg1 quad)))
        (else (closest-point-on-boundary s5-0 a1-5 arg0 s3-1) (vector+! arg0 arg0 (-> s5-0 origin))))))
  arg0)

(defmethod find-poly ((this nav-control) (arg0 vector))
  (find-poly (-> this mesh)
             (vector-! (new 'stack-no-clear 'vector) arg0 (-> this mesh origin))
             (-> this nearest-y-threshold)
             (the-as (pointer nav-control-flags) #f)))

(defmethod project-point-into-tri-3d ((this nav-control) (arg0 nav-poly) (arg1 vector) (arg2 vector))
  (project-point-into-tri-3d (-> this mesh) arg0 arg1 (vector-! (new 'stack-no-clear 'vector) arg2 (-> this mesh origin)))
  (vector+! arg1 arg1 (-> this mesh origin))
  arg1)

(defmethod is-in-mesh? ((this nav-control) (arg0 vector) (arg1 float))
  (let ((v1-1 (vector-! (new 'stack-no-clear 'vector) arg0 (-> this mesh origin)))
        (a1-1 (-> this mesh)))
    (is-in-mesh? a1-1 v1-1 arg1 (-> this nearest-y-threshold))))

(defmethod debug-draw ((this nav-control))
  (local-vars
    (sv-192 vector)
    (sv-208 uint)
    (sv-224 int)
    (sv-240 (function _varargs_ object))
    (sv-256 string)
    (sv-272 int)
    (sv-288 (function _varargs_ object)))
  (let ((a0-1 this))
    (when (and *display-nav-marks* (logtest? (-> a0-1 flags) (nav-control-flags display-marks)))
      (let ((s5-0 (-> this mesh))
            (s4-0 (new 'stack-no-clear 'vector)))
        (= (-> this mesh debug-time) (logand (-> *display* actual-frame-counter) 255))
        (when #t
          (set! (-> s5-0 debug-time) (the-as uint (-> *display* actual-frame-counter)))
          (add-debug-sphere (logtest? (-> this flags) (nav-control-flags navcf1))
                            (bucket-id debug)
                            (-> s5-0 bounds)
                            (-> s5-0 bounds w)
                            (new 'static 'rgba :r #xff :g #xff :a #x20))
          (add-debug-vector #t (bucket-id debug-no-zbuf) (-> s5-0 origin) *x-vector* (meters 1) *color-red*)
          (add-debug-vector #t (bucket-id debug-no-zbuf) (-> s5-0 origin) *z-vector* (meters 1) *color-blue*)
          (when (logtest? (-> this flags) (nav-control-flags navcf2))
            (dotimes (s3-0 (-> s5-0 vertex-count))
              (add-debug-x #t
                           (bucket-id debug-no-zbuf)
                           (vector+! s4-0 (-> s5-0 origin) (the-as vector (-> s5-0 vertex s3-0)))
                           *color-green*)))
          (when #f
            (dotimes (s3-1 (-> s5-0 node-count))
              (let ((a1-4 (-> s5-0 nodes s3-1))
                    (a0-15 (new 'stack-no-clear 'vector))
                    (v1-20 (new 'stack-no-clear 'vector)))
                (set! sv-192 (new 'stack-no-clear 'vector))
                (let ((s2-0 (new 'stack-no-clear 'vector))
                      (s1-0 (new 'stack-no-clear 'vector))
                      (s0-0 (new 'stack-no-clear 'vector)))
                  (set! sv-208 (the-as uint #x80808080))
                  (set! (-> a0-15 quad) (-> a1-4 center quad))
                  (set! (-> v1-20 quad) (-> a1-4 radius quad))
                  (set! (-> sv-192 w) 1.0)
                  (set! (-> s2-0 w) 1.0)
                  (set! (-> s1-0 w) 1.0)
                  (set! (-> s0-0 w) 1.0)
                  (set! (-> sv-192 x) (+ (-> a0-15 x) (-> v1-20 x)))
                  (set! (-> sv-192 y) (+ (-> a0-15 y) (-> v1-20 y)))
                  (set! (-> sv-192 z) (+ (-> a0-15 z) (-> v1-20 z)))
                  (set! (-> s2-0 x) (+ (-> a0-15 x) (-> v1-20 x)))
                  (set! (-> s2-0 y) (+ (-> a0-15 y) (-> v1-20 y)))
                  (set! (-> s2-0 z) (- (-> a0-15 z) (-> v1-20 z)))
                  (set! (-> s1-0 x) (- (-> a0-15 x) (-> v1-20 x)))
                  (set! (-> s1-0 y) (+ (-> a0-15 y) (-> v1-20 y)))
                  (set! (-> s1-0 z) (- (-> a0-15 z) (-> v1-20 z)))
                  (set! (-> s0-0 x) (- (-> a0-15 x) (-> v1-20 x)))
                  (set! (-> s0-0 y) (+ (-> a0-15 y) (-> v1-20 y)))
                  (set! (-> s0-0 z) (+ (-> a0-15 z) (-> v1-20 z)))
                  (add-debug-line #t (bucket-id debug-no-zbuf) sv-192 s2-0 (the-as rgba sv-208) #f (the-as rgba -1))
                  (add-debug-line #t (bucket-id debug-no-zbuf) s2-0 s1-0 (the-as rgba sv-208) #f (the-as rgba -1))
                  (add-debug-line #t (bucket-id debug-no-zbuf) s1-0 s0-0 (the-as rgba sv-208) #f (the-as rgba -1))
                  (let ((t9-7 add-debug-line)
                        (a0-19 #t)
                        (a1-13 68)
                        (t1-5 #f)
                        (t2-3 -1))
                    (t9-7 a0-19 (the-as bucket-id a1-13) s0-0 sv-192 (the-as rgba sv-208) t1-5 (the-as rgba t2-3)))))))
          (when (logtest? (-> this flags) (nav-control-flags navcf3))
            (dotimes (s3-2 (-> s5-0 poly-count))
              (let ((s2-1 (-> s5-0 poly s3-2)))
                (debug-draw-poly s5-0
                                 s2-1
                                 (cond
                                   ((logtest? (-> s2-1 pat) 1) (new 'static 'rgba :g #x80 :b #x80 :a #x40))
                                   ((logtest? (-> s2-1 pat) 2) (new 'static 'rgba :g #xff :b #x80 :a #x40))
                                   ((logtest? (-> s2-1 pat) 4) (new 'static 'rgba :g #xff :b #x40 :a #x80))
                                   ((logtest? (-> s2-1 pat) 8) (new 'static 'rgba :g #x80 :b #x40 :a #xff))
                                   ((logtest? (-> s2-1 pat) 16) (new 'static 'rgba :g #x80 :b #x40 :a #xff))
                                   (else (new 'static 'rgba :g #x80 :b #xff :a #x80))))
                (when (logtest? (-> this flags) (nav-control-flags navcf4))
                  (let ((s1-1 add-debug-text-3d)
                        (s0-1 #t))
                    (set! sv-224 68)
                    (set! sv-240 format)
                    (let ((a0-23 (clear *temp-string*))
                          (a1-15 "~D")
                          (a2-13 (-> s2-1 id)))
                      (sv-240 a0-23 a1-15 a2-13))
                    (set! sv-256 *temp-string*)
                    (let ((a3-10 (tri-centroid-world s5-0 s2-1 s4-0))
                          (t0-8 8)
                          (t1-6 #f))
                      (s1-1 s0-1 (the-as bucket-id sv-224) sv-256 a3-10 (the-as font-color t0-8) (the-as vector2h t1-6))))))))
          (when (logtest? (-> this flags) (nav-control-flags navcf5))
            (if (-> this next-poly) (debug-draw-poly s5-0 (-> this next-poly) *color-cyan*))
            (if (-> this target-poly) (debug-draw-poly s5-0 (-> this target-poly) *color-yellow*))
            (if (-> this current-poly) (debug-draw-poly s5-0 (-> this current-poly) *color-red*)))
          (when (logtest? (-> this flags) (nav-control-flags navcf7))
            (dotimes (s3-3 (the-as int (-> s5-0 static-sphere-count)))
              (let ((s2-2 (-> s5-0 static-sphere s3-3)))
                (add-debug-sphere #t (bucket-id debug) (the-as vector s2-2) (-> s2-2 trans w) *color-blue*)
                (let ((s1-2 add-debug-text-3d)
                      (s0-2 #t))
                  (set! sv-272 68)
                  (set! sv-288 format)
                  (let ((a0-32 (clear *temp-string*))
                        (a1-22 "~D")
                        (a2-20 s3-3))
                    (sv-288 a0-32 a1-22 a2-20))
                  (let ((a2-21 *temp-string*)
                        (t0-10 8)
                        (t1-7 #f))
                    (s1-2 s0-2 (the-as bucket-id sv-272) a2-21 (the-as vector s2-2) (the-as font-color t0-10) (the-as vector2h t1-7))))))))
        (when (logtest? (-> this flags) (nav-control-flags navcf6))
          (when (and (-> this portal 0) (-> this portal 1))
            (let ((v1-80 (-> s5-0 origin))
                  (a2-22 (new 'stack-no-clear 'vector))
                  (a3-13 (new 'stack-no-clear 'vector)))
              (add-debug-line #t
                              (bucket-id debug-no-zbuf)
                              (vector+! a2-22 v1-80 (the-as vector (-> this portal 0)))
                              (vector+! a3-13 v1-80 (the-as vector (-> this portal 1)))
                              (new 'static 'rgba :r #xff :g #x80 :b #x40 :a #x80)
                              #f
                              (the-as rgba -1))))
          (add-debug-vector #t
                            (bucket-id debug-no-zbuf)
                            (-> this shape trans)
                            (-> this travel)
                            (meters 0.00024414062)
                            *color-white*)
          (add-debug-vector #t
                            (bucket-id debug-no-zbuf)
                            (vector+! (new 'stack-no-clear 'vector) (-> this shape trans) (new 'static 'vector :y 4096.0 :w 1.0))
                            (-> this old-travel)
                            (meters 0.00024414062)
                            *color-yellow*)
          (add-debug-x #t
                       (bucket-id debug-no-zbuf)
                       (vector+! (new 'stack-no-clear 'vector) (-> this shape trans) (-> this travel))
                       (new 'static 'rgba :r #xff :g #xff :b #xff :a #x80)))
        (when (logtest? (-> this flags) (nav-control-flags navcf7))
          (add-debug-sphere #t
                            (bucket-id debug-no-zbuf)
                            (the-as vector (-> this shape root-prim prim-core))
                            (-> this shape nav-radius)
                            (new 'static 'rgba :g #xff :b #xff :a #x20))
          (dotimes (s3-4 (-> this num-spheres))
            (let ((v1-95 (-> this sphere s3-4)))
              (vector+! s4-0 (-> s5-0 origin) (the-as vector (&-> v1-95 x)))
              (add-debug-sphere #t
                                (bucket-id debug-no-zbuf)
                                s4-0
                                (- (-> v1-95 w) (-> this shape nav-radius))
                                (new 'static 'rgba :g #xff :b #xff :a #x20))))))
      0))
  (none))

(defmethod mem-usage ((this nav-mesh) (arg0 memory-usage-block) (arg1 int))
  (set! (-> arg0 length) (max 46 (-> arg0 length)))
  (set! (-> arg0 data 45 name) "nav-mesh")
  (+! (-> arg0 data 45 count) 1)
  (let ((v1-6 (asize-of this))) (+! (-> arg0 data 45 used) v1-6) (+! (-> arg0 data 45 total) (logand -16 (+ v1-6 15))))
  (set! (-> arg0 length) (max 46 (-> arg0 length)))
  (set! (-> arg0 data 45 name) "nav-mesh")
  (+! (-> arg0 data 45 count) 1)
  (let ((v1-16 (* (-> this vertex-count) 16)))
    (+! (-> arg0 data 45 used) v1-16)
    (+! (-> arg0 data 45 total) (logand -16 (+ v1-16 15))))
  (set! (-> arg0 length) (max 46 (-> arg0 length)))
  (set! (-> arg0 data 45 name) "nav-mesh")
  (+! (-> arg0 data 45 count) 1)
  (let ((v1-26 (* (-> this poly-count) 8)))
    (+! (-> arg0 data 45 used) v1-26)
    (+! (-> arg0 data 45 total) (logand -16 (+ v1-26 15))))
  (set! (-> arg0 length) (max 46 (-> arg0 length)))
  (set! (-> arg0 data 45 name) "nav-mesh")
  (+! (-> arg0 data 45 count) 1)
  (let ((v1-38 (/ (* (* (-> this poly-count) (-> this poly-count)) 2) 8)))
    (+! (-> arg0 data 45 used) v1-38)
    (+! (-> arg0 data 45 total) (logand -16 (+ v1-38 15))))
  (the-as nav-mesh 0))

(defmethod set-current-poly! ((this nav-control) (arg0 nav-poly))
  (set! (-> this current-poly) arg0)
  (logior! (-> this flags) (nav-control-flags navcf9))
  0
  (none))

(defmethod nav-control-method-30 ((this nav-control) (arg0 vector) (arg1 vector) (arg2 vector))
  (let ((s5-0 (the-as sphere #f)))
    (let ((f30-0 -0.000001))
      (countdown (s1-0 (-> this num-spheres))
        (let* ((s0-0 (-> this sphere s1-0))
               (f0-1 (ray-circle-intersect arg0 arg1 (the-as vector (&-> s0-0 x)) (-> s0-0 w))))
          (when (< f30-0 f0-1)
            (set! s5-0 s0-0)
            (set! f30-0 f0-1))))
      (set! (-> arg2 x) f30-0))
    s5-0))

(defmethod intersect-ray-line-segment? ((this nav-control) (arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (ray-line-segment-intersection? arg0 arg1 arg2 arg3))

(defun add-nav-sphere ((arg0 nav-control) (arg1 vector))
  (when (< (-> arg0 num-spheres) (-> arg0 max-spheres))
    (let* ((s4-0 (-> arg0 sphere (-> arg0 num-spheres)))
           (f1-0 (vector-vector-xz-distance-squared arg1 (-> arg0 shape trans)))
           (f0-1 (+ (-> arg1 w) (-> arg0 shape nav-radius)))
           (f2-2 (+ 40960.0 f0-1)))
      (when (< f1-0 (* f2-2 f2-2))
        (vector-! (the-as vector s4-0) arg1 (-> arg0 mesh origin))
        (set! (-> s4-0 w) f0-1)
        (+! (-> arg0 num-spheres) 1))))
  0
  (none))

(defun add-collide-shape-spheres ((arg0 nav-control) (arg1 collide-shape) (arg2 vector))
  (when (logtest? (-> arg1 nav-flags) (nav-flags navf0))
    (set! (-> arg2 quad) (-> arg1 root-prim prim-core world-sphere quad))
    (set! (-> arg2 w) (-> arg1 nav-radius))
    (let ((s4-0 arg0)
          (s3-0 arg2))
      (when (< (-> s4-0 num-spheres) (-> s4-0 max-spheres))
        (let* ((s2-0 (-> s4-0 sphere (-> s4-0 num-spheres)))
               (f1-0 (vector-vector-xz-distance-squared s3-0 (-> s4-0 shape trans)))
               (f0-2 (+ (-> s3-0 w) (-> s4-0 shape nav-radius)))
               (f2-2 (+ 40960.0 f0-2)))
          (when (< f1-0 (* f2-2 f2-2))
            (vector-! (the-as vector s2-0) s3-0 (-> s4-0 mesh origin))
            (set! (-> s2-0 w) f0-2)
            (+! (-> s4-0 num-spheres) 1)))))
    0)
  (when (logtest? (-> arg1 nav-flags) (nav-flags navf1))
    (let ((s5-1 (-> arg1 process nav extra-nav-sphere)))
      (when (< (-> arg0 num-spheres) (-> arg0 max-spheres))
        (let* ((s4-1 (-> arg0 sphere (-> arg0 num-spheres)))
               (f1-1 (vector-vector-xz-distance-squared s5-1 (-> arg0 shape trans)))
               (f0-4 (+ (-> s5-1 w) (-> arg0 shape nav-radius)))
               (f2-7 (+ 40960.0 f0-4)))
          (when (< f1-1 (* f2-7 f2-7))
            (vector-! (the-as vector s4-1) s5-1 (-> arg0 mesh origin))
            (set! (-> s4-1 w) f0-4)
            (+! (-> arg0 num-spheres) 1)))))
    0)
  (none))

(defmethod nav-control-method-28 ((this nav-control) (arg0 collide-kind))
  (local-vars
    (sv-32 nav-control)
    (sv-48 sphere)
    (sv-64 sphere)
    (sv-80 nav-control)
    (sv-96 sphere)
    (sv-112 vector)
    (sv-128 sphere))
  (set! (-> this num-spheres) 0)
  (let ((s4-0 (-> this mesh user-list))
        (s3-0 (new 'stack-no-clear 'vector)))
    (when (and *target*
               (or (logtest? (-> this flags) (nav-control-flags navcf11))
                   (logtest? (-> *target* state-flags)
                             (state-flags being-attacked invulnerable timed-invulnerable invuln-powerup do-not-notice dying))))
      (let ((s2-0 this)
            (s1-0 (-> *target* control)))
        (let ((s0-0 s3-0))
          (when (logtest? (-> s1-0 nav-flags) (nav-flags navf0))
            (set! (-> s0-0 quad) (-> s1-0 root-prim prim-core world-sphere quad))
            (set! (-> s0-0 w) (-> s1-0 nav-radius))
            (set! sv-32 s2-0)
            (when (< (-> sv-32 num-spheres) (-> sv-32 max-spheres))
              (set! sv-48 (-> sv-32 sphere (-> sv-32 num-spheres)))
              (let* ((f1-0 (vector-vector-xz-distance-squared s0-0 (-> sv-32 shape trans)))
                     (f0-2 (+ (-> s0-0 w) (-> sv-32 shape nav-radius)))
                     (f2-2 (+ 40960.0 f0-2)))
                (when (< f1-0 (* f2-2 f2-2))
                  (vector-! (the-as vector sv-48) s0-0 (-> sv-32 mesh origin))
                  (set! (-> sv-48 w) f0-2)
                  (+! (-> sv-32 num-spheres) 1))))
            0))
        (when (logtest? (-> s1-0 nav-flags) (nav-flags navf1))
          (let ((s1-1 (-> s1-0 process nav extra-nav-sphere)))
            (when (< (-> s2-0 num-spheres) (-> s2-0 max-spheres))
              (let* ((s0-1 (-> s2-0 sphere (-> s2-0 num-spheres)))
                     (f1-1 (vector-vector-xz-distance-squared s1-1 (-> s2-0 shape trans)))
                     (f0-4 (+ (-> s1-1 w) (-> s2-0 shape nav-radius)))
                     (f2-7 (+ 40960.0 f0-4)))
                (when (< f1-1 (* f2-7 f2-7))
                  (vector-! (the-as vector s0-1) s1-1 (-> s2-0 mesh origin))
                  (set! (-> s0-1 w) f0-4)
                  (+! (-> s2-0 num-spheres) 1)))))
          0)))
    (when (logtest? (-> this flags) (nav-control-flags navcf13))
      (countdown (s2-1 (-> this mesh static-sphere-count))
        (let ((s1-2 this)
              (s0-2 (-> this mesh static-sphere s2-1)))
          (when (< (-> s1-2 num-spheres) (-> s1-2 max-spheres))
            (set! sv-64 (-> s1-2 sphere (-> s1-2 num-spheres)))
            (let* ((f1-2 (vector-vector-xz-distance-squared (the-as vector s0-2) (-> s1-2 shape trans)))
                   (f0-6 (+ (-> s0-2 trans w) (-> s1-2 shape nav-radius)))
                   (f2-12 (+ 40960.0 f0-6)))
              (when (< f1-2 (* f2-12 f2-12))
                (vector-! (the-as vector sv-64) (the-as vector s0-2) (-> s1-2 mesh origin))
                (set! (-> sv-64 w) f0-6)
                (+! (-> s1-2 num-spheres) 1)))))
        0))
    (let* ((v1-71 (-> s4-0 alive-list next0))
           (s2-2 (-> v1-71 next0)))
      (while (!= v1-71 (-> s4-0 alive-list-end))
        (let ((s0-3 (the-as collide-shape (-> (the-as connection v1-71) param3))))
          (when (not (or (= s0-3 (-> this shape)) (not (logtest? arg0 (-> s0-3 root-prim prim-core collide-as)))))
            (let ((s1-3 this))
              (set! sv-112 s3-0)
              (when (logtest? (-> s0-3 nav-flags) (nav-flags navf0))
                (set! (-> sv-112 quad) (-> s0-3 root-prim prim-core world-sphere quad))
                (set! (-> sv-112 w) (-> s0-3 nav-radius))
                (set! sv-80 s1-3)
                (when (< (-> sv-80 num-spheres) (-> sv-80 max-spheres))
                  (set! sv-96 (-> sv-80 sphere (-> sv-80 num-spheres)))
                  (let* ((f1-3 (vector-vector-xz-distance-squared sv-112 (-> sv-80 shape trans)))
                         (f0-9 (+ (-> sv-112 w) (-> sv-80 shape nav-radius)))
                         (f2-17 (+ 40960.0 f0-9)))
                    ;; HACK: added a NaN check here. it seems like some enemies have bogus positions (maybe on their first frame?)
                    ;; and in these cases we shouldn't bother including them in the mesh.
                    (when (and (< f1-3 (* f2-17 f2-17)) (not (is-nan? (-> sv-112 x))))
                      (let ((v1-94 sv-96)
                            (a0-47 (-> sv-80 mesh origin)))
                        (vector-! (the-as vector v1-94) sv-112 a0-47))
                      (set! (-> sv-96 w) f0-9)
                      (+! (-> sv-80 num-spheres) 1))))
                0)
              (when (logtest? (-> s0-3 nav-flags) (nav-flags navf1))
                (let ((s0-4 (-> s0-3 process nav extra-nav-sphere)))
                  (when (< (-> s1-3 num-spheres) (-> s1-3 max-spheres))
                    (set! sv-128 (-> s1-3 sphere (-> s1-3 num-spheres)))
                    (let* ((f1-4 (vector-vector-xz-distance-squared s0-4 (-> s1-3 shape trans)))
                           (f0-11 (+ (-> s0-4 w) (-> s1-3 shape nav-radius)))
                           (f2-22 (+ 40960.0 f0-11)))
                      (when (< f1-4 (* f2-22 f2-22))
                        (vector-! (the-as vector sv-128) s0-4 (-> s1-3 mesh origin))
                        (set! (-> sv-128 w) f0-11)
                        (+! (-> s1-3 num-spheres) 1)))))
                0))))
        (set! v1-71 s2-2)
        (set! s2-2 (-> s2-2 next0)))))
  0
  (none))

(defmethod nav-control-method-35 ((this nav-control) (arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 float))
  (let ((v1-0 (new 'stack-no-clear 'vector)))
    (vector-! v1-0 arg1 (-> this mesh origin))
    (nav-control-method-32 this arg0 v1-0 arg2 arg3 arg4))
  (none))

(deftype cfs-travel-vec (structure)
  ((dir         vector :inline)
   (delta-angle float)))

(deftype cfs-work (structure)
  ((desired-travel-dist float)
   (desired-angle       float)
   (max-dist            float)
   (old-angle           float)
   (modified            int32)
   (blocked-mask        uint64)
   (travel              vector :inline)
   (current             vector :inline)
   (new-travel          cfs-travel-vec 2 :inline)
   (temp-travel         cfs-travel-vec 2 :inline)
   (prev-dir            vector :inline)
   (attempt-dir         vector :inline)
   (tangent             vector 2 :inline)))

(deftype nav-control-cfs-work (structure)
  ((in-dir              vector :inline)
   (right-dir           vector :inline)
   (best-dir            vector 2 :inline)
   (temp-dir            vector 2 :inline)
   (away-dir            vector :inline)
   (best-dir-angle      degrees 2)
   (ignore-mask         uint64)
   (initial-ignore-mask uint64)
   (i-sphere            int32)
   (i-first-sphere      int32)
   (i-inside-sphere     int32)
   (inside-sphere-dist  float)
   (sign                float)
   (travel-len          float)
   (dist2               float)
   (inside-dist         float)
   (rand-angle          float)
   (dir-update          basic)
   (debug-offset        vector :inline)))

(defun circle-tangent-directions ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (let ((s2-0 (new 'stack-no-clear 'vector))
        (s3-0 (new 'stack-no-clear 'vector))
        (s5-0 (new 'stack-no-clear 'vector)))
    (vector-! s2-0 arg1 arg0)
    (set! (-> s2-0 y) 0.0)
    (vector-normalize-copy! s3-0 s2-0 1.0)
    (set! (-> s5-0 quad) (-> s3-0 quad))
    (set! (-> s5-0 x) (-> s3-0 z))
    (set! (-> s5-0 z) (- (-> s3-0 x)))
    (let ((f0-4 (-> arg1 w))
          (f1-1 (vector-dot s3-0 s2-0)))
      (if (< f1-1 f0-4) (set! f0-4 f1-1))
      (let* ((f2-0 f1-1)
             (f2-2 (* f2-0 f2-0))
             (f3-0 f0-4)
             (f2-4 (sqrtf (- f2-2 (* f3-0 f3-0))))
             (f3-4 (/ 1.0 f1-1))
             (f1-3 (* f2-4 f3-4))
             (f30-0 (* f0-4 f3-4))
             (s2-1 (new 'stack-no-clear 'vector)))
        (vector-float*! s2-1 s3-0 f1-3)
        (vector+*! arg2 s2-1 s5-0 f30-0)
        (vector+*! arg3 s2-1 s5-0 (- f30-0))))))

(defun find-closest-circle-ray-intersection ((arg0 vector) (arg1 vector) (arg2 float) (arg3 int) (arg4 (inline-array vector)) (arg5 int))
  (let ((f30-0 1.0)
        (gp-0 -1))
    (let ((s1-0 (new 'stack-no-clear 'vector)))
      (vector-float*! s1-0 arg1 arg2)
      (dotimes (s0-0 arg3)
        (when (not (logtest? arg5 (ash 1 s0-0)))
          (let* ((v1-7 (-> arg4 s0-0))
                 (f0-2 (ray-circle-intersect arg0 s1-0 v1-7 (-> v1-7 w))))
            (when (and (>= f0-2 0.0) (< f0-2 f30-0))
              (set! f30-0 f0-2)
              (set! gp-0 s0-0))))))
    gp-0))

(defun sign-bit ((arg0 int))
  (local-vars (v1-1 int))
  (let ((v1-0 arg0)) (shift-arith-right-32 v1-1 v1-0 31))
  (logand v1-1 1))

(defun compute-dir-parm ((arg0 vector) (arg1 vector) (arg2 vector))
  (local-vars (a2-3 int))
  (let ((v1-0 (new 'static 'vector :x 1.0 :y -1.0)))
    (let ((a2-2 (vector-dot arg0 arg2))) (shift-arith-right-32 a2-3 a2-2 31))
    (* (-> v1-0 data (logand a2-3 1)) (- 1.0 (vector-dot arg0 arg1)))))

(defmethod nav-control-method-32 ((this nav-control) (arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 float))
  (local-vars (v0-3 symbol) (v1-38 int) (a0-29 int) (a3-7 int) (sv-208 sphere))
  (let ((gp-0 (new 'stack-no-clear 'nav-control-cfs-work)))
    (set! (-> gp-0 in-dir quad) (-> arg2 quad))
    (set! (-> gp-0 in-dir y) 0.0)
    (vector-normalize! (-> gp-0 in-dir) 1.0)
    (set! (-> gp-0 travel-len) (vector-dot (-> gp-0 in-dir) arg2))
    (set! (-> gp-0 right-dir quad) (-> gp-0 in-dir quad))
    (set! (-> gp-0 right-dir x) (- (-> gp-0 in-dir z)))
    (set! (-> gp-0 right-dir z) (-> gp-0 in-dir x))
    (set! (-> gp-0 best-dir 0 quad) (-> gp-0 in-dir quad))
    (set! (-> gp-0 best-dir 1 quad) (-> gp-0 in-dir quad))
    (set! (-> gp-0 best-dir-angle 0) 0.0)
    (set! (-> gp-0 best-dir-angle 1) 0.0)
    (set! (-> gp-0 initial-ignore-mask) (the-as uint 0))
    (set! (-> gp-0 i-inside-sphere) -1)
    (set! (-> gp-0 inside-sphere-dist) 0.0)
    (dotimes (s0-0 (-> this num-spheres))
      (set! sv-208 (-> this sphere s0-0))
      (set! (-> gp-0 dist2) (vector-vector-xz-distance-squared arg1 sv-208))
      (let ((f0-10 (-> gp-0 dist2))
            (f1-0 (-> sv-208 w)))
        (when (< f0-10 (* f1-0 f1-0))
          (vector-! (the-as vector (-> gp-0 temp-dir)) arg1 (the-as vector sv-208))
          (set! (-> gp-0 temp-dir 0 y) 0.0)
          (if (< 0.0 (vector-dot (the-as vector (-> gp-0 temp-dir)) (-> gp-0 in-dir)))
            (+! (-> gp-0 initial-ignore-mask) (ash 1 s0-0)))
          (set! (-> gp-0 inside-dist) (- (-> sv-208 w) (sqrtf (-> gp-0 dist2))))
          (when (< (-> gp-0 inside-sphere-dist) (-> gp-0 inside-dist))
            (set! (-> gp-0 i-inside-sphere) s0-0)
            (set! (-> gp-0 inside-sphere-dist) (-> gp-0 inside-dist))))))
    (set! (-> gp-0 i-first-sphere)
          (find-closest-circle-ray-intersection arg1
                                                (-> gp-0 in-dir)
                                                (-> gp-0 travel-len)
                                                (-> this num-spheres)
                                                (-> this sphere)
                                                (the-as int (-> gp-0 initial-ignore-mask))))
    (when (= (-> gp-0 i-first-sphere) -1)
      (set! (-> arg0 quad) (-> arg2 quad))
      (set! v0-3 #f)
      (b! #t cfg-46 :delay (nop!))
      (the-as none 0))
    (+! (-> gp-0 initial-ignore-mask) (ash 1 (-> gp-0 i-first-sphere)))
    (let ((a1-9 (-> this sphere (-> gp-0 i-first-sphere))))
      (circle-tangent-directions arg1 a1-9 (the-as vector (-> gp-0 temp-dir)) (-> gp-0 temp-dir 1)))
    (dotimes (v1-31 2)
      (let ((a0-28 (vector-dot (-> gp-0 right-dir) (-> gp-0 temp-dir v1-31)))) (shift-arith-right-32 a0-29 a0-28 31))
      (let ((a0-30 (logand a0-29 1))
            (f0-21 (- 1.0 (vector-dot (-> gp-0 in-dir) (-> gp-0 temp-dir v1-31)))))
        (set! (-> gp-0 best-dir a0-30 quad) (-> gp-0 temp-dir v1-31 quad))
        (set! (-> gp-0 best-dir-angle a0-30) f0-21)))
    0
    (set! (-> gp-0 sign) 1.0)
    (dotimes (s1-1 2)
      (-> gp-0 i-first-sphere)
      (set! (-> gp-0 dir-update) #t)
      (set! (-> gp-0 ignore-mask) (-> gp-0 initial-ignore-mask))
      (b! #t cfg-31 :delay (nop!))
      (label cfg-22)
      (+! (-> gp-0 ignore-mask) (ash 1 v1-38))
      (circle-tangent-directions arg1 (-> this sphere v1-38) (the-as vector (-> gp-0 temp-dir)) (-> gp-0 temp-dir 1))
      (set! (-> gp-0 dir-update) #f)
      (dotimes (v1-40 2)
        (let ((f0-23 (-> gp-0 sign))
              (a1-22 (-> gp-0 temp-dir v1-40))
              (a0-39 (-> gp-0 in-dir))
              (a3-4 (-> gp-0 right-dir))
              (a2-10 (new 'static 'vector :x 1.0 :y -1.0)))
          (let ((a3-6 (vector-dot a1-22 a3-4))) (shift-arith-right-32 a3-7 a3-6 31))
          (let ((f0-24 (* f0-23 (* (-> a2-10 data (logand a3-7 1)) (- 1.0 (vector-dot a1-22 a0-39))))))
            (when (< (-> gp-0 best-dir-angle s1-1) f0-24)
              (set! (-> gp-0 best-dir s1-1 quad) (-> gp-0 temp-dir v1-40 quad))
              (set! (-> gp-0 best-dir-angle s1-1) f0-24)
              (set! (-> gp-0 dir-update) #t)))))
      (label cfg-31)
      (when (-> gp-0 dir-update)
        (set! v1-38
              (find-closest-circle-ray-intersection arg1
                                                    (-> gp-0 best-dir s1-1)
                                                    (-> gp-0 travel-len)
                                                    (-> this num-spheres)
                                                    (-> this sphere)
                                                    (the-as int (-> gp-0 ignore-mask))))
        (b! (!= v1-38 -1) cfg-22 :delay (nop!)))
      (set! (-> gp-0 sign) (* -1.0 (-> gp-0 sign))))
    (when (!= (-> gp-0 i-inside-sphere) -1)
      (let ((s2-1 (-> this sphere (-> gp-0 i-inside-sphere))))
        (vector-! (-> gp-0 away-dir) arg1 (the-as vector s2-1))
        (set! (-> gp-0 away-dir y) 0.0)
        (when (>= 40.96 (vector-length (-> gp-0 away-dir)))
          (set! (-> gp-0 rand-angle) (* 65536.0 (rand-vu)))
          (set! (-> gp-0 away-dir x) (cos (-> gp-0 rand-angle)))
          (set! (-> gp-0 away-dir z) (sin (-> gp-0 rand-angle))))
        (vector-normalize! (-> gp-0 away-dir) 1.0)
        (let ((f30-2 (/ (-> gp-0 inside-sphere-dist) (-> s2-1 w))))
          (vector-lerp! (the-as vector (-> gp-0 best-dir)) (the-as vector (-> gp-0 best-dir)) (-> gp-0 away-dir) f30-2)
          (vector-lerp! (-> gp-0 best-dir 1) (-> gp-0 best-dir 1) (-> gp-0 away-dir) f30-2)))
      (vector-normalize! (the-as vector (-> gp-0 best-dir)) 1.0)
      (vector-normalize! (-> gp-0 best-dir 1) 1.0))
    (let ((f30-3 (vector-dot (-> gp-0 in-dir) (the-as vector (-> gp-0 best-dir))))
          (f28-0 (vector-dot (-> gp-0 in-dir) (-> gp-0 best-dir 1))))
      (when (< (fmax f30-3 f28-0) (cos 1820.4445))
        (set! f30-3 (vector-dot arg3 (the-as vector (-> gp-0 best-dir))))
        (set! f28-0 (vector-dot arg3 (-> gp-0 best-dir 1))))
      (vector-float*! arg0 (-> gp-0 best-dir (if (< f28-0 f30-3) 0 1)) (-> gp-0 travel-len))))
  0
  (set! v0-3 #t)
  (label cfg-46)
  v0-3)

(defmethod nav-control-method-23 ((this nav-control) (arg0 vector) (arg1 check-vector-collision-with-nav-spheres-info))
  (let ((s2-1 (vector-! (new 'stack-no-clear 'vector) (-> this shape trans) (-> this mesh origin)))
        (f30-0 -1.0))
    (let ((s3-0 -1))
      (countdown (s1-0 (-> this num-spheres))
        (let* ((s0-0 (-> this sphere s1-0))
               (f0-1 (ray-circle-intersect s2-1 arg0 (the-as vector (&-> s0-0 x)) (-> s0-0 w))))
          (when (>= f0-1 0.0)
            (let ((v1-5 (new 'stack-no-clear 'vector)))
              (vector-! v1-5 (the-as vector (&-> s0-0 x)) s2-1)
              (when (>= (vector-dot v1-5 arg0) 0.0)
                (when (or (< f30-0 0.0) (< f0-1 f30-0))
                  (set! f30-0 f0-1)
                  (set! s3-0 s1-0)))))))
      (when arg1
        (set! (-> arg1 u) f30-0)
        (when (>= f30-0 0.0)
          (vector+float*! (-> arg1 intersect) (-> this shape trans) arg0 f30-0)
          (let ((a1-4 (-> this sphere s3-0))
                (v1-19 (new 'stack-no-clear 'vector)))
            (vector+! v1-19 (the-as vector (&-> a1-4 x)) (-> this mesh origin))
            (vector-! (-> arg1 normal) (-> arg1 intersect) v1-19))
          (set! (-> arg1 normal w) 1.0)
          (vector-normalize! (-> arg1 normal) 1.0))))
    f30-0))

(defmethod nav-control-method-33 ((this nav-control) (arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 float))
  (let ((f30-0 (vector-xz-length (-> this travel))))
    (when (nav-control-method-32 this arg0 arg1 arg2 arg3 f30-0)
      (set! (-> this blocked-travel quad) (-> this travel quad))
      (let ((f0-0 (vector-xz-length (-> this travel))))
        (when (and (>= f30-0 f0-0) (< f0-0 204.8))
          (logior! (-> this flags) (nav-control-flags navcf17))
          (set-time! (-> this block-time))
          (+! (-> this block-count) 1.0)
          (if (-> this block-event) (send-event (the-as process-tree (-> this process)) (the-as symbol (-> this block-event)) this))))
      #t)))

(define *debug-ray-test* (the-as nav-ray #f))

(define *debug-ray-test-capture-mode* #f)

(define *debug-ray-test-capture-output* #f)

(define *test-ray-start-poly-id* 62)

(define *test-ray-src-pos* (new 'static 'vector :x -705089.6 :y 9769.325 :z -963339.0 :w 1.0))

(define *test-ray-dest-pos* (new 'static 'vector :x -722887.7 :y 9532.475 :z -958862.25 :w 1.0))

(defmethod nav-control-method-19 ((this nav-control) (arg0 vector) (arg1 collide-shape-moving) (arg2 vector) (arg3 float))
  (local-vars (sv-48 float))
  (let ((f30-0 (* arg3 (seconds-per-frame)))
        (s0-0 arg1)
        (s1-0 arg2)
        (s2-0 deg-diff))
    (set! sv-48 (y-angle s0-0))
    (let* ((a1-1 (vector-y-angle (vector-! (new 'stack-no-clear 'vector) s1-0 (-> s0-0 trans))))
           (f28-0 (s2-0 sv-48 a1-1)))
      (cond
        ((or (vector= arg2 (-> this target-pos)) (< (fabs f28-0) 364.0889))
         (logior! (-> this flags) (nav-control-flags navcf21))
         (set! (-> arg0 quad) (-> arg2 quad)))
        (else
         (let ((s2-1 (vector-z-quaternion! (new 'stack-no-clear 'vector) (-> arg1 quat))))
           (vector-rotate-y! s2-1 s2-1 (fmax (fmin f28-0 f30-0) (- f30-0)))
           (vector-normalize! s2-1 819.2)
           (logclear! (-> this flags) (nav-control-flags navcf21))
           (vector+! arg0 (-> arg1 trans) s2-1))
         (when (or (not (nav-control-method-16 this arg0))
                   (logtest? (nav-control-flags navcf17) (-> this flags))
                   (not (logtest? (-> this flags) (nav-control-flags navcf10))))
           (logior! (-> this flags) (nav-control-flags navcf21))
           (vector-! (-> this travel) arg2 (-> arg1 trans))
           (set! (-> arg0 quad) (-> arg2 quad)))))))
  (none))

(defmethod nav-control-method-16 ((this nav-control) (arg0 vector))
  (find-poly-fast (-> this mesh)
                  (vector-! (new 'stack-no-clear 'vector) arg0 (-> this mesh origin))
                  (-> this nearest-y-threshold)))

(defmethod nav-control-method-21 ((this nav-control) (arg0 vector))
  (find-poly-fast (-> this mesh)
                  (vector-! (new 'stack-no-clear 'vector) arg0 (-> this mesh origin))
                  (-> this nearest-y-threshold)))

(defmethod nav-control-method-22 ((this nav-control) (arg0 vector) (arg1 float))
  (the-as symbol
          (and (find-poly-fast (-> this mesh)
                               (vector-! (new 'stack-no-clear 'vector) arg0 (-> this mesh origin))
                               (-> this nearest-y-threshold))
               (< (-> arg0 y) (+ (-> this mesh origin y) arg1)))))

(defun debug-nav-validate-current-poly ((arg0 nav-mesh) (arg1 nav-poly) (arg2 vector))
  (when (not (point-in-poly? arg0 arg1 arg2))
    (let ((s3-0 (new 'stack-no-clear 'vector)))
      (project-point-into-tri-2d arg0 arg1 s3-0 arg2)
      (vector-vector-xz-distance arg2 s3-0))
    #f))

(defmethod nav-control-method-26 ((this nav-control))
  0
  (none))

(defmethod nav-control-method-27 ((this nav-control))
  (local-vars (v1-7 symbol))
  (cond
    ((-> this current-poly)
     (let ((s4-0 (-> this mesh))
           (s5-0 (new 'stack-no-clear 'nav-ray)))
       (let ((s3-0 0))
         (set! (-> s5-0 current-poly) (-> this current-poly))
         (vector-! (-> s5-0 current-pos) (-> this prev-pos) (-> s4-0 origin))
         (vector-! (-> s5-0 dest-pos) (-> this shape trans) (-> s4-0 origin))
         (init-ray s5-0)
         (until v1-7
           (+! s3-0 1)
           (move-along-nav-ray! s4-0 s5-0)
           (set! v1-7 (or (>= s3-0 15) (-> s5-0 terminated)))))
       (set! (-> this current-poly) (-> s5-0 current-poly))
       (when (-> s5-0 reached-dest)
         (if (not (point-in-poly? s4-0 (-> s5-0 current-poly) (-> s5-0 dest-pos))) (set! (-> s5-0 reached-dest) #f)))
       (cond
         ((-> s5-0 reached-dest) (set! (-> this prev-pos quad) (-> this shape trans quad)))
         (else
          (set! (-> this prev-pos quad) (-> this shape trans quad))
          (set! (-> this current-poly) (find-poly this (-> this shape trans))))))
     0)
    (else
     (set! (-> this prev-pos quad) (-> this shape trans quad))
     (set! (-> this current-poly) (find-poly this (-> this shape trans)))))
  (nav-control-method-26 this)
  0
  (none))

(defmethod nav-mesh-method-16 ((this nav-mesh) (arg0 vector) (arg1 nav-poly) (arg2 vector) (arg3 symbol) (arg4 float) (arg5 clip-travel-vector-to-mesh-return-info))
  (local-vars (v1-10 symbol) (sv-96 symbol) (sv-112 int) (sv-128 int))
  (when arg5
    (set! (-> arg5 found-boundary) #f)
    (set! (-> arg5 gap-poly) #f))
  (let ((s0-0 (new 'stack-no-clear 'nav-ray)))
    (set! sv-96 (the-as symbol #f))
    (set! sv-112 0)
    (set! (-> s0-0 current-poly) arg1)
    (set! (-> s0-0 current-pos quad) (-> arg0 quad))
    (vector+! (-> s0-0 dest-pos) arg0 arg2)
    (until sv-96
      (set! sv-128 0)
      (init-ray s0-0)
      (until v1-10
        (set! sv-128 (+ sv-128 1))
        (move-along-nav-ray! this s0-0)
        (set! v1-10 (or (>= sv-128 15) (or (>= (-> s0-0 len) (fmax 20480.0 arg4)) (-> s0-0 terminated)))))
      (cond
        ((and (-> s0-0 hit-boundary) (and (< (-> s0-0 len) arg4) (!= (-> s0-0 last-edge) -1) (< sv-112 1)))
         (set! sv-112 (+ sv-112 1))
         (let* ((v1-22 (-> this vertex (-> s0-0 current-poly vertex (-> *edge-vert0-table* (-> s0-0 last-edge)))))
                (a0-15 (-> this vertex (-> s0-0 current-poly vertex (-> *edge-vert1-table* (-> s0-0 last-edge)))))
                (f2-1 (- (-> v1-22 z) (-> a0-15 z)))
                (f3-0 (- (-> a0-15 x) (-> v1-22 x)))
                (f1-5 (-> arg2 x))
                (f0-4 (-> arg2 z))
                (f4-0 f2-1)
                (f4-2 (* f4-0 f4-0))
                (f5-0 f3-0)
                (f4-4 (sqrtf (+ f4-2 (* f5-0 f5-0))))
                (f4-6 (/ 1.0 f4-4))
                (f2-2 (* f2-1 f4-6))
                (f3-1 (* f3-0 f4-6)))
           (when arg5
             (set! (-> arg5 found-boundary) #t)
             (vector+! (-> arg5 intersection) (-> s0-0 current-pos) (-> this origin))
             (set! (-> arg5 boundary-normal x) f2-2)
             (set! (-> arg5 boundary-normal y) 0.0)
             (set! (-> arg5 boundary-normal z) f3-1)
             (set! (-> arg5 poly) (-> s0-0 current-poly))
             (set! (-> arg5 edge) (-> s0-0 last-edge))
             (vector+! (-> arg5 vert-0) (the-as vector v1-22) (-> this origin))
             (vector+! (-> arg5 vert-1) (the-as vector a0-15) (-> this origin)))
           (set! (-> s0-0 dest-pos quad) (-> s0-0 current-pos quad))
           (cond
             (arg3
              (let* ((f4-9 (* 1.01 (+ (* f2-2 f1-5) (* f3-1 f0-4))))
                     (f1-6 (- f1-5 (* f2-2 f4-9)))
                     (f0-5 (- f0-4 (* f3-1 f4-9))))
                (+! (-> s0-0 dest-pos x) f1-6)
                (+! (-> s0-0 dest-pos z) f0-5)))
             (else (set! sv-96 #t) sv-96))))
        ((-> s0-0 hit-gap) (if arg5 (set! (-> arg5 gap-poly) (-> s0-0 next-poly))) (set! sv-96 #t) sv-96)
        (else (set! sv-96 #t) sv-96)))
    (vector-! arg2 (-> s0-0 current-pos) arg0))
  0
  0
  (none))

(defmethod nav-control-method-24 ((this nav-control) (arg0 float) (arg1 clip-travel-vector-to-mesh-return-info))
  (let ((v1-0 (new 'stack-no-clear 'vector)))
    (vector-! v1-0 (-> this shape trans) (-> this mesh origin))
    (nav-mesh-method-16 (-> this mesh)
                        v1-0
                        (-> this current-poly)
                        (-> this travel)
                        (not (logtest? (-> this flags) (nav-control-flags navcf12)))
                        arg0
                        arg1))
  (when arg1
    (if (-> arg1 gap-poly) (set! (-> this next-poly) (-> arg1 gap-poly))))
  0
  (none))

(defun test-xz-point-on-line-segment? ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  (let ((v0-2 (>= arg3 (fmin (vector-vector-xz-distance arg0 arg1) (vector-vector-xz-distance arg0 arg2)))))
    (when (not v0-2)
      (let* ((f0-3 (- (-> arg2 x) (-> arg1 x)))
             (f1-2 (- (-> arg2 z) (-> arg1 z)))
             (f2-1 f0-3)
             (f2-3 (* f2-1 f2-1))
             (f3-0 f1-2)
             (f2-5 (sqrtf (+ f2-3 (* f3-0 f3-0))))
             (f3-3 f2-5)
             (f3-5 (/ 1.0 f3-3))
             (f4-2 (* f3-5 (- f1-2)))
             (f5-0 (* f3-5 f0-3))
             (f3-7 (- (-> arg0 x) (-> arg1 x)))
             (f6-2 (- (-> arg0 z) (-> arg1 z))))
        (when (>= arg3 (fabs (+ (* f3-7 f4-2) (* f6-2 f5-0))))
          (let ((f0-5 (+ (* f3-7 f0-3) (* f6-2 f1-2)))) (set! v0-2 (and (>= f0-5 0.0) (>= (* f2-5 f2-5) f0-5)))))))
    v0-2))

(defun choose-travel-portal-vertex ((arg0 nav-mesh) (arg1 nav-route-portal) (arg2 nav-poly) (arg3 vector))
  (let ((f0-1 (- (-> arg1 vertex 1 x) (-> arg1 vertex 0 x)))
        (f1-2 (- (-> arg1 vertex 1 z) (-> arg1 vertex 0 z))))
    0.0
    (let ((s2-0 (new 'stack-no-clear 'nav-route-portal))
          (s1-0 0))
      (let* ((s0-0 #f)
             (f2-2 f0-1)
             (f2-4 (* f2-2 f2-2))
             (f3-0 f1-2)
             (f2-6 (sqrtf (+ f2-4 (* f3-0 f3-0))))
             (f2-8 (/ 1.0 f2-6))
             (f30-0 (* f0-1 f2-8))
             (f28-0 (* f1-2 f2-8))
             (f26-0 (* -0.5
                       (+ (* f30-0 (-> arg1 vertex 0 x))
                          (* f28-0 (-> arg1 vertex 0 z))
                          (+ (* f30-0 (-> arg1 vertex 1 x)) (* f28-0 (-> arg1 vertex 1 z)))))))
        (set! (-> s2-0 vertex 0) (-> arg1 vertex 0))
        (set! (-> s2-0 vertex 1) (-> arg1 vertex 1))
        (set! (-> s2-0 next-poly) (-> arg1 next-poly))
        (until s0-0
          (setup-portal arg0 (-> s2-0 next-poly) arg2 s2-0)
          (cond
            ((-> s2-0 next-poly)
             (let ((v1-18 0))
               (dotimes (a0-2 2)
                 (if (< 0.0 (+ (* f30-0 (-> s2-0 vertex a0-2 x)) (* f28-0 (-> s2-0 vertex a0-2 z)) f26-0)) (+! v1-18 1)))
               (when (= v1-18 2)
                 (set! s1-0 1)
                 (set! s0-0 #t))
               (when (zero? v1-18)
                 (set! s1-0 0)
                 (set! s0-0 #t)))
             0)
            (else
             (let ((v1-22 (if (< (vector-vector-xz-distance (-> s2-0 vertex 0) arg3) (vector-vector-xz-distance (-> s2-0 vertex 1) arg3)) 0 1)))
               (set! s1-0 (if (< 0.0 (+ (* f30-0 (-> arg1 vertex v1-22 x)) (* f28-0 (-> arg1 vertex v1-22 z)) f26-0)) 1 0)))
             (set! s0-0 #t)
             0))))
      s1-0)))

(defmethod nav-control-method-13 ((this nav-control) (arg0 vector) (arg1 vector))
  (local-vars (sv-80 vector) (sv-84 vector) (sv-88 nav-poly) (sv-92 vector) (sv-96 nav-route-portal) (sv-100 symbol))
  (set! sv-80 (new 'stack-no-clear 'vector))
  (set! sv-84 (new 'stack-no-clear 'vector))
  (set! sv-88 (-> this current-poly))
  (set! sv-92 (new 'stack-no-clear 'vector))
  (set! sv-96 (new 'stack-no-clear 'nav-route-portal))
  (set! sv-100 (the-as symbol #f))
  (vector-! sv-80 (-> this shape trans) (-> this mesh origin))
  (set! (-> sv-84 quad) (-> sv-80 quad))
  (let ((s4-0 (new 'stack-no-clear 'vector)))
    (vector-! s4-0 arg0 (-> this mesh origin))
    (set! (-> this target-poly) (find-poly (-> this mesh) s4-0 (-> this nearest-y-threshold) (&-> this flags)))
    (if (not (-> this target-poly)) (set! (-> this target-poly) (-> this current-poly)))
    (project-point-into-tri-2d (-> this mesh) (-> this target-poly) sv-92 s4-0))
  (vector-! (-> this travel) sv-92 sv-80)
  (setup-portal (-> this mesh) (-> this current-poly) (-> this target-poly) sv-96)
  (cond
    ((not (-> sv-96 next-poly)) (set! (-> this next-poly) #f) (set! (-> this portal 0) #f) (set! (-> this portal 1) #f))
    (else
     (set! (-> this next-poly) (-> sv-96 next-poly))
     (set! (-> this portal 0) (the-as nav-route-portal (-> sv-96 vertex 0)))
     (set! (-> this portal 1) (the-as nav-route-portal (-> sv-96 vertex 1)))
     (set! sv-100 #t)
     (while (and sv-100
                 (-> sv-96 next-poly)
                 (test-xz-point-on-line-segment? sv-80 (-> sv-96 vertex 0) (-> sv-96 vertex 1) 409.59998))
       (when #t
         #t
         (vector-segment-distance-point! sv-80 (-> sv-96 vertex 0) (-> sv-96 vertex 1) sv-84)
         (vector-! (-> this travel) sv-92 sv-84)
         0)
       (cond
         ((logtest? (-> sv-96 next-poly pat) 1)
          (vector-segment-distance-point! sv-80 (-> sv-96 vertex 0) (-> sv-96 vertex 1) sv-92)
          (vector-! (-> this travel) sv-92 sv-84)
          (set! (-> this next-poly) (-> sv-96 next-poly))
          (set! sv-100 (the-as symbol #f)))
         ((begin
            (set! sv-88 (-> sv-96 next-poly))
            (setup-portal (-> this mesh) (-> sv-96 next-poly) (-> this target-poly) sv-96))
          (set! (-> this next-poly) (-> sv-96 next-poly))
          (set! (-> this portal 0) (the-as nav-route-portal (-> sv-96 vertex 0)))
          (set! (-> this portal 1) (the-as nav-route-portal (-> sv-96 vertex 1)))
          0)
         (else
          (set! (-> this next-poly) #f)
          (set! (-> this portal 0) #f)
          (set! (-> this portal 1) #f)
          (set! sv-100 (the-as symbol #f)))))))
  (when sv-100
    (when (not (ray-ccw-line-segment-intersection? sv-84
                                                   (-> this travel)
                                                   (the-as vector (-> this portal 0))
                                                   (the-as vector (-> this portal 1))))
      (let ((a0-18 (choose-travel-portal-vertex (-> this mesh) sv-96 (-> this target-poly) sv-92)))
        (vector-! (-> this travel) (the-as vector (-> sv-96 vertex a0-18)) sv-84))))
  (nav-control-method-33 this (-> this travel) sv-84 (-> this travel) arg1 40960.0)
  (let ((s5-1 (new 'stack-no-clear 'clip-travel-vector-to-mesh-return-info)))
    (nav-mesh-method-16 (-> this mesh)
                        sv-84
                        sv-88
                        (-> this travel)
                        (not (logtest? (-> this flags) (nav-control-flags navcf12)))
                        204.8
                        s5-1)
    (if (-> s5-1 gap-poly) (set! (-> this next-poly) (-> s5-1 gap-poly))))
  (let ((v1-82 (new 'stack-no-clear 'vector)))
    (vector-! v1-82 sv-84 sv-80)
    (vector+! (-> this travel) (-> this travel) v1-82))
  (set! (-> this travel y) 0.0)
  (-> this travel))

(defmethod nav-control-method-12 ((this nav-control) (arg0 nav-gap-info))
  (when (and (-> this next-poly) (logtest? (-> this next-poly pat) 1))
    (let ((s4-0 (-> this next-poly))
          (s3-1 (vector-! (new 'stack-no-clear 'vector) (-> this shape trans) (-> this mesh origin))))
      (while (and s4-0 (logtest? (-> s4-0 pat) 1))
        (set! s4-0 (get-adj-poly (-> this mesh) s4-0 (-> this target-poly) #f)))
      (when (and s4-0 (-> this gap-event))
        (let ((s2-0 (-> this event-temp)))
          (closest-point-on-boundary (-> this mesh) s4-0 s2-0 s3-1)
          (vector+! s2-0 s2-0 (-> this mesh origin))
          (set! (-> arg0 dest quad) (-> s2-0 quad)))
        (set! (-> arg0 poly) s4-0)
        #t))))

(defmethod nav-control-method-11 ((this nav-control) (arg0 vector))
  (set! (-> this old-travel quad) (-> this travel quad))
  (-> this block-count)
  (seek! (-> this block-count) 0.0 0.016666668)
  (logclear! (-> this flags) (nav-control-flags navcf9 navcf17 navcf18 navcf19))
  (nav-control-method-27 this)
  (if (logtest? (-> this flags) (nav-control-flags navcf8)) (nav-control-method-28 this (the-as collide-kind -1)))
  (nav-control-method-13 this arg0 (-> this old-travel))
  (-> this mesh)
  (vector-! (new 'stack-no-clear 'vector) (-> this shape trans) (-> this mesh origin))
  (let ((s5-1 (new 'stack-no-clear 'nav-gap-info)))
    (when (< (vector-xz-length (-> this travel)) 204.8)
      (cond
        ((logtest? (nav-control-flags navcf17) (-> this flags)))
        ((-> this next-poly)
         (cond
           ((nav-control-method-12 this s5-1)
            (set! (-> this next-poly) #f)
            (send-event (the-as process-tree (-> this process)) (the-as symbol (-> this gap-event)) (-> s5-1 dest) (-> s5-1 poly)))
           (else)))
        (else (logior! (-> this flags) (nav-control-flags navcf19))))))
  (-> this travel))

(defun-debug nav-sphere-from-cam ()
  (let ((v1-0 (camera-pos))) (format #t "SPHEREM(~4,,1M, ~4,,1M, ~4,,1M, 1.0)~%" (-> v1-0 x) (-> v1-0 y) (-> v1-0 z)))
  0
  (none))
