godotVmcSharp/VmcMessages/VmcExtRootPos.cs

/*
    godotVmcSharp
    Copyright (C) 2023  Cassandra de la Cruz-Munoz

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as published
    by the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.
    */

using Godot;
using godotOscSharp;
using System.Collections.Generic;

namespace godotVmcSharp
{
    public class VmcExtRootPos : VmcMessage
    {
        public readonly string Name;
        public readonly Transform3D Transform;

        public readonly Vector3? Scale;
        public readonly Vector3? Offset;

        public static VmcExtRootPos CreateFromMessage(OscMessage m)
        {
            switch (m.Data.Count)
            {
                case 8:
                    return new VmcExtRootPos(m.Data[0].GetAsString(), new Transform3D(new Basis(new Quaternion(m.Data[4].GetAsFloat(), m.Data[5].GetAsFloat(), m.Data[6].GetAsFloat(), m.Data[7].GetAsFloat())), new Vector3(m.Data[1].GetAsFloat(), m.Data[2].GetAsFloat(), m.Data[3].GetAsFloat())));
                case 14:
                    return new VmcExtRootPos(m.Data[0].GetAsString(), new Transform3D(new Basis(new Quaternion(m.Data[4].GetAsFloat(), m.Data[5].GetAsFloat(), m.Data[6].GetAsFloat(), m.Data[7].GetAsFloat())), new Vector3(m.Data[1].GetAsFloat(), m.Data[2].GetAsFloat(), m.Data[3].GetAsFloat())), new Vector3(m.Data[8].GetAsFloat(), m.Data[9].GetAsFloat(), m.Data[10].GetAsFloat()), new Vector3(m.Data[11].GetAsFloat(), m.Data[12].GetAsFloat(), m.Data[13].GetAsFloat()));
                default:
                    throw new MissingArgumentsException(m.Address, "{8, 14}", m.Data.Count);
            }
        }

        public VmcExtRootPos(string name, Transform3D transform) : base(new OscAddress("/VMC/Ext/Root/Pos"))
        {
            Name = name;
            Transform = transform;
        }

        public VmcExtRootPos(string name, Transform3D transform, Vector3 scale, Vector3 offset) : base(new OscAddress("/VMC/Ext/Root/Pos"))
        {
            Name = name;
            Transform = transform;
            Scale = scale;
            Offset = offset;
        }

        public new OscMessage ToMessage()
        {
            var quat = Transform.Basis.GetRotationQuaternion();
            if (!Scale.HasValue)
            {
                return new OscMessage(Addr, new List<OscArgument>{
                    OscArgument.CreateFromParams(Name, 's')!,
                    OscArgument.CreateFromParams(Transform.Origin.X, 'f')!,
                    OscArgument.CreateFromParams(Transform.Origin.Y, 'f')!,
                    OscArgument.CreateFromParams(Transform.Origin.Z, 'f')!,
                    OscArgument.CreateFromParams(quat.X, 'f')!,
                    OscArgument.CreateFromParams(quat.Y, 'f')!,
                    OscArgument.CreateFromParams(quat.Z, 'f')!,
                    OscArgument.CreateFromParams(quat.W, 'f')!,
                });
            }
            return new OscMessage(Addr, new List<OscArgument>{
                OscArgument.CreateFromParams(Name, 's')!,
                OscArgument.CreateFromParams(Transform.Origin.X, 'f')!,
                OscArgument.CreateFromParams(Transform.Origin.Y, 'f')!,
                OscArgument.CreateFromParams(Transform.Origin.Z, 'f')!,
                OscArgument.CreateFromParams(quat.X, 'f')!,
                OscArgument.CreateFromParams(quat.Y, 'f')!,
                OscArgument.CreateFromParams(quat.Z, 'f')!,
                OscArgument.CreateFromParams(quat.W, 'f')!,
                OscArgument.CreateFromParams(Scale!.Value.X, 'f')!,
                OscArgument.CreateFromParams(Scale!.Value.Y, 'f')!,
                OscArgument.CreateFromParams(Scale!.Value.Z, 'f')!,
                OscArgument.CreateFromParams(Offset!.Value.X, 'f')!,
                OscArgument.CreateFromParams(Offset!.Value.Y, 'f')!,
                OscArgument.CreateFromParams(Offset!.Value.Z, 'f')!,
            });
        }
    }
}