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

;;-*-Lisp-*-
(in-package goal)
(bundles "ENGINE.CGO" "GAME.CGO")
(require "engine/geometry/geometry-h.gc")
(require "engine/math/quaternion.gc")
(require "engine/gfx/font.gc")

(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)
         (source :class vf)
         (normal :class vf)
         (in-plane :class vf))
    (init-vf0-vector)
    (.lvf source (&-> src quad))
    (.lvf normal (&-> plane-normal quad))
    (.mov.vf.w in-plane vf0)
    ;; use the triple-product to compute this without needing
    ;; a slow divide.
    ;; vec x normal gives us a vector with the right magnitude,
    ;; and in plane, but rotated 90 degrees.
    (.outer.product.a.vf acc source normal)
    (.outer.product.b.vf in-plane normal source acc)
    ;; rotate by 90 about normal of plane to get the final vector.
    (.outer.product.a.vf acc normal in-plane)
    (.outer.product.b.vf in-plane in-plane normal acc)
    (.svf (&-> dst quad) in-plane)
    dst))

(defun vector-reflect! ((dst vector) (src vector) (plane-normal vector))
  "Reflect src across the plane through the origin with unit normal plane-normal. dst may be
   either input and receives w = 1.0. plane-normal must be unit length; the magnitude of src is
   preserved only if it is."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (source :class vf)
         (normal :class vf)
         (in-plane :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 source (&-> src quad))
    (.lvf normal (&-> plane-normal quad))
    (.mov.vf.w in-plane vf0)
    ;; triple-product to project onto the plane
    (.outer.product.a.vf acc source normal)
    (.outer.product.b.vf in-plane normal source acc)
    (.outer.product.a.vf acc normal in-plane)
    (.outer.product.b.vf in-plane in-plane normal acc)
    ;; add in-plane to itself to get 2 * T
    (.add.vf.xyz acc in-plane in-plane)
    ;; subtract src.
    (.sub.mul.w.vf.xyz in-plane source vf0 acc)
    (.svf (&-> dst quad) in-plane)
    dst))

(defun vector-reflect-flat! ((dst vector) (src vector) (plane-normal vector))
  "Flatten src onto the plane, then add `plane-normal`.
   This is a strange operation, since `src` is typically a velocity and is 1000's of times
   larger than plane-normal. Removing the addition makes no obvious difference in collision.
   My theory is that this is a slight optimization and very slightly pushes collision geometry
   away from the triangle, preventing a future collision. Without this, rounding errors may
   cause geometry to enter the triangle again and take another iteration.
   The existance of vector-reflect-true-flat! (without this addition) acknowledges that this
   version is somehow an adjust reflect-flat."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (source :class vf)
         (normal :class vf)
         (in-plane :class vf))
    (init-vf0-vector)
    (.lvf source (&-> src quad))
    (.lvf normal (&-> plane-normal quad))
    (.mov.vf.w in-plane vf0)
    ;; triple-product to project onto plane
    (.outer.product.a.vf acc source normal)
    (.outer.product.b.vf in-plane normal source acc)
    (.outer.product.a.vf acc normal in-plane)
    (.outer.product.b.vf in-plane in-plane normal acc)
    ;; add normal
    (.add.vf.xyz in-plane in-plane normal)
    (.svf (&-> dst quad) in-plane)
    dst))

(defun vector-reflect-true-flat! ((dst vector) (src vector) (plane-normal vector))
  "Project src onto the plane with unit normal plane-normal; equivalent to vector-flatten!."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (source :class vf)
         (normal :class vf)
         (in-plane :class vf))
    (init-vf0-vector)
    (.lvf source (&-> src quad))
    (.lvf normal (&-> plane-normal quad))
    (.mov.vf.w in-plane vf0)
    (.outer.product.a.vf acc source normal)
    (.outer.product.b.vf in-plane normal source acc)
    (.outer.product.a.vf acc normal in-plane)
    (.outer.product.b.vf in-plane in-plane normal acc)
    (.svf (&-> dst quad) in-plane)
    dst))

(defun vector-reflect-flat-above! ((dst vector) (src vector) (plane-normal vector))
  "Project src onto the plane with unit normal plane-normal, then lift the result off the plane by
   0.32 times its own length, capped at four meters. dst is used as scratch, so it must not alias
   plane-normal, though it may alias src."
   ;; this may just have a bug, the normal-component is always 0.
   ;; in any case, this is a bit of a softer "bounce" than reflect.
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (source :class vf)
         (normal :class vf)
         (in-plane :class vf))
    (init-vf0-vector)
    (.lvf source (&-> src quad))
    (.lvf normal (&-> plane-normal quad))
    (.mov.vf.w in-plane vf0)
    (.outer.product.a.vf acc source normal)
    (.outer.product.b.vf in-plane normal source acc)
    (.outer.product.a.vf acc normal in-plane)
    (.outer.product.b.vf in-plane in-plane normal acc)
    (.svf (&-> dst quad) in-plane)
    ;; dst is now the normal part of src
    (let* ((in-plane-length (vector-length dst)) ;; len of normal component
           (normal-component (vector-dot dst plane-normal)) ;; always zero?
           (lift (- (* 0.02 in-plane-length) normal-component)))
      ;; scale down and limit the normal component
      (vector+float*! dst dst plane-normal (fmin (meters 4) (* 16.0 lift))))))

