From e83d8bc5f53076280e3e6bf1a9512272dc68e6da Mon Sep 17 00:00:00 2001
From: David Thompson <dthompson2@worcester.edu>
Date: Tue, 16 Apr 2024 21:49:00 -0400
Subject: graphics: model: Fix glTF world matrix computation.

---
 chickadee/graphics/model.scm | 102 +++++++++++++++++++++++++++----------------
 1 file changed, 64 insertions(+), 38 deletions(-)

diff --git a/chickadee/graphics/model.scm b/chickadee/graphics/model.scm
index 5746e5a..2fe7131 100644
--- a/chickadee/graphics/model.scm
+++ b/chickadee/graphics/model.scm
@@ -103,7 +103,7 @@
   (mesh model-node-mesh)
   (matrix model-node-matrix)
   (world-matrix model-node-world-matrix)
-  (children model-node-children))
+  (children model-node-children set-model-node-children!))
 
 (define* (make-model-node #:key
                           (name "anonymous")
@@ -114,20 +114,20 @@
   (%make-model-node name mesh matrix world-matrix children))
 
 (define (draw-model-node node state view-matrix camera-position skybox lights)
-  (for-each (lambda (child)
-              (draw-model-node child state view-matrix camera-position
-                               skybox lights))
-            (model-node-children node))
-  (let ((mesh (model-node-mesh node)))
-    (when mesh
-      (render-state-world-model-matrix-mult! state
-                                             (model-node-world-matrix node))
-      (draw-mesh mesh
-                 #:model-matrix (render-state-world-model-matrix state)
-                 #:view-matrix view-matrix
-                 #:camera-position camera-position
-                 #:skybox skybox
-                 #:lights lights))))
+  (match node
+    (($ <model-node> name mesh matrix world-matrix children)
+     (for-each (lambda (child)
+                 (draw-model-node child state view-matrix camera-position
+                                  skybox lights))
+               children)
+     (when mesh
+       (render-state-world-model-matrix-mult! state world-matrix)
+       (draw-mesh mesh
+                  #:model-matrix (render-state-world-model-matrix state)
+                  #:view-matrix view-matrix
+                  #:camera-position camera-position
+                  #:skybox skybox
+                  #:lights lights)))))
 
 
 ;;;
@@ -961,7 +961,7 @@
           (weights (number-array-ref/optional obj "weights")))
       ;; TODO: Support weights.
       (make-mesh name primitives)))
-  (define (parse-node obj parse-child parent-matrix meshes)
+  (define (parse-node obj meshes)
     ;; TODO: Parse all fields of nodes.
     (let* ((name (or (string-ref/optional obj "name") "anonymous"))
            ;; TODO: Parse camera.
@@ -987,36 +987,62 @@
                                    (match translation
                                      (#(x y z)
                                       (matrix4-translate (vec3 x y z))))))))
-           (world-matrix (matrix4* matrix parent-matrix))
            (mesh (match (number-ref/optional obj "mesh")
                    (#f #f)
                    (n (vector-ref meshes n))))
            ;; TODO: Parse weights.
-           (weights #f)
-           (children (map (lambda (i)
-                            (parse-child i world-matrix))
-                          (vector->list
-                           (or (array-ref/optional obj "children")
-                               #())))))
+           (weights #f))
       (make-model-node #:name name
-                       #:children children
                        #:matrix matrix
-                       #:world-matrix world-matrix
                        #:mesh mesh)))
   (define (parse-nodes array meshes)
-    (define nodes (make-vector (vector-length array) #f))
-    (define (parse-node* i matrix)
-      (let ((node (vector-ref nodes i)))
-        (or node
-            (let ((node (parse-node (vector-ref array i)
-                                    parse-node*
-                                    matrix
-                                    meshes)))
-              (vector-set! nodes i node)
-              node))))
-    (for-range ((i (vector-length array)))
-      (parse-node* i (make-identity-matrix4)))
-    nodes)
+    (let* ((k (vector-length array))
+           (nodes (make-vector k)))
+      ;; Creating the node tree is a multi-phase process.  First, we
+      ;; parse all of the nodes in order, ignoring children since the
+      ;; array is not guaranteed to be topologically sorted and the
+      ;; child nodes may not exist yet.  Then, we take another pass to
+      ;; initialize children now that all nodes can be referenced.
+      ;; Next, we topologically sort the nodes and initialize world
+      ;; matrices.  The topological sort ensures that we initialize the
+      ;; matrices of parents before their children.
+      (for-range ((i k))
+        (vector-set! nodes i (parse-node (vector-ref array i) meshes)))
+      (for-range ((i k))
+        (let ((node (vector-ref nodes i))
+              (children (or (array-ref/optional (vector-ref array i) "children")
+                            #())))
+          (set-model-node-children! node (map (lambda (j)
+                                                (vector-ref nodes j))
+                                              (vector->list children)))))
+      (let ((visited (make-hash-table))
+            (top-nodes '()))
+        (define (visit node)
+          (match (hashq-ref visited node)
+            (#f
+             (hashq-set! visited node 'temp)
+             (for-each visit (model-node-children node))
+             (hashq-set! visited node 'perm)
+             (set! top-nodes (cons node top-nodes)))
+            ('temp (error "cycle in glTF node tree"))
+            ('perm #t)))
+        (for-range ((i k))
+          (visit (vector-ref nodes i)))
+        ;; Reuse the visited table for world matrix initialization.
+        (hash-clear! visited)
+        (define (init-world-matrix node parent-matrix)
+          (match node
+            (($ <model-node> name mesh matrix world-matrix children)
+             (matrix4-mult! world-matrix parent-matrix matrix)
+             (hashq-set! visited node #t)
+             (for-each (lambda (child)
+                         (init-world-matrix child world-matrix))
+                       children))))
+        (for-each (lambda (node)
+                    (unless (hashq-ref visited node)
+                      (init-world-matrix node (make-identity-matrix4))))
+                  top-nodes))
+      nodes))
   (define (parse-scene obj nodes)
     (let ((name (or (string-ref/optional obj "name") "anonymous"))
           (children
-- 
cgit v1.2.3