NanoBrain-unitypackage/src/Angle.cs

using System;

namespace LinearAlgebra {

    public struct AngleFloat {
        public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F;
        public const float Deg2Rad = (float)Math.PI * 2 / 360.0f; //57.29578F;

        private AngleFloat(float degrees) {
            this.value = degrees;
        }
        private readonly float value;

        public static AngleFloat Degrees(float degrees) {
            // Reduce it to (-180..180]
            if (float.IsFinite(degrees)) {
                while (degrees < -180)
                    degrees += 360;
                while (degrees >= 180)
                    degrees -= 360;
            }
            return new AngleFloat(degrees);
        }

        public static AngleFloat Radians(float radians) {
            // Reduce it to (-pi..pi]
            if (float.IsFinite(radians)) {
                while (radians <= -Math.PI)
                    radians += 2 * (float)Math.PI;
                while (radians > Math.PI)
                    radians -= 2 * (float)Math.PI;
            }

            return new AngleFloat(radians * Rad2Deg);
        }

        public static AngleFloat Revolutions(float revolutions) {
            // reduce it to (-0.5 .. 0.5]
            if (float.IsFinite(revolutions)) {
                // Get the integer part
                int integerPart = (int)revolutions;

                // Get the decimal part
                revolutions -= integerPart;
                if (revolutions < -0.5)
                    revolutions += 1;
                if (revolutions >= 0.5)
                    revolutions -= 1;
            }
            return new AngleFloat(revolutions * 360);
        }

        public readonly float inDegrees => this.value;

        public readonly float inRadians => this.value * Deg2Rad;
        
        public readonly float inRevolutions => this.value / 360.0f;

        public override string ToString() {
               return $"{this.inDegrees} deg.";
        }

        public static readonly AngleFloat zero = Degrees(0);
        public static readonly AngleFloat deg90 = Degrees(90);
        public static readonly AngleFloat deg180 = Degrees(180);

        /// <summary>
        /// Get the sign of the angle
        /// </summary>
        /// <param name="a">The angle</param>
        /// <returns>-1 when the angle is negative, 1 when it is positive and 0 in all other cases</returns>
        public static int Sign(AngleFloat a) {
            if (a.value < 0)
                return -1;
            if (a.value > 0)
                return 1;
            return 0;
        }

        /// <summary>
        /// Returns the magnitude of the angle
        /// </summary>
        /// <param name="a">The angle</param>
        /// <returns>The positive magnitude of the angle</returns>
        /// Negative values are negated to get a positive result
        public static AngleFloat Abs(AngleFloat a) {
            if (Sign(a) < 0)
                return -a;
            else
                return a;
        }

        /// <summary>
        /// Tests the equality of two angles
        /// </summary>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <returns>True when the angles are equal, false otherwise</returns>
        /// <remarks>The equality is determine within the limits of precision of a float</remarks>
        public static bool operator ==(AngleFloat a1, AngleFloat a2) {
            return a1.value == a2.value;
        }

        /// <summary>
        /// Tests the inequality of two angles
        /// </summary>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <returns>True when the angles are not equal, false otherwise</returns>
        /// <remarks>The equality is determine within the limits of precision of a float</remarks>
        public static bool operator !=(AngleFloat a1, AngleFloat a2) {
            return a1.value != a2.value;
        }

        public override readonly bool Equals(object obj) {
            if (obj is AngleFloat other) {
                return this == other;
            }
            return false;
        }

        public override readonly int GetHashCode() {
            return this.value.GetHashCode();
        }


        /// <summary>
        /// Tests if the first angle is greater than the second
        /// </summary>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <returns>True when a1 is greater than a2, False otherwise</returns>
        public static bool operator >(AngleFloat a1, AngleFloat a2) {
            return a1.value > a2.value;
        }

        /// <summary>
        /// Tests if the first angle is greater than or equal to the second
        /// </summary>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <returns>True when a1 is greater than or equal to a2, False otherwise</returns>
        public static bool operator >=(AngleFloat a1, AngleFloat a2) {
            return a1.value >= a2.value;
        }

        /// <summary>
        /// Tests if the first angle is less than the second
        /// </summary>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <returns>True when a1 is less than a2, False otherwise</returns>
        public static bool operator <(AngleFloat a1, AngleFloat a2) {
            return a1.value < a2.value;
        }

        /// <summary>
        /// Tests if the first angle is less than or equal to the second
        /// </summary>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <returns>True when a1 is less than or equal to a2, False otherwise</returns>
        public static bool operator <=(AngleFloat a1, AngleFloat a2) {
            return a1.value <= a2.value;
        }

        /// <summary>
        /// Negate the angle
        /// </summary>
        /// <param name="a">The angle</param>
        /// <returns>The negated angle</returns>
        /// The negation of -180 is still -180 because the range is (-180..180]
        public static AngleFloat operator -(AngleFloat a) {
            AngleFloat r = new(-a.value);
            return r;
        }

        /// <summary>
        /// Subtract two angles
        /// </summary>
        /// <param name="a1">Angle 1</param>
        /// <param name="a2">Angle 2</param>
        /// <returns>The result of the subtraction</returns>
        public static AngleFloat operator -(AngleFloat a1, AngleFloat a2) {
            AngleFloat r = new(a1.value - a2.value);
            return r;
        }
        /// <summary>
        /// Add two angles
        /// </summary>
        /// <param name="a1">Angle 1</param>
        /// <param name="a2">Angle 2</param>
        /// <returns>The result of the addition</returns>
        public static AngleFloat operator +(AngleFloat a1, AngleFloat a2) {
            AngleFloat r = new(a1.value + a2.value);
            return r;
        }

        /// <summary>
        /// Multiplies the angle
        /// </summary>
        /// <param name="a">The angle to multiply</param>
        /// <param name="factor">The factor by which the angle is multiplied</param>
        /// <returns>The multiplied angle</returns>
        public static AngleFloat operator *(AngleFloat a, float factor) {
            return Degrees(a.inDegrees * factor);
        }
        public static AngleFloat operator *(float factor, AngleFloat a) {
            return Degrees(factor * a.inDegrees);
        }

        /// <summary>
        /// Clamp the angle between the given min and max values
        /// </summary>
        /// <param name="angle">The angle to clamp</param>
        /// <param name="min">The minimum angle</param>
        /// <param name="max">The maximum angle</param>
        /// <returns>The clamped angle</returns>
        /// Angles are normalized
        public static float Clamp(AngleFloat angle, AngleFloat min, AngleFloat max) {
            return Float.Clamp(angle.inDegrees, min.inDegrees, max.inDegrees);
        }

        /// @brief Calculates the cosine of an angle
        /// @param angle The given angle
        /// @return The cosine of the angle
        public static float Cos(AngleFloat angle) {
            return MathF.Cos(angle.inRadians);
        }
        /// @brief Calculates the sine of an angle
        /// @param angle The given angle
        /// @return The sine of the angle
        public static float Sin(AngleFloat angle) {
            return MathF.Sin(angle.inRadians);
        }
        /// @brief Calculates the tangent of an angle
        /// @param angle The given angle
        /// @return The tangent of the angle
        public static float Tan(AngleFloat angle) {
            return MathF.Tan(angle.inRadians);
        }

        /// @brief Calculates the arc cosine angle
        /// @param f The value
        /// @return The arc cosine for the given value
        public static AngleFloat Acos(float f) {
            return Radians(MathF.Acos(f));
        }
        /// @brief Calculates the arc sine angle
        /// @param f The value
        /// @return The arc sine for the given value
        public static AngleFloat Asin(float f) {
            return Radians(MathF.Asin(f));
        }
        /// @brief Calculates the arc tangent angle
        /// @param f The value
        /// @return The arc tangent for the given value
        public static AngleFloat Atan(float f) {
            return Radians(MathF.Atan(f));
        }
        /// @brief Calculates the tangent for the given values
        /// @param y The vertical value
        /// @param x The horizontal value
        /// @return The tanget for the given values
        /// Uses the y and x signs to compute the quadrant
        public static AngleFloat Atan2(float y, float x) {
            return Radians(MathF.Atan2(y, x));
        }

        /// <summary>
        /// Rotate from one angle to the other with a maximum degrees
        /// </summary>
        /// <param name="fromAngle">Starting angle</param>
        /// <param name="toAngle">Target angle</param>
        /// <param name="maxAngle">Maximum angle to rotate</param>
        /// <returns>The resulting angle</returns>
        /// This function is compatible with radian and degrees angles
        public static AngleFloat MoveTowards(AngleFloat fromAngle, AngleFloat toAngle, float maxDegrees) {
            maxDegrees = Math.Max(0, maxDegrees);  // filter out negative distances
            AngleFloat d = toAngle - fromAngle;
            float dDegrees = Abs(d).inDegrees;
            d = Degrees(Float.Clamp(dDegrees, 0, maxDegrees));
            if (Sign(d) < 0)
                d = -d;
            return fromAngle + d;
        }
    }


    /// <summary>
    /// %Angle utilities
    /// </summary>
    public static class Angles {
        public const float pi = 3.1415927410125732421875F;
        // public static float Rad2Deg = 360.0f / ((float)Math.PI * 2);
        // public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f;


        /// <summary>
        /// Determine the angle difference, result is a normalized angle
        /// </summary>
        /// <param name="a">First first angle</param>
        /// <param name="b">The second angle</param>
        /// <returns>the angle between the two angles</returns>
        /// Angle values should be degrees
        public static float Difference(float a, float b) {
            float r = Normalize(b - a);
            return r;
        }

        /// <summary>
        /// Normalize an angle to the range -180 < angle <= 180
        /// </summary>
        /// <param name="angle">The angle to normalize</param>
        /// <returns>The normalized angle in interval (-180..180] </returns>
        /// Angle values should be in degrees
        public static float Normalize(float angle) {
            if (float.IsInfinity(angle))
                return angle;

            while (angle <= -180) angle += 360;
            while (angle > 180) angle -= 360;
            return angle;
        }

        /// <summary>
        /// Map interval of angles between vectors [0..Pi] to interval [0..1]
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns>The resulting factor in interval [0..1]</returns>
        /// Vectors a and b must be normalized
        /// \deprecated Please use Vector2.ToFactor instead.
        // [Obsolete("Please use Vector2.ToFactor instead.")]
        // public static float ToFactor(Vector2Float v1, Vector2Float v2) {
        //     return (1 - Vector2Float.Dot(v1, v2)) / 2;
        // }

    }

}