(defun vector-segment-distance-point! ((point vector) (segment-start vector) (segment-end vector) (closest-point vector))
  "Compute the distance from a point to the closest point on the line segment.
   Write that point to closest-point unless it is #f. The projected distance along
   the segment is clamped before measuring the remaining perpendicular displacement.
   The return value is a true distance, not a squared one. closest-point may alias any input and
   receives w = 1.0. A zero-length segment divides by zero and makes both results unusable."
  (local-vars (perpendicular-distance float) (segment-length float) (along-raw float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (segment :class vf)
         (length-work :class vf)
         (start :class vf)
         (end :class vf)
         (probe-point :class vf)
         (start-to-point :class vf)
         (along :class vf)
         (closest :class vf))
    (init-vf0-vector)
    (nop!)
    (.lvf start (&-> segment-start quad))
    (.lvf end (&-> segment-end quad))
    (.lvf probe-point (&-> point quad))
    (.sub.vf segment end start)
    (.sub.vf start-to-point probe-point start)
    ;; Sum the squared lanes into w, then take the square root of that one lane.
    (.mul.vf length-work segment segment)
    (.mul.x.vf.w acc vf0 length-work)
    (.add.mul.y.vf.w acc vf0 length-work acc)
    (.add.mul.z.vf.w length-work vf0 length-work acc)
    (.sqrt.vf Q length-work :ftf #b11)
    (.wait.vf)
    (.add.vf.x length-work vf0 Q)
    (.nop.vf)
    (.nop.vf)
    (.div.vf Q vf0 length-work :fsf #b11 :ftf #b0)
    (.mov segment-length length-work)
    (let ((segment-max segment-length))
      (.wait.vf)
      ;; segment becomes the unit direction, so the dot product below is a length along it.
      (.mul.vf segment segment Q)
      (.mul.vf along segment start-to-point)
      (let ((segment-min 0.0))
        (.add.y.vf.x along along along)
        (.add.z.vf.x along along along)
        (.mov along-raw along)
        (let ((along-clamped along-raw))
          (b! (< along-clamped segment-min) clamp-done :likely-delay (set! along-clamped segment-min))
          (b! (< segment-max along-clamped) clamp-done :likely-delay (set! along-clamped segment-max))
          (label clamp-done)
          (let ((clamped-bits along-clamped)) (.mov along clamped-bits)))))
    ;; segment becomes the offset from segment-start to the closest point.
    (.mul.x.vf segment segment along)
    (b! (= closest-point #f) measure-perpendicular :delay (.mov.vf.w closest vf0))
    (.add.vf.xyz closest start segment)
    (.svf (&-> closest-point quad) closest)
    (label measure-perpendicular)
    (.sub.vf length-work start-to-point segment)
    (.mul.vf length-work length-work length-work)
    (.mul.x.vf.w acc vf0 length-work)
    (.add.mul.y.vf.w acc vf0 length-work acc)
    (.add.mul.z.vf.w length-work vf0 length-work acc)
    (.sqrt.vf Q length-work :ftf #b11)
    (.wait.vf)
    (.add.vf.x length-work vf0 Q)
    (.nop.vf)
    (.mov perpendicular-distance length-work)
    perpendicular-distance))

(defun vector-line-distance ((point vector) (line-point vector) (line-point-2 vector))
  "Return the distance from point to the infinite line through line-point and line-point-2. This
   is a true distance, and the line is not clipped to the segment between the two points. The two
   points must differ, or the normalize divides by zero."
  (let* ((line-direction (vector-normalize! (vector-! (new-stack-vector0) line-point-2 line-point) 1.0))
         (point-offset (vector-! (new-stack-vector0) point line-point))
         (distance-along-line (vector-dot line-direction point-offset))
         (projected-offset (vector-float*! (new-stack-vector0) line-direction distance-along-line)))
    (vector-length (vector-! (new-stack-vector0) point-offset projected-offset))))

(defun vector-line-distance-point! ((point vector) (line-point vector) (line-point-2 vector) (closest-point vector))
  "Return the distance from point to an infinite line and optionally write its closest point.
   closest-point may be #f, and is not clamped to the segment between the two line points, so it
   can land outside them. It receives w = 1.0 and may alias any input."
  (let* ((line-direction (vector-normalize! (vector-! (new-stack-vector0) line-point-2 line-point) 1.0))
         (point-offset (vector-! (new-stack-vector0) point line-point))
         (distance-along-line (vector-dot line-direction point-offset))
         (projected-offset (vector-float*! (new-stack-vector0) line-direction distance-along-line)))
    (if closest-point (vector+! closest-point line-point projected-offset))
    (vector-length (vector-! (new-stack-vector0) point-offset projected-offset))))

(defun vector-orient-by-quat! ((dst vector) (src vector) (rotation quaternion))
  "Rotate src by rotation and write the result to dst. rotation must be unit; a longer quaternion
   scales the result by its squared norm. dst may alias src and keeps src's w."
  ;; Rather than call quaternion->matrix, this builds the three rotation rows directly out of the
  ;; quaternion's own lanes. Each row starts as a permutation of (w, z, -y) and friends, is crossed
  ;; with 2q, and picks up the 1.0 on its diagonal from vf0's w.
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (quat :class vf)
         (row0 :class vf)
         (row1 :class vf)
         (row2 :class vf)
         (quat-doubled :class vf)
         (vec :class vf))
    (init-vf0-vector)
    (.lvf quat (&-> rotation vec quad))
    (.lvf vec (&-> src quad))
    (.add.vf quat-doubled quat quat)
    (.add.w.vf.x row0 vf0 quat)
    (.add.z.vf.y row0 vf0 quat)
    (.sub.y.vf.z row0 vf0 quat)
    (.sub.w.vf.w row0 vf0 vf0)
    (.sub.z.vf.x row1 vf0 quat)
    (.add.w.vf.y row1 vf0 quat)
    (.add.x.vf.z row1 vf0 quat)
    (.sub.w.vf.w row1 vf0 vf0)
    (.add.y.vf.x row2 vf0 quat)
    (.sub.x.vf.y row2 vf0 quat)
    (.add.w.vf.z row2 vf0 quat)
    (.sub.w.vf.w row2 vf0 vf0)
    (.outer.product.a.vf acc quat-doubled row0)
    (.outer.product.b.vf row0 row0 quat-doubled acc)
    (.outer.product.a.vf acc quat-doubled row1)
    (.outer.product.b.vf row1 row1 quat-doubled acc)
    (.outer.product.a.vf acc quat-doubled row2)
    (.outer.product.b.vf row2 row2 quat-doubled acc)
    (.add.w.vf.x row0 row0 vf0)
    (.add.w.vf.y row1 row1 vf0)
    (.add.w.vf.z row2 row2 vf0)
    (.mul.w.vf acc vf0 vec)
    (.add.mul.x.vf acc row0 vec acc)
    (.add.mul.y.vf acc row1 vec acc)
    (.add.mul.z.vf vec row2 vec acc)
    (.svf (&-> dst quad) vec)
    dst))

;; 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 ((dst matrix) (forward vector) (down vector))
  "Build an inverse rotation matrix whose +z axis follows forward and -y points toward down."
  (vector-normalize-copy! (-> dst vector 2) forward 1.0)
  (vector-cross! (the-as vector (-> dst vector)) (-> dst vector 2) down)
  (vector-normalize! (the-as vector (-> dst vector)) 1.0)
  (vector-cross! (-> dst vector 1) forward (the-as vector (-> dst vector)))
  (vector-normalize! (-> dst vector 1) 1.0)
  (set! (-> dst vector 3 quad) (the-as uint128 0))
  (set! (-> dst vector 0 w) 0.0)
  (set! (-> dst vector 1 w) 0.0)
  (set! (-> dst vector 2 w) 0.0)
  (set! (-> dst vector 3 w) 1.0)
  dst)

(defun forward-down-nopitch->inv-matrix ((dst matrix) (forward vector) (down vector))
  "Build an inverse rotation matrix from forward and down, taking pitch from down."
  (vector-normalize-copy! (-> dst vector 1) down 1.0)
  (vector-negate! (-> dst vector 1) (-> dst vector 1))
  (vector-cross! (-> dst vector 0) (-> dst vector 1) forward)
  (vector-normalize! (-> dst vector 0) 1.0)
  (vector-cross! (-> dst vector 2) (-> dst vector 0) (-> dst vector 1))
  (vector-normalize! (-> dst vector 2) 1.0)
  (vector-zero! (-> dst vector 3))
  (set! (-> dst vector 0 w) 0.0)
  (set! (-> dst vector 1 w) 0.0)
  (set! (-> dst vector 2 w) 0.0)
  (set! (-> dst vector 3 w) 1.0)
  dst)

(defun forward-up-nopitch->inv-matrix ((dst matrix) (forward vector) (up vector))
  "Build an inverse rotation matrix from forward and up, taking pitch from up."
  (forward-down-nopitch->inv-matrix dst forward (vector-negate! (new-stack-vector0) up)))

(defun forward-up-nopitch->quaternion ((dst quaternion) (forward vector) (up vector))
  "Build a quaternion from forward and up, taking pitch from up."
  (matrix->quaternion dst (forward-up-nopitch->inv-matrix (new-stack-matrix0) forward up)))

(defun forward-up->quaternion ((dst quaternion) (forward vector) (up vector))
  "Build a quaternion from forward and up while retaining the pitch in forward."
  (matrix->quaternion dst
                      (forward-down->inv-matrix (new-stack-matrix0) forward (vector-negate! (new 'stack-no-clear 'vector) up))))

(defun quaternion-from-two-vectors! ((dst quaternion) (from vector) (to vector))
  "Build the shortest-arc quaternion rotating unit vector from onto unit vector to.
   Parallel and antiparallel inputs are singular because the cross-product axis has zero length."
  ;; cross(from,to) is the axis times sin(angle) and dot(from, to) is cos(angle), so the half-angle
  ;; identities give the quaternion without ever forming the angle itself.
  (let* ((axis (vector-cross! (new-stack-vector0) from to))
         (sin-angle (vector-length axis))
         (cos-angle (vector-dot from to)))
    (let ((scale (/ (sqrtf (* 0.5 (- 1.0 cos-angle))) sin-angle)))
      (set! (-> dst x) (* (-> axis x) scale))
      (set! (-> dst y) (* (-> axis y) scale))
      (set! (-> dst z) (* (-> axis z) scale)))
    (set! (-> dst w) (sqrtf (* 0.5 (+ 1.0 cos-angle)))))
  dst)

(defun quaternion-from-two-vectors-max-angle! ((dst quaternion) (from vector) (to vector) (max-angle float))
  "Build the shortest-arc quaternion between two unit vectors, capped at max-angle rotation units."
  (let* ((axis (vector-cross! (new-stack-vector0) from to))
         (sin-angle (vector-length axis))
         (cos-angle (vector-dot from to))
         (sin-half-angle (sqrtf (* 0.5 (- 1.0 cos-angle)))))
    (let ((sin-half-max (sin (* 0.5 max-angle))))
      (cond
        ((< sin-half-max sin-half-angle) (set! sin-half-angle sin-half-max) (set! (-> dst w) (cos (* 0.5 max-angle))))
        (else (set! (-> dst w) (sqrtf (* 0.5 (+ 1.0 cos-angle)))))))
    (let ((scale (/ sin-half-angle sin-angle)))
      (set! (-> dst x) (* (-> axis x) scale))
      (set! (-> dst y) (* (-> axis y) scale))
      (set! (-> dst z) (* (-> axis z) scale))))
  dst)

;; Every matrix-from-two-vectors variant crosses to with from, not from with to. That is not a
;; mistake. matrix-axis-sin-cos! writes the textbook column-vector
;; Rodrigues matrix, while GOAL multiplies row vector by matrix, so we correct things for GOAL here.
;;
;; All four take unit inputs. from . to is the cosine directly, and the sine comes back from
;; sqrt(1 - cos^2), which is always nonnegative, so these build the shortest arc and never the long
;; way around.

(defun matrix-from-two-vectors! ((dst matrix) (from vector) (to vector))
  "Build the shortest-arc rotation matrix between two unit vectors. dst is a full affine matrix
   with zero translation."
  (let* ((axis (vector-normalize! (vector-cross! (new-stack-vector0) to from) 1.0))
         (cos-angle (vector-dot from to))
         (sin-angle (sqrtf (- 1.0 (square cos-angle)))))
    (matrix-axis-sin-cos! dst axis sin-angle cos-angle)))

(defun matrix-from-two-vectors-max-angle! ((dst matrix) (from vector) (to vector) (max-angle float))
  "Build the shortest-arc rotation between two unit vectors, capped at max-angle."
  (let ((axis (vector-normalize! (vector-cross! (new-stack-vector0) to from) 1.0))
        (cos-angle (vector-dot from to))
        (cos-max (cos max-angle)))
    (if (< cos-angle cos-max)
        (matrix-axis-sin-cos! dst axis (sin max-angle) cos-max)
        (matrix-axis-sin-cos! dst axis (sqrtf (- 1.0 (square cos-angle))) cos-angle))))

;; note: these two interpolation functions may be equivalent to slerp.

(defun matrix-from-two-vectors-max-angle-partial! ((dst matrix) (from vector) (to vector) (max-angle float) (fraction float))
  "Build a fractional shortest-arc rotation capped at max-angle. This interpolates the cosine,
   which doesn't linearly interpolate the rotation angle, but is faster."
  (let* ((axis (vector-normalize! (vector-cross! (new-stack-vector0) to from) 1.0))
         (cos-angle (vector-dot from to))
         (max-cos (cos max-angle))
         (blended-cos (+ 1.0 (* (+ -1.0 cos-angle) fraction))))
    (if (< blended-cos max-cos)
        (matrix-axis-sin-cos! dst axis (sin max-angle) max-cos)
        (matrix-axis-sin-cos! dst axis (sqrtf (- 1.0 (square blended-cos))) blended-cos))))

(defun matrix-from-two-vectors-partial-linear! ((dst matrix) (from vector) (to vector) (fraction float))
  "Build a fractional shortest-arc rotation between two unit vectors by scaling the angle."
  (let ((axis (vector-normalize! (vector-cross! (new-stack-vector0) to from) 1.0))
        (cos-angle (vector-dot from to)))
    (cond
      ((< 0.9999 (fabs cos-angle))
       (matrix-identity! dst))
      (else
       (let ((blended-cos (cos (* fraction (acos cos-angle)))))
         (matrix-axis-sin-cos! dst axis (sqrtf (- 1.0 (square blended-cos))) blended-cos))))))

(defun matrix-remove-z-rot ((rotation matrix) (reference matrix))
  "Rotate rotation about its own third row so that its second row lines up with reference's first
   row flattened into that plane, removing the roll between them. rotation is modified in place and
   returned; reference is only read, and only its first row is used. Rows within 0.99999 of aligned
   are left alone."
  ;; The target direction is reference's x axis negated and projected into the plane perpendicular
  ;; to rotation's z axis, so it is the closest in-plane heading to "behind the reference". The
  ;; correction angle comes from the cross product of rotation's y axis with that target: its
  ;; length is the sine and its sign relative to rotation's z axis says which way. The sine is
  ;; negated when the cross agrees with z, because a row-vector product by the matrix
  ;; matrix-axis-sin-cos! builds rotates by the negative of the angle it was handed.
  (let ((target (new-stack-vector0)))
    0.0
    0.0
    (let ((twist (new-stack-matrix0)))
      (vector-negate! target (the-as vector reference))
      (vector-flatten! target target (-> rotation vector 2))
      (vector-normalize! target 1.0)
      (let ((cos-twist (vector-dot (-> rotation vector 1) target)))
        (when (< cos-twist 0.99999)
          (vector-cross! target (-> rotation vector 1) target)
          (let ((sin-twist (vector-length target)))
            (if (< 0.0 (vector-dot target (-> rotation vector 2))) (set! sin-twist (- sin-twist)))
            (matrix-axis-sin-cos! twist (-> rotation vector 2) sin-twist cos-twist))
          (matrix*! rotation rotation twist)))))
  rotation)

(defun matrix-rot-diff! ((axis vector) (from matrix) (to matrix))
  "Return the rotation-unit angle between two rotation matrices and write its unit axis to axis.
   The returned angle is never negative, because the difference quaternion is flipped to the
   positive-w hemisphere first. axis is negated relative to the difference quaternion's vector part
   and gets w = 1.0. Both matrices must be rotations; a scale or shear makes matrix->quaternion
   meaningless."
  ;; When from and to are equal the difference quaternion is the identity, the axis is exactly zero,
  ;; and the normalize below has nothing to work with. The y = 1.0 fallback only sets y, which is
  ;; enough on the EE: vrsqrt of zero saturates to max-float and zero times that is still zero. The
  ;; PC build divides by zero instead, so x and z come back NaN. Nothing calls this.
  (let ((from-rot (new-stack-quaternion0))
        (to-rot (new-stack-quaternion0))
        (difference (new-stack-quaternion0)))
    0.0
    (matrix->quaternion from-rot from)
    (matrix->quaternion to-rot to)
    (quaternion-conjugate! difference from-rot)
    (quaternion*! difference to-rot difference)
    (quaternion-normalize! difference)
    (if (< (-> difference w) 0.0) (quaternion-negate! difference difference))
    (let ((angle (* 2.0 (acos (-> difference w)))))
      (set! (-> axis quad) (-> difference vec quad))
      (vector-negate! axis axis)
      (if (= (vector-normalize-ret-len! axis 1.0) 0.0) (set! (-> axis y) 1.0))
      angle)))

(defun quaternion-seek ((value quaternion) (from quaternion) (to quaternion) (unused-rate float) (max-angle float))
  "Rotate value toward to by at most max-angle, using only the forward axes of from and to.
   unused-rate is ignored. The implementation is a bit strange and doesn't handle roll."
  ;; Only the third row of each matrix is read, so any roll difference between from and to is
  ;; discarded and value keeps its own. value is both the accumulator and the output: the step is
  ;; post-multiplied onto whatever it already holds, which is not required to agree with from.
  (let ((from-matrix (new-stack-matrix0))
        (to-matrix (new-stack-matrix0)))
    (quaternion->matrix from-matrix from)
    (quaternion->matrix to-matrix to)
    (let ((step (new-stack-quaternion0)))
      (quaternion-from-two-vectors-max-angle! step (-> from-matrix vector 2) (-> to-matrix vector 2) max-angle)
      (quaternion-normalize! (quaternion*! value value step)))))

(defun vector-deg-seek ((dst vector) (from vector) (to vector) (max-angle float))
  "Rotate from toward to by at most max-angle rotation units and write the result to dst. from and
   to must be unit length."
  (let ((step (new-stack-matrix0)))
    (matrix-from-two-vectors-max-angle! step from to max-angle)
    (vector-matrix*! dst from step)))

(defun vector-deg-slerp ((dst vector) (from vector) (to vector) (t float))
  "Spherically interpolate from one vector direction to another by t, preserving from's magnitude.
   The inputs are normalized internally."
  (cond
    ((>= 0.0 t) (vector-copy! dst from) dst)
    ((>= t 1.0) (vector-copy! dst to) dst)
    (else
     (let ((step (new-stack-matrix0)))
       (let ((from-unit (vector-normalize-copy! (new 'stack-no-clear 'vector) from 1.0))
             (to-unit (vector-normalize-copy! (new 'stack-no-clear 'vector) to 1.0)))
         (matrix-from-two-vectors-partial-linear! step from-unit to-unit t))
       (vector-matrix*! dst from step)))))

(defun vector-vector-deg-slerp! ((dst vector) (from vector) (to vector) (t float) (up vector))
  "Interpolate direction from from to to around a shared up vector while linearly blending
   magnitude. Both directions are turned into full orientations against up before the slerp, so up
   must not be parallel to either one."
  (cond
    ((>= 0.0 t)
     (vector-copy! dst from))
    ((>= t 1.0)
     (vector-copy! dst to))
    (else
     (let* ((from-unit (vector-normalize-copy! (new 'stack-no-clear 'vector) from 1.0))
            (to-unit (vector-normalize-copy! (new 'stack-no-clear 'vector) to 1.0))
            (from-rot (forward-up->quaternion (new 'stack-no-clear 'quaternion) from-unit up))
            (to-rot (forward-up->quaternion (new 'stack-no-clear 'quaternion) to-unit up))
            (blended-rot (quaternion-slerp! (new 'stack-no-clear 'quaternion) from-rot to-rot t)))
       (vector-normalize-copy! dst
                               (vector-z-quaternion! (new 'stack-no-clear 'vector) blended-rot)
                               (lerp (vector-length from) (vector-length to) t)))))
  dst)

(defun normal-of-plane ((dst vector) (point-a vector) (point-b vector) (point-c vector))
  "Compute a unit normal for the plane through three points. The normal follows the right-hand rule
   for the points taken in a, b, c order, so reversing any two arguments flips it."
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (a-to-b :class vf)
         (c-to-b :class vf)
         (vertex-b :class vf)
         (normal :class vf)
         (length-work :class vf))
    (init-vf0-vector)
    ;; point-a and point-c are loaded into the registers that immediately become the two edges.
    (.lvf vertex-b (&-> point-b quad))
    (.lvf a-to-b (&-> point-a quad))
    (.lvf c-to-b (&-> point-c quad))
    (.sub.vf a-to-b vertex-b a-to-b)
    (.sub.vf c-to-b vertex-b c-to-b)
    (.outer.product.a.vf acc c-to-b a-to-b)
    (.outer.product.b.vf normal a-to-b c-to-b acc)
    (.mul.vf length-work normal normal)
    (.add.y.vf.x length-work length-work length-work)
    (.add.z.vf.x length-work length-work length-work)
    (.isqrt.vf Q vf0 length-work :fsf #b11 :ftf #b0)
    (.mov.vf.w normal vf0)
    (.wait.vf)
    (.mul.vf.xyz normal normal Q)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> dst quad) normal)
    dst))

