RoboidControl-csharp/LinearAlgebra/src/Spherical.cs

using System;
#if UNITY_5_3_OR_NEWER
//using Vector3Float = UnityEngine.Vector3;
using Vector3 = UnityEngine.Vector3;
#endif

namespace LinearAlgebra {
    /// <summary>
    /// A spherical vector
    /// </summary>
    public class Spherical {

        /// <summary>
        /// Create a default vector with zero distance
        /// </summary>
        public Spherical() {
            this.distance = 0;
            this.direction = new Direction();
        }

        /// <summary>
        /// Create a spherical vector
        /// </summary>
        /// <param name="distance">The distance in meters</param>
        /// <param name="direction">The direction of the vector</param>
        public Spherical(float distance, Direction direction) {
            this.distance = distance;
            this.direction = direction;
        }

        /// <summary>
        /// Create spherical vector. All given angles are in degrees
        /// </summary>
        /// <param name="distance">The distance in meters</param>
        /// <param name="horizontal">The horizontal angle in degrees</param>
        /// <param name="vertical">The vertical angle in degrees</param>
        /// <returns></returns>
        public static Spherical Degrees(float distance, float horizontal, float vertical) {
            Direction direction = Direction.Degrees(horizontal, vertical);
            Spherical s = new(distance, direction);
            return s;
        }

        public static Spherical Radians(float distance, float horizontal, float vertical) {
            Direction direction = Direction.Radians(horizontal, vertical);
            Spherical s = new(distance, direction);
            return s;
        }

        /// <summary>
        /// The distance in meters
        /// </summary>
        /// @remark The distance should never be negative
        public float distance;
        /// <summary>
        /// The direction of the vector
        /// </summary>
        public Direction direction;

        /// <summary>
        /// A spherical vector with zero degree angles and distance
        /// </summary>
        public readonly static Spherical zero = new(0, Direction.forward);
        /// <summary>
        /// A normalized forward-oriented vector
        /// </summary>
        public readonly static Spherical forward = new(1, Direction.forward);


        // public static Spherical FromVector3Float(Vector3Float v) {
        //     float distance = v.magnitude;
        //     if (distance == 0.0f)
        //         return Spherical.zero;
        //     else {
        //         float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg);
        //         float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
        //         return Spherical.Degrees(distance, horizontalAngle, verticalAngle);
        //     }
        // }

        public static Spherical FromVector3(Vector3 v) {
            float distance = v.magnitude;
            if (distance == 0.0f)
                return Spherical.zero;
            else {
                float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg);
                float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
                return Spherical.Degrees(distance, horizontalAngle, verticalAngle);
            }
        }

        // public Vector3Float ToVector3Float() {
        //     float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad;
        //     float horizontalRad = this.direction.horizontal * Angle.Deg2Rad;
        //     float cosVertical = (float)Math.Cos(verticalRad);
        //     float sinVertical = (float)Math.Sin(verticalRad);
        //     float cosHorizontal = (float)Math.Cos(horizontalRad);
        //     float sinHorizontal = (float)Math.Sin(horizontalRad);

        //     float x = this.distance * sinVertical * sinHorizontal;
        //     float y = this.distance * cosVertical;
        //     float z = this.distance * sinVertical * cosHorizontal;

        //     Vector3Float v = new(x, y, z);
        //     return v;
        // }

        public Vector3 ToVector3() {
            float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad;
            float horizontalRad = this.direction.horizontal * Angle.Deg2Rad;
            float cosVertical = (float)Math.Cos(verticalRad);
            float sinVertical = (float)Math.Sin(verticalRad);
            float cosHorizontal = (float)Math.Cos(horizontalRad);
            float sinHorizontal = (float)Math.Sin(horizontalRad);

            float x = this.distance * sinVertical * sinHorizontal;
            float y = this.distance * cosVertical;
            float z = this.distance * sinVertical * cosHorizontal;

            Vector3 v = new(x, y, z);
            return v;
        }

        public static Spherical operator +(Spherical s1, Spherical s2) {
            // let's do it the easy way...
            Vector3 v1 = s1.ToVector3();
            Vector3 v2 = s2.ToVector3();
            Vector3 v = v1 + v2;
            Spherical r = FromVector3(v);
            return r;
        }

    }
}