source: OniSplit/Totoro/AnimationDaeWriter.cs

using System;
using System.Diagnostics;
using System.Collections.Generic;

namespace Oni.Totoro
{
    internal static class AnimationDaeWriter
    {
        public static void AppendFrames(Animation anim1, Animation anim2)
        {
            var isOverlay = (anim2.Flags & AnimationFlags.Overlay) != 0;

            if (isOverlay)
            {
                Console.Error.WriteLine("Cannot merge {0} because it's an overlay animation", anim2.Name);
                return;
            }

            if (anim1.FrameSize == 0)
            {
                anim1.FrameSize = anim2.FrameSize;
            }
            else if (anim1.FrameSize != anim2.FrameSize)
            {
                Console.Error.WriteLine("Cannot merge {0} because its frame size doesn't match the frame size of the previous animation", anim2.Name);
                return;
            }

            anim1.Velocities.AddRange(anim2.Velocities);
            anim1.Heights.AddRange(anim2.Heights);

            if (anim1.Rotations.Count == 0)
            {
                anim1.Rotations.AddRange(anim2.Rotations);
            }
            else
            {
                for (int i = 0; i < anim1.Rotations.Count; i++)
                    anim1.Rotations[i].AddRange(anim2.Rotations[i]);
            }
        }

        public static void Write(Dae.Node root, Animation animation, int startFrame = 0)
        {
            var velocities = animation.Velocities;
            var heights = animation.Heights;
            var rotations = animation.Rotations;
            var isCompressed = animation.FrameSize == 6;
            var isOverlay = (animation.Flags & AnimationFlags.Overlay) != 0;
            var isRealWorld = (animation.Flags & AnimationFlags.RealWorld) != 0;
            var activeBones = (uint)(animation.OverlayUsedBones | animation.OverlayReplacedBones);

            var nodes = FindNodes(root);

            if (!isOverlay && !isRealWorld)
            {
                var pelvis = nodes[0];

                //
                // Write pelvis position animation
                //

                var positions = new Vector2[velocities.Count + 1];

                for (int i = 1; i < positions.Length; i++)
                    positions[i] = positions[i - 1] + velocities[i - 1];

                CreateAnimationCurve(startFrame, positions.Select(p => p.X).ToList(), pelvis, "pos", "X");
                CreateAnimationCurve(startFrame, positions.Select(p => p.Y).ToList(), pelvis, "pos", "Z");

                //
                // Write pelvis height animation
                //

                CreateAnimationCurve(startFrame, heights.ToList(), pelvis, "pos", "Y");
            }

            //
            // Write rotation animations for all bones
            //

            bool plot = true;

            for (int i = 0; i < rotations.Count; i++)
            {
                if (isOverlay && (activeBones & (1u << i)) == 0)
                    continue;

                var node = nodes[i];
                var keys = rotations[i];

                int length;

                if (plot)
                    length = keys.Sum(k => k.Duration);
                else
                    length = keys.Count;

                var times = new float[length];
                var xAngles = new float[length];
                var yAngles = new float[length];
                var zAngles = new float[length];

                if (plot)
                {
                    //
                    // Transform key frames to quaternions.
                    //

                    var quats = new Quaternion[keys.Count];

                    for (int k = 0; k < keys.Count; k++)
                    {
                        var key = keys[k];

                        if (isCompressed)
                        {
                            quats[k] = Quaternion.CreateFromAxisAngle(Vector3.UnitX, MathHelper.ToRadians(key.Rotation.X))
                                     * Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.ToRadians(key.Rotation.Y))
                                     * Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathHelper.ToRadians(key.Rotation.Z));
                        }
                        else
                        {
                            quats[k] = new Quaternion(key.Rotation);
                        }
                    }

                    //
                    // Interpolate the quaternions.
                    //

                    int frame = 0;

                    for (int k = 0; k < keys.Count; k++)
                    {
                        var duration = keys[k].Duration;

                        var q1 = quats[k];
                        var q2 = (k == keys.Count - 1) ? quats[k] : quats[k + 1];

                        for (int t = 0; t < duration; t++)
                        {
                            var q = Quaternion.Lerp(q1, q2, (float)t / (float)duration);
                            var euler = q.ToEulerXYZ();

                            times[frame] = (frame + startFrame) * (1.0f / 60.0f);

                            xAngles[frame] = euler.X;
                            yAngles[frame] = euler.Y;
                            zAngles[frame] = euler.Z;

                            frame++;
                        }
                    }

                    MakeRotationCurveContinuous(xAngles);
                    MakeRotationCurveContinuous(yAngles);
                    MakeRotationCurveContinuous(zAngles);
                }
                else
                {
                    int frame = 0;

                    for (int k = 0; k < keys.Count; k++)
                    {
                        var key = keys[k];

                        times[k] = (frame + startFrame) * (1.0f / 60.0f);
                        frame += key.Duration;

                        if (isCompressed)
                        {
                            xAngles[k] = key.Rotation.X;
                            yAngles[k] = key.Rotation.Y;
                            zAngles[k] = key.Rotation.Z;
                        }
                        else
                        {
                            var euler = new Quaternion(key.Rotation).ToEulerXYZ();

                            xAngles[k] = euler.X;
                            yAngles[k] = euler.Y;
                            zAngles[k] = euler.Z;
                        }
                    }
                }

                CreateAnimationCurve(times, xAngles, node, "rotX", "ANGLE");
                CreateAnimationCurve(times, yAngles, node, "rotY", "ANGLE");
                CreateAnimationCurve(times, zAngles, node, "rotZ", "ANGLE");
            }
        }

        private static void MakeRotationCurveContinuous(float[] curve)
        {
            for (int i = 1; i < curve.Length; i++)
            {
                float v1 = curve[i - 1];
                float v2 = curve[i];

                if (Math.Abs(v2 - v1) > 180.0f)
                {
                    if (v2 > v1)
                        v2 -= 360.0f;
                    else
                        v2 += 360.0f;

                    curve[i] = v2;
                }
            }
        }

        private static void CreateAnimationCurve(int startFrame, IList<float> values, Dae.Node targetNode, string targetSid, string targetValue)
        {
            if (values.Count == 0)
                return;

            var times = new float[values.Count];

            for (int i = 0; i < times.Length; i++)
                times[i] = (i + startFrame) * (1.0f / 60.0f);

            CreateAnimationCurve(times, values, targetNode, targetSid, targetValue);
        }

        private static void CreateAnimationCurve(IList<float> times, IList<float> values, Dae.Node targetNode, string targetSid, string targetValue)
        {
            Debug.Assert(times.Count > 0);
            Debug.Assert(times.Count == values.Count);

            var interpolations = new string[times.Count];

            for (int i = 0; i < interpolations.Length; i++)
                interpolations[i] = "LINEAR";

            var targetTransform = targetNode.Transforms.Find(x => x.Sid == targetSid);

            targetTransform.BindAnimation(targetValue, new Dae.Sampler {
                Inputs = {
                    new Dae.Input(Dae.Semantic.Input, new Dae.Source(times, 1)),
                    new Dae.Input(Dae.Semantic.Output, new Dae.Source(values, 1)),
                    new Dae.Input(Dae.Semantic.Interpolation, new Dae.Source(interpolations, 1))
                }
            });
        }

        private static List<Dae.Node> FindNodes(Dae.Node root)
        {
            var nodes = new List<Dae.Node>(19);
            FindNodesRecursive(root, nodes);
            return nodes;
        }

        private static void FindNodesRecursive(Dae.Node node, List<Dae.Node> result)
        {
            result.Add(node);

            foreach (var child in node.Nodes)
                FindNodesRecursive(child, result);
        }
    }
}