341 lines
13 KiB
C#
341 lines
13 KiB
C#
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}\u00B0";
|
|
}
|
|
|
|
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;
|
|
// }
|
|
|
|
}
|
|
|
|
} |