(defun vector-3pt-cross! ((dst vector) (origin vector) (point-a vector) (point-b vector))
  "Cross the displacements from origin to point-a and point-b without normalizing the result. The
   length is therefore twice the area of the triangle, which callers use as an area or winding test
   as well as a normal direction."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (base :class vf)
         (base-to-a :class vf)
         (base-to-b :class vf)
         (cross :class vf))
    (init-vf0-vector)
    (.lvf base (&-> origin quad))
    (.lvf base-to-a (&-> point-a quad))
    (.lvf base-to-b (&-> point-b quad))
    (.add.x.vf.w cross vf0 vf0)
    (.sub.vf base-to-a base-to-a base)
    (.sub.vf base-to-b base-to-b base)
    (.outer.product.a.vf acc base-to-a base-to-b)
    (.outer.product.b.vf cross base-to-b base-to-a acc)
    (.svf (&-> dst quad) cross)
    dst))

(defun closest-pt-in-triangle ((dst vector) (point vector) (triangle matrix) (normal vector))
  "Write the closest point on triangle to point.
   The function projects onto the plane, then clamps to the nearest edge when the projection lies outside.
   triangle's first three rows are the vertices and its fourth is ignored. normal must be the
   triangle's own unit normal"
  ;; The three edge tests are cross products of the two triangle edges leaving a shared vertex with
  ;; the vector to point, dotted against the normal. Each dot lands in lane y so that the wide
  ;; register move below carries it in the high word of a 64-bit value, where its sign bit is the
  ;; integer sign bit. The three signs become bits 0, 1 and 2 of one mask, and the mask selects the
  ;; region: zero is the interior, one bit is an edge, two bits are the corner between them. All
  ;; three bits set is geometrically impossible for a consistent normal and shares the last branch.
  ;; (declare (print-asm))
  (local-vars (edge-01-bits uint)
              (mask-minus-1 uint)
              (mask-minus-2 uint)
              (mask-minus-3 uint)
              (mask-minus-4 uint)
              (mask-minus-5 uint)
              (edge-20-bits uint)
              (edge-12-bits uint))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (plane-normal :class vf)
         (v0-to-point :class vf)
         (edge-01-side :class vf)
         (edge-12-side :class vf)
         (edge-20-side :class vf)
         (edge-01-cross :class vf)
         (edge-12-cross :class vf)
         (edge-20-cross :class vf)
         (plane-offset :class vf)
         (projected :class vf)
         (vertex-0 :class vf)
         (vertex-1 :class vf)
         (vertex-2 :class vf)
         (probe-point :class vf)
         (v2-to-v1 :class vf)
         (point-to-v1 :class vf)
         (v0-to-v1 :class vf)
         (v2-to-point :class vf))
    (init-vf0-vector)
    (nop!)
    (nop!)
    (.lvf vertex-1 (&-> triangle vector 1 quad))
    (nop!)
    (.lvf vertex-2 (&-> triangle vector 2 quad))
    (nop!)
    (.lvf probe-point (&-> point quad))
    (.sub.vf v2-to-v1 vertex-1 vertex-2)
    (.lvf vertex-0 (&-> triangle vector 0 quad))
    (.sub.vf point-to-v1 vertex-1 probe-point)
    (.lvf plane-normal (&-> normal quad))
    (.sub.vf v0-to-v1 vertex-1 vertex-0)
    (.sub.vf v2-to-point probe-point vertex-2)
    (.sub.vf v0-to-point probe-point vertex-0)
    (.outer.product.a.vf acc point-to-v1 v0-to-v1)
    (.outer.product.b.vf edge-01-cross v0-to-v1 point-to-v1 acc)
    (.outer.product.a.vf acc v2-to-v1 point-to-v1)
    (.outer.product.b.vf edge-12-cross point-to-v1 v2-to-v1 acc)
    (.mul.vf edge-01-side edge-01-cross plane-normal)
    (.outer.product.a.vf acc v2-to-point v0-to-point)
    (.outer.product.b.vf edge-20-cross v0-to-point v2-to-point acc)
    (.mul.vf edge-12-side edge-12-cross plane-normal)
    (.add.x.vf.y edge-01-side edge-01-side edge-01-side)
    (.mul.vf edge-20-side edge-20-cross plane-normal)
    (.add.x.vf.y edge-12-side edge-12-side edge-12-side)
    (.add.x.vf.y edge-20-side edge-20-side edge-20-side)
    (.add.z.vf.y edge-01-side edge-01-side edge-01-side)
    (.add.z.vf.y edge-12-side edge-12-side edge-12-side)
    (.add.z.vf.y edge-20-side edge-20-side edge-20-side)
    ;; og:preserve-this these types were changed to uint to make this copy 64 bits.
    (.mov edge-01-bits edge-01-side)
    (.mov edge-12-bits edge-12-side)
    (.mov edge-20-bits edge-20-side)
    (let* ((edge-01-outside (shr (the-as int edge-01-bits) 63))
           (edge-12-outside (shr (the-as int edge-12-bits) 63))
           (edge-20-outside (shr (the-as int edge-20-bits) 63))
           (edge-12-bit (* edge-12-outside 2))
           (edge-20-bit (* edge-20-outside 4))
           (outside-mask (logior (logior edge-01-outside edge-12-bit) edge-20-bit)))
      (b! (nonzero? outside-mask) outside-triangle :delay (set! mask-minus-1 (the-as uint (+ outside-mask -1)))))
    ;; Inside. Project point onto the triangle's plane through vertex 0 with the double cross.
    (.sub.vf plane-offset probe-point vertex-0)
    (.mov.vf.w projected vf0)
    (.outer.product.a.vf acc plane-offset plane-normal)
    (.outer.product.b.vf projected plane-normal plane-offset acc)
    (.outer.product.a.vf acc plane-normal projected)
    (.outer.product.b.vf projected projected plane-normal acc)
    (.add.vf.xyz projected projected vertex-0)
    (b! #t closest-point-done :delay (.svf (&-> dst quad) projected))
    (nop!)
    (label outside-triangle)
    (b! (nonzero? mask-minus-1) test-edge-12 :delay (set! mask-minus-2 (the-as uint (+ mask-minus-1 -1))))
    (vector-segment-distance-point! point (the-as vector (-> triangle vector)) (-> triangle vector 1) dst)
    (goto closest-point-done)
    (label test-edge-12)
    (b! (nonzero? mask-minus-2) test-vertex-1 :delay (set! mask-minus-3 (the-as uint (+ mask-minus-2 -1))))
    (vector-segment-distance-point! point (-> triangle vector 1) (-> triangle vector 2) dst)
    (goto closest-point-done)
    (label test-vertex-1)
    (b! (nonzero? mask-minus-3) test-edge-20 :delay (set! mask-minus-4 (the-as uint (+ mask-minus-3 -1))))
    (let ((first-distance (vector-segment-distance-point! point (-> triangle vector 1) (the-as vector (-> triangle vector)) dst))
          (second-point (new 'stack-no-clear 'vector)))
      (if (< (vector-segment-distance-point! point (-> triangle vector 1) (-> triangle vector 2) second-point) first-distance)
        (vector-copy! dst second-point)))
    (goto closest-point-done)
    (label test-edge-20)
    (b! (nonzero? mask-minus-4) test-vertex-0 :delay (set! mask-minus-5 (the-as uint (+ mask-minus-4 -1))))
    (vector-segment-distance-point! point (-> triangle vector 2) (the-as vector (-> triangle vector)) dst)
    (goto closest-point-done)
    (label test-vertex-0)
    (b! (nonzero? mask-minus-5) vertex-2-region)
    (let ((first-distance (vector-segment-distance-point! point (the-as vector (-> triangle vector)) (-> triangle vector 1) dst))
          (second-point (new 'stack-no-clear 'vector)))
      (if (< (vector-segment-distance-point! point (the-as vector (-> triangle vector)) (-> triangle vector 2) second-point) first-distance)
        (vector-copy! dst second-point)))
    (goto closest-point-done)
    (label vertex-2-region)
    (let ((first-distance (vector-segment-distance-point! point (-> triangle vector 2) (the-as vector (-> triangle vector)) dst))
          (second-point (new 'stack-no-clear 'vector)))
      (if (< (vector-segment-distance-point! point (-> triangle vector 2) (-> triangle vector 1) second-point) first-distance)
        (vector-copy! dst second-point)))
    (label closest-point-done)
    0
    (none)))

