convert.lisp

(in-package #:org.shirakumo.fraf.manifolds)

(defparameter *dbg-start-time* (get-internal-real-time))

(defun dbg (&rest stuff)
  (format *debug-io* "~&~6,2f> ~{~a~^ ~}~%"
          (float (/ (- (get-internal-real-time) *dbg-start-time*) internal-time-units-per-second)) stuff))

(defstruct (node
            (:constructor %node (location bsize faces &key number level)))
  (location NIL :type vec3)
  (bsize NIL :type vec3)
  (level 0 :type (unsigned-byte 32))
  (number 1 :type (unsigned-byte 32)) ; Number of occupied nodes in this branch
  (faces NIL :type (simple-array (unsigned-byte 32) (*)))
  (children NIL :type (or null (simple-array node (8))))
  (neighbors (make-array 6 :initial-element NIL) :type (simple-vector 6))
  (empty-neighbors (make-array 6 :initial-element NIL) :type (simple-vector 6)))

(defmethod print-object ((node node) stream)
  (print-unreadable-object (node stream :type T)
    (format stream "~a/~d [~d occupied] ~d faces"
            (node-location node) (node-level node) (node-number node)
            (length (node-faces node)))))

;; The children are oriented as follows:
;; 0 => x- y- z-
;; 1 => x- y- z+
;; 2 => x- y+ z-
;; 3 => x- y+ z+
;; 4 => x+ y- z-
;; 5 => x+ y- z+
;; 6 => x+ y+ z-
;; 7 => x+ y+ z+
;; The neighbors as follows:
;; 0 => x+
;; 1 => y+
;; 2 => z+
;; 3 => x-
;; 4 => y-
;; 5 => z-

(defun node-occupied-p (node)
  (< 0 (length (node-faces node))))

;; NOTE: what the fuck? This property is computed but for all I can tell it's just the same as occupied-p???
(defun node-exterior-p (node)
  (not (node-occupied-p node)))

(defun exterior-p (node point)
  (let ((location (node-location node))
        (bsize (node-bsize node)))
    (cond ((or (not (<= (- (vx location) (vx bsize)) (vx point) (+ (vx location) (vx bsize))))
               (not (<= (- (vy location) (vy bsize)) (vy point) (+ (vy location) (vy bsize))))
               (not (<= (- (vz location) (vz bsize)) (vz point) (+ (vz location) (vz bsize)))))
           T) ;; Point lies outside the node entirely.
          ((not (node-occupied-p node))
           (node-exterior-p node))
          ((= 0 (node-level node))
           NIL)
          (T
           (let ((i (+ (if (< (vz point) (vz location)) 1 0)
                       (if (< (vy point) (vy location)) 2 0)
                       (if (< (vx point) (vx location)) 4 0))))
             (exterior-p (aref (node-children node) i) point))))))

(defun build-octtree (vertices faces &key (resolution 1000))
  (check-type vertices (simple-array single-float (*)))
  (check-type faces (simple-array (unsigned-byte 32) (*)))
  (multiple-value-bind (location bsize) (bounding-box vertices)
    (let ((node (%node location bsize faces)))
      (loop while (< (node-number node) resolution)
            do (split-octtree node vertices faces))
      (build-neighborhood node))))

(defun split-octtree (node vertices faces)
  (check-type vertices (simple-array single-float (*)))
  (check-type faces (simple-array (unsigned-byte 32) (*)))
  (incf (node-level node))
  (setf (node-number node) 0)
  (cond ((= 1 (node-level node))
         (let ((bsize (v* (node-bsize node) 0.5))
               (children (make-array 8)))
           (setf (node-children node) children)
           (loop for i from 0 below 8
                 for location = (nv+ (nv* (vec (1- (* (ldb (byte 1 2) i) 2))
                                               (1- (* (ldb (byte 1 1) i) 2))
                                               (1- (* (ldb (byte 1 0) i) 2)))
                                          bsize)
                                     (node-location node))
                 for child-faces = (faces-in-volume vertices faces location bsize)
                 for child = (%node location bsize child-faces)
                 do (when (< 0 (length child-faces))
                      (setf (node-number child) 1)
                      (incf (node-number node)))
                    (setf (aref children i) child))))
        (T
         (loop for child across (node-children node)
               do (when (node-occupied-p child)
                    (split-octtree child vertices faces)
                    (incf (node-number node) (node-number child))))))
  node)

(defun build-neighborhood (node)
  (when (< 0 (node-level node))
    (map NIL #'build-neighborhood (node-children node))
    (labels ((recur-full (l r li ri order)
               (setf (aref (node-neighbors l) ri) r)
               (setf (aref (node-neighbors r) li) l)
               (when (and (node-children l) (node-children r))
                 (loop for (j . i) across order
                       do (recur (aref (node-children l) i)
                                 (aref (node-children r) j)
                                 li ri order))))
             (recur-empty (l r li ri order)
               (cond ((and (node-occupied-p l) (node-occupied-p r))
                      (unless (= 0 (node-level l))
                        (when (and (node-children l) (node-children r))
                          (loop for (j . i) across order
                                do (recur-empty (aref (node-children l) i)
                                                (aref (node-children r) j)
                                                li ri order)))))
                     ((not (node-occupied-p l))
                      (setf (aref (node-empty-neighbors r) ri) l)
                      (when (node-children r)
                        (loop for (j . i) across order
                              do (recur-empty l (aref (node-children r) j)
                                              li ri order))))
                     ((not (node-occupied-p r))
                      (setf (aref (node-empty-neighbors l) li) r)
                      (when (node-children l)
                        (loop for (j . i) across order
                              do (recur-empty (aref (node-children l) i) r
                                              li ri order))))))
             (recur (l r li ri order)
               (recur-full l r li ri order)
               (recur-empty l r li ri order)))
      (let ((xo #((0 . 4) (1 . 5) (2 . 6) (3 . 7)))
            (yo #((0 . 2) (1 . 3) (4 . 6) (5 . 7)))
            (zo #((0 . 1) (2 . 3) (4 . 5) (6 . 7))))
        (loop for face from 0 below 6
              for (xi . xj) across xo
              for (yi . yj) across yo
              for (zi . zj) across zo
              do (recur (aref (node-children node) xi) (aref (node-children node) xj) 0 3 xo)
                 (recur (aref (node-children node) yi) (aref (node-children node) yj) 1 4 yo)
                 (recur (aref (node-children node) zi) (aref (node-children node) zj) 2 5 zo)))))
  node)

(defun construct-quad-manifold (tree)
  (let ((vcolor (make-hash-table :test 'equalp))
        (vertices (make-array 0 :element-type 'vec3 :adjustable T :fill-pointer T))
        (quad-faces (make-array 0 :element-type '(unsigned-byte 32) :adjustable T :fill-pointer T))
        (vertex-faces (make-array 0 :adjustable T :fill-pointer T :initial-element ()))
        (offsets #2A((#.(vec 1 0 0) #.(vec 1 0 1) #.(vec 1 1 1) #.(vec 1 1 0))
                     (#.(vec 0 1 0) #.(vec 1 1 0) #.(vec 1 1 1) #.(vec 0 1 1))
                     (#.(vec 0 0 1) #.(vec 0 1 1) #.(vec 1 1 1) #.(vec 1 0 1))
                     (#.(vec 0 0 0) #.(vec 0 1 0) #.(vec 0 1 1) #.(vec 0 0 1))
                     (#.(vec 0 0 0) #.(vec 0 0 1) #.(vec 1 0 1) #.(vec 1 0 0))
                     (#.(vec 0 0 0) #.(vec 1 0 0) #.(vec 1 1 0) #.(vec 0 1 0)))))
    (labels ((make-face (node start offset)
               (let* ((vid (v* (v+ start offset) 2))
                      (id (gethash vid vcolor)))
                 (unless id
                   (let ((d (v+ (v- (node-location node) (node-bsize node))
                                (v* offset 2 (node-bsize node)))))
                     (setf id (length vertices))
                     (setf (gethash vid vcolor) id)
                     (vector-push-extend d vertices)
                     (vector-push-extend () vertex-faces)))
                 (loop for face across (node-faces node)
                       do (pushnew face (aref vertex-faces id)))
                 (vector-push-extend id quad-faces)))
             (recurse-face (node start)
               (cond ((= 0 (node-level node))
                      (when (node-occupied-p node)
                        (loop for i from 0 below 6
                              for empty across (node-empty-neighbors node)
                              do (when (and empty (node-exterior-p empty))
                                   (dotimes (j 4)
                                     (make-face node start (aref offsets i j)))))))
                     (T
                      (loop for i from 0
                            for child across (node-children node)
                            do (when (node-occupied-p child)
                                 (recurse-face child (v+ (v* start 2)
                                                           (vec (ldb (byte 1 2) i)
                                                                (ldb (byte 1 1) i)
                                                                (ldb (byte 1 0) i))))))))))
      (recurse-face tree (vec 0 0 0)))
    (values vcolor vertices quad-faces vertex-faces)))

(defun construct-triangle-mesh (vcolor vertices quad-faces vertex-faces tree)
  (let ((v-info (make-array (hash-table-count vcolor) :adjustable T :fill-pointer T))
        (e-info (make-hash-table :test 'equal)) ; (E1 . E2) => [(V1 . V2)]
        (marked-v (make-hash-table :test 'eql))
        (faces (make-array 0 :element-type '(unsigned-byte 32) :adjustable T :fill-pointer T))
        (half-len (* 0.5 (vdistance (aref vertices (aref quad-faces 1)) (aref vertices (aref quad-faces 0)))))
        (unit-len 0.0)
        (odd-edges (make-hash-table :test 'eql))
        (even-edges (make-hash-table :test 'eql))
        (odd-vertices (make-hash-table :test 'eql))
        (even-vertices (make-hash-table :test 'eql)))
    (loop for vid being the hash-keys of vcolor using (hash-value id)
          do (setf (aref v-info id) vid))
    (loop for i from 0 below (length quad-faces) by 4
          do (loop for j from 0 below 4
                   for edge = (cons (aref quad-faces (+ i j))
                                    (aref quad-faces (+ i (mod (1+ j) 4))))
                   do (when (< (cdr edge) (car edge)) (rotatef (cdr edge) (car edge)))
                      (let ((face-list (gethash edge e-info)))
                        (unless face-list
                          (setf face-list (make-array 0 :adjustable T :fill-pointer T))
                          (setf (gethash edge e-info) face-list))
                        (vector-push-extend (cons (truncate i 4) j) face-list))))
    (loop for (x . y) being the hash-keys of e-info using (hash-value face-list)
          do (when (< 2 (length face-list))
               (setf (gethash x marked-v) T)
               (setf (gethash y marked-v) T)))
    (flet ((emit-face (a b c)
             (assert (not (= a b c)))
             (vector-push-extend a faces)
             (vector-push-extend b faces)
             (vector-push-extend c faces)))
      ;; First loop to construct vertices.
      (flet ((maybe-emit-vertex (point a b &optional (vertex a))
               (let ((index (gethash point vcolor)))
                 (unless index
                   (setf index (length vertices))
                   (setf (gethash point vcolor) index)
                   (vector-push-extend point v-info)
                   (vector-push-extend (nv* (v+ (aref vertices a) (aref vertices b)) 0.5) vertices)
                   (vector-push-extend (aref vertex-faces vertex) vertex-faces))
                 index)))
        (loop for face-i from 0 below (length quad-faces) by 4
              for a = (aref quad-faces (+ face-i 0))
              for b = (aref quad-faces (+ face-i 1))
              for c = (aref quad-faces (+ face-i 2))
              for d = (aref quad-faces (+ face-i 3))
              for tt = 0
              do (loop while (and (< tt 4) (gethash (aref quad-faces (+ face-i tt)) marked-v))
                       do (incf tt)
                          (rotatef a b c d))
                 (cond ((= 4 tt) ; If the entire quad face is in the proper configuration, just emit it.
                        (dbg face-i "|" "-" "-" "-" "-")
                        (emit-face a c b)
                        (emit-face a d c))
                       (T          ; Otherwise possibly split the face
                        (dbg face-i "|" a b c d)
                        (let* ((flag1 (gethash a marked-v))
                               (flag2 (gethash b marked-v))
                               (flag3 (gethash c marked-v))
                               (pt1 (nv* (v+ (aref v-info a) (aref v-info b)) 0.5))
                               (pt2 (nv* (v+ (aref v-info a) (aref v-info d)) 0.5))
                               (pt3 (nv* (v+ (aref v-info c) (aref v-info d)) 0.5))
                               (pt4 (nv* (v+ (aref v-info b) (aref v-info c)) 0.5))
                               (a* (maybe-emit-vertex pt1 a b))
                               (b* (maybe-emit-vertex pt2 a d))
                               (d* (maybe-emit-vertex pt4 b c (if flag1 b c)))
                               (c* (maybe-emit-vertex pt3 c d (if flag2 c d))))
                          (cond ((and (not flag1) (not flag2) (not flag3))
                                 (emit-face a* c b)
                                 (emit-face b* c a*)
                                 (emit-face d c b*))
                                ((and (not flag1) (not flag2) flag3)
                                 (emit-face a* b* c*)
                                 (emit-face a* c* c)
                                 (emit-face a* c b))
                                ((and (not flag1) flag2 (not flag3))
                                 (emit-face a* d* b)
                                 (emit-face a* b* d*)
                                 (emit-face b* d c*)
                                 (emit-face b* c* d*))
                                ((and (not flag1) flag2 flag3)
                                 (emit-face a* d* b)
                                 (emit-face a* b* d*)
                                 (emit-face b* c* d*))
                                ((and flag1 (not flag2) (not flag3))
                                 (emit-face a* b* d*)
                                 (emit-face d* b* d)
                                 (emit-face d* d c))
                                ((and flag1 (not flag2) flag3)
                                 (emit-face a* b* d*)
                                 (emit-face d* b* c*)
                                 (emit-face d* c* c))
                                ((and flag1 flag2 (not flag3))
                                 (emit-face a* b* d*)
                                 (emit-face b* c* d*)
                                 (emit-face b* d c*))
                                ((and flag1 flag2 flag3)
                                 (emit-face a* b* c*)
                                 (emit-face a* c* d*))))))))
      ;; This secondary loop seems to add vertices along the node boundaries.
      (flet ((maybe-emit-vertex (vertex off)
               (let* ((vec (v+ (aref v-info vertex) off))
                      (id (gethash vec vcolor)))
                 (unless id
                   (setf id (length vertices))
                   (setf (gethash vec vcolor) id)
                   (vector-push-extend vec v-info)
                   (vector-push-extend (v+ (aref vertices vertex) (v* off half-len)) vertices)
                   (vector-push-extend (aref vertex-faces vertex) vertex-faces))
                 id)))
        (loop for vertex being the hash-keys of marked-v
              for p = (aref vertices vertex)
              do (do-directions (x y z -1 +1)
                   (when (exterior-p tree (v+ p (* x half-len) (* y half-len) (* z half-len)))
                     (let* ((id1 (maybe-emit-vertex vertex (vec x 0 0)))
                            (id2 (maybe-emit-vertex vertex (vec 0 y 0)))
                            (id3 (maybe-emit-vertex vertex (vec 0 0 z)))
                            (normal (vc (v- (aref vertices id2) (aref vertices id1))
                                        (v- (aref vertices id3) (aref vertices id1)))))
                       (if (< (v. normal (vec x y z)) 0)
                           (emit-face id1 id3 id2)
                           (emit-face id1 id2 id3)))))))
      ;; Figure out if vertex is odd or even [why tables and not a single property vector?]
      (loop for vertex from 0 below (length vertices)
            for location = (aref v-info vertex)
            do (flet ((frob (off)
                        (let* ((ind (v+ location off))
                               (index (gethash ind vcolor)))
                          (when index
                            (setf unit-len (vdistance (aref vertices index) (aref vertices vertex)))
                            (if (< vertex index)
                                (gethash (cons vertex index) e-info)
                                (gethash (cons index vertex) e-info))))))
                 (cond ((or (= 1 (mod (vx location) 2))
                            (= 1 (mod (vy location) 2))
                            (= 1 (mod (vz location) 2)))
                        (setf (gethash vertex odd-vertices) T)
                        (setf (gethash vertex odd-edges) ()))
                       ((and (frob (vec -2 0 0))
                             (frob (vec 0 -2 0))
                             (frob (vec 0 0 -2))
                             (frob (vec +2 0 0))
                             (frob (vec 0 +2 0))
                             (frob (vec 0 0 +2)))
                        (setf (gethash vertex even-vertices) T)
                        (setf (gethash vertex even-edges) ())))))
      ;; Gather odd/even edge lists [isn't this pretty similar to just computing vertex-faces?]
      (loop for face from 0 below (length faces) by 3
            do (loop for face-i from 0 below 3
                     for vertex = (aref faces (+ face face-i))
                     do (when (gethash vertex odd-vertices)
                          (pushnew (cons face face-i) (gethash vertex odd-edges)))
                        (when (gethash vertex even-vertices)
                          (pushnew (cons face face-i) (gethash vertex even-edges)))))
      ;; Compute leftover verts and faces
      (flet ((frob (vertex dir len)
               (loop for (i . j) in (gethash vertex even-edges)
                     do (when (< (v. dir (v- (aref vertices (aref faces (+ i (mod (1+ j) 3)))) (aref vertices vertex))) 0)
                          (setf (aref faces (+ i j)) (length vertices))))
               (nv+* (aref vertices vertex) dir (* 0.5 len))
               (vector-push-extend (aref vertex-faces vertex) vertex-faces)
               (vector-push-extend (aref vertices vertex) vertices)
               (nv+* (aref vertices (1- (length vertices))) dir (- len))))
        (loop for vertex being the hash-keys of even-vertices
              for dir = (vec 0 0 0)
              for count = 0
              do (do-directions (x y z -1 +1)
                   (let ((d (nv* (nvunit (vec x y z)) unit-len 0.5)))
                     (when (exterior-p tree (v+ (aref vertices vertex) d))
                       (setf dir (nvunit d))
                       (incf count))))
                 (when (< count 3)
                   (frob vertex dir unit-len)))
        (loop for vertex being the hash-keys of odd-vertices
              for location = (aref v-info vertex)
              for k = (cond ((/= 0 (mod (vx location) 2)) +vx+)
                            ((/= 0 (mod (vy location) 2)) +vy+)
                            ((/= 0 (mod (vz location) 2)) +vz+))
              for id1 = (v- location k)
              for id2 = (v+ location k)
              for x = (gethash id1 vcolor)
              for y = (gethash id2 vcolor)
              do (when (< y x) (rotatef x y))
                 (when (< 2 (length (gethash (cons x y) e-info)))
                   (let* ((vert (v- (aref vertices x) (aref vertices y)))
                          (len (vlength vert))
                          (dir (vec (* 0.5 len) (* 0.5 len) (* 0.5 len))))
                     (nv/ vert len)
                     (nv+* dir vert (- (v. dir vert)))
                     (unless (exterior-p tree (v+ (aref vertices vertex) dir))
                       (setf dir (vc vert dir)))
                     (nvunit dir)
                     (frob vertex dir len)))))
      (values vertices faces vertex-faces))))

(defun project-manifold (vertices faces vertex-faces orig-vertices orig-faces &key (iterations 20))
  (check-type vertices (vector vec3))
  (check-type faces (vector (unsigned-byte 32)))
  (check-type orig-vertices (simple-array single-float (*)))
  (check-type orig-faces (simple-array (unsigned-byte 32) (*)))
  (let* ((vertex-count (length vertices))
         (len (min (vdistance (aref vertices (aref faces 0)) (aref vertices (aref faces 1)))
                   (vdistance (aref vertices (aref faces 0)) (aref vertices (aref faces 2)))))
         (face-normals (make-array (truncate (length faces) 3)))
         (invalid-vertices (make-array 0 :element-type '(unsigned-byte 32) :adjustable T :fill-pointer T))
         (invalid-indices (make-array (length vertex-faces) :element-type '(signed-byte 32) :initial-element -1))
         (visited (make-array vertex-count :element-type 'bit))
         (min-step (/ 2.0 iterations)))
    (labels ((convex-p (vertex normal)
               (loop for face in (aref vertex-faces vertex)
                     do (dotimes (i 3)
                          (when (< 0 (v. (v- (aref vertices (aref faces (+ i (* 3 face))))
                                             (aref vertices vertex))
                                         normal))
                            (return-from convex-p NIL)))
                     finally (return T)))
             (closest-on-orig (vertex)
               (let ((cpoint NIL)
                     (normal (vec 0 0 0))
                     (v (aref vertices vertex)))
                 (loop for face in (aref vertex-faces vertex)
                       ;; I do *not* understand how this worked in the original code. The vertex-faces array
                       ;; can, at this point, include face indices that were not part of the orig-faces array
                       ;; and can, thus, point outside its range. The construct-triangle-mesh function emits
                       ;; new vertices and faces and also updates the vertex-faces array as part of that.
                       do (when (< (* 3 face) (length orig-faces))
                            (let ((p (closest-point-on-triangle orig-vertices orig-faces face v)))
                              (when (or (null cpoint) (< (vdistance p v) (vdistance cpoint v)))
                                (setf normal (face-normal orig-vertices orig-faces face))
                                (setf cpoint p)
                                (when (< (v. normal (v- v cpoint)) 0)
                                  (nv- normal))))))
                 (if cpoint
                     (nv+* cpoint normal 5e-4)
                     v)))
             (mark-invalid (vertex)
               (setf (aref invalid-indices vertex) (length invalid-vertices))
               (vector-push-extend vertex invalid-vertices))
             (update-step-size (vertex v-faces move-dir step)
               (loop for face in v-faces
                     for a = (aref faces (+ 0 (* 3 face)))
                     for b = (aref faces (+ 1 (* 3 face)))
                     for c = (aref faces (+ 2 (* 3 face)))
                     do (loop until (= a vertex)
                              do (rotatef a b c))
                        (let* ((dir (nvunit* (v- (aref vertices c) (aref vertices b))))
                               (h (v+ (aref vertices b)
                                      (v* (v. (v- (aref vertices a) (aref vertices b)) dir) dir)
                                      (v- (aref vertices a))))
                               (h-len (vlength h)))
                          (when (/= 0 h-len)
                            (nv/ h h-len)
                            (let ((h-step (* (v. h move-dir) step)))
                              (when (< (* 0.7 h-len) h-step)
                                (setf step (* step h-len 0.7 (/ h-step))))))))
               step))
      ;; Main projection loop
      (dotimes (iter iterations)
        (face-normals* vertices faces face-normals)
        (dotimes (vertex vertex-count)
          (when (= 0 (sbit visited vertex))
            (let* ((v-faces (aref vertex-faces vertex))
                   (closest (closest-on-orig vertex))
                   (move-dir (v- closest (aref vertices vertex)))
                   (orig-step (vlength move-dir))
                   (step orig-step)
                   (flag (< step 1e15))
                   (normal (nvunit* (reduce #'nv+ v-faces
                                            :initial-value (vec 0 0 0)
                                            :key (lambda (f) (aref face-normals f))))))
              (when (< 0 orig-step)
                (nv/ move-dir orig-step))
              (when flag
                (if (convex-p vertex normal)
                    (loop for face in v-faces
                          do (when (< 0 (v. (aref face-normals face) move-dir))
                               (return (setf flag NIL))))
                    (loop for face in v-faces
                          do (when (< (v. (aref face-normals face) move-dir) 0)
                               (return (setf flag T))))))
              (cond (flag
                     (setf step (min (* min-step len) step))
                     (let ((new-step (update-step-size vertex v-faces move-dir step)))
                       (when (/= step new-step)
                         (mark-invalid vertex)
                         (setf step new-step)))
                     (cond ((< (abs (- step orig-step)) 1e-6)
                            (v<- (aref vertices vertex) closest)
                            (nv+* (aref vertices vertex) normal len)
                            (setf step (max 0.0 (- step 1e-4)))
                            (setf (sbit visited vertex) 1))
                           (T
                            (nv+* (aref vertices vertex) move-dir step)))
                     (loop for face in v-faces
                           do (setf (aref face-normals face) (face-normal* vertices faces face))))
                    (T
                     (mark-invalid vertex)))))))
      ;; Fixup invalid vertices
      (fill visited 0)
      (dotimes (i (length invalid-vertices))
        (when (= 0 (sbit visited i))
          (setf (sbit visited i) 1)
          (let ((queue (make-array 0 :adjustable T :fill-pointer T)))
            (vector-push-extend i queue)
            (loop for f from 0
                  while (< f (length queue))
                  do (dolist (face (aref vertex-faces (aref queue f)))
                       (loop for fi from (* 3 face) repeat 3
                             for index = (aref invalid-indices (aref faces fi))
                             do (when (and (/= -1 index) (not (sbitp visited index)))
                                  (setf (sbitp visited index) T)
                                  (vector-push-extend index queue)))))
            (loop for it across queue
                  for midpoint = (vec 0 0 0)
                  for count = 0
                  for vertex = (aref invalid-vertices it)
                  for v-faces = (aref vertex-faces vertex)
                  do (dolist (face v-faces)
                       (loop for fi from (* 3 face) repeat 3
                             for index = (aref faces fi)
                             do (when (or (= -1 (aref invalid-indices index))
                                          (not (sbitp visited (aref invalid-indices index))))
                                  (nv+ midpoint (aref vertices index))
                                  (incf count))))
                     (setf (sbitp visited it) NIL)
                     (let* ((move-dir (v- (v/ midpoint count) (aref vertices vertex)))
                            (step (vlength move-dir)))
                       (unless (or (= 0 step) (= 0 count))
                         (nv/ move-dir step)
                         (setf step (update-step-size v-faces vertex move-dir step))
                         (nv+* (aref vertices vertex) move-dir step))))
            (loop for index across queue
                  do (setf (sbitp visited index) T))))))
    (values vertices faces)))

(defun transfer-vertices (vertices faces)
  (check-type vertices (vector vec3))
  (check-type faces (vector (unsigned-byte 32)))
  ;; Compacts the vertices and faces down again into a packed array.
  (let ((map (make-array (length vertices) :element-type '(unsigned-byte 32) :initial-element 0))
        (count 0))
    (loop for i from 0 below (length faces)
          do (when (= 0 (aref map (aref faces i)))
               (setf (aref map (aref faces i)) 1)
               (incf count 3)))
    (let ((out-vertices (make-array count :element-type 'single-float))
          (out-faces (make-array (length faces) :element-type '(unsigned-byte 32)))
          (out-i 0))
      (loop for vertex from 0 below (length map)
            for location = (aref vertices vertex)
            do (when (< 0 (aref map vertex))
                 (setf (aref map vertex) (truncate out-i 3))
                 (setf (aref out-vertices (+ 0 out-i)) (vx location))
                 (setf (aref out-vertices (+ 1 out-i)) (vy location))
                 (setf (aref out-vertices (+ 2 out-i)) (vz location))
                 (incf out-i 3)))
      (loop for i from 0 below (length out-faces)
            do (setf (aref out-faces i) (aref map (aref faces i))))
      (values out-vertices out-faces))))

(defun normalize-vertices (vertices faces)
  (check-type vertices (simple-array single-float (*)))
  (check-type faces (simple-array (unsigned-byte 32) (*)))
  ;; Some kinda smoothing operation? I'm not entirely sure to be honest.
  (let ((displacements (make-array (length vertices) :element-type 'single-float))
        (weights (make-array (truncate (length vertices) 3) :element-type '(unsigned-byte 32))))
    (labels ((transfer (a b)
               (incf (aref displacements (+ 0 a)) (aref vertices (+ 0 b)))
               (incf (aref displacements (+ 1 a)) (aref vertices (+ 1 b)))
               (incf (aref displacements (+ 2 a)) (aref vertices (+ 2 b))))
             (record (a b)
               (incf (aref weights a))
               (incf (aref weights b))
               (transfer (* 3 a) (* 3 b))
               (transfer (* 3 b) (* 3 a))))
      (loop for i from 0 below (length faces) by 3
            for a = (aref faces (+ 0 i))
            for b = (aref faces (+ 1 i))
            for c = (aref faces (+ 2 i))
            do (record a b)
               (record b c)
               (record c a)))
    (loop for i from 0 below (length vertices) by 3
          for weight across weights
          do (when (< 0 weight)
               (let ((weight (/ weight)))
                 (setf (aref vertices (+ 0 i)) (* (aref displacements (+ 0 i)) weight))
                 (setf (aref vertices (+ 1 i)) (* (aref displacements (+ 1 i)) weight))
                 (setf (aref vertices (+ 2 i)) (* (aref displacements (+ 2 i)) weight))))))
  (values vertices faces))

(defun manifold (vertices faces &key (resolution 10) project)
  (check-type vertices (simple-array single-float (*)))
  (check-type faces (simple-array (unsigned-byte 32) (*)))
  (let ((tree (build-octtree vertices faces :resolution resolution)))
    (multiple-value-call #'normalize-vertices
      (multiple-value-call #'transfer-vertices
        (multiple-value-call (if project #'project-manifold #'values)
          (multiple-value-call #'construct-triangle-mesh
            (construct-quad-manifold tree) tree)
          vertices faces)))))