From 0da592707a861835ae9485675e13fdafc3d361fe Mon Sep 17 00:00:00 2001
From: Shinmera <shinmera@tymoon.eu>
Date: Mon, 19 Feb 2024 14:44:40 +0100
Subject: [PATCH] Start working on an optimized-convex-mesh...

---
 formats/gltf.lisp       |  8 +++---
 physics/primitives.lisp | 58 ++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 62 insertions(+), 4 deletions(-)

diff --git a/formats/gltf.lisp b/formats/gltf.lisp
index 4218e6e3b..61c27e837 100644
--- a/formats/gltf.lisp
+++ b/formats/gltf.lisp
@@ -369,9 +369,11 @@
                                 args))
       (gltf:convex-shape
        (let ((mesh (ensure-mesh (gltf:mesh shape))))
-         (apply #'trial:make-convex-mesh :vertices (trial:reordered-vertex-data mesh '(trial:location))
-                                         :faces (trial::simplify (trial:faces mesh) '(unsigned-byte 16))
-                                         args)))
+         ;; KLUDGE: If we have a mesh with few faces, skip the more elaborate hill climbing.
+         (apply (if (< (length (trial:faces mesh)) 8) #'trial:make-convex-mesh #'trial::make-optimized-convex-mesh)
+                :vertices (trial:reordered-vertex-data mesh '(trial:location))
+                :faces (trial::simplify (trial:faces mesh) '(unsigned-byte 16))
+                args)))
       (gltf:cylinder-shape
        (apply #'trial:make-cylinder :height (float (* 0.5 (gltf:height shape)) 0f0)
                                     :radius (max (float (gltf:radius-top shape) 0f0)
diff --git a/physics/primitives.lisp b/physics/primitives.lisp
index d0b9abae2..d7d11fdcb 100644
--- a/physics/primitives.lisp
+++ b/physics/primitives.lisp
@@ -574,7 +574,7 @@
         (vert (vec3))
         (furthest most-negative-single-float))
     (declare (dynamic-extent vert))
-    (declare (optimize (safety 0)))
+    (declare (optimize speed (safety 0)))
     ;; FIXME: this is O(n)
     (loop for i from 0 below (length verts) by 3
           do (setf (vx vert) (aref verts (+ i 0)))
@@ -585,6 +585,56 @@
                  (setf furthest dist)
                  (v<- next vert))))))
 
+(define-primitive-type (optimized-convex-mesh convex-mesh)
+    ((adjacency-list (make-array 0) :type simple-vector))
+  (let ((offset (recenter-vertices (general-mesh-vertices primitive))))
+    (!m* (primitive-local-transform primitive)
+         offset
+         (primitive-local-transform primitive)))
+  ;; Precompute the adjacency list. As a further optimisation we already pre-multiply
+  ;; the adjacents to be vertex array indices rather than vertex numbers, and turn it
+  ;; into a simple-array for more compact storage.
+  (let ((adjacent (org.shirakumo.fraf.manifolds:vertex-adjacency-list (convex-mesh-faces primitive))))
+    (dotimes (i (length adjacent))
+      (setf (aref adjacent i)
+            (map '(simple-array (unsigned-byte 16) (*))
+                 (lambda (x) (* 3 x)) (aref adjacent i))))
+    (setf (optimized-convex-mesh-adjacency-list primitive) adjacent)))
+
+(define-transfer optimized-convex-mesh optimized-convex-mesh-adjacency-list)
+
+(define-support-function optimized-convex-mesh (dir best)
+  (let* ((verts (convex-mesh-vertices primitive))
+         (adjacents (optimized-convex-mesh-adjacency-list primitive))
+         (vert (vec3))
+         (vertex 0)
+         (iterations 0))
+    (declare (optimize speed))
+    (declare (dynamic-extent vert))
+    (declare (type (unsigned-byte 16) vertex iterations))
+    (flet ((try-vertex (i)
+             (setf (vx vert) (aref verts (+ i 0)))
+             (setf (vy vert) (aref verts (+ i 1)))
+             (setf (vz vert) (aref verts (+ i 2)))
+             (when (< 0 (v. (nv- vert best) dir))
+               (setf vertex i)
+               (nv+ best vert))))
+      (tagbody
+       next
+         ;; KLUDGE: Hill climbing can get stuck in a coplanar local minimum. To catch this we
+         ;;         cap the number of iterations and fall back to the O(n) method. It'll be
+         ;;         super slow at that point, but at least we'll terminate with a correct
+         ;;         result
+         (when (= 256 (incf iterations))
+           (dbg "HILL CLIMBING STUCK")
+           (call-next-method)
+           (go done))
+         (loop for i of-type (unsigned-byte 16) across
+               (the (simple-array (unsigned-byte 16) (*)) (aref adjacents (truncate vertex 3)))
+               do (when (try-vertex i)
+                    (go next)))
+       done))))
+
 (defmacro with-mesh-construction ((constructor &optional (finalizer 'finalize)) &body body)
   (let ((vertices (gensym "VERTICES"))
         (faces (gensym "FACES"))
@@ -871,6 +921,12 @@
          :faces (simplify (faces mesh) '(unsigned-byte 16))
          args))
 
+(defmethod coerce-object ((primitive convex-mesh) (type (eql 'optimized-convex-mesh)) &rest args &key &allow-other-keys)
+  (apply #'make-optimized-convex-mesh
+         :vertices (convex-mesh-vertices primitive)
+         :faces (convex-mesh-faces primitive)
+         args))
+
 (defmethod replace-vertex-data (target (primitive primitive) &rest args &key &allow-other-keys)
   (apply #'replace-vertex-data target (coerce-object primitive 'mesh-data) args))