NanoBrain-unitypackage/LinearAlgebra/src/Direction.cs

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

namespace LinearAlgebra
{

    /// <summary>
    /// A direction in 3D space
    /// </summary>
    /// A direction is represented using two angles:
    /// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive)
    /// degrees which is a rotation in the horizontal plane
    /// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees
    /// which is the rotation in the up/down direction applied after the horizontal
    /// rotation has been applied.
    /// The angles are automatically normalized to stay within the abovenmentioned
    /// ranges.
    public struct Direction
    {
        /// @brief horizontal angle, range = (-180..180] degrees
        public AngleFloat horizontal;
        /// @brief vertical angle, range in degrees = (-90..90] degrees
        public AngleFloat vertical;

        /// <summary>
        /// Create a new direction
        /// </summary>
        /// <param name="horizontal">The horizontal angle</param>
        /// <param name="vertical">The vertical angle</param>
        /// <remarks>The direction will be normalized automatically
        /// to ensure the angles are within the allowed ranges</remarks>
        public Direction(AngleFloat horizontal, AngleFloat vertical)
        {
            this.horizontal = horizontal;
            this.vertical = vertical;
            this.Normalize();
        }

        /// <summary>
        /// Create a direction using angle values in degrees
        /// </summary>
        /// <param name="horizontal">The horizontal angle in degrees</param>
        /// <param name="vertical">The vertical angle in degrees</param>
        /// <returns>The direction</returns>
        /// <remarks>The direction will be normalized automatically
        /// to ensure the angles are within the allowed ranges</remarks>
        public static Direction Degrees(float horizontal, float vertical)
        {
            Direction d = new()
            {
                horizontal = AngleFloat.Degrees(horizontal),
                vertical = AngleFloat.Degrees(vertical)
            };
            d.Normalize();
            return d;
        }
        /// <summary>
        /// Create a direction using angle values in radians
        /// </summary>
        /// <param name="horizontal">The horizontal angle in radians</param>
        /// <param name="vertical">The vertical angle in radians</param>
        /// <returns>The direction</returns>
        public static Direction Radians(float horizontal, float vertical)
        {
            Direction d = new()
            {
                horizontal = AngleFloat.Radians(horizontal),
                vertical = AngleFloat.Radians(vertical)
            };
            d.Normalize();
            return d;
        }

        public override readonly string ToString() {
            return $"Direction(h: {this.horizontal}, v: {this.vertical})";
        }

        /// <summary>
        /// A forward direction with zero for both angles
        /// </summary>
        public readonly static Direction forward = Degrees(0, 0);
        /// <summary>
        /// A backward direction with horizontal angle -180 and zero vertical
        /// angle
        /// </summary>
        public readonly static Direction backward = Degrees(-180, 0);
        /// <summary>
        /// A upward direction with zero horizontal angle and vertical angle 90
        /// </summary>
        public readonly static Direction up = Degrees(0, 90);
        /// <summary>
        /// A downward direction with zero horizontal angle and vertical angle
        /// -90
        /// </summary>
        public readonly static Direction down = Degrees(0, -90);
        /// <summary>
        /// A left-pointing direction with horizontal angle -90 and zero
        /// vertical angle
        /// </summary>
        public readonly static Direction left = Degrees(-90, 0);
        /// <summary>
        /// A right-pointing direction with horizontal angle 90 and zero
        /// vertical angle
        /// </summary>
        public readonly static Direction right = Degrees(90, 0);

        private void Normalize()
        {
            if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90)
            {
                this.horizontal += AngleFloat.deg180;
                this.vertical = AngleFloat.Degrees(180 - this.vertical.inDegrees);
            }
        }

#if UNITY_5_3_OR_NEWER
        /// <summary>
        /// Convert the direction into a carthesian vector
        /// </summary>
        /// <returns>The carthesian vector corresponding to this direction.</returns>
        public readonly UnityEngine.Vector3 ToVector3() {
            // Convert degrees to radians
            float radH = this.horizontal.inRadians;
            float radV = this.vertical.inRadians;

            // Calculate Vector
            float cosV = MathF.Cos(radV);
            float sinV = MathF.Sin(radV);

            float x = cosV * MathF.Sin(radH);
            float y = sinV;
            float z = cosV * MathF.Cos(radH);

            return new UnityEngine.Vector3(x, y, z);
        }

        /// <summary>
        /// Convert a carthesian vector into a direction
        /// </summary>
        /// <param name="v">The carthesian vector</param>
        /// <returns>The direction</returns>
        /// <remarks>Information about the length of the carthesian vector is not
        /// included in this transformation</remarks>
        public static Direction FromVector3(UnityEngine.Vector3 v) {
            AngleFloat horizontal = AngleFloat.Atan2(v.x, v.z);
            AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.y);
            Direction d = new(horizontal, vertical);
            return d;
        }
#else
        /// <summary>
        /// Convert the direction into a carthesian vector
        /// </summary>
        /// <returns>The carthesian vector corresponding to this direction.</returns>
        public readonly Vector3Float ToVector3() {
            // Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0);
            // Vector3Float v = q * Vector3Float.forward;
            // return v;

            float radH = this.horizontal.inRadians;
            float radV = this.vertical.inRadians;

            float cosV = MathF.Cos(radV);
            float sinV = MathF.Sin(radV);

            float horizontal = cosV * MathF.Sin(radH);
            float vertical = sinV;
            float depth = cosV * MathF.Cos(radH);

            return new Vector3Float(horizontal, vertical, depth);
        }

        /// <summary>
        /// Convert a carthesian vector into a direction
        /// </summary>
        /// <param name="v">The carthesian vector</param>
        /// <returns>The direction</returns>
        /// <remarks>Information about the length of the carthesian vector is not
        /// included in this transformation</remarks>
        public static Direction FromVector3(Vector3Float v)
        {
            AngleFloat horizontal = AngleFloat.Atan2(v.horizontal, v.depth);
            AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.vertical);
            Direction d = new(horizontal, vertical);
            return d;
        }
#endif        

        public static Direction operator -(Direction d) {
            AngleFloat horizontal = d.horizontal + AngleFloat.deg180;
            AngleFloat vertical = -d.vertical;
            return new Direction(horizontal, vertical);
        }

        /// <summary>
        /// Tests the equality of two directions
        /// </summary>
        /// <param name="d1"></param>
        /// <param name="d2"></param>
        /// <returns>True when the direction angles are equal, false otherwise.</returns>
        public static bool operator ==(Direction d1, Direction d2)
        {
            bool horizontalEq = d1.horizontal == d2.horizontal;
            bool verticalEq = d1.vertical == d2.vertical;
            return horizontalEq && verticalEq;
        }

        /// <summary>
        /// Tests the inequality of two directions
        /// </summary>
        /// <param name="d1"></param>
        /// <param name="d2"></param>
        /// <returns>True when the direction angles are not equal, false otherwise.</returns>
        public static bool operator !=(Direction d1, Direction d2)
        {
            bool horizontalNEq = d1.horizontal != d2.horizontal;
            bool verticalNEq = d1.vertical != d2.vertical;
            return horizontalNEq || verticalNEq;
        }

        public override readonly bool Equals(object obj)
        {
            if (obj is not Direction d)
                return false;

            bool horizontalEq = this.horizontal == d.horizontal;
            bool verticalEq = this.vertical == d.vertical;
            return horizontalEq && verticalEq;
        }


        public override readonly int GetHashCode()
        {
            return HashCode.Combine(horizontal, vertical);
        }

        public static AngleFloat UnsignedAngle(Direction d1, Direction d2) {
            // Convert angles from degrees to radians
            float horizontal1Rad = d1.horizontal.inRadians;
            float vertical1Rad = d1.vertical.inRadians;

            float horizontal2Rad = d2.horizontal.inRadians;
            float vertical2Rad = d2.vertical.inRadians;

            // Calculate the cosine of the angle using the spherical law of cosines
            float cosTheta = MathF.Sin(vertical1Rad) * MathF.Sin(vertical2Rad) +
                             MathF.Cos(vertical1Rad) * MathF.Cos(vertical2Rad) *
                             MathF.Cos(horizontal1Rad - horizontal2Rad);

            // Clip cosTheta to the valid range for acos
            cosTheta = Float.Clamp(cosTheta, -1.0f, 1.0f);

            // Calculate the angle
            AngleFloat angle = AngleFloat.Acos(cosTheta);
            return angle;
        }
    }

}