RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Updated Angle to match C++/Python

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-26 18:25:31 +02:00
parent babffeefc0
commit 9b2a04746d
6 changed files with 336 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
src/Angle.cs
View File

@ -1,17 +1,128 @@
using System;
using System.Reflection.Emit;
namespace LinearAlgebra {
/// <summary>
/// %Angle utilities
/// </summary>
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; }
}
/// <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(Angle 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 Angle Abs(Angle a) {
if (Sign(a) < 0)
return -a;
else
return a;
}
/// <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 Angle operator -(Angle a) {
Angle 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 Angle operator -(Angle a1, Angle a2) {
Angle 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 Angle operator +(Angle a1, Angle a2) {
Angle r = new(a1.value + a2.value);
return r;
}
/// <summary>
/// Clamp the angle between the given min and max values
@ -21,11 +132,38 @@ namespace LinearAlgebra {
/// <param name="max">The maximum angle</param>
/// <returns>The clamped angle</returns>
/// 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);
}
/// <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 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;
}
}
/// <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>
@ -53,21 +191,6 @@ namespace LinearAlgebra {
return angle;
}
/// <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 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;
}
/// <summary>
/// Map interval of angles between vectors [0..Pi] to interval [0..1]
/// </summary>

6
src/Direction.cs
View File

@ -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);

4
src/Spherical.cs
View File

@ -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);

236
test/AngleTest.cs
View File

@ -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");
}
}

17
test/DirectionTest.cs Normal file
View File

@ -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);
}
};
}

27
test/SphericalTest.cs
View File

@ -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)");
}
}
}