(defun point-in-triangle-cross ((point vector) (normal vector) (vertex-a vector) (vertex-b vector) (vertex-c vector))
  "Test whether a coplanar point lies inside a triangle using consistently oriented edge crosses.
   point is assumed already in the triangle's plane; nothing here measures how far off it is, so a
   point anywhere along the prism through the triangle reports true. normal must point the way
   normal-of-plane would for the vertices in a, b, c order, and only its direction matters. Points
   exactly on an edge count as inside. Returns true when the point is inside."
  ;; One test per edge: cross the two vectors leaving the shared vertex, dot against the normal, and
  ;; ask whether the result is negative. The dot is summed into lane y so that the 64-bit register
  ;; move puts it in the high word of an int, where a negative float has the integer sign bit set.
  ;; Oring the three carriers together and testing the sign then answers "was any of the three
  ;; negative" in a single comparison, without three branches and without leaving the accumulator.
  (local-vars (ca-bits int) (bc-bits int) (ab-bits int))
  (rlet ((acc :class vf)
         (plane-normal :class vf)
         (a-to-point :class vf)
         (bc-side :class vf)
         (ab-side :class vf)
         (ca-side :class vf)
         (probe-point :class vf)
         (c-to-b :class vf)
         (point-to-b :class vf)
         (a-to-b :class vf)
         (c-to-point :class vf))
    ;; The three vertices are loaded into the registers that go on to hold the three edge results.
    (.lvf ab-side (&-> vertex-b quad))
    (.lvf ca-side (&-> vertex-c quad))
    (.lvf probe-point (&-> point quad))
    (.lvf bc-side (&-> vertex-a quad))
    (.lvf plane-normal (&-> normal quad))
    (.sub.vf c-to-b ab-side ca-side)
    (.sub.vf point-to-b ab-side probe-point)
    (.sub.vf a-to-b ab-side bc-side)
    (.sub.vf c-to-point probe-point ca-side)
    (.sub.vf a-to-point probe-point bc-side)
    (.outer.product.a.vf acc c-to-b point-to-b)
    (.outer.product.b.vf bc-side point-to-b c-to-b acc)
    (.outer.product.a.vf acc point-to-b a-to-b)
    (.outer.product.b.vf ab-side a-to-b point-to-b acc)
    (.outer.product.a.vf acc c-to-point a-to-point)
    (.outer.product.b.vf ca-side a-to-point c-to-point acc)
    (.mul.vf bc-side bc-side plane-normal)
    (.mul.vf ab-side ab-side plane-normal)
    (.nop.vf)
    (.mul.vf ca-side ca-side plane-normal)
    (.add.x.vf.y bc-side bc-side bc-side)
    (.add.x.vf.y ab-side ab-side ab-side)
    (.add.x.vf.y ca-side ca-side ca-side)
    (.nop.vf)
    (.add.z.vf.y bc-side bc-side bc-side)
    (.add.z.vf.y ab-side ab-side ab-side)
    (.add.z.vf.y ca-side ca-side ca-side)
    (.nop.vf)
    (.mov bc-bits bc-side)
    (.mov ab-bits ab-side)
    (.mov ca-bits ca-side)
    (>= (the-as int (logior (logior bc-bits ab-bits) ca-bits)) 0)))

