RoboidControl-csharp/src/SwingTwist.cs

using System.Numerics;
#if UNITY_5_3_OR_NEWER
using Quaternion = UnityEngine.Quaternion;
#endif

namespace LinearAlgebra {

    public class SwingTwist {
        public Direction swing;
        public float twist;

        public static readonly SwingTwist zero = new SwingTwist(0, 0, 0);

        public SwingTwist(Direction swing, float twist) {
            this.swing = swing;
            this.twist = twist;
        }
        public SwingTwist(float horizontalSwing, float verticalSwing, float twist) {
            this.swing = Direction.Degrees(horizontalSwing, verticalSwing);
            this.swing.Normalize();
            this.twist = twist;
        }
        public static SwingTwist FromQuat32(Quat32 q32) {
            // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
            // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
            q32.ToAngles(out float right, out float up, out float forward);
            SwingTwist r = new SwingTwist(up, right, forward);
            return r;
        }

#if UNITY_5_3_OR_NEWER
        public static SwingTwist FromQuaternion(Quaternion q) {
            // q.ToAngles(out float right, out float up, out float forward);
            UnityEngine.Vector3 angles = q.eulerAngles;
            SwingTwist r = new SwingTwist(angles.y, angles.x, angles.z);
            return r;
        }

        public Quaternion ToQuaternion() {
            Quaternion q = Quaternion.Euler(-this.swing.vertical,
                                   this.swing.horizontal,
                                   this.twist);
            return q;
        }
#endif        
    }

}