From 4491ff04262d11cf8f66e1bd6145c1a8d30bb503 Mon Sep 17 00:00:00 2001
From: Benji <64439681+benjitrosch@users.noreply.github.com>
Date: Wed, 17 Apr 2024 02:10:48 -0400
Subject: [PATCH] Replaced SQUAD quat interpolation with cubic hermite to align
 with gltf 2.0 spec (#3920)

---
 src/raymath.h | 30 ++++++++++++++++++++++--------
 src/rmodels.c | 28 ++++++++++++++++++++--------
 2 files changed, 42 insertions(+), 16 deletions(-)

diff --git a/src/raymath.h b/src/raymath.h
index 8b2bbed5..19625f10 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -956,7 +956,7 @@ RMAPI Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount)
 }
 
 // Calculate cubic hermite interpolation between two vectors and their tangents
-// taken directly from: https://en.wikipedia.org/wiki/Cubic_Hermite_spline
+// as described in the GLTF 2.0 specification: https://registry.khronos.org/glTF/specs/2.0/glTF-2.0.html#interpolation-cubic
 RMAPI Vector3 Vector3CubicHermite(Vector3 v1, Vector3 tangent1, Vector3 v2, Vector3 tangent2, float amount)
 {
     Vector3 result = { 0 };
@@ -2213,15 +2213,29 @@ RMAPI Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount)
     return result;
 }
 
-// Calculate quaternion cubic spline interpolation using the SQUAD algorithm
-// roughly adapted from the SQUAD algorithm presented here: https://roboop.sourceforge.io/htmldoc/robotse9.html
-RMAPI Quaternion QuaternionCubicSpline(Quaternion q1, Quaternion tangent1, Quaternion q2, Quaternion tangent2, float amount)
+// Calculate quaternion cubic spline interpolation using Cubic Hermite Spline algorithm
+// as described in the GLTF 2.0 specification: https://registry.khronos.org/glTF/specs/2.0/glTF-2.0.html#interpolation-cubic
+RMAPI Quaternion QuaternionCubicHermiteSpline(Quaternion q1, Quaternion outTangent1, Quaternion q2, Quaternion inTangent2, float t)
 {
-    Quaternion slerp1 = QuaternionSlerp(q1, q2, amount);
-    Quaternion slerp2 = QuaternionSlerp(tangent1, tangent2, amount);
-    float t = 2 * amount * (1 - amount);
+    float t2 = t * t;
+    float t3 = t2 * t;
+    float h00 = 2 * t3 - 3 * t2 + 1;
+    float h10 = t3 - 2 * t2 + t;    
+    float h01 = -2 * t3 + 3 * t2;   
+    float h11 = t3 - t2;            
+
+    Quaternion p0 = QuaternionScale(q1, h00);
+    Quaternion m0 = QuaternionScale(outTangent1, h10);
+    Quaternion p1 = QuaternionScale(q2, h01);
+    Quaternion m1 = QuaternionScale(inTangent2, h11);
+
+    Quaternion result = { 0 };
+
+    result = QuaternionAdd(p0, m0);
+    result = QuaternionAdd(result, p1);
+    result = QuaternionAdd(result, m1);
+    result = QuaternionNormalize(result);
 
-    Quaternion result = QuaternionSlerp(slerp1, slerp2, t);
     return result;
 }
 
diff --git a/src/rmodels.c b/src/rmodels.c
index 04b2f129..0154a36f 100644
--- a/src/rmodels.c
+++ b/src/rmodels.c
@@ -5380,7 +5380,8 @@ static bool GetPoseAtTimeGLTF(cgltf_interpolation_type interpolationType, cgltf_
         }
     }
     
-    float t = (time - tstart)/fmax((tend - tstart), EPSILON);
+    float duration = fmax((tend - tstart), EPSILON);
+    float t = (time - tstart)/duration;
     t = (t < 0.0f)? 0.0f : t;
     t = (t > 1.0f)? 1.0f : t;
 
@@ -5419,9 +5420,9 @@ static bool GetPoseAtTimeGLTF(cgltf_interpolation_type interpolationType, cgltf_
                 Vector3 v1 = {tmp[0], tmp[1], tmp[2]};
                 cgltf_accessor_read_float(output, 3*keyframe+2, tmp, 3);
                 Vector3 tangent1 = {tmp[0], tmp[1], tmp[2]};
-                cgltf_accessor_read_float(output, 3*(keyframe+1), tmp, 3);
-                Vector3 v2 = {tmp[0], tmp[1], tmp[2]};
                 cgltf_accessor_read_float(output, 3*(keyframe+1)+1, tmp, 3);
+                Vector3 v2 = {tmp[0], tmp[1], tmp[2]};
+                cgltf_accessor_read_float(output, 3*(keyframe+1), tmp, 3);
                 Vector3 tangent2 = {tmp[0], tmp[1], tmp[2]};
                 Vector3 *r = data;
 
@@ -5462,14 +5463,25 @@ static bool GetPoseAtTimeGLTF(cgltf_interpolation_type interpolationType, cgltf_
                 cgltf_accessor_read_float(output, 3*keyframe+1, tmp, 4);
                 Vector4 v1 = {tmp[0], tmp[1], tmp[2], tmp[3]};
                 cgltf_accessor_read_float(output, 3*keyframe+2, tmp, 4);
-                Vector4 tangent1 = {tmp[0], tmp[1], tmp[2]};
-                cgltf_accessor_read_float(output, 3*(keyframe+1), tmp, 4);
-                Vector4 v2 = {tmp[0], tmp[1], tmp[2], tmp[3]};
+                Vector4 outTangent1 = {tmp[0], tmp[1], tmp[2]};
                 cgltf_accessor_read_float(output, 3*(keyframe+1)+1, tmp, 4);
-                Vector4 tangent2 = {tmp[0], tmp[1], tmp[2]};
+                Vector4 v2 = {tmp[0], tmp[1], tmp[2], tmp[3]};
+                cgltf_accessor_read_float(output, 3*(keyframe+1), tmp, 4);
+                Vector4 inTangent2 = {tmp[0], tmp[1], tmp[2]};
                 Vector4 *r = data;
 
-                *r = QuaternionCubicSpline(v1, tangent1, v2, tangent2, t);
+                v1 = QuaternionNormalize(v1);
+                v2 = QuaternionNormalize(v2);
+
+                if (Vector4DotProduct(v1, v2) < 0.0f)
+                {
+                    v2 = Vector4Negate(v2);
+                }
+                
+                outTangent1 = Vector4Scale(outTangent1, duration);
+                inTangent2 = Vector4Scale(inTangent2, duration);
+
+                *r = QuaternionCubicHermiteSpline(v1, outTangent1, v2, inTangent2, t);
             } break;
         }
     }