NanoBrain-unitypackage/src/SwingTwist.cs

#if !UNITY_5_3_OR_NEWER
using UnityEngine;
#endif

namespace LinearAlgebra {

    /// <summary>
    /// An orientation using swing and twist angles
    /// </summary>
    /// <param name="swing">The swing rotation</param>
    /// <param name="twist">The twist rotation</param>
    public struct SwingTwist {
        public Direction swing;
        public AngleFloat twist;

        public SwingTwist(Direction swing, AngleFloat twist) {
            this.swing = swing;
            this.twist = twist;
        }

        /// <summary>
        /// Create a swing/twist rotation using angles in degrees
        /// </summary>
        /// <param name="horizontalSwing">The swing angle in the horizontal plane in degrees</param>
        /// <param name="verticalSwing">The swing angle in the vertical plan in degrees</param>
        /// <param name="twist">The twist angle in degrees</param>
        /// <returns>The swing/twist rotation</returns>
        public static SwingTwist Degrees(float horizontalSwing, float verticalSwing, float twist) {
            Direction swing = Direction.Degrees(horizontalSwing, verticalSwing);
            AngleFloat twistAngle = AngleFloat.Degrees(twist);
            SwingTwist s = new(swing, twistAngle);
            return s;
        }

        /// <summary>
        /// Create a swing/twist rotation using angles in degrees
        /// </summary>
        /// <param name="horizontalSwing">The swing angle in the horizontal plane in degrees</param>
        /// <param name="verticalSwing">The swing angle in the vertical plan in degrees</param>
        /// <param name="twist">The twist angle in degrees</param>
        /// <returns>The swing/twist rotation</returns>
        public static SwingTwist Radians(float horizontalSwing, float verticalSwing, float twist) {
            Direction swing = Direction.Radians(horizontalSwing, verticalSwing);
            AngleFloat twistAngle = AngleFloat.Radians(twist);
            SwingTwist s = new(swing, twistAngle);
            return s;
        }

#if !UNITY_5_3_OR_NEWER
        /// <summary>
        /// A zero angle rotation
        /// </summary>
        public static readonly SwingTwist zero = Degrees(0, 0, 0);

        public Spherical ToAngleAxis() {
            LinearAlgebra.Quaternion q = this.ToQuaternion();
            q.ToAngleAxis(out float angle, out Vector3Float axis);
            Direction direction = Direction.FromVector3(axis);

            Spherical r = new(angle, direction);
            return r;
        }

        public static SwingTwist FromAngleAxis(Spherical r) {
            Vector3Float vectorAxis = r.direction.ToVector3();
            LinearAlgebra.Quaternion q = LinearAlgebra.Quaternion.AngleAxis(AngleFloat.Degrees(r.distance), vectorAxis);
            return FromQuaternion(q);
        }

        /// <summary>
        /// Convert a quaternion in a swing/twist rotation
        /// </summary>
        /// <param name="q">The quaternion to convert</param>
        /// <returns>The swing/twist rotation</returns>
        public static SwingTwist FromQuaternion(LinearAlgebra.Quaternion q) {
            Vector3Float v = LinearAlgebra.Quaternion.ToAngles(q);
            SwingTwist r = Degrees(v.vertical, v.horizontal, v.depth);
            return r;
        }

        public LinearAlgebra.Quaternion ToQuaternion() {
            LinearAlgebra.Quaternion q = LinearAlgebra.Quaternion.Euler(this.swing.vertical.inDegrees,
                                       this.swing.horizontal.inDegrees,
                                       this.twist.inDegrees);
            return q;

        }

        public static SwingTwist FromQuat32(Quat32 q32) {
            q32.ToAngles(out float right, out float up, out float forward);
            SwingTwist r = Degrees(up, right, forward);
            return r;
        }
#endif
#if UNITY_5_3_OR_NEWER
        /// <summary>
        /// Convert a quaternion in a swing/twist rotation
        /// </summary>
        /// <param name="q">The quaternion to convert</param>
        /// <returns>The swing/twist rotation</returns>
        public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) {
            UnityEngine.Vector3 angles = q.eulerAngles;
            SwingTwist r = Degrees(angles.y, -angles.x, -angles.z);
            return r;
        }

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

}