Fix 3: Apply hierarchical transforms to models

KeepersCompound.Lightmapper/MeshBuilder.cs

@@ -127,7 +127,7 @@ public class MeshBuilder
             var model = new ModelFile(modelPath);
             pos -= model.Header.Center;
             
-
+            // Calculate base model transform
             var scalePart = Matrix4x4.CreateScale(scale);
             var rotPart = Matrix4x4.Identity;
             rotPart *= Matrix4x4.CreateRotationX(float.DegreesToRadians(rot.X));
@@ -136,27 +136,62 @@ public class MeshBuilder
             var transPart = Matrix4x4.CreateTranslation(pos);
             var modelTrans = scalePart * rotPart * transPart;
             
-            // for each object modify the vertices
+            // Calculate base transforms for every subobj (including joint)
-            // TODO: Handle nested sub objects
+            var objCount = model.Objects.Length;
-            foreach (var subObj in model.Objects)
+            var subObjTransforms = new Matrix4x4[objCount];
+            for (var i = 0; i < objCount; i++)
             {
-                var jointTrans = Matrix4x4.Identity;
+                var subObj = model.Objects[i];
+                var objTrans = Matrix4x4.Identity;
                 if (subObj.Joint != -1)
                 {
                     var ang =  float.DegreesToRadians(joints[subObj.Joint]);
+                    // TODO: Is this correct? Should I use a manual rotation matrix?
                     var jointRot = Matrix4x4.CreateFromYawPitchRoll(0, ang, 0);
-                    var objTrans = subObj.Transform;
+                    objTrans = jointRot * subObj.Transform;
-                    jointTrans = jointRot * objTrans;
                 }
                
-                var transform = jointTrans * modelTrans;
+                subObjTransforms[i] = objTrans;
+            }
+            
+            // Build map of objects to their parent id
+            var parentIds = new int[objCount];
+            for (var i = 0; i < objCount; i++)
+            {
+                parentIds[i] = -1;
+            }
+            for (var i = 0; i < objCount; i++)
+            {
+                var subObj = model.Objects[i];
+                var childIdx = subObj.Child;
+                while (childIdx != -1)
+                {
+                    parentIds[childIdx] = i;
+                    childIdx = model.Objects[childIdx].Next;
+                }
+            }
+            
+            // Apply sub object transforms + the base object transform to each vertex
+            for (var i = 0; i < objCount; i++)
+            {
+                var subObj = model.Objects[i];
+                var transform = subObjTransforms[i];
+            
+                var parentId = parentIds[i];
+                while (parentId != -1)
+                {
+                    transform *= subObjTransforms[parentId];
+                    parentId = parentIds[parentId];
+                }
+            
+                transform *= modelTrans;
                 
                 var start = subObj.PointIdx;
                 var end = start + subObj.PointCount;
-                for (var i = start; i < end; i++)
+                for (var j = start; j < end; j++)
                 {
-                    var v = model.Vertices[i];
+                    var v = model.Vertices[j];
-                    model.Vertices[i] = Vector3.Transform(v, transform);
+                    model.Vertices[j] = Vector3.Transform(v, transform);
                 }
             }