From 05d76c74a61444f1daba82100a0b69e5d66520ed Mon Sep 17 00:00:00 2001
From: VitoTringolo <vito8899@gmail.com>
Date: Fri, 28 Jun 2024 09:53:29 +0200
Subject: [PATCH] Fix GLTF animations (#4107)

Co-authored-by: Vito Tringolo <vito.tringolo@recognitionrobotics.com>
---
 src/raymath.h | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++
 src/rmodels.c | 38 ++++++++++++++++++--------------------
 2 files changed, 68 insertions(+), 20 deletions(-)

diff --git a/src/raymath.h b/src/raymath.h
index 8b69cbd53c0a..63947ee8d74a 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -2525,4 +2525,54 @@ RMAPI int QuaternionEquals(Quaternion p, Quaternion q)
     return result;
 }
 
+// Decompose a transformation matrix into its rotational, translational and scaling components
+RMAPI void MatrixDecompose(Matrix mat, Vector3 *translation, Quaternion *rotation, Vector3 *scale)
+{
+    // Extract translation.
+    translation->x = mat.m12;
+    translation->y = mat.m13;
+    translation->z = mat.m14;
+
+    // Extract upper-left for determinant computation.
+    const float a = mat.m0;
+    const float b = mat.m4;
+    const float c = mat.m8;
+    const float d = mat.m1;
+    const float e = mat.m5;
+    const float f = mat.m9;
+    const float g = mat.m2;
+    const float h = mat.m6;
+    const float i = mat.m10;
+    const float A = e * i - f * h;
+    const float B = f * g - d * i;
+    const float C = d * h - e * g;
+
+    // Extract scale.
+    const float det = a * A + b * B + c * C;
+    float scalex = Vector3Length((Vector3) {a, b, c});
+    float scaley = Vector3Length((Vector3) {d, e, f});
+    float scalez = Vector3Length((Vector3) {g, h, i});
+    Vector3 s = {scalex, scaley, scalez};
+
+    if (det < 0) s = Vector3Negate(s);
+
+    *scale = s;
+
+    // Remove scale from the matrix if it is not close to zero.
+    Matrix clone = mat;
+    if (!FloatEquals(det, 0))
+    {
+        clone.m0 /= s.x;
+        clone.m5 /= s.y;
+        clone.m10 /= s.z;
+        // Extract rotation
+        *rotation = QuaternionFromMatrix(clone);
+    }
+    else
+    {
+        // Set to identity if close to zero
+        *rotation = QuaternionIdentity();
+    }
+}
+
 #endif  // RAYMATH_H
diff --git a/src/rmodels.c b/src/rmodels.c
index a323292ca7d8..f99ba5149522 100644
--- a/src/rmodels.c
+++ b/src/rmodels.c
@@ -4878,13 +4878,13 @@ static BoneInfo *LoadBoneInfoGLTF(cgltf_skin skin, int *boneCount)
         }
 
         // Find parent bone index
-        unsigned int parentIndex = -1;
+        int parentIndex = -1;
 
         for (unsigned int j = 0; j < skin.joints_count; j++)
         {
             if (skin.joints[j] == node.parent)
             {
-                parentIndex = j;
+                parentIndex = (int)j;
                 break;
             }
         }
@@ -5459,22 +5459,17 @@ static Model LoadGLTF(const char *fileName)
 
             for (int i = 0; i < model.boneCount; i++)
             {
-                cgltf_node node = *skin.joints[i];
-                model.bindPose[i].translation.x = node.translation[0];
-                model.bindPose[i].translation.y = node.translation[1];
-                model.bindPose[i].translation.z = node.translation[2];
-
-                model.bindPose[i].rotation.x = node.rotation[0];
-                model.bindPose[i].rotation.y = node.rotation[1];
-                model.bindPose[i].rotation.z = node.rotation[2];
-                model.bindPose[i].rotation.w = node.rotation[3];
-
-                model.bindPose[i].scale.x = node.scale[0];
-                model.bindPose[i].scale.y = node.scale[1];
-                model.bindPose[i].scale.z = node.scale[2];
+                cgltf_node* node = skin.joints[i];
+                cgltf_float worldTransform[16];
+                cgltf_node_transform_world(node, worldTransform);
+                Matrix worldMatrix = {
+                    worldTransform[0], worldTransform[4], worldTransform[8], worldTransform[12],
+                    worldTransform[1], worldTransform[5], worldTransform[9], worldTransform[13],
+                    worldTransform[2], worldTransform[6], worldTransform[10], worldTransform[14],
+                    worldTransform[3], worldTransform[7], worldTransform[11], worldTransform[15]
+                };
+                MatrixDecompose(worldMatrix, &(model.bindPose[i].translation), &(model.bindPose[i].rotation), &(model.bindPose[i].scale));
             }
-
-            BuildPoseFromParentJoints(model.bones, model.boneCount, model.bindPose);
         }
         else if (data->skins_count > 1)
         {
@@ -5651,6 +5646,9 @@ static bool GetPoseAtTimeGLTF(cgltf_interpolation_type interpolationType, cgltf_
         }
     }
 
+    // Constant animation, no need to interpolate
+    if (FloatEquals(tend, tstart)) return false;
+
     float duration = fmaxf((tend - tstart), EPSILON);
     float t = (time - tstart)/duration;
     t = (t < 0.0f)? 0.0f : t;
@@ -5880,9 +5878,9 @@ static ModelAnimation *LoadModelAnimationsGLTF(const char *fileName, int *animCo
 
                     for (int k = 0; k < animations[i].boneCount; k++)
                     {
-                        Vector3 translation = {0, 0, 0};
-                        Quaternion rotation = {0, 0, 0, 1};
-                        Vector3 scale = {1, 1, 1};
+                        Vector3 translation = {skin.joints[k]->translation[0], skin.joints[k]->translation[1], skin.joints[k]->translation[2]};
+                        Quaternion rotation = {skin.joints[k]->rotation[0], skin.joints[k]->rotation[1], skin.joints[k]->rotation[2], skin.joints[k]->rotation[3]};
+                        Vector3 scale = {skin.joints[k]->scale[0], skin.joints[k]->scale[1], skin.joints[k]->scale[2]};
 
                         if (boneChannels[k].translate)
                         {