From a8bdf37097c3e73adc333a22070fa54cdb57e6eb Mon Sep 17 00:00:00 2001
From: Jarrod Doyle <thejarroddoyle@gmail.com>
Date: Mon, 23 Dec 2024 17:46:45 +0000
Subject: [PATCH] Add joint transformation to modelfile

---
 KeepersCompound.LGS/ModelFile.cs | 67 ++++++++++++++++++++++++++++++++
 1 file changed, 67 insertions(+)

diff --git a/KeepersCompound.LGS/ModelFile.cs b/KeepersCompound.LGS/ModelFile.cs
index 276f5e0..38f1d83 100644
--- a/KeepersCompound.LGS/ModelFile.cs
+++ b/KeepersCompound.LGS/ModelFile.cs
@@ -288,6 +288,73 @@ public class ModelFile
         }
     }
 
+    // TODO: Apply transforms to normals and stuff
+    public void ApplyJoints(float[] joints)
+    {
+        // Build map of objects to their parent id
+        var objCount = Objects.Length;
+        var parentIds = new int[objCount];
+        for (var i = 0; i < objCount; i++)
+        {
+            parentIds[i] = -1;
+        }
+        for (var i = 0; i < objCount; i++)
+        {
+            var subObj = Objects[i];
+            var childIdx = subObj.Child;
+            while (childIdx != -1)
+            {
+                parentIds[childIdx] = i;
+                childIdx = Objects[childIdx].Next;
+            }
+        }
+        
+        // Calculate base transforms for every subobj (including joint)
+        var subObjTransforms = new Matrix4x4[objCount];
+        for (var i = 0; i < objCount; i++)
+        {
+            var subObj = 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);
+                objTrans = jointRot * subObj.Transform;
+            }
+           
+            subObjTransforms[i] = objTrans;
+        }
+        
+        // Apply sub object transforms
+        for (var i = 0; i < objCount; i++)
+        {
+            var subObj = Objects[i];
+            var transform = subObjTransforms[i];
+        
+            // Build compound transformation
+            var parentId = parentIds[i];
+            while (parentId != -1)
+            {
+                transform *= subObjTransforms[parentId];
+                parentId = parentIds[parentId];
+            }
+
+            for (var j = 0; j < subObj.VhotCount; j++)
+            {
+                var v = VHots[subObj.VhotIdx + j];
+                v.Position = Vector3.Transform(v.Position, transform);
+                VHots[subObj.VhotIdx + j] = v;
+            }
+
+            for (var j = 0; j < subObj.PointCount; j++)
+            {
+                var v = Vertices[subObj.PointIdx + j];
+                Vertices[subObj.PointIdx + j] = Vector3.Transform(v, transform);
+            }
+        }
+    }
+
     public bool TryGetVhot(VhotId id, out VHot vhot)
     {
         foreach (var v in VHots)