ReFuel.Gltf/ReFuel.Gltf/GltfNode.cs

using System.Numerics;
using System.Runtime.InteropServices;
using System.Text.Json;
using ReFuel.Gltf.Serialization;

namespace ReFuel.Gltf
{
    public class GltfNode : GltfIdObject
    {
        public override GltfObjectKind Kind => GltfObjectKind.Node;

        private GltfSmartId<GltfIdObject> camera;
        private GltfSmartId<GltfIdObject> skin;
        private GltfSmartId<GltfMesh> mesh;
        private float[]? weights = null;
        private TransformMode transformMode = TransformMode.Matrix;
        private TransformUnion transform = new TransformUnion() { Matrix = Matrix4x4.Identity };

        public string? Name { get; set; }
        public object? Camera { get; set; }
        public GltfRefList<GltfNode> Children { get; }
        public object? Skin { get; set; }

        public Matrix4x4 Matrix
        {
            get => transformMode == TransformMode.Matrix ? transform.Matrix : transform.ToMatrix().Matrix;
            set
            {
                transformMode = TransformMode.Matrix;
                transform.Matrix = value;
            }
        }

        public Quaternion Rotation
        {
            get => transformMode == TransformMode.SRT ? transform.Rotation : transform.ToSRT().Rotation;

            set
            {
                if (transformMode != TransformMode.SRT)
                {
                    transformMode = TransformMode.SRT;
                    transform = transform.ToSRT();
                }

                transform.Rotation = value;
            }
        }

        public Vector3 Scale
        {
            get => transformMode == TransformMode.SRT ?
                transform.Scale :
                transform.ToSRT().Scale;
            set
            {
                if (transformMode != TransformMode.SRT)
                {
                    transformMode = TransformMode.SRT;
                    transform = transform.ToSRT();
                }

                transform.Scale = value;
            }
        }

        public Vector3 Translation
        {
            get => transformMode == TransformMode.SRT ?
                transform.Translation :
                transform.ToSRT().Translation;
            set
            {
                if (transformMode != TransformMode.SRT)
                {
                    transformMode = TransformMode.SRT;
                    transform = transform.ToSRT();
                }

                transform.Translation = value;
            }
        }

        public GltfMesh? Mesh
        {
            get => mesh.IsSet ? mesh.Value : null;
            set
            {
                if (value is null)
                    mesh.Reset();
                else
                    mesh.Value = value;
            }
        }
        public float[]? Weights => weights;

        internal GltfNode(GltfDocument document) : base(document)
        {
            Children = new GltfRefList<GltfNode>(document.Nodes);
            mesh = new GltfSmartId<GltfMesh>(document.Meshes);
        }

        /// <summary>
        /// Optimize this node for matrix mode transforms.
        /// </summary>
        /// <returns>Self</returns>
        public GltfNode MatrixMode()
        {
            if (transformMode == TransformMode.Matrix)
                return this;

            transformMode = TransformMode.Matrix;
            transform = transform.ToMatrix();
            return this;
        }

        /// <summary>
        /// Optimize this node for scale rotation translation transforms.
        /// </summary>
        /// <returns>Self</returns>
        public GltfNode SrtMode()
        {
            if (transformMode == TransformMode.SRT)
                return this;

            transformMode = TransformMode.SRT;
            transform = transform.ToSRT();

            return this;
        }

        internal override void Deserialize(JsonElement element)
        {
            if (element.ValueKind != JsonValueKind.Object)
                throw new Exception("Expected node to be an object.");

            if (element.TryGetProperty("name", out JsonElement nameProperty))
                Name = nameProperty.ToString();

            if (element.TryGetProperty("camera", out JsonElement cameraProperty))
            {
                if (!cameraProperty.TryGetInt32(out int cameraId))
                    throw new Exception("Expected camera id to be a number.");

                camera.Id = cameraId;
            }

            if (element.TryGetProperty("skin", out JsonElement skinElement))
            {
                if (!skinElement.TryGetInt32(out int skinId))
                    throw new Exception("Expected skin id to be a number.");

                skin.Id = skinId;
            }

            if (element.TryGetProperty("mesh", out JsonElement meshElement))
            {
                if (!meshElement.TryGetInt32(out int meshId))
                    throw new Exception("Expected mesh id to be a number.");

                mesh.Id = meshId;
            }

            if (element.TryGetProperty("children", out JsonElement childrenElement))
            {
                if (childrenElement.ValueKind != JsonValueKind.Array)
                    throw new Exception("Expected a JSON array for children.");

                foreach (JsonElement child in childrenElement.EnumerateArray())
                {
                    if (!child.TryGetInt32(out int id))
                        throw new Exception("Expected a number for child id.");

                    Children.Add(id);
                }
            }

            if (element.TryGetProperty("matrix", out JsonElement matrixElement))
            {
                transformMode = TransformMode.Matrix;
                transform.Matrix = matrixElement.ToSingleMatrix(4, 4, Matrix4x4.Identity);
            }
            else
            {
                transformMode = TransformMode.SRT;
                transform = new TransformUnion()
                {
                    Scale = Vector3.One,
                    Rotation = Quaternion.Identity,
                    Translation = Vector3.Zero
                };

                if (element.TryGetProperty("scale", out JsonElement scaleElement))
                {
                    transform.Scale = scaleElement.ToSingleVector(3, Vector3.One);
                }

                if (element.TryGetProperty("rotation", out JsonElement rotationElement))
                {
                    transform.Rotation = rotationElement.ToSingleVector(4, Quaternion.Identity);
                }

                if (element.TryGetProperty("translation", out JsonElement translationElement))
                {
                    transform.Translation = translationElement.ToSingleVector(3, Vector3.One);
                }
            }

            DeserializeCommon(element);
        }

