cubic splines WIP

src/Shared/_Extensions.cs

@@ -159,123 +159,7 @@ namespace SharpGLTF
 
             return dst;
         }
-
-        internal static (T, T, float, float) GetSample<T>(this IEnumerable<(float, T)> sequence, float offset)
-        {
-            (float, T)? left = null;
-            (float, T)? right = null;
-            (float, T)? prev = null;
-
-            if (offset < 0) offset = 0;
-
-            foreach (var item in sequence)
-            {
-                if (item.Item1 == offset)
-                {
-                    left = item; continue;
-                }
-
-                if (item.Item1 > offset)
-                {
-                    if (left == null) left = prev;
-                    right = item;
-                    break;
-                }
-
-                prev = item;
-            }
-
-            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);
-
-            var delta = 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, delta);
-        }
-
-        internal static Func<float, Vector3> GetLinearSamplerFunc(this IEnumerable<(float, Vector3)> collection)
-        {
-            if (collection == null) return null;
-
-            Vector3 _sampler(float offset)
-            {
-                var sample = collection.GetSample(offset);
-                return Vector3.Lerp(sample.Item1, sample.Item2, sample.Item3);
-            }
-
-            return _sampler;
-        }
-
-        internal static Func<float, Quaternion> GetLinearSamplerFunc(this IEnumerable<(float, Quaternion)> collection)
-        {
-            if (collection == null) return null;
-
-            Quaternion _sampler(float offset)
-            {
-                var sample = collection.GetSample(offset);
-                return Quaternion.Slerp(sample.Item1, sample.Item2, sample.Item3);
-            }
-
-            return _sampler;
-        }
-
-        internal static Func<float, float[]> GetLinearSamplerFunc(this IEnumerable<(float, float[])> collection)
-        {
-            if (collection == null) return null;
-
-            float[] _sampler(float offset)
-            {
-                var sample = collection.GetSample(offset);
-                var result = new float[sample.Item1.Length];
-
-                for (int i = 0; i < result.Length; ++i)
-                {
-                    result[i] = sample.Item1[i] * (1 - sample.Item3) + sample.Item2[i] * sample.Item3;
-                }
-
-                return result;
-            }
-
-            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

@@ -8,6 +8,8 @@ using SharpGLTF.Collections;
 
 namespace SharpGLTF.Schema2
 {
+    using Transforms;
+
     [System.Diagnostics.DebuggerDisplay("Animation[{LogicalIndex}] {Name}")]
     public sealed partial class Animation
     {
@@ -159,15 +161,15 @@ namespace SharpGLTF.Schema2
             return channel.Sampler.AsLinearVector3KeyFrames();
         }
 
-        public Transforms.AffineTransform GetLocalTransform(Node node, float time)
+        public AffineTransform GetLocalTransform(Node node, float time)
         {
             Guard.MustShareLogicalParent(this, node, nameof(node));
 
             var xform = node.LocalTransform;
 
-            var sfunc = this.FindScaleChannel(node).GetLinearSamplerFunc();
-            var rfunc = this.FindRotationChannel(node).GetLinearSamplerFunc();
-            var tfunc = this.FindTranslationChannel(node).GetLinearSamplerFunc();
+            var sfunc = this.FindScaleChannel(node).CreateLinearSamplerFunc();
+            var rfunc = this.FindRotationChannel(node).CreateLinearSamplerFunc();
+            var tfunc = this.FindTranslationChannel(node).CreateLinearSamplerFunc();
 
             if (sfunc != null) xform.Scale = sfunc(time);
             if (rfunc != null) xform.Rotation = rfunc(time);
@@ -186,9 +188,9 @@ namespace SharpGLTF.Schema2
             var channel = _channels.FirstOrDefault(item => item.TargetNode == node && item.TargetNodePath == PropertyPath.weights);
             if (channel == null) return morphWeights;
 
-            var frames = channel.Sampler.AsLinearArrayKeyFrames(morphWeights.Count);
-
-            var mfunc = frames.GetLinearSamplerFunc();
+            var mfunc = channel.Sampler
+                .AsLinearArrayKeyFrames(morphWeights.Count)
+                .CreateLinearSamplerFunc();
 
             return mfunc(time);
         }
@@ -427,6 +429,10 @@ namespace SharpGLTF.Schema2
         public void SetVector3Keys(IReadOnlyDictionary<Single, (Vector3, Vector3, Vector3)> keyframes)
         {
             var kv = _Split(keyframes);
+
+            kv.Item2[0] = Vector3.Zero;
+            kv.Item2[kv.Item2.Length - 1] = Vector3.Zero;
+
             _input = this._CreateInputAccessor(kv.Item1).LogicalIndex;
             _output = this._CreateOutputAccessor(kv.Item2).LogicalIndex;
         }
@@ -441,6 +447,10 @@ namespace SharpGLTF.Schema2
         public void SetQuaternionKeys(IReadOnlyDictionary<Single, (Quaternion, Quaternion, Quaternion)> keyframes)
         {
             var kv = _Split(keyframes);
+
+            kv.Item2[0] = new Quaternion(0, 0, 0, 0);
+            kv.Item2[kv.Item2.Length - 1] = new Quaternion(0, 0, 0, 0);
+
             _input = this._CreateInputAccessor(kv.Item1).LogicalIndex;
             _output = this._CreateOutputAccessor(kv.Item2).LogicalIndex;
         }

src/SharpGLTF.Core/Transforms/AnimationSamplerFactory.cs

@@ -0,0 +1,150 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+
+namespace SharpGLTF.Transforms
+{
+    internal static class AnimationSamplerFactory
+    {
+        private static (T, T, float) _GetSample<T>(this IEnumerable<(float, T)> sequence, float offset)
+        {
+            (float, T)? left = null;
+            (float, T)? right = null;
+            (float, T)? prev = null;
+
+            if (offset < 0) offset = 0;
+
+            foreach (var item in sequence)
+            {
+                if (item.Item1 == offset)
+                {
+                    left = item; continue;
+                }
+
+                if (item.Item1 > offset)
+                {
+                    if (left == null) left = prev;
+                    right = item;
+                    break;
+                }
+
+                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);
+
+            var delta = 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);
+        }
+
+        internal static Func<float, Vector3> CreateLinearSamplerFunc(this IEnumerable<(float, Vector3)> collection)
+        {
+            if (collection == null) return null;
+
+            Vector3 _sampler(float offset)
+            {
+                var sample = collection._GetSample(offset);
+                return Vector3.Lerp(sample.Item1, sample.Item2, sample.Item3);
+            }
+
+            return _sampler;
+        }
+
+        internal static Func<float, Quaternion> CreateLinearSamplerFunc(this IEnumerable<(float, Quaternion)> collection)
+        {
+            if (collection == null) return null;
+
+            Quaternion _sampler(float offset)
+            {
+                var sample = collection._GetSample(offset);
+                return Quaternion.Slerp(sample.Item1, sample.Item2, sample.Item3);
+            }
+
+            return _sampler;
+        }
+
+        internal static Func<float, float[]> CreateLinearSamplerFunc(this IEnumerable<(float, float[])> collection)
+        {
+            if (collection == null) return null;
+
+            float[] _sampler(float offset)
+            {
+                var sample = collection._GetSample(offset);
+                var result = new float[sample.Item1.Length];
+
+                for (int i = 0; i < result.Length; ++i)
+                {
+                    result[i] = sample.Item1[i] * (1 - sample.Item3) + sample.Item2[i] * sample.Item3;
+                }
+
+                return result;
+            }
+
+            return _sampler;
+        }
+
+        internal static Func<float, Vector3> CreateCubicSamplerFunc(this IEnumerable<(float, (Vector3, Vector3, Vector3))> collection)
+        {
+            return CreateCubicSamplerFunc<Vector3>(collection, Hermite);
+        }
+
+        internal static Func<float, Quaternion> CreateCubicSamplerFunc(this IEnumerable<(float, (Quaternion, Quaternion, Quaternion))> collection)
+        {
+            return CreateCubicSamplerFunc<Quaternion>(collection, Hermite);
+        }
+
+        internal static Func<float, T> CreateCubicSamplerFunc<T>(this IEnumerable<(float, (T, T, T))> collection, Func<T, T, T, T, float, T> hermiteFunc)
+        {
+            if (collection == null) return null;
+
+            T _sampler(float offset)
+            {
+                var sample = collection._GetSample(offset);
+
+                return hermiteFunc(sample.Item1.Item2, sample.Item1.Item3, sample.Item2.Item2, sample.Item2.Item1, sample.Item3);
+            }
+
+            return _sampler;
+        }
+
+        internal static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount)
+        {
+            // http://mathworld.wolfram.com/HermitePolynomial.html
+
+            var squared = amount * amount;
+            var cubed = amount * squared;
+
+            var part1 = (2.0f * cubed) - (3.0f * squared) + 1.0f;
+            var part2 = (-2.0f * cubed) + (3.0f * squared);
+            var part3 = (cubed - (2.0f * squared)) + amount;
+            var part4 = cubed - squared;
+
+            return (value1 * part1) + (value2 * part2) + (tangent1 * part3) + (tangent2 * part4);
+        }
+
+        internal static Quaternion Hermite(Quaternion value1, Quaternion tangent1, Quaternion value2, Quaternion tangent2, float amount)
+        {
+            // http://mathworld.wolfram.com/HermitePolynomial.html
+
+            var squared = amount * amount;
+            var cubed = amount * squared;
+
+            var part1 = (2.0f * cubed) - (3.0f * squared) + 1.0f;
+            var part2 = (-2.0f * cubed) + (3.0f * squared);
+            var part3 = (cubed - (2.0f * squared)) + amount;
+            var part4 = cubed - squared;
+
+            return Quaternion.Normalize((value1 * part1) + (value2 * part2) + (tangent1 * part3) + (tangent2 * part4));
+        }
+    }
+}

tests/SharpGLTF.Tests/AnimationSamplingTests.cs

@@ -0,0 +1,55 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+
+using NUnit.Framework;
+
+namespace SharpGLTF
+{
+    [TestFixture]
+    [Category("Core")]
+    public class AnimationSamplingTests
+    {
+        private static (float, (Vector3, Vector3, Vector3))[] _TransAnim = new []
+        {
+            (0.0f, (new Vector3(0, 0, 0), new Vector3(-1, 0, 0),new Vector3(0, 0, 0))),
+            (1.0f, (new Vector3(0, 0, 0), new Vector3(+1, 0, 0),new Vector3(0, 3, 0))),
+            (2.0f, (new Vector3(0, 0, 0), new Vector3(-1, 0, 0),new Vector3(0, 0, 0)))
+        };
+
+        private static (float, (Quaternion, Quaternion, Quaternion))[] _RotAnim = new[]
+        {
+            (0.0f, (new Quaternion(0,0,0,0), Quaternion.CreateFromYawPitchRoll(+1.6f, 0, 0), new Quaternion(0,0,0,0))),
+            (1.0f, (new Quaternion(0,0,0,0), Quaternion.Identity, new Quaternion(0,0,0,0))),
+            (2.0f, (new Quaternion(0,0,0,0), Quaternion.CreateFromYawPitchRoll(-1.6f, 0, 0), new Quaternion(0,0,0,0))),
+            (3.0f, (new Quaternion(0,0,0,0), Quaternion.Identity, new Quaternion(0,0,0,0))),
+            (4.0f, (new Quaternion(0,0,0,0), Quaternion.CreateFromYawPitchRoll(+1.6f, 0, 0), new Quaternion(0,0,0,0))),
+        };
+
+        [Test]
+        public void TestVector3CubicSplineSampling()
+        {
+            var hermite = Transforms.AnimationSamplerFactory.Hermite(new Vector3(1, 0, 0), new Vector3(0, 2, 0), new Vector3(3, 0, 0), new Vector3(0, -2, 0), 0.5f);
+
+            var sampler = Transforms.AnimationSamplerFactory.CreateCubicSamplerFunc(_TransAnim);
+
+            var a = sampler(0);
+            var b = sampler(1);
+            var bc = sampler(1.5f);
+            var c = sampler(2);
+        }
+
+        [Test]
+        public void TestQuaternionCubicSplineSampling()
+        {
+            var sampler = Transforms.AnimationSamplerFactory.CreateCubicSamplerFunc(_RotAnim);
+
+            var a = sampler(0);
+            var b = sampler(1);
+            var bc = sampler(1.5f);
+            var c = sampler(2);
+        }
+
+    }
+}