(defun point-in-plane-<-point+normal! ((dst vector) (point vector) (normal vector))
  "Construct another point in the plane through point with the supplied normal.
   Two coordinates are offset and the coordinate with the largest normal component is solved from the plane equation.
   The two free coordinates are offset by exactly one meter, so dst is a second plane point roughly
   a meter and a half away rather than a direction. normal need not be unit."
  ;; Which coordinate to solve for is decided by magnitude so the division is by the largest
  ;; component available. Exact ties fall through to z.
  (let ((plane-offset (+ (+ (* (-> normal x) (-> point x)) (* (-> normal y) (-> point y))) (* (-> normal z) (-> point z)))))
    (set! (-> dst w) 1.0)
    (let ((abs-x (fabs (-> normal x)))
          (abs-y (fabs (-> normal y)))
          (abs-z (fabs (-> normal z))))
      (cond
        ((and (< abs-y abs-x) (< abs-z abs-x))
         (set! (-> dst y) (+ (meters 1) (-> point y)))
         (set! (-> dst z) (+ (meters 1) (-> point z)))
         (set! (-> dst x) (/ (+ (- (- (* (-> normal y) (-> dst y))) (* (-> normal z) (-> dst z))) plane-offset) (-> normal x))))
        ((and (< abs-x abs-y) (< abs-z abs-y))
         (set! (-> dst x) (+ (meters 1) (-> point x)))
         (set! (-> dst z) (+ (meters 1) (-> point z)))
         (set! (-> dst y) (/ (- (- plane-offset (* (-> normal x) (-> dst x))) (* (-> normal z) (-> dst z))) (-> normal y))))
        (else
         (set! (-> dst x) (+ (meters 1) (-> point x)))
         (set! (-> dst y) (+ (meters 1) (-> point y)))
         (set! (-> dst z) (/ (+ (- (- (* (-> normal x) (-> dst x))) (* (-> normal y) (-> dst y))) plane-offset) (-> normal z)))))))
  dst)


(defun circle-circle-xz-intersect ((circle-a sphere) (circle-b sphere) (intersection-a vector) (intersection-b vector))
  "Intersect two circles in the xz plane. Unused and not ported."
  (format 0 "circle-circle-xz-intersect~%")
  (crash!)
  0)

(defun circle-test ()
  "Test the circle-circle-xz-intersect function."
  (let ((circle-a (new 'stack 'sphere))
        (circle-b (new 'stack 'sphere))
        (intersection-a (new-stack-vector0))
        (intersection-b (new-stack-vector0)))
    (set-vector! circle-a 0.0 0.0 0.0 1.0)
    (set-vector! circle-b 100.0 0.0 0.0 10000.0)
    (let ((result (circle-circle-xz-intersect circle-a circle-b intersection-a intersection-b)))
      (format #t "res = ~d~%" result))
    (format #t "(~f, ~f)~%" (-> intersection-a x) (-> intersection-a z))
    (format #t "(~f, ~f)~%" (-> intersection-b x) (-> intersection-b z)))
  (none))

(defun vector-circle-tangent-new ((point-a vector) (point-b vector) (tangent-a vector) (tangent-b vector))
  "Write the xz tangent contact points from the position point-a to the circle centered on point-b
   with radius point-b's w. point-a's w is ignored, so this takes a point and a circle despite both
   arguments being vectors. Returns nothing, and the intersection count from
   circle-circle-xz-intersect is discarded, so a point-a inside the circle leaves the outputs
   untouched with no indication."
  ;; Thales' construction: the tangent contact points are exactly where the circle with the segment
  ;; from point-a to point-b as its diameter crosses the target circle, because the radius meets a
  ;; tangent at a right angle and any angle inscribed on a diameter is a right angle. So the whole
  ;; function is building that circle -- center at the midpoint, radius half the xz distance -- and
  ;; handing both circles to the general intersector. Only the x and z lanes take part; the
  ;; midpoint's y is computed and then ignored.
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (thales :class vf)
         (half :class vf)
         (circle-center :class vf)
         (from-point :class vf)
         (half-offset :class vf))
    (init-vf0-vector)
    (let ((thales-circle (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 ((half-value 0.5)) (.lvf circle-center (&-> point-b quad)) (.mov half half-value))
      (.lvf from-point (&-> point-a quad))
      (.add.vf thales circle-center from-point)
      (.sub.vf half-offset from-point circle-center)
      (.mul.x.vf thales thales half)
      (.mul.x.vf half-offset half-offset half)
      (.mul.vf.xz half-offset half-offset half-offset)
      (.add.z.vf.x half-offset half-offset half-offset)
      (.sqrt.vf Q half-offset :ftf #b0)
      (.wait.vf)
      ;; The Thales radius goes in w, which is where the sphere type keeps it.
      (.mul.vf.w thales vf0 Q)
      (.nop.vf)
      (.nop.vf)
      (.svf (&-> thales-circle quad) thales)
      (circle-circle-xz-intersect (the-as sphere point-b) thales-circle tangent-a tangent-b))
    0
    (none)))

(defun vector-circle-tangent ((point vector) (circle vector) (tangent-a vector) (tangent-b vector))
  "Write the two xz tangent points from an external point to a circle. circle's xz is the center and
   its w is the radius; y is ignored throughout. point must lie outside the circle: the arc cosine of
   radius over distance is not a number once the point is inside, the comparison below then fails,
   and the function falls into the general case and writes nonsense."
  ;; The angle at the center between the direction to point and the direction to either contact point
  ;; is acos(radius / distance), so both contacts are the center-to-point direction turned by plus and
  ;; minus that angle and walked out one radius. The negated radius appears because the stored
  ;; displacement runs from point toward the center, the opposite of the direction being walked.
  (let* ((to-center (vector-! (the-as vector (new 'stack-no-clear 'sphere)) circle point))
         (center-distance (vector-xz-length to-center))
         (tangent-angle (acos (/ (-> circle w) center-distance))))
    (cond
      ;; Very small tangent angles are numerically unstable; use the perpendicular fallback.
      ;; The fallback returns two points a full center-distance to either side of point rather than
      ;; points on the circle, which is only close to right because a tiny angle means point is
      ;; nearly on the circle already.
      ((>= (degrees 3) tangent-angle)
       (set! (-> tangent-a x) (- (-> point x) (-> to-center z)))
       (set! (-> tangent-a y) 0.0)
       (set! (-> tangent-a z) (+ (-> point z) (-> to-center x)))
       (set! (-> tangent-b x) (+ (-> point x) (-> to-center z)))
       (set! (-> tangent-b y) 0.0)
       (set! (-> tangent-b z) (- (-> point z) (-> to-center x))))
      (else
       (let ((center-angle (atan (-> to-center z) (-> to-center x)))
             (neg-radius (- (-> circle w)))
             (sines (new 'stack-no-clear 'vector)))
         (let ((cosines (new 'stack-no-clear 'vector)))
           (let ((angles (new 'stack-no-clear 'vector)))
             (set! (-> angles x) (- center-angle tangent-angle))
             (set! (-> angles y) (+ center-angle tangent-angle))
             (vector-sincos! sines cosines angles))
           (set! (-> tangent-a x) (+ (-> circle x) (* neg-radius (-> cosines x))))
           (set! (-> tangent-a z) (+ (-> circle z) (* neg-radius (-> sines x))))
           (set! (-> tangent-b x) (+ (-> circle x) (* neg-radius (-> cosines y)))))
         (set! (-> tangent-b z) (+ (-> circle z) (* neg-radius (-> sines y))))))))
  0
  (none))

;;;;;;;;;;;;;;;;;;;
;; curve
;;;;;;;;;;;;;;;;;;;

;; Curves are clamped cubic nonuniform B-splines. For N control vertices, a degree-three spline
;; requires N + 4 knots: with last control index n = N - 1 and degree p = 3, the last knot index is
;; m = n + p + 1 = N + 3, so the knot count is m + 1 = N + 4. Knots must be nondecreasing.
;;
;; Valid span indices are 3 through N - 1. Span i blends controls i - 3 through i, so the first
;; span reads controls 0..3 and the last reads controls N-4..N-1. Repeating the first and last knot
;; four times gives cubic endpoint clamping and makes the curve interpolate its endpoint controls.
;; For example, seven controls use eleven knots:
;;   [0 0 0 0  .25 .5 .75  1 1 1 1]
;; whose four spans select controls 0..3, 1..4, 2..5, and 3..6.
;;
;; Evaluation computes u = clamp(input * last-knot, first-knot, last-knot). Curve data uses a
;; zero first knot, so input [0, 1] maps to the full knot domain; this is not the general
;; first + input * (last - first) mapping. After locating u's span, the evaluator computes four
;; Cox-de Boor basis weights and blends that span's four controls.
;;
;; Input is parameter progress, not arc-length progress: .25 does not generally travel one quarter
;; of the curve's distance. Changing an interior knot while retaining the same controls changes the
;; basis weights of several neighboring spans and generally changes both shape and speed, not just
;; the point where one polynomial begins. Applying the same affine transform to every knot preserves
;; the curve when u receives the same transform; this evaluator handles uniform scaling directly
;; because first-knot is zero.
;;
;; The fast span candidate is for zero-origin, unit-spaced knots:
;;   [0 0 0 0  1 2 3 ...  last last last last]
;; When u is in [k, k+1), int(u) + 3 is the expected span: the +3 skips the first three repeated
;; zero entries before span 3. The candidate is checked against its actual knot bounds, so other
;; spacing falls back to binary search.

(defun find-knot-span ((high-span int) (low-span int) (value float) (knots (pointer float)))
  "Find the knot span containing value, using an integer-derived fast candidate before binary search.
   The highest candidate span comes first and the lowest second, which is the reverse of the usual
   order; for a clamped cubic spline they are knot-count - 5 and 3. The returned span i means
   knots[i] <= value < knots[i + 1], except for value exactly at the top knot, which is reported as
   the last span rather than as out of range."
  (let* ((top-knot (-> knots (+ high-span 1)))
         (candidate-span (+ (the int value) 3))
         (candidate-knots (&-> knots candidate-span)))
    (cond
      ;; The upper endpoint belongs to the final span despite its half-open interval.
      ((= value top-knot)
       high-span)
      ;; Unit-spaced knots make int(value) + 3 the likely span. Accept it only when its
      ;; actual knot interval contains value.
      ((and (>= value (-> candidate-knots 0))
            (< value (-> candidate-knots 1)))
       candidate-span)
      (else
       ;; Maintain [search-low, search-high) around the result. This also advances across
       ;; repeated knots, selecting the first nonempty span whose upper knot exceeds value.
       (let ((search-low low-span)
             (search-high (+ high-span 1)))
         (while (> (- search-high search-low) 1)
           (let ((mid-span (/ (+ search-low search-high) 2)))
             (if (< value (-> knots mid-span))
                 (set! search-high mid-span)
                 (set! search-low mid-span))))
         search-low)))))

(defun calculate-basis-functions-vector! ((dst vector) (span int) (value float) (knots (pointer float)))
  "Compute the four nonzero cubic B-spline basis weights for value in span."
  ;; Unrolled Cox-de Boor recurrence. left-N and right-N are the distances from value
  ;; to the knots on either side of the selected span.
  (let* ((span-knot (&-> knots span))
         (left-1 (- value (-> span-knot 0)))
         (left-2 (- value (-> span-knot -1)))
         (left-3 (- value (-> span-knot -2)))
         (right-1 (- (-> span-knot 1) value))
         (right-2 (- (-> span-knot 2) value))
         (right-3 (- (-> span-knot 3) value))
         ;; Linear basis functions.
         (linear-denominator (+ right-1 left-1))
         (linear-0 (/ right-1 linear-denominator))
         (linear-1 (/ left-1 linear-denominator))
         ;; Quadratic basis functions.
         (quadratic-0 (* right-1 (/ linear-0 (+ right-1 left-2))))
         (quadratic-1 (+ (* left-2 (/ linear-0 (+ right-1 left-2)))
                         (* right-2 (/ linear-1 (+ right-2 left-1)))))
         (quadratic-2 (* left-1 (/ linear-1 (+ right-2 left-1))))
         ;; Cubic basis functions.
         (basis-0 (* right-1 (/ quadratic-0 (+ right-1 left-3))))
         (basis-1 (+ (* left-3 (/ quadratic-0 (+ right-1 left-3)))
                     (* right-2 (/ quadratic-1 (+ right-2 left-2)))))
         (basis-2 (+ (* left-2 (/ quadratic-1 (+ right-2 left-2)))
                     (* right-3 (/ quadratic-2 (+ right-3 left-1)))))
         (basis-3 (* left-1 (/ quadratic-2 (+ right-3 left-1)))))
    (set! (-> dst x) basis-0)
    (set! (-> dst y) basis-1)
    (set! (-> dst z) basis-2)
    (set! (-> dst w) basis-3))
  dst)

(defun curve-evaluate! ((dst vector)
        (input float)
        (control-points (inline-array vector))
        (control-point-count int)
        (knots (pointer float))
        (knot-count int))
  "Evaluate a clamped cubic nonuniform B-spline at normalized input."
  (let* ((first-knot (-> knots 0))
         (last-knot (-> knots (+ knot-count -1)))
         (u (fmax (fmin (* input last-knot) last-knot) first-knot))
         (last-span (+ knot-count -5))
         (span (find-knot-span last-span 3 u knots))
         (basis (new 'stack-no-clear 'vector))
         (first-control (- span 3))
         (control-0 (-> control-points first-control))
         (control-1 (-> control-points (+ first-control 1)))
         (control-2 (-> control-points (+ first-control 2)))
         (control-3 (-> control-points (+ first-control 3))))
    (calculate-basis-functions-vector! basis span u knots)
    (set! (-> dst x) (+ (* (-> control-0 x) (-> basis x))
                        (* (-> control-1 x) (-> basis y))
                        (* (-> control-2 x) (-> basis z))
                        (* (-> control-3 x) (-> basis w))))
    (set! (-> dst y) (+ (* (-> control-0 y) (-> basis x))
                        (* (-> control-1 y) (-> basis y))
                        (* (-> control-2 y) (-> basis z))
                        (* (-> control-3 y) (-> basis w))))
    (set! (-> dst z) (+ (* (-> control-0 z) (-> basis x))
                        (* (-> control-1 z) (-> basis y))
                        (* (-> control-2 z) (-> basis z))
                        (* (-> control-3 z) (-> basis w))))
    (set! (-> dst w) 1.0))
  dst)

(defun curve-get-pos! ((dst vector) (input float) (curve-data curve))
  "Evaluate curve-data at normalized input and write its position to dst. dst.w becomes 1.0. The
   curve's cached length field is not consulted; input is parameter progress."
  (curve-evaluate! dst
                   input
                   (-> curve-data cverts)
                   (-> curve-data num-cverts)
                   (-> curve-data knots)
                   (-> curve-data num-knots)))

(defun curve-length ((curve-data curve))
  "Estimate total curve length with three uniformly spaced parameter samples per control point.
   This value does not make the curve distance-parameterized.
   The estimate is the length of the inscribed polyline through those samples, so it always
   undershoots the true arc length, and it undershoots more the tighter the curvature. It does not
   read or update the curve's cached length field."
  ;; The first sample uses knots[0] rather than 0.0 as the parameter. Curve data has a zero first
  ;; knot, so the two agree, but a curve whose first knot is nonzero would start the walk at the
  ;; clamped endpoint either way.
  (let ((previous-point (new-stack-vector0))
        (current-point (new-stack-vector0))
        (segment-count (* 3 (-> curve-data num-cverts)))
        (total-length 0.0))
    (curve-evaluate! current-point
                     (-> curve-data knots 0)
                     (-> curve-data cverts)
                     (-> curve-data num-cverts)
                     (-> curve-data knots)
                     (-> curve-data num-knots))
    (dotimes (i segment-count)
      (set! (-> previous-point quad) (-> current-point quad))
      (curve-evaluate! current-point
                       (/ (the float (+ i 1)) (the float segment-count))
                       (-> curve-data cverts)
                       (-> curve-data num-cverts)
                       (-> curve-data knots)
                       (-> curve-data num-knots))
      (+! total-length (vector-vector-distance previous-point current-point)))
    total-length))

(defun curve-copy! ((dst curve) (src curve))
  "Shallow-copy a curve descriptor, retaining its control-point and knot references. The two curves
   share the same control-point and knot storage afterwards, so editing one edits both."
  (set! (-> dst cverts) (-> src cverts))
  (set! (-> dst num-cverts) (-> src num-cverts))
  (set! (-> dst knots) (-> src knots))
  (set! (-> dst num-knots) (-> src num-knots))
  (set! (-> dst length) (-> src length))
  dst)

(defun curve-closest-point ((curve-data curve) (point vector) (initial-input float) (search-distance float) (iterations int) (input-padding float))
  "Refine a normalized parameter toward the curve point nearest point.
   Each iteration compares the two ends of the current interval, moves the center toward the
   better end, and halves the interval; distance arguments are converted with the estimated length.
   search-distance and input-padding are world distances, divided by the estimated curve length to
   reach parameter units; a search-distance of zero or less means half the curve. This is a local
   refinement, not a global search: it converges on whichever nearby minimum the starting interval
   brackets. The padding is subtracted before the search and added back afterwards, so the returned
   parameter is not clamped and can leave zero to one even though the interior iterations are."
  (local-vars (search-radius float))
  (set! search-radius search-distance)
  (let ((remaining iterations)
        (padding input-padding)
        (curve-total-length (curve-length curve-data))
        (low-point (new 'stack-no-clear 'vector))
        (high-point (new 'stack-no-clear 'vector)))
    0.0
    0.0
    0.0
    0.0
    (let ((half-width 0.5))
      0.0
      (if (< 0.0 search-radius) (set! half-width (/ search-radius curve-total-length)))
      (let* ((center (- initial-input (/ padding curve-total-length)))
             (low-input (- center half-width))
             (high-input (+ center half-width)))
        (curve-get-pos! low-point low-input curve-data)
        (curve-get-pos! high-point high-input curve-data)
        ;; Squared distances are enough, since only the comparison between the two ends matters.
        (let ((low-distance-squared (vector-vector-distance-squared low-point point))
              (high-distance-squared (vector-vector-distance-squared high-point point)))
          (while (> remaining 0)
            (+! remaining -1)
            (set! half-width (* 0.5 half-width))
            ;; The losing end is replaced by the center, and the center steps toward the winner.
            (let ((next-center (cond
                                 ((< low-distance-squared high-distance-squared)
                                  (curve-get-pos! high-point center curve-data)
                                  (set! high-distance-squared (vector-vector-distance-squared high-point point))
                                  (set! high-input center)
                                  (- center half-width))
                                 (else
                                  (curve-get-pos! low-point center curve-data)
                                  (set! low-distance-squared (vector-vector-distance-squared low-point point))
                                  (set! low-input center)
                                  (+ center half-width)))))
              (set! center (fmin 1.0 (fmax 0.0 next-center)))))
          (+ (if (< low-distance-squared high-distance-squared) low-input high-input) (/ padding curve-total-length)))))))

(defun vector-plane-distance ((point vector) (plane-pt vector) (normal vector))
  "Return the signed distance from point to plane-data along normal."
  (vector-dot (vector-! (new 'stack-no-clear 'vector) point plane-pt) normal))