Replaced SQUAD quat interpolation with cubic hermite to align with gltf 2.0 spec (#3920)

1 files changed, 22 insertions, 8 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;
 }