WIP with animations

src/Shared/_Extensions.cs

@@ -160,7 +160,7 @@ namespace SharpGLTF
             return dst;
         }
 
-        internal static (T, T, float) GetSample<T>(this IEnumerable<(float, T)> sequence, float offset)
+        internal static (T, T, float, float) GetSample<T>(this IEnumerable<(float, T)> sequence, float offset)
         {
             (float, T)? left = null;
             (float, T)? right = null;
@@ -185,17 +185,19 @@ namespace SharpGLTF
                 prev = item;
             }
 
-            if (left == null && right == null) return (default(T), default(T), 0);
-            if (left == null) return (right.Value.Item2, right.Value.Item2, 0);
-            if (right == null) return (left.Value.Item2, left.Value.Item2, 0);
+            if (left == null && right == null) return (default(T), default(T), 0, 0);
+            if (left == null) return (right.Value.Item2, right.Value.Item2, 0, 0);
+            if (right == null) return (left.Value.Item2, left.Value.Item2, 0, 0);
 
-            System.Diagnostics.Debug.Assert(left.Value.Item1 < right.Value.Item1);
+            var delta = right.Value.Item1 - left.Value.Item1;
 
-            var amount = (offset - left.Value.Item1) / (right.Value.Item1 - left.Value.Item1);
+            System.Diagnostics.Debug.Assert(delta > 0);
+
+            var amount = (offset - left.Value.Item1) / delta;
 
             System.Diagnostics.Debug.Assert(amount >= 0 && amount <= 1);
 
-            return (left.Value.Item2, right.Value.Item2, amount);
+            return (left.Value.Item2, right.Value.Item2, amount, delta);
         }
 
         internal static Func<float, Vector3> GetLinearSamplerFunc(this IEnumerable<(float, Vector3)> collection)
@@ -244,6 +246,36 @@ namespace SharpGLTF
             return _sampler;
         }
 
+        internal static Func<float, Vector3> GetCubicSamplerFunc(this IEnumerable<(float, (Vector3, Vector3, Vector3))> collection)
+        {
+            // http://www.hugi.scene.org/online/coding/hugi%2012%20-%20cosplqua.htm
+
+            if (collection == null) return null;
+
+            Vector3 _sampler(float offset)
+            {
+                var sample = collection.GetSample(offset);
+
+                var t = sample.Item3;
+                var dt = sample.Item4;
+
+                var tt = t * t;
+                var ttt = tt * t;
+
+                var p0 = sample.Item1.Item1;
+                var m0 = dt * sample.Item1.Item3;
+
+                var p1 = sample.Item2.Item1;
+                var m1 = dt * sample.Item1.Item2;
+
+                var p = (2 * ttt - 3 * tt + 1) * p0 + (ttt - 2 * tt + t) * m0 + (-2 * ttt + 3 * tt) * p1 + (ttt - tt) * m1;
+
+                return p;
+            }
+
+            return _sampler;
+        }
+
         #endregion
 
         #region linq

src/SharpGLTF.Core/Schema2/gltf.Animations.cs

@@ -471,6 +471,48 @@ namespace SharpGLTF.Schema2
             return keys.Zip(frames, (key, val) => (key, val));
         }
 
+        public IEnumerable<(Single, (Vector3, Vector3, Vector3))> AsCubicVector3KeyFrames()
+        {
+            var keys = this.Input.AsScalarArray();
+            var frames = GroupThree(this.Output.AsVector3Array());
+
+            return keys.Zip(frames, (key, val) => (key, val));
+        }
+
+        public IEnumerable<(Single, (Quaternion, Quaternion, Quaternion))> AsCubicQuaternionKeyFrames()
+        {
+            var keys = this.Input.AsScalarArray();
+            var frames = GroupThree(this.Output.AsQuaternionArray());
+
+            return keys.Zip(frames, (key, val) => (key, val));
+        }
+
+        public IEnumerable<(Single, (Single[], Single[], Single[]))> AsCubicArrayKeyFrames(int dimensions)
+        {
+            var keys = this.Input.AsScalarArray();
+            var frames = GroupThree(this.Output.AsMultiArray(dimensions));
+
+            return keys.Zip(frames, (key, val) => (key, val));
+        }
+
+        private static IEnumerable<(T, T, T)> GroupThree<T>(IEnumerable<T> collection)
+        {
+            using (var ptr = collection.GetEnumerator())
+            {
+                while (true)
+                {
+                    if (!ptr.MoveNext()) break;
+                    var a = ptr.Current;
+                    if (!ptr.MoveNext()) break;
+                    var b = ptr.Current;
+                    if (!ptr.MoveNext()) break;
+                    var c = ptr.Current;
+
+                    yield return (a, b, c);
+                }
+            }
+        }
+
         #endregion
     }