        internal override void Serialize(Utf8JsonWriter writer)
        {
            throw new NotImplementedException();
        }

        [StructLayout(LayoutKind.Explicit)]
        private struct TransformUnion
        {
            [FieldOffset(0 * sizeof(float))] public Matrix4x4 Matrix;

            [FieldOffset(0 * sizeof(float))] public Vector3 Scale;
            [FieldOffset(4 * sizeof(float))] public Quaternion Rotation;
            [FieldOffset(8 * sizeof(float))] public Vector3 Translation;

            public TransformUnion ToSRT()
            {
                Vector3 r0 = new Vector3(Matrix.M11, Matrix.M12, Matrix.M13);
                Vector3 r1 = new Vector3(Matrix.M11, Matrix.M12, Matrix.M13);
                Vector3 r2 = new Vector3(Matrix.M11, Matrix.M12, Matrix.M13);

                float r0l = r0.Length();
                float r1l = r1.Length();
                float r2l = r2.Length();

                Vector3 scale = new Vector3(r0l, r1l, r2l);
                Vector3 translation = new Vector3(Matrix.M41, Matrix.M42, Matrix.M43);

                // The following code segment has been adapted from OpenTK.Mathematics (MIT), which adapted from
                // Blender (GPLv2 or more permissive).
                // However, it is probably not copyrighted considering it is math.

                r0 /= r0l;
                r1 /= r1l;
                r2 /= r2l;

                Quaternion rotation = default;
                float trace = 0.25f * (r0.X + r1.Y + r2.Z + 1f);

                if (trace > 0)
                {
                    float sq = MathF.Sqrt(trace);

                    rotation.W = sq;
                    sq = 1.0f / (4.0f * sq);
                    rotation.X = (r1.Z - r2.Y) * sq;
                    rotation.Y = (r2.X - r0.Z) * sq;
                    rotation.Z = (r0.Y - r1.X) * sq;
                }
                else if (r0.X > r1.Y && r0.X > r2.Z)
                {
                    float sq = 2.0f * MathF.Sqrt(1.0f + r0.X - r1.Y - r2.Z);

                    rotation.X = 0.25f * sq;
                    sq = 1.0f / sq;
                    rotation.W = (r2.Y - r1.Z) * sq;
                    rotation.Y = (r1.X + r0.Y) * sq;
                    rotation.Z = (r2.X + r0.Z) * sq;
                }
                else if (r1.Y > r2.Z)
                {
                    float sq = 2.0f * MathF.Sqrt(1.0f + r1.Y - r0.X - r2.Z);

                    rotation.Y = 0.25f * sq;
                    sq = 1.0f / sq;
                    rotation.W = (r2.X - r0.Z) * sq;
                    rotation.X = (r1.X + r0.Y) * sq;
                    rotation.Z = (r2.Y + r1.Z) * sq;
                }
                else
                {
                    float sq = 2.0f * MathF.Sqrt(1.0f + r2.Z - r0.X - r1.Y);

                    rotation.Z = (float)(0.25 * sq);
                    sq = 1.0f / sq;
                    rotation.W = (float)((r1.X - r0.Y) * sq);
                    rotation.X = (float)((r2.X + r0.Z) * sq);
                    rotation.Y = (float)((r2.Y + r1.Z) * sq);
                }

                rotation = Quaternion.Normalize(rotation);

                return new TransformUnion() { Scale = scale, Rotation = rotation, Translation = translation };
            }

            public TransformUnion ToMatrix()
            {
                Matrix4x4 matrix = Matrix4x4.CreateScale(Scale) *
                                   Matrix4x4.CreateFromQuaternion(Rotation) *
                                   Matrix4x4.CreateTranslation(Translation);

                return new TransformUnion() { Matrix = matrix };
            }
        }

        private enum TransformMode
        {
            Matrix,
            SRT
        }
    }
}