diff --git a/src/Angle.cs b/src/Angle.cs index f056e5a..1490983 100644 --- a/src/Angle.cs +++ b/src/Angle.cs @@ -1,17 +1,128 @@ using System; +using System.Reflection.Emit; namespace LinearAlgebra { - /// - /// %Angle utilities - /// - public static class Angle { - 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; - + public class Angle { public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F; - public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F; + public const float Deg2Rad = (float)Math.PI * 2 / 360.0f; //57.29578F; + + private Angle(float degrees) { + this.value = degrees; + } + private readonly float value = 0; + + public static readonly Angle zero = new(0); + + public static Angle Degrees(float degrees) { + // Reduce it to (-180..180] + if (float.IsFinite(degrees)) { + while (degrees < -180) + degrees += 360; + while (degrees >= 180) + degrees -= 360; + } + return new Angle(degrees); + } + + public static Angle 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 Angle(radians * Rad2Deg); + + } + + public static Angle 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 Angle(revolutions * 360); + } + + public float inDegrees { + get { return this.value; } + } + + public float inRadians { + get { return this.value * Deg2Rad; } + } + + public float inRevolutions { + get { return this.value / 360.0f; } + } + + /// + /// Get the sign of the angle + /// + /// The angle + /// -1 when the angle is negative, 1 when it is positive and 0 in all other cases + public static int Sign(Angle a) { + if (a.value < 0) + return -1; + if (a.value > 0) + return 1; + return 0; + } + + /// + /// Returns the magnitude of the angle + /// + /// The angle + /// The positive magnitude of the angle + /// Negative values are negated to get a positive result + public static Angle Abs(Angle a) { + if (Sign(a) < 0) + return -a; + else + return a; + } + + /// + /// Negate the angle + /// + /// The angle + /// The negated angle + /// The negation of -180 is still -180 because the range is (-180..180] + public static Angle operator -(Angle a) { + Angle r = new(-a.value); + return r; + } + + /// + /// Subtract two angles + /// + /// Angle 1 + /// Angle 2 + /// The result of the subtraction + public static Angle operator -(Angle a1, Angle a2) { + Angle r = new(a1.value - a2.value); + return r; + } + /// + /// Add two angles + /// + /// Angle 1 + /// Angle 2 + /// The result of the addition + public static Angle operator +(Angle a1, Angle a2) { + Angle r = new(a1.value + a2.value); + return r; + } /// /// Clamp the angle between the given min and max values @@ -21,11 +132,38 @@ namespace LinearAlgebra { /// The maximum angle /// The clamped angle /// Angles are normalized - public static float Clamp(float angle, float min, float max) { - float normalizedAngle = Normalize(angle); - return Float.Clamp(normalizedAngle, min, max); + public static float Clamp(Angle angle, Angle min, Angle max) { + return Float.Clamp(angle.inDegrees, min.inDegrees, max.inDegrees); } + /// + /// Rotate from one angle to the other with a maximum degrees + /// + /// Starting angle + /// Target angle + /// Maximum angle to rotate + /// The resulting angle + /// This function is compatible with radian and degrees angles + public static Angle MoveTowards(Angle fromAngle, Angle toAngle, float maxDegrees) { + maxDegrees = Math.Max(0, maxDegrees); // filter out negative distances + Angle d = toAngle - fromAngle; + float dDegrees = Abs(d).inDegrees; + d = Degrees(Float.Clamp(dDegrees, 0, maxDegrees)); + if (Sign(d) < 0) + d = -d; + return fromAngle + d; + } + } + + /// + /// %Angle utilities + /// + 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; + + /// /// Determine the angle difference, result is a normalized angle /// @@ -53,21 +191,6 @@ namespace LinearAlgebra { return angle; } - /// - /// Rotate from one angle to the other with a maximum degrees - /// - /// Starting angle - /// Target angle - /// Maximum angle to rotate - /// The resulting angle - /// This function is compatible with radian and degrees angles - public static float MoveTowards(float fromAngle, float toAngle, float maxAngle) { - float d = toAngle - fromAngle; - d = Normalize(d); - d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle); - return fromAngle + d; - } - /// /// Map interval of angles between vectors [0..Pi] to interval [0..1] /// diff --git a/src/Direction.cs b/src/Direction.cs index 6039bd5..297eff9 100644 --- a/src/Direction.cs +++ b/src/Direction.cs @@ -25,6 +25,10 @@ namespace LinearAlgebra horizontal = 0; vertical = 0; } + public Direction(Angle horizontal, Angle vertical) { + this.horizontal = horizontal.inDegrees; + + } // public Direction(float horizontal, float vertical) { // this.horizontal = horizontal; // this.vertical = vertical; @@ -64,7 +68,7 @@ namespace LinearAlgebra public Vector3Float ToVector3() { - float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad; + float verticalRad = ((float)Math.PI / 2) - this.vertical * Angle.Deg2Rad; float horizontalRad = this.horizontal * Angle.Deg2Rad; float cosVertical = (float)Math.Cos(verticalRad); float sinVertical = (float)Math.Sin(verticalRad); diff --git a/src/Spherical.cs b/src/Spherical.cs index 456646f..183aa01 100644 --- a/src/Spherical.cs +++ b/src/Spherical.cs @@ -83,7 +83,7 @@ namespace LinearAlgebra { if (distance == 0.0f) return Spherical.zero; else { - float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg); + float verticalAngle = (float)(Math.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); } @@ -106,7 +106,7 @@ namespace LinearAlgebra { // } public Vector3 ToVector3() { - float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + float verticalRad = (float)(Math.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); diff --git a/test/AngleTest.cs b/test/AngleTest.cs index c248465..00e4981 100644 --- a/test/AngleTest.cs +++ b/test/AngleTest.cs @@ -1,169 +1,207 @@ #if !UNITY_5_6_OR_NEWER +using System; +using System.Formats.Asn1; using NUnit.Framework; -namespace LinearAlgebra.Test -{ - public class Tests - { +namespace LinearAlgebra.Test { + public class AngleTests { [SetUp] - public void Setup() - { + public void Setup() { } [Test] - public void Normalize() - { - float r = 0; + public void Construct() { + // Degrees + float angle = 0.0f; + Angle a = Angle.Degrees(angle); + Assert.AreEqual(angle, a.inDegrees); - r = Angle.Normalize(90); - Assert.AreEqual(r, 90, "Normalize 90"); + angle = -180.0f; + a = Angle.Degrees(angle); + Assert.AreEqual(angle, a.inDegrees); - r = Angle.Normalize(-90); - Assert.AreEqual(r, -90, "Normalize -90"); + angle = 270.0f; + a = Angle.Degrees(angle); + Assert.AreEqual(-90, a.inDegrees); - r = Angle.Normalize(270); - Assert.AreEqual(r, -90, "Normalize 270"); + // Radians + angle = 0.0f; + a = Angle.Radians(angle); + Assert.AreEqual(angle, a.inRadians); - r = Angle.Normalize(270 + 360); - Assert.AreEqual(r, -90, "Normalize 270+360"); + angle = (float)-Math.PI; + a = Angle.Radians(angle); + Assert.AreEqual(angle, a.inRadians); - r = Angle.Normalize(-270); - Assert.AreEqual(r, 90, "Normalize -270"); + angle = (float)Math.PI * 1.5f; + a = Angle.Radians(angle); + Assert.AreEqual(-Math.PI * 0.5f, a.inRadians); - r = Angle.Normalize(-270 - 360); - Assert.AreEqual(r, 90, "Normalize -270-360"); + // Revolutions + angle = 0.0f; + a = Angle.Revolutions(angle); + Assert.AreEqual(angle, a.inRevolutions); - r = Angle.Normalize(0); - Assert.AreEqual(r, 0, "Normalize 0"); + angle = -0.5f; + a = Angle.Revolutions(angle); + Assert.AreEqual(angle, a.inRevolutions); - r = Angle.Normalize(float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + angle = 0.75f; + a = Angle.Revolutions(angle); + Assert.AreEqual(-0.25f, a.inRevolutions); - r = Angle.Normalize(float.NegativeInfinity); - Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); } + // [Test] + // public void Normalize() { + // float r = 0; + + // r = Angle.Normalize(90); + // Assert.AreEqual(r, 90, "Normalize 90"); + + // r = Angle.Normalize(-90); + // Assert.AreEqual(r, -90, "Normalize -90"); + + // r = Angle.Normalize(270); + // Assert.AreEqual(r, -90, "Normalize 270"); + + // r = Angle.Normalize(270 + 360); + // Assert.AreEqual(r, -90, "Normalize 270+360"); + + // r = Angle.Normalize(-270); + // Assert.AreEqual(r, 90, "Normalize -270"); + + // r = Angle.Normalize(-270 - 360); + // Assert.AreEqual(r, 90, "Normalize -270-360"); + + // r = Angle.Normalize(0); + // Assert.AreEqual(r, 0, "Normalize 0"); + + // r = Angle.Normalize(float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + + // r = Angle.Normalize(float.NegativeInfinity); + // Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); + // } + [Test] - public void Clamp() - { + public void Clamp() { float r = 0; - r = Angle.Clamp(1, 0, 2); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 1, "Clamp 1 0 2"); - r = Angle.Clamp(-1, 0, 2); + r = Angle.Clamp(Angle.Degrees(-1), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 0, "Clamp -1 0 2"); - r = Angle.Clamp(3, 0, 2); + r = Angle.Clamp(Angle.Degrees(3), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 2, "Clamp 3 0 2"); - r = Angle.Clamp(1, 0, 0); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(0)); Assert.AreEqual(r, 0, "Clamp 1 0 0"); - r = Angle.Clamp(0, 0, 0); + r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(0), Angle.Degrees(0)); Assert.AreEqual(r, 0, "Clamp 0 0 0"); - r = Angle.Clamp(0, 1, -1); + r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(1), Angle.Degrees(-1)); Assert.AreEqual(r, 1, "Clamp 0 1 -1"); - r = Angle.Clamp(1, 0, float.PositiveInfinity); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(float.PositiveInfinity)); Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY"); - r = Angle.Clamp(1, float.NegativeInfinity, 1); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(float.NegativeInfinity), Angle.Degrees(1)); Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1"); } - [Test] - public void Difference() - { - float r = 0; + // [Test] + // public void Difference() { + // float r = 0; - r = Angle.Difference(0, 90); - Assert.AreEqual(r, 90, "Difference 0 90"); + // r = Angle.Difference(0, 90); + // Assert.AreEqual(r, 90, "Difference 0 90"); - r = Angle.Difference(0, -90); - Assert.AreEqual(r, -90, "Difference 0 -90"); + // r = Angle.Difference(0, -90); + // Assert.AreEqual(r, -90, "Difference 0 -90"); - r = Angle.Difference(0, 270); - Assert.AreEqual(r, -90, "Difference 0 270"); + // r = Angle.Difference(0, 270); + // Assert.AreEqual(r, -90, "Difference 0 270"); - r = Angle.Difference(0, -270); - Assert.AreEqual(r, 90, "Difference 0 -270"); + // r = Angle.Difference(0, -270); + // Assert.AreEqual(r, 90, "Difference 0 -270"); - r = Angle.Difference(90, 0); - Assert.AreEqual(r, -90, "Difference 90 0"); + // r = Angle.Difference(90, 0); + // Assert.AreEqual(r, -90, "Difference 90 0"); - r = Angle.Difference(-90, 0); - Assert.AreEqual(r, 90, "Difference -90 0"); + // r = Angle.Difference(-90, 0); + // Assert.AreEqual(r, 90, "Difference -90 0"); - r = Angle.Difference(0, 0); - Assert.AreEqual(r, 0, "Difference 0 0"); + // r = Angle.Difference(0, 0); + // Assert.AreEqual(r, 0, "Difference 0 0"); - r = Angle.Difference(90, 90); - Assert.AreEqual(r, 0, "Difference 90 90"); + // r = Angle.Difference(90, 90); + // Assert.AreEqual(r, 0, "Difference 90 90"); - r = Angle.Difference(0, float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); + // r = Angle.Difference(0, float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); - r = Angle.Difference(0, float.NegativeInfinity); - Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); + // r = Angle.Difference(0, float.NegativeInfinity); + // Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); - r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); - } + // r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); + // } [Test] - public void MoveTowards() - { - float r = 0; + public void MoveTowards() { + Angle r = Angle.zero; - r = Angle.MoveTowards(0, 90, 30); - Assert.AreEqual(r, 30, "MoveTowards 0 90 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 30); + Assert.AreEqual(r.inDegrees, 30, "MoveTowards 0 90 30"); - r = Angle.MoveTowards(0, 90, 90); - Assert.AreEqual(r, 90, "MoveTowards 0 90 90"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 90); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 90"); - r = Angle.MoveTowards(0, 90, 180); - Assert.AreEqual(r, 90, "MoveTowards 0 90 180"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 180); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 180"); - r = Angle.MoveTowards(0, 90, 270); - Assert.AreEqual(r, 90, "MoveTowrads 0 90 270"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 270); + Assert.AreEqual(r.inDegrees, 90, "MoveTowrads 0 90 270"); - r = Angle.MoveTowards(0, 90, -30); - Assert.AreEqual(r, -30, "MoveTowards 0 90 -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 90 -30"); - r = Angle.MoveTowards(0, -90, -30); - Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -30"); - r = Angle.MoveTowards(0, -90, -90); - Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -90); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -90"); - r = Angle.MoveTowards(0, -90, -180); - Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -180); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -180"); - r = Angle.MoveTowards(0, -90, -270); - Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -270); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 -90 -270"); - r = Angle.MoveTowards(0, 90, 0); - Assert.AreEqual(r, 0, "MoveTowards 0 90 0"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 0); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 90 0"); - r = Angle.MoveTowards(0, 0, 0); - Assert.AreEqual(r, 0, "MoveTowards 0 0 0"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 0); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 0 0"); - r = Angle.MoveTowards(0, 0, 30); - Assert.AreEqual(r, 0, "MoveTowrads 0 0 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 0 30"); - r = Angle.MoveTowards(0, 90, float.PositiveInfinity); - Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), float.PositiveInfinity); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 INFINITY"); - r = Angle.MoveTowards(0, float.PositiveInfinity, 30); - Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(float.PositiveInfinity), 30); + Assert.AreEqual(r.inDegrees, 30, "MoveTowrads 0 INFINITY 30"); - r = Angle.MoveTowards(0, -90, float.NegativeInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), float.NegativeInfinity); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -INFINITY"); - r = Angle.MoveTowards(0, float.NegativeInfinity, -30); - Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(float.NegativeInfinity), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 -INFINITY -30"); } } diff --git a/test/DirectionTest.cs b/test/DirectionTest.cs new file mode 100644 index 0000000..e31af4c --- /dev/null +++ b/test/DirectionTest.cs @@ -0,0 +1,17 @@ +using NUnit.Framework; + +namespace LinearAlgebra.Test { + public class DirectionTest { + [SetUp] + public void Setup() { + } + + [Test] + public void Compare() { + Direction d = Direction.Degrees(45, 135); + bool r; + r = d == new Direction(Angle.Degrees(45), Angle.Degrees(135)); + Assert.True(r); + } + }; +} \ No newline at end of file diff --git a/test/SphericalTest.cs b/test/SphericalTest.cs index 3ede4f4..8539dc0 100644 --- a/test/SphericalTest.cs +++ b/test/SphericalTest.cs @@ -1,4 +1,5 @@ #if !UNITY_5_6_OR_NEWER +using System; using NUnit.Framework; namespace LinearAlgebra.Test { @@ -13,7 +14,19 @@ namespace LinearAlgebra.Test { Spherical s = Spherical.FromVector3(v); Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1"); Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 0 1"); - Assert.AreEqual(0.0f, s.direction.vertical, "s.vert 0 0 1"); + Assert.AreEqual(0.0f, s.direction.vertical, 1.0E-05F, "s.vert 0 0 1"); + + v = new(0, 1, 0); + s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 0 1 0"); + Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 1 0"); + Assert.AreEqual(90.0f, s.direction.vertical, "s.vert 0 1 0"); + + v = new(1, 0, 0); + s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 1 0 0"); + Assert.AreEqual(90.0f, s.direction.horizontal, "s.hor 1 0 0"); + Assert.AreEqual(0.0f, s.direction.vertical, 1.0E-05F, "s.vert 1 0 0"); } [Test] @@ -23,7 +36,17 @@ namespace LinearAlgebra.Test { Spherical r = Spherical.zero; r = v1 + v2; - Assert.AreEqual(v1.distance, r.distance, "Addition(0,0,0)"); + Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)"); + + r = v1; + r += v2; + Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)"); + + v2 = Spherical.Degrees(1, 0, 90); + r = v1 + v2; + Assert.AreEqual(Math.Sqrt(2), r.distance, 1.0E-05F, "Addition(1 0 90)"); + Assert.AreEqual(45.0f, r.direction.horizontal, "Addition(1 0 90)"); + Assert.AreEqual(45.0f, r.direction.vertical, "Addition(1 0 90)"); } } }