jak-project/goal_src/engine/geometry/geometry.gc

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

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

;; Geometry functions are common vector/plane utilities + the "curve" stuff
;; The curves are likely bezier splines, but not 100% sure yet.

(defun vector-flatten! ((dst vector) (src vector) (plane-normal vector))
  "Get the projection of src onto a plane with the given normal
   The normal should have magnitude 1.0."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf) ;; src
         (vf2 :class vf) ;; normal
         (vf3 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> src quad))
    (.lvf vf2 (&-> plane-normal quad))
    (.mov.vf vf3 vf0 :mask #b1000)
    (.outer.product.vf vf3 vf1 vf2) ;; has the right magnitude, but rotation is off by 90 degrees
    (.outer.product.vf vf3 vf2 vf3) ;; rotate by 90 about normal of plane
    (.svf (&-> dst quad) vf3)
    dst
    )
  )

(defun vector-reflect! ((dst vector) (src vector) (plane-normal vector))
  "Reflect a vector off of a plane."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    ;; we want to split the vector into normal / tangent components.
    ;; let src = T + N, where T dot plane-normal = 0.
    ;; then the reflection is T - N = 2 * T - src.
    ;; we can compute T from vector-flatten!'s trick
    (init-vf0-vector)
    (.lvf vf1 (&-> src quad))
    (.lvf vf2 (&-> plane-normal quad))
    (.mov.vf vf3 vf0 :mask #b1000)
    (.outer.product.vf vf3 vf1 vf2)
    (.outer.product.vf vf3 vf2 vf3) ;; vf3 is the projection on the plane
    (.add.vf acc vf3 vf3 :mask #b111) ;; double that part
    (.sub.mul.w.vf vf3 vf1 vf0 acc :mask #b111) ;; and subtract the original
    (.svf (&-> dst quad) vf3)
    dst
    )
  )

(defun vector-reflect-flat! ((dst vector) (src vector) (plane-normal vector))
  "This is a weird one. It doesn't care about the value of src dot normal
   and it effectively replaces the component of src normal to the plane with
   the plane's normal.  I think this requires src/normal to both be unit vectors
   in order to make sense.
   NOTE: src should point from positive halfspace to negative otherwise it
   doesn't work."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> src quad))
    (.lvf vf2 (&-> plane-normal quad))
    (.mov.vf vf3 vf0 :mask #b1000)
    (.outer.product.vf vf3 vf1 vf2)
    (.outer.product.vf vf3 vf2 vf3) ;; part on the plane (requires normal to be unit)
    (.add.vf vf3 vf3 vf2 :mask #b111) ;; add normal to that.
    (.svf (&-> dst quad) vf3)
    dst
    )
  )

(defun vector-reflect-true-flat! ((dst vector) (src vector) (plane-normal vector))
  "Not really a reflect. Same as flatten"
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> src quad))
    (.lvf vf2 (&-> plane-normal quad))
    (.mov.vf vf3 vf0 :mask #b1000)
    (.outer.product.vf vf3 vf1 vf2)
    (.outer.product.vf vf3 vf2 vf3)
    (.svf (&-> dst quad) vf3)
    dst
    )
  )

(defun vector-reflect-flat-above! ((dst vector) (src vector) (plane-normal vector))
  "A hacked up version of reflect, probably to make their collision system work.
   It is a less aggressive version of reflect that also has a limit to the output
   normal component"
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> src quad))
    (.lvf vf2 (&-> plane-normal quad))
    (.mov.vf vf3 vf0 :mask #b1000)
    (.outer.product.vf vf3 vf1 vf2)
    (.outer.product.vf vf3 vf2 vf3)
    (.svf (&-> dst quad) vf3)

    ;; dst is now the normal part of src
    (let* ((f0-0 (vector-length dst)) ;; len of normal component
           (f1-1 (vector-dot dst plane-normal)) ;; always zero?
           ;; f1-3 = .02 * length of normal.  f1-2 is always zero here
           (f1-2 (- (* 0.02 f0-0) f1-1))
           )

      ;; scale down and limit the normal component
      (vector+float*! dst dst plane-normal (fmin 16384.0 (* 16.0 f1-2)))
      )
    )
  )

(defun vector-segment-distance-point! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (local-vars (v0-0 float) (v1-0 float) (v1-1 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         (vf8 :class vf)
         )
    (init-vf0-vector)
    (nop!)
    (.lvf vf3 (&-> arg1 quad))
    (.lvf vf4 (&-> arg2 quad))
    (.lvf vf5 (&-> arg0 quad))
    (.sub.vf vf1 vf4 vf3)
    (.sub.vf vf6 vf5 vf3)
    (.mul.vf vf2 vf1 vf1)
    (.mul.x.vf acc vf0 vf2 :mask #b1000)
    (.add.mul.y.vf acc vf0 vf2 acc :mask #b1000)
    (.add.mul.z.vf vf2 vf0 vf2 acc :mask #b1000)
    (.sqrt.vf Q vf2 :ftf #b11)
    (.wait.vf)
    (.add.vf vf2 vf0 Q :mask #b1)
    (.nop.vf)
    (.nop.vf)
    (.div.vf Q vf0 vf2 :fsf #b11 :ftf #b0)
    (.mov v1-0 vf2)
    (let ((f2-0 v1-0))
      (.wait.vf)
      (.mul.vf vf1 vf1 Q)
      (.mul.vf vf7 vf1 vf6)
      (let ((f1-0 0.0))
        (.add.y.vf vf7 vf7 vf7 :mask #b1)
        (.add.z.vf vf7 vf7 vf7 :mask #b1)
        (.mov v1-1 vf7)
        (let ((f0-0 v1-1))
          (b! (< f0-0 f1-0) cfg-4 :likely-delay (set! f0-0 f1-0))
          (b! (< f2-0 f0-0) cfg-4 :likely-delay (set! f0-0 f2-0))
          (label cfg-4)
          (let ((v1-2 f0-0))
            (.mov vf7 v1-2)
            )
          )
        )
      )
    (.mul.x.vf vf1 vf1 vf7)
    (b! (= arg3 #f) cfg-6 :delay (.mov.vf vf8 vf0 :mask #b1000))
    (.add.vf vf8 vf3 vf1 :mask #b111)
    (.svf (&-> arg3 quad) vf8)
    (label cfg-6)
    (.sub.vf vf2 vf6 vf1)
    (.mul.vf vf2 vf2 vf2)
    (.mul.x.vf acc vf0 vf2 :mask #b1000)
    (.add.mul.y.vf acc vf0 vf2 acc :mask #b1000)
    (.add.mul.z.vf vf2 vf0 vf2 acc :mask #b1000)
    (.sqrt.vf Q vf2 :ftf #b11)
    (.wait.vf)
    (.add.vf vf2 vf0 Q :mask #b1)
    (.nop.vf)
    (.mov v0-0 vf2)
    v0-0
    )
  )

(defun vector-line-distance ((arg0 vector) (arg1 vector) (arg2 vector))
  (let* ((a1-3 (vector-normalize! (vector-! (new-stack-vector0) arg2 arg1) 1.0))
         (gp-1 (vector-! (new-stack-vector0) arg0 arg1))
         (f0-1 (vector-dot a1-3 gp-1))
         (v1-3 (vector-float*! (new-stack-vector0) a1-3 f0-1))
         )
    (vector-length (vector-! (new-stack-vector0) gp-1 v1-3))
    )
  )

(defun vector-line-distance-point! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (let* ((a1-3 (vector-normalize! (vector-! (new-stack-vector0) arg2 arg1) 1.0))
         (s4-1 (vector-! (new-stack-vector0) arg0 arg1))
         (f0-1 (vector-dot a1-3 s4-1))
         (v1-4 (vector-float*! (new-stack-vector0) a1-3 f0-1))
         )
    (if arg3
        (vector+! arg3 arg1 v1-4)
        )
    (vector-length (vector-! (new-stack-vector0) s4-1 v1-4))
    )
  )

(defun vector-orient-by-quat! ((arg0 vector) (arg1 vector) (arg2 quaternion))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg2 vec quad))
    (.lvf vf6 (&-> arg1 quad))
    (.add.vf vf5 vf1 vf1)
    (.add.w.vf vf2 vf0 vf1 :mask #b1)
    (.add.z.vf vf2 vf0 vf1 :mask #b10)
    (.sub.y.vf vf2 vf0 vf1 :mask #b100)
    (.sub.w.vf vf2 vf0 vf0 :mask #b1000)
    (.sub.z.vf vf3 vf0 vf1 :mask #b1)
    (.add.w.vf vf3 vf0 vf1 :mask #b10)
    (.add.x.vf vf3 vf0 vf1 :mask #b100)
    (.sub.w.vf vf3 vf0 vf0 :mask #b1000)
    (.add.y.vf vf4 vf0 vf1 :mask #b1)
    (.sub.x.vf vf4 vf0 vf1 :mask #b10)
    (.add.w.vf vf4 vf0 vf1 :mask #b100)
    (.sub.w.vf vf4 vf0 vf0 :mask #b1000)
    (.outer.product.vf vf2 vf5 vf2)
    (.outer.product.vf vf3 vf5 vf3)
    (.outer.product.vf vf4 vf5 vf4)
    (.add.w.vf vf2 vf2 vf0 :mask #b1)
    (.add.w.vf vf3 vf3 vf0 :mask #b10)
    (.add.w.vf vf4 vf4 vf0 :mask #b100)
    (.mul.w.vf acc vf0 vf6)
    (.add.mul.x.vf acc vf2 vf6 acc)
    (.add.mul.y.vf acc vf3 vf6 acc)
    (.add.mul.z.vf vf6 vf4 vf6 acc)
    (.svf (&-> arg0 quad) vf6)
    arg0
    )
  )

;; The "forward down" function take a direction for forward (+z) and down (-y)
;; and convert to a transform.
;; Note that the normal functions take their pitch from the forward vector, but
;; the "nopitch" ones use the pitch from the up/down.  Of course, if you are
;; consistent and provide orthogonal forward/down, they do the same thing.

(defun forward-down->inv-matrix ((arg0 matrix) (arg1 vector) (arg2 vector))
  (vector-normalize-copy! (-> arg0 vector 2) arg1 1.0)
  (vector-cross! (the-as vector (-> arg0 vector)) (-> arg0 vector 2) arg2)
  (vector-normalize! (the-as vector (-> arg0 vector)) 1.0)
  (vector-cross! (-> arg0 vector 1) arg1 (the-as vector (-> arg0 vector)))
  (vector-normalize! (-> arg0 vector 1) 1.0)
  (set! (-> arg0 vector 3 quad) (the-as uint128 0))
  (set! (-> arg0 vector 0 w) 0.0)
  (set! (-> arg0 vector 1 w) 0.0)
  (set! (-> arg0 vector 2 w) 0.0)
  (set! (-> arg0 vector 3 w) 1.0)
  arg0
  )

(defun forward-down-nopitch->inv-matrix ((arg0 matrix) (arg1 vector) (arg2 vector))
  (vector-normalize-copy! (-> arg0 vector 1) arg2 1.0)
  (vector-negate! (-> arg0 vector 1) (-> arg0 vector 1))
  (vector-cross! (the-as vector (-> arg0 vector)) (-> arg0 vector 1) arg1)
  (vector-normalize! (the-as vector (-> arg0 vector)) 1.0)
  (vector-cross!
   (-> arg0 vector 2)
   (the-as vector (-> arg0 vector))
   (-> arg0 vector 1)
   )
  (vector-normalize! (-> arg0 vector 2) 1.0)
  (set! (-> arg0 vector 3 quad) (the-as uint128 0))
  (set! (-> arg0 vector 0 w) 0.0)
  (set! (-> arg0 vector 1 w) 0.0)
  (set! (-> arg0 vector 2 w) 0.0)
  (set! (-> arg0 vector 3 w) 1.0)
  arg0
  )

(defun forward-up-nopitch->inv-matrix ((arg0 matrix) (arg1 vector) (arg2 vector))
  (forward-down-nopitch->inv-matrix arg0 arg1 (vector-negate! (new-stack-vector0) arg2))
  )

(defun forward-up-nopitch->quaternion ((arg0 quaternion) (arg1 vector) (arg2 vector))
  (matrix->quaternion arg0 (forward-up-nopitch->inv-matrix (new-stack-matrix0) arg1 arg2))
  )

(defun forward-up->quaternion ((arg0 quaternion) (arg1 vector) (arg2 vector))
  (matrix->quaternion arg0
                      (forward-down->inv-matrix
                       (new-stack-matrix0)
                       arg1
                       (vector-negate! (new 'stack-no-clear 'vector) arg2)
                       )
                      )
  )

(defun quaternion-from-two-vectors! ((arg0 quaternion) (arg1 vector) (arg2 vector))
  (let* ((s5-0 (vector-cross! (new-stack-vector0) arg1 arg2))
         (f0-0 (vector-length s5-0))
         (f1-1 (vector-dot arg1 arg2))
         )
    (let ((f0-1 (/ (sqrtf (* 0.5 (- 1.0 f1-1))) f0-0)))
      (set! (-> arg0 x) (* (-> s5-0 x) f0-1))
      (set! (-> arg0 y) (* (-> s5-0 y) f0-1))
      (set! (-> arg0 z) (* (-> s5-0 z) f0-1))
      )
    (set! (-> arg0 w) (sqrtf (* 0.5 (+ 1.0 f1-1))))
    )
  arg0
  )

(defun quaternion-from-two-vectors-max-angle! ((arg0 quaternion) (arg1 vector) (arg2 vector) (arg3 float))
  (let* ((s5-0 (vector-cross! (new-stack-vector0) arg1 arg2))
         (f30-0 (vector-length s5-0))
         (f26-0 (vector-dot arg1 arg2))
         (f28-0 (sqrtf (* 0.5 (- 1.0 f26-0))))
         )
    (let ((f0-5 (sin (* 0.5 arg3))))
      (cond
        ((< f0-5 f28-0)
         (set! f28-0 f0-5)
         (set! (-> arg0 w) (cos (* 0.5 arg3)))
         )
        (else
         (set! (-> arg0 w) (sqrtf (* 0.5 (+ 1.0 f26-0))))
         )
        )
      )
    (let ((f0-12 (/ f28-0 f30-0)))
      (set! (-> arg0 x) (* (-> s5-0 x) f0-12))
      (set! (-> arg0 y) (* (-> s5-0 y) f0-12))
      (set! (-> arg0 z) (* (-> s5-0 z) f0-12))
      )
    )
  arg0
  )

(defun matrix-from-two-vectors! ((arg0 matrix) (arg1 vector) (arg2 vector))
  (let* ((a1-3 (vector-normalize! (vector-cross! (new-stack-vector0) arg2 arg1) 1.0))
         (f0-1 (vector-dot arg1 arg2))
         (f1-0 1.0)
         (f2-0 f0-1)
         (f1-2 (sqrtf (- f1-0 (* f2-0 f2-0))))
         )
    (matrix-axis-sin-cos! arg0 a1-3 f1-2 f0-1)
    )
  )

(defun matrix-from-two-vectors-max-angle! ((arg0 matrix) (arg1 vector) (arg2 vector) (arg3 float))
  (let
      ((s4-1 (vector-normalize! (vector-cross! (new-stack-vector0) arg2 arg1) 1.0))
       (f30-0 (vector-dot arg1 arg2))
       (f28-0 (cos arg3))
       )
    (cond
      ((< f30-0 f28-0)
       (matrix-axis-sin-cos! arg0 s4-1 (sin arg3) f28-0)
       )
      (else
       ;; todo looks bad because of inline (square x)
       (let ((t9-5 matrix-axis-sin-cos!)
             (a0-6 arg0)
             (a1-4 s4-1)
             (f0-1 1.0)
             (f1-0 f30-0)
             )
         (t9-5 a0-6 a1-4 (sqrtf (- f0-1 (* f1-0 f1-0))) f30-0)
         )
       )
      )
    )
  )

(defun matrix-from-two-vectors-max-angle-partial! ((arg0 matrix) (arg1 vector) (arg2 vector) (arg3 float) (arg4 float))
  (let* ((s4-1 (vector-normalize! (vector-cross! (new-stack-vector0) arg2 arg1) 1.0))
         (f28-0 (vector-dot arg1 arg2))
         (f30-0 (cos arg3))
         (f0-2 (+ 1.0 (* (+ -1.0 f28-0) arg4)))
         )
    (cond
      ((< f0-2 f30-0)
       (matrix-axis-sin-cos! arg0 s4-1 (sin arg3) f30-0)
       )
      (else
       ;; todo looks bad because of inline (square x)
       (let ((t9-5 matrix-axis-sin-cos!)
             (a0-6 arg0)
             (a1-4 s4-1)
             (f1-3 1.0)
             (f2-1 f0-2)
             )
         (t9-5 a0-6 a1-4 (sqrtf (- f1-3 (* f2-1 f2-1))) f0-2)
         )
       )
      )
    )
  )

(defun matrix-from-two-vectors-partial-linear! ((arg0 matrix) (arg1 vector) (arg2 vector) (arg3 float))
  (let ((gp-1 (vector-normalize! (vector-cross! (new-stack-vector0) arg2 arg1) 1.0))
        (f0-1 (vector-dot arg1 arg2))
        )
    (cond
      ((< 0.9999 (fabs f0-1))
       (matrix-identity! arg0)
       )
      (else
       (let* ((f0-4 (cos (* arg3 (acos f0-1))))
              (t9-5 matrix-axis-sin-cos!)
              (a0-6 arg0)
              (f1-1 1.0)
              (f2-1 f0-4)
              )
         (t9-5 a0-6 gp-1 (sqrtf (- f1-1 (* f2-1 f2-1))) f0-4)
         )
       )
      )
    )
  )

(defun matrix-remove-z-rot ((arg0 matrix) (arg1 matrix))
  (let ((s4-0 (new-stack-vector0)))
    0.0
    0.0
    (let ((s5-0 (new-stack-matrix0)))
      (vector-negate! s4-0 (the-as vector arg1))
      (vector-flatten! s4-0 s4-0 (-> arg0 vector 2))
      (vector-normalize! s4-0 1.0)
      (let ((f30-0 (vector-dot (-> arg0 vector 1) s4-0)))
        (when (< f30-0 0.99999)
          (vector-cross! s4-0 (-> arg0 vector 1) s4-0)
          (let ((f0-4 (vector-length s4-0)))
            (if (< 0.0 (vector-dot s4-0 (-> arg0 vector 2)))
                (set! f0-4 (- f0-4))
                )
            (matrix-axis-sin-cos! s5-0 (-> arg0 vector 2) f0-4 f30-0)
            )
          (matrix*! arg0 arg0 s5-0)
          )
        )
      )
    )
  arg0
  )

(defun matrix-rot-diff! ((arg0 vector) (arg1 matrix) (arg2 matrix))
  (let ((s3-0 (new-stack-quaternion0))
        (s2-0 (new-stack-quaternion0))
        (s5-0 (new-stack-quaternion0))
        )
    0.0
    (matrix->quaternion s3-0 arg1)
    (matrix->quaternion s2-0 arg2)
    (quaternion-conjugate! s5-0 s3-0)
    (quaternion*! s5-0 s2-0 s5-0)
    (quaternion-normalize! s5-0)
    (if (< (-> s5-0 w) 0.0)
        (quaternion-negate! s5-0 s5-0)
        )
    (let ((f30-1 (* 2.0 (acos (-> s5-0 w)))))
      (set! (-> arg0 quad) (-> s5-0 vec quad))
      (vector-negate! arg0 arg0)
      (if (= (vector-normalize-ret-len! arg0 1.0) 0.0)
          (set! (-> arg0 y) 1.0)
          )
      f30-1
      )
    )
  )


(defun quaternion-seek ((arg0 quaternion) (arg1 quaternion) (arg2 quaternion) (arg3 float) (arg4 float))
  (let ((s5-0 (new-stack-matrix0))
        (s4-0 (new-stack-matrix0))
        )
    (quaternion->matrix s5-0 arg1)
    (quaternion->matrix s4-0 arg2)
    (let ((s2-1 (new-stack-quaternion0)))
      (quaternion-from-two-vectors-max-angle!
       s2-1
       (-> s5-0 vector 2)
       (-> s4-0 vector 2)
       arg4
       )
      (quaternion-normalize! (quaternion*! arg0 arg0 s2-1))
      )
    )
  )

(defun vector-deg-seek ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  (let ((s4-0 (new-stack-matrix0)))
    (matrix-from-two-vectors-max-angle! s4-0 arg1 arg2 arg3)
    (vector-matrix*! arg0 arg1 s4-0)
    )
  )

(defun vector-deg-slerp ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  (cond
    ((>= 0.0 arg3)
     (set! (-> arg0 quad) (-> arg1 quad))
     arg0
     )
    ((>= arg3 1.0)
     (set! (-> arg0 quad) (-> arg2 quad))
     arg0
     )
    (else
     (let ((s1-0 (new-stack-matrix0)))
       (let
           ((s2-0 (vector-normalize-copy! (new 'stack-no-clear 'vector) arg1 1.0))
            (a2-3 (vector-normalize-copy! (new 'stack-no-clear 'vector) arg2 1.0))
            )
         (matrix-from-two-vectors-partial-linear! s1-0 s2-0 a2-3 arg3)
         )
       (vector-matrix*! arg0 arg1 s1-0)
       )
     )
    )
  )

(defun vector-vector-deg-slerp! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float) (arg4 vector))
  (local-vars (sv-112 (function float float float float)))
  (cond
    ((>= 0.0 arg3)
     (set! (-> arg0 quad) (-> arg1 quad))
     )
    ((>= arg3 1.0)
     (set! (-> arg0 quad) (-> arg2 quad))
     )
    (else
     (let*
         ((s0-0 (vector-normalize-copy! (new 'stack-no-clear 'vector) arg1 1.0))
          (s1-0 (vector-normalize-copy! (new 'stack-no-clear 'vector) arg2 1.0))
          (s0-1 (forward-up->quaternion (new 'stack-no-clear 'quaternion) s0-0 arg4))
          (a2-5 (forward-up->quaternion (new 'stack-no-clear 'quaternion) s1-0 arg4))
          (a1-6 (quaternion-slerp! (new 'stack-no-clear 'quaternion) s0-1 a2-5 arg3))
          (s2-1 vector-normalize-copy!)
          (s1-1 arg0)
          (s0-2 (vector-z-quaternion! (new 'stack-no-clear 'vector) a1-6))
          )
       (set! sv-112 lerp)
       (let ((s3-1 (vector-length arg1))
             (a1-7 (vector-length arg2))
             )
         (s2-1 s1-1 s0-2 (sv-112 s3-1 a1-7 arg3))
         )
       )
     )
    )
  arg0
  )

(defun normal-of-plane ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         )
   (init-vf0-vector)
   (.lvf vf3 (&-> arg2 quad))
   (.lvf vf1 (&-> arg1 quad))
   (.lvf vf2 (&-> arg3 quad))
   (.sub.vf vf1 vf3 vf1)
   (.sub.vf vf2 vf3 vf2)
   (.outer.product.vf vf4 vf2 vf1)
   (.mul.vf vf5 vf4 vf4)
   (.add.y.vf vf5 vf5 vf5 :mask #b1)
   (.add.z.vf vf5 vf5 vf5 :mask #b1)
   (.isqrt.vf Q vf0 vf5 :fsf #b11 :ftf #b0)
   (.mov.vf vf4 vf0 :mask #b1000)
   (.wait.vf)
   (.mul.vf vf4 vf4 Q :mask #b111)
   (.nop.vf)
   (.nop.vf)
   (.svf (&-> arg0 quad) vf4)
   arg0
   )
  )

(defun vector-3pt-cross! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         )
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.lvf vf3 (&-> arg3 quad))
    (.add.x.vf vf4 vf0 vf0 :mask #b1000)
    (.sub.vf vf2 vf2 vf1)
    (.sub.vf vf3 vf3 vf1)
    (.outer.product.vf vf4 vf2 vf3)
    (.svf (&-> arg0 quad) vf4)
    arg0
    )
  )

(defun closest-pt-in-triangle ((arg0 vector) (arg1 vector) (arg2 matrix) (arg3 vector))
  "arg2 is probably the triangle"
  ;; (declare (print-asm))
  (local-vars
   (v1-0 uint)
   (v1-4 uint)
   (v1-5 uint)
   (v1-6 uint)
   (v1-7 uint)
   (v1-10 uint)
   (a0-1 uint)
   (a1-1 uint)
   )
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf10 :class vf)
         (vf11 :class vf)
         (vf12 :class vf)
         (vf13 :class vf)
         (vf14 :class vf)
         (vf15 :class vf)
         (vf16 :class vf)
         (vf17 :class vf)
         (vf18 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         (vf8 :class vf)
         (vf9 :class vf)
         )
    (init-vf0-vector)
    (nop!)
    (nop!)
    (.lvf vf3 (&-> arg2 vector 1 quad))
    (nop!)
    (.lvf vf4 (&-> arg2 vector 2 quad))
    (nop!)
    (.lvf vf5 (&-> arg1 quad))
    (.sub.vf vf6 vf3 vf4)
    (.lvf vf2 (&-> arg2 vector 0 quad))
    (.sub.vf vf7 vf3 vf5)
    (.lvf vf1 (&-> arg3 quad))
    (.sub.vf vf8 vf3 vf2)
    (.sub.vf vf9 vf5 vf4)
    (.sub.vf vf10 vf5 vf2)
    (.outer.product.vf vf14 vf7 vf8)
    (.outer.product.vf vf15 vf6 vf7)
    (.mul.vf vf11 vf14 vf1)
    (.outer.product.vf vf16 vf9 vf10)
    (.mul.vf vf12 vf15 vf1)
    (.add.x.vf vf11 vf11 vf11 :mask #b10)
    (.mul.vf vf13 vf16 vf1)
    (.add.x.vf vf12 vf12 vf12 :mask #b10)
    (.add.x.vf vf13 vf13 vf13 :mask #b10)
    (.add.z.vf vf11 vf11 vf11 :mask #b10)
    (.add.z.vf vf12 vf12 vf12 :mask #b10)
    (.add.z.vf vf13 vf13 vf13 :mask #b10)
    ;; note: these types were changed to uint to make this copy 64 bits.
    (.mov v1-0 vf11)
    (.mov a1-1 vf12)
    (.mov a0-1 vf13)
    (let* ((v1-1 (shr (the-as int v1-0) 63))
           (a1-2 (shr (the-as int a1-1) 63))
           (a0-2 (shr (the-as int a0-1) 63))
           (a1-3 (* a1-2 2))
           (a0-3 (* a0-2 4))
           (v1-3 (logior (logior v1-1 a1-3) a0-3))
           )
      (b! (nonzero? v1-3) cfg-3 :delay (set! v1-4 (the-as uint (+ v1-3 -1))))
      )
    (.sub.vf vf17 vf5 vf2)
    (.mov.vf vf18 vf0 :mask #b1000)
    (.outer.product.vf vf18 vf17 vf1)
    (.outer.product.vf vf18 vf1 vf18)
    (.add.vf vf18 vf18 vf2 :mask #b111)
    (b! #t cfg-24 :delay (.svf (&-> arg0 quad) vf18))
    (nop!)
    (label cfg-3)
    (b! (nonzero? v1-4) cfg-6 :delay (set! v1-5 (the-as uint (+ v1-4 -1))))
    (vector-segment-distance-point!
     arg1
     (the-as vector (-> arg2 vector))
     (-> arg2 vector 1)
     arg0
     )
    (goto cfg-24)
    (label cfg-6)
    (b! (nonzero? v1-5) cfg-9 :delay (set! v1-6 (the-as uint (+ v1-5 -1))))
    (vector-segment-distance-point!
     arg1
     (-> arg2 vector 1)
     (-> arg2 vector 2)
     arg0
     )
    (goto cfg-24)
    (label cfg-9)
    (b! (nonzero? v1-6) cfg-14 :delay (set! v1-7 (the-as uint (+ v1-6 -1))))
    (let ((f30-0 (vector-segment-distance-point!
                  arg1
                  (-> arg2 vector 1)
                  (the-as vector (-> arg2 vector))
                  arg0
                  )
            )
          (s3-0 (new 'stack-no-clear 'vector))
          )
      (if (< (vector-segment-distance-point!
              arg1
              (-> arg2 vector 1)
              (-> arg2 vector 2)
              s3-0
              )
             f30-0
             )
          (set! (-> arg0 quad) (-> s3-0 quad))
          )
      )
    (goto cfg-24)
    (label cfg-14)
    (b! (nonzero? v1-7) cfg-17 :delay (set! v1-10 (the-as uint (+ v1-7 -1))))
    (vector-segment-distance-point!
     arg1
     (-> arg2 vector 2)
     (the-as vector (-> arg2 vector))
     arg0
     )
    (goto cfg-24)
    (label cfg-17)
    (b! (nonzero? v1-10) cfg-22)
    (let ((f30-1 (vector-segment-distance-point!
                  arg1
                  (the-as vector (-> arg2 vector))
                  (-> arg2 vector 1)
                  arg0
                  )
            )
          (s3-1 (new 'stack-no-clear 'vector))
          )
      (if (< (vector-segment-distance-point!
              arg1
              (the-as vector (-> arg2 vector))
              (-> arg2 vector 2)
              s3-1
              )
             f30-1
             )
          (set! (-> arg0 quad) (-> s3-1 quad))
          )
      )
    (goto cfg-24)
    (label cfg-22)
    (let ((f30-2 (vector-segment-distance-point!
                  arg1
                  (-> arg2 vector 2)
                  (the-as vector (-> arg2 vector))
                  arg0
                  )
            )
          (s3-2 (new 'stack-no-clear 'vector))
          )
      (if (< (vector-segment-distance-point!
              arg1
              (-> arg2 vector 2)
              (-> arg2 vector 1)
              s3-2
              )
             f30-2
             )
          (set! (-> arg0 quad) (-> s3-2 quad))
          )
      )
    (label cfg-24)
    0
    (none)
    )
  )

(defun point-in-triangle-cross ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector) (arg4 vector))
  (local-vars (v1-0 int) (a0-1 int) (a1-1 int))
  (rlet ((acc :class vf)
         (vf1 :class vf)
         (vf10 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf)
         (vf8 :class vf)
         (vf9 :class vf)
         )
   (.lvf vf3 (&-> arg3 quad))
   (.lvf vf4 (&-> arg4 quad))
   (.lvf vf5 (&-> arg0 quad))
   (.lvf vf2 (&-> arg2 quad))
   (.lvf vf1 (&-> arg1 quad))
   (.sub.vf vf6 vf3 vf4)
   (.sub.vf vf7 vf3 vf5)
   (.sub.vf vf8 vf3 vf2)
   (.sub.vf vf9 vf5 vf4)
   (.sub.vf vf10 vf5 vf2)
   (.outer.product.vf vf2 vf6 vf7)
   (.outer.product.vf vf3 vf7 vf8)
   (.outer.product.vf vf4 vf9 vf10)
   (.mul.vf vf2 vf2 vf1)
   (.mul.vf vf3 vf3 vf1)
   (.nop.vf)
   (.mul.vf vf4 vf4 vf1)
   (.add.x.vf vf2 vf2 vf2 :mask #b10)
   (.add.x.vf vf3 vf3 vf3 :mask #b10)
   (.add.x.vf vf4 vf4 vf4 :mask #b10)
   (.nop.vf)
   (.add.z.vf vf2 vf2 vf2 :mask #b10)
   (.add.z.vf vf3 vf3 vf3 :mask #b10)
   (.add.z.vf vf4 vf4 vf4 :mask #b10)
   (.nop.vf)
   (.mov a0-1 vf2)
   (.mov a1-1 vf3)
   (.mov v1-0 vf4)
   (>= (the-as int (logior (logior a0-1 a1-1) v1-0)) 0)
   )
  )

(defun point-in-plane-<-point+normal! ((arg0 vector) (arg1 vector) (arg2 vector))
  (let
   ((f0-3
     (+
      (+ (* (-> arg2 x) (-> arg1 x)) (* (-> arg2 y) (-> arg1 y)))
      (* (-> arg2 z) (-> arg1 z))
      )
     )
    )
   (set! (-> arg0 w) 1.0)
   (let ((f1-7 (fabs (-> arg2 x)))
         (f2-3 (fabs (-> arg2 y)))
         (f3-1 (fabs (-> arg2 z)))
         )
    (cond
     ((and (< f2-3 f1-7) (< f3-1 f1-7))
      (set! (-> arg0 y) (+ 4096.0 (-> arg1 y)))
      (set! (-> arg0 z) (+ 4096.0 (-> arg1 z)))
      (set!
       (-> arg0 x)
       (/
        (+ (- (- (* (-> arg2 y) (-> arg0 y))) (* (-> arg2 z) (-> arg0 z))) f0-3)
        (-> arg2 x)
        )
       )
      )
     ((and (< f1-7 f2-3) (< f3-1 f2-3))
      (set! (-> arg0 x) (+ 4096.0 (-> arg1 x)))
      (set! (-> arg0 z) (+ 4096.0 (-> arg1 z)))
      (set!
       (-> arg0 y)
       (/
        (- (- f0-3 (* (-> arg2 x) (-> arg0 x))) (* (-> arg2 z) (-> arg0 z)))
        (-> arg2 y)
        )
       )
      )
     (else
      (set! (-> arg0 x) (+ 4096.0 (-> arg1 x)))
      (set! (-> arg0 y) (+ 4096.0 (-> arg1 y)))
      (set!
       (-> arg0 z)
       (/
        (+ (- (- (* (-> arg2 x) (-> arg0 x))) (* (-> arg2 y) (-> arg0 y))) f0-3)
        (-> arg2 z)
        )
       )
      )
     )
    )
   )
  arg0
  )


(defun circle-circle-xz-intersect ((arg0 sphere) (arg1 sphere) (arg2 vector) (arg3 vector))
  ;; this function is unused and really complicated, so not implementing it for now.
  (format 0 "circle-circle-xz-intersect~%")
  (crash!)
  0
  )

(defun circle-test ()
  "Test the circle-circle-xz-intersect function."
  ;; doesn't work because circle-circle-xz-intersect hasn't been ported.
  (let ((s4-0 (new 'stack 'sphere))
        (a1-2 (new 'stack 'sphere))
        (s5-0 (new-stack-vector0))
        (gp-0 (new-stack-vector0))
        )
    (set-vector! s4-0 0.0 0.0 0.0 1.0)
    (set-vector! a1-2 100.0 0.0 0.0 10000.0)
    (let ((a2-1 (circle-circle-xz-intersect s4-0 a1-2 s5-0 gp-0)))
      (format #t "res = ~d~%" a2-1)
      )
    (format #t "(~f, ~f)~%" (-> s5-0 x) (-> s5-0 z))
    (format #t "(~f, ~f)~%" (-> gp-0 x) (-> gp-0 z))
    )
  (none)
  )


(defun vector-circle-tangent-new ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  "Unused."
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         )
   (init-vf0-vector)
   (let ((a1-2 (new 'stack 'sphere)))
    (set! (-> (new 'stack-no-clear 'vector) quad) (the-as uint128 0))
    (set! (-> (new 'stack-no-clear 'vector) quad) (the-as uint128 0))
    (let ((v1-3 0.5))
     (.lvf vf3 (&-> arg1 quad))
     (.mov vf2 v1-3)
     )
    (.lvf vf4 (&-> arg0 quad))
    (.add.vf vf1 vf3 vf4)
    (.sub.vf vf5 vf4 vf3)
    (.mul.x.vf vf1 vf1 vf2)
    (.mul.x.vf vf5 vf5 vf2)
    (.mul.vf vf5 vf5 vf5 :mask #b101)
    (.add.z.vf vf5 vf5 vf5 :mask #b1)
    (.sqrt.vf Q vf5 :ftf #b0)
    (.wait.vf)
    (.mul.vf vf1 vf0 Q :mask #b1000)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> a1-2 quad) vf1)
    (circle-circle-xz-intersect (the-as sphere arg1) a1-2 arg2 arg3)
    )
   0
   (none)
   )
  )

(defun vector-circle-tangent ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 vector))
  "Also unused."
  (let* ((s3-1 (vector-! (the-as vector (new 'stack-no-clear 'sphere)) arg1 arg0))
         (f0-0 (vector-xz-length s3-1))
         (f28-0 (acos (/ (-> arg1 w) f0-0)))
         )
    (cond
      ((>= 546.13336 f28-0) ;; 2/15 of a meter...
       (set! (-> arg2 x) (- (-> arg0 x) (-> s3-1 z)))
       (set! (-> arg2 y) 0.0)
       (set! (-> arg2 z) (+ (-> arg0 z) (-> s3-1 x)))
       (set! (-> arg3 x) (+ (-> arg0 x) (-> s3-1 z)))
       (set! (-> arg3 y) 0.0)
       (set! (-> arg3 z) (- (-> arg0 z) (-> s3-1 x)))
       )
      (else
       (let ((f0-15 (atan (-> s3-1 z) (-> s3-1 x)))
             (f30-0 (- (-> arg1 w)))
             (s3-2 (new 'stack-no-clear 'vector))
             )
         (let ((s2-1 (new 'stack-no-clear 'vector)))
           (let ((a2-1 (new 'stack-no-clear 'vector)))
             (set! (-> a2-1 x) (- f0-15 f28-0))
             (set! (-> a2-1 y) (+ f0-15 f28-0))
             (vector-sincos! s3-2 s2-1 a2-1)
             )
           (set! (-> arg2 x) (+ (-> arg1 x) (* f30-0 (-> s2-1 x))))
           (set! (-> arg2 z) (+ (-> arg1 z) (* f30-0 (-> s3-2 x))))
           (set! (-> arg3 x) (+ (-> arg1 x) (* f30-0 (-> s2-1 y))))
           )
         (set! (-> arg3 z) (+ (-> arg1 z) (* f30-0 (-> s3-2 y))))
         )
       )
      )
    )
  0
  (none)
  )

(defun find-knot-span ((arg0 int) (arg1 int) (arg2 float) (arg3 (inline-array vector)))
  "Binary serach over knots to find which contains the value float in (arg0 arg1). Unused."
  (local-vars (v0-0 int))

  ;; if the knot after this, is exactly target value, return that.
  (b! (= arg2 (-> arg3 (+ arg0 1))) cfg-11 :delay (set! v0-0 arg0))

  ;; convert to int?
  (let ((v1-3 (the int arg2)))
    (let* ((a2-1 (+ v1-3 3))
           ;;(t0-1 (+ (* a2-1 4) (the-as int arg3)))
           (t0-1 (&-> arg3 0 data a2-1))
           ;;(f1-2 (dynamic-array-field-access t0-1 PLACEHOLDER))
           (f1-2 (-> t0-1 0))
           (f2-0 (-> t0-1 1))
           )
      (b! (> f1-2 arg2) cfg-4)
      (b! (>= arg2 f2-0) cfg-4 :delay (set! v0-0 a2-1))
      )

    ;; don't think this is hit normally.
    (b! #t cfg-11)

    ;; loop setup
    (label cfg-4)
    (let ((a1-1 arg1)       ;; current 
          (a0-1 (+ arg0 1)) ;; next
          )

      ;; loop top
      (label cfg-5)

      (let ((a2-3 (/ (+ a1-1 a0-1) 2)))
        (let ((t0-3 (&-> arg3 0 data a2-3)))
          (b! (>= arg2 (-> t0-3 0)) cfg-7)
          (b! #t cfg-5 :delay (set! a0-1 a2-3))
          (label cfg-7)
          (b! (< arg2 (-> t0-3 1)) cfg-9)
          )
        (b! #t cfg-5 :delay (set! a1-1 a2-3))
        (label cfg-9)
        (set! v0-0 a2-3)
        )
      )
    (b! (= v0-0 v1-3) cfg-11)
    )
  (nop!)
  (nop!)
  (label cfg-11)
  v0-0
  )

(defun calculate-basis-functions-vector! ((arg0 (pointer float)) (arg1 int) (arg2 float) (arg3 (pointer float)))
  (local-vars (v1-0 int) (v1-1 object))
  ;;(.sll v1-0 arg1 2)
  (set! v1-0 (* 4 arg1)) ;; originally used 32-bit asm
  (let ((a1-1 #x3f800000)
        (f3-0 arg2)
        )
    ;;(.addu v1-1 arg3 v1-0)
    (set! v1-1 (&+ arg3 v1-0))
    (let* ((f1-0 (the-as float a1-1)) ;; trick to load float constant.
           (f5-0 f1-0)
           )
      0.0
      0.0
      (let* ((f0-2 (-> (the-as (pointer float) v1-1) 0))
             (f2-0 (-> (the-as (pointer float) v1-1) 1))
             (f0-3 (- f3-0 f0-2))
             (f4-0 (- f2-0 f3-0))
             (f10-0 (/ f1-0 (+ f4-0 f0-3)))
             (f2-2 (-> (the-as (pointer float) v1-1) -1))
             (f8-0 (-> (the-as (pointer float) v1-1) 2))
             (f2-3 (- f3-0 f2-2))
             (f9-0 (+ f4-0 f2-3))
             (f6-0 (-> (the-as (pointer float) v1-1) -2))
             (f7-0 (-> (the-as (pointer float) v1-1) 3))
             (f9-1 (/ f1-0 f9-0))
             (f5-1 (* f5-0 f10-0))
             (f11-0 (* f4-0 f5-1))
             (f10-1 (* f0-3 f5-1))
             (f5-2 (- f8-0 f3-0))
             (f8-1 (* f11-0 f9-1))
             (f11-1 (/ f1-0 (+ f5-2 f0-3)))
             (f9-3 (* f4-0 f8-1))
             (f8-2 (* f2-3 f8-1))
             (f11-2 (* f10-1 f11-1))
             (f10-3 (+ (* f5-2 f11-2) f8-2))
             (f8-3 (* f0-3 f11-2))
             (f6-1 (- f3-0 f6-0))
             (f3-1 (- f7-0 f3-0))
             (f7-3 (* f9-3 (/ f1-0 (+ f4-0 f6-1))))
             (f4-1 (* f4-0 f7-3))
             (f6-2 (* f6-1 f7-3))
             (f7-6 (* f10-3 (/ f1-0 (+ f5-2 f2-3))))
             (f5-4 (+ (* f5-2 f7-6) f6-2))
             (f2-4 (* f2-3 f7-6))
             (f1-2 (* f8-3 (the-as float (/ f1-0 (+ f3-1 f0-3)))))
             (f2-5 (+ (* f3-1 f1-2) f2-4))
             (f0-4 (* f0-3 f1-2))
             )
        (set! (-> arg0 0) f4-1)
        (set! (-> arg0 1) f5-4)
        (set! (-> arg0 2) f2-5)
        (set! (-> arg0 3) f0-4)
        )
      )
    )
  arg0
  )

(defun curve-evaluate! ((arg0 vector) (arg1 float) (arg2 pointer) (arg3 int) (arg4 (inline-array vector)) (arg5 int))
  (local-vars (v1-7 int) (v1-8 int) (v1-10 float) (s3-0 int))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
   (init-vf0-vector)
   (let ((s4-0 (new 'static 'array float 4 0.0 0.0 0.0 0.0)))
    0
    (let* ((f0-0 (-> arg4 0 x))
           (f1-0 (-> (&-> arg4 0 data (+ arg5 -1)) 0))
           (a2-1 (fmax (fmin (* arg1 f1-0) f1-0) f0-0))
           )
     (let* ((a1-1 (+ arg5 -5))
            (a3-1 3)
            (f0-2 a2-1)
            (v1-5 arg4)
            (f0-3 f0-2)
            )
      (b! (= f0-3 (-> (&-> v1-5 0 data (+ a1-1 1)) 0))
       cfg-11
       :delay (set! s3-0 a1-1)
       )
      (let ((a0-4 (the int f0-3)))
       (let* ((t1-1 (+ a0-4 3))
              (t2-1 (&-> v1-5 0 data t1-1))
              (f1-4 (-> t2-1 0))
              (f2-3 (-> t2-1 1))
              )
        (b! (> f1-4 f0-3) cfg-4)
        (b! (>= f0-3 f2-3) cfg-4 :delay (set! s3-0 t1-1))
        )
       (b! #t cfg-11)
       (label cfg-4)
       (let ((a3-2 a3-1)
             (a1-2 (+ a1-1 1))
             )
        (label cfg-5)
        (let ((t1-3 (/ (+ a3-2 a1-2) 2)))
         (let ((t2-3 (&-> v1-5 0 data t1-3)))
          (b! (>= f0-3 (-> t2-3 0)) cfg-7)
          (b! #t cfg-5 :delay (set! a1-2 t1-3))
          (label cfg-7)
          (b! (< f0-3 (-> t2-3 1)) cfg-9)
          )
         (b! #t cfg-5 :delay (set! a3-2 t1-3))
         (label cfg-9)
         (set! s3-0 t1-3)
         )
        )
       (b! (= s3-0 a0-4) cfg-11)
       )
      )
     (nop!)
     (nop!)
     (label cfg-11)
     (calculate-basis-functions-vector!
      s4-0
      s3-0
      a2-1
      (the-as (pointer float) arg4)
      )
     )
    ;;(.addiu v1-7 s3-0 -3)
    (set! v1-7 (- s3-0 3))
    (.lvf vf6 s4-0)
    )
   ;;(.sll v1-8 v1-7 4)
   (set! v1-8 (* v1-7 16))
   (.add.x.vf vf1 vf0 vf0 :mask #b1000)
   (let ((v1-9 (+ v1-8 (the-as int arg2))))
    (nop!)
    (nop!)
    (.lvf vf2 (&-> (the-as (pointer int128) v1-9)))
    (nop!)
    (.lvf vf3 (+ v1-9 16))
    (nop!)
    (.lvf vf4 (+ v1-9 32))
    (nop!)
    (.lvf vf5 (+ v1-9 48))
    )
   (.mul.x.vf acc vf2 vf6)
   (nop!)
   (.add.mul.y.vf acc vf3 vf6 acc :mask #b111)
   (nop!)
   (.add.mul.z.vf acc vf4 vf6 acc :mask #b111)
   (nop!)
   (.add.mul.w.vf vf1 vf5 vf6 acc :mask #b111)
   (nop!)
   (nop!)
   (nop!)
   (nop!)
   (nop!)
   (nop!)
   (nop!)
   (nop!)
   (.svf (&-> arg0 quad) vf1)
   (.mov v1-10 vf1)
   arg0
   )
  )

(defun curve-get-pos! ((arg0 vector) (arg1 float) (arg2 curve))
  (curve-evaluate!
   arg0
   arg1
   (-> arg2 cverts)
   (-> arg2 num-cverts)
   (-> arg2 knots)
   (-> arg2 num-knots)
   )
  )

(defun curve-length ((arg0 curve))
  (let ((s5-0 (new-stack-vector0))
        (s4-0 (new-stack-vector0))
        (s3-0 (* 3 (-> arg0 num-cverts)))
        (f30-0 0.0)
        )
   (curve-evaluate!
    s4-0
    (-> arg0 knots 0 x)
    (-> arg0 cverts)
    (-> arg0 num-cverts)
    (-> arg0 knots)
    (-> arg0 num-knots)
    )
   (dotimes (s2-0 s3-0)
    (set! (-> s5-0 quad) (-> s4-0 quad))
    (curve-evaluate!
     s4-0
     (/ (the float (+ s2-0 1)) (the float s3-0))
     (-> arg0 cverts)
     (-> arg0 num-cverts)
     (-> arg0 knots)
     (-> arg0 num-knots)
     )
    (+! f30-0 (vector-vector-distance s5-0 s4-0))
    )
   f30-0
   )
  )

(defun curve-copy! ((arg0 curve) (arg1 curve))
  (set! (-> arg0 cverts) (-> arg1 cverts))
  (set! (-> arg0 num-cverts) (-> arg1 num-cverts))
  (set! (-> arg0 knots) (-> arg1 knots))
  (set! (-> arg0 num-knots) (-> arg1 num-knots))
  (set! (-> arg0 length) (-> arg1 length))
  arg0
  )

(defun curve-closest-point ((arg0 curve) (arg1 vector) (arg2 float) (arg3 float) (arg4 int) (arg5 float))
  (local-vars (sv-48 float))
  (set! sv-48 arg3)
  (let ((s3-0 arg4)
        (gp-0 arg5)
        (f30-0 (curve-length arg0))
        (s2-0 (new 'stack-no-clear 'vector))
        (s1-0 (new 'stack-no-clear 'vector))
        )
    0.0
    0.0
    0.0
    0.0
    (let ((f28-0 0.5))
      0.0
      (if (< 0.0 sv-48)
          (set! f28-0 (/ sv-48 f30-0))
          )
      (let* ((s0-1 (- arg2 (/ gp-0 f30-0)))
             (f26-0 (- s0-1 f28-0))
             (f24-0 (+ s0-1 f28-0))
             )
        (curve-get-pos! s2-0 f26-0 arg0)
        (curve-get-pos! s1-0 f24-0 arg0)
        (let ((f22-0 (vector-vector-distance-squared s2-0 arg1))
              (f20-0 (vector-vector-distance-squared s1-0 arg1))
              )
          (while (> s3-0 0)
            (+! s3-0 -1)
            (set! f28-0 (* 0.5 f28-0))
            (let ((v1-4 (cond
                          ((< f22-0 f20-0)
                           (curve-get-pos! s1-0 s0-1 arg0)
                           (set! f20-0 (vector-vector-distance-squared s1-0 arg1))
                           (set! f24-0 s0-1)
                           (- s0-1 f28-0)
                           )
                          (else
                           (curve-get-pos! s2-0 s0-1 arg0)
                           (set! f22-0 (vector-vector-distance-squared s2-0 arg1))
                           (set! f26-0 s0-1)
                           (+ s0-1 f28-0)
                           )
                          )
                    )
                  )
              (set! s0-1 (fmin 1.0 (fmax 0.0 v1-4)))
              )
            )
          (+ (if (< f22-0 f20-0)
                 f26-0
                 f24-0
                 )
             (/ gp-0 f30-0)
             )
          )
        )
      )
    )
  )

(defun vector-plane-distance ((arg0 vector) (arg1 plane) (arg2 vector))
  (vector-dot
   (vector-! (new 'stack-no-clear 'vector) arg0 (the-as vector (&-> arg1 x)))
   arg2
   )
  )