From c7bce31f34da99b68c709cef496eacfe099fc49f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 7 Mar 2025 14:48:03 +0100 Subject: [PATCH] Restructuring --- LinearAlgebra/src/Angle.cs | 110 ++++++ .../src}/Direction.cs | 0 LinearAlgebra/src/Float.cs | 41 ++ LinearAlgebra/src/LinearAlgebra.csproj | 14 + .../src}/Matrix.cs | 0 .../src}/Quat32.cs | 0 .../src}/Quaternion.cs | 0 .../src}/Spherical.cs | 0 .../src}/SwingTwist.cs | 0 LinearAlgebra/src/Vector2.cs | 363 ++++++++++++++++++ LinearAlgebra/src/Vector3.cs | 179 +++++++++ .../src}/float16.cs | 0 LinearAlgebra/test/AngleTest.cs | 162 ++++++++ LinearAlgebra/test/LinearAlgebra_Test.csproj | 19 + RoboidControl.sln | 24 +- src/LinearAlgebra/Angle.cs | 11 - src/LinearAlgebra/Vector2.cs | 51 --- src/LinearAlgebra/Vector3.cs | 33 -- src/RoboidControl.csproj | 2 +- ...{test.csproj => RoboidControl_Test.csproj} | 0 20 files changed, 909 insertions(+), 100 deletions(-) create mode 100644 LinearAlgebra/src/Angle.cs rename {src/LinearAlgebra => LinearAlgebra/src}/Direction.cs (100%) create mode 100644 LinearAlgebra/src/Float.cs create mode 100644 LinearAlgebra/src/LinearAlgebra.csproj rename {src/LinearAlgebra => LinearAlgebra/src}/Matrix.cs (100%) rename {src/LinearAlgebra => LinearAlgebra/src}/Quat32.cs (100%) rename {src/LinearAlgebra => LinearAlgebra/src}/Quaternion.cs (100%) rename {src/LinearAlgebra => LinearAlgebra/src}/Spherical.cs (100%) rename {src/LinearAlgebra => LinearAlgebra/src}/SwingTwist.cs (100%) create mode 100644 LinearAlgebra/src/Vector2.cs create mode 100644 LinearAlgebra/src/Vector3.cs rename {src/LinearAlgebra => LinearAlgebra/src}/float16.cs (100%) create mode 100644 LinearAlgebra/test/AngleTest.cs create mode 100644 LinearAlgebra/test/LinearAlgebra_Test.csproj delete mode 100644 src/LinearAlgebra/Angle.cs delete mode 100644 src/LinearAlgebra/Vector2.cs delete mode 100644 src/LinearAlgebra/Vector3.cs rename test/{test.csproj => RoboidControl_Test.csproj} (100%) diff --git a/LinearAlgebra/src/Angle.cs b/LinearAlgebra/src/Angle.cs new file mode 100644 index 0000000..78ae979 --- /dev/null +++ b/LinearAlgebra/src/Angle.cs @@ -0,0 +1,110 @@ +using System; + +namespace Passer.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 const float Deg2Rad = 360.0f / ((float)Math.PI * 2); //0.0174532924F; + public const float Rad2Deg = ((float)Math.PI * 2) / 360.0f; //57.29578F; + + /// + /// Clamp the angle between the given min and max values + /// + /// The angle to clamp + /// The minimum angle + /// 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); + } + + /// + /// Determine the angle difference, result is a normalized angle + /// + /// First first angle + /// The second angle + /// the angle between the two angles + /// Angle values should be degrees + public static float Difference(float a, float b) { + float r = Normalize(b - a); + return r; + } + + /// + /// Normalize an angle to the range -180 < angle <= 180 + /// + /// The angle to normalize + /// The normalized angle in interval (-180..180] + /// 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; + } + + /// + /// 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] + /// + /// The first vector + /// The second vector + /// The resulting factor in interval [0..1] + /// Vectors a and b must be normalized + /// \deprecated Please use Vector2.ToFactor instead. + [Obsolete("Please use Vector2.ToFactor instead.")] + public static float ToFactor(Vector2 v1, Vector2 v2) { + return (1 - Vector2.Dot(v1, v2)) / 2; + } + + // Normalize all vector angles to the range -180 < angle < 180 + //public static Vector3 Normalize(Vector3 angles) { + // float x = Normalize(angles.x); + // float y = Normalize(angles.y); + // float z = Normalize(angles.z); + // return new Vector3(x, y, z); + //} + + // Returns the signed angle in degrees between from and to. + //public static float SignedAngle(Vector3 from, Vector3 to) { + // float angle = Vector3.Angle(from, to); + // Vector3 cross = Vector3.Cross(from, to); + // if (cross.y < 0) angle = -angle; + // return angle; + //} + + // Returns the signed angle in degrees between from and to. + //public static float SignedAngle(Vector2 from, Vector2 to) { + // float sign = Math.Sign(from.y * to.x - from.x * to.y); + // return Vector2.Angle(from, to) * sign; + //} + + //public static Quaternion ToQuaternion(Rotation orientation) { + // return new Quaternion(orientation.x, orientation.y, orientation.z, orientation.w); + //} + } +} \ No newline at end of file diff --git a/src/LinearAlgebra/Direction.cs b/LinearAlgebra/src/Direction.cs similarity index 100% rename from src/LinearAlgebra/Direction.cs rename to LinearAlgebra/src/Direction.cs diff --git a/LinearAlgebra/src/Float.cs b/LinearAlgebra/src/Float.cs new file mode 100644 index 0000000..4959b75 --- /dev/null +++ b/LinearAlgebra/src/Float.cs @@ -0,0 +1,41 @@ +namespace Passer.LinearAlgebra { + + /// + /// Float number utilities + /// + public class Float { + /// + /// The precision of float numbers + /// + public const float epsilon = 1E-05f; + /// + /// The square of the float number precision + /// + public const float sqrEpsilon = 1e-10f; + + /// + /// Clamp the value between the given minimum and maximum values + /// + /// The value to clamp + /// The minimum value + /// The maximum value + /// The clamped value + public static float Clamp(float f, float min, float max) { + if (f < min) + return min; + if (f > max) + return max; + return f; + } + + /// + /// Clamp the value between to the interval [0..1] + /// + /// The value to clamp + /// The clamped value + public static float Clamp01(float f) { + return Clamp(f, 0, 1); + } + } + +} \ No newline at end of file diff --git a/LinearAlgebra/src/LinearAlgebra.csproj b/LinearAlgebra/src/LinearAlgebra.csproj new file mode 100644 index 0000000..14d3947 --- /dev/null +++ b/LinearAlgebra/src/LinearAlgebra.csproj @@ -0,0 +1,14 @@ + + + + false + false + net5.0 + + + + + + + + diff --git a/src/LinearAlgebra/Matrix.cs b/LinearAlgebra/src/Matrix.cs similarity index 100% rename from src/LinearAlgebra/Matrix.cs rename to LinearAlgebra/src/Matrix.cs diff --git a/src/LinearAlgebra/Quat32.cs b/LinearAlgebra/src/Quat32.cs similarity index 100% rename from src/LinearAlgebra/Quat32.cs rename to LinearAlgebra/src/Quat32.cs diff --git a/src/LinearAlgebra/Quaternion.cs b/LinearAlgebra/src/Quaternion.cs similarity index 100% rename from src/LinearAlgebra/Quaternion.cs rename to LinearAlgebra/src/Quaternion.cs diff --git a/src/LinearAlgebra/Spherical.cs b/LinearAlgebra/src/Spherical.cs similarity index 100% rename from src/LinearAlgebra/Spherical.cs rename to LinearAlgebra/src/Spherical.cs diff --git a/src/LinearAlgebra/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs similarity index 100% rename from src/LinearAlgebra/SwingTwist.cs rename to LinearAlgebra/src/SwingTwist.cs diff --git a/LinearAlgebra/src/Vector2.cs b/LinearAlgebra/src/Vector2.cs new file mode 100644 index 0000000..c7d00a4 --- /dev/null +++ b/LinearAlgebra/src/Vector2.cs @@ -0,0 +1,363 @@ +using System; +using System.Numerics; + +namespace Passer.LinearAlgebra { + + public class Vector2Of where T : IComparable { + public T x; + public T y; + + public Vector2Of(T x, T y) { + this.x = x; + this.y = y; + } + } + + public class Vector2Int : Vector2Of { + public Vector2Int(int x, int y) : base(x, y) { } + + public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) { + return new Vector2Int(v1.x - v2.x, v1.y - v2.y); + } + + public float magnitude { + get { + return (float)Math.Sqrt(this.x * this.x + this.y * this.y); + } + } + + public static float Distance(Vector2Int v1, Vector2Int v2) { + return (v1 - v2).magnitude; + } + } + + public class Vector2Float : Vector2Of { + public Vector2Float(float x, float y) : base(x, y) { } + + public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) { + return new Vector2Float(v1.x - v2.x, v1.y - v2.y); + } + + public float magnitude { + get { + return (float)Math.Sqrt(this.x * this.x + this.y * this.y); + } + } + + public static float Distance(Vector2Float v1, Vector2Float v2) { + return (v1 - v2).magnitude; + } + } + + /// + /// 2-dimensional vectors + /// + public struct Vector2 : IEquatable { + + /// + /// The right axis of the vector + /// + public float x; // left/right + /// + /// The upward/forward axis of the vector + /// + public float y; // forward/backward + // directions are to be inline with Vector3 as much as possible... + + /// + /// Create a new 2-dimensional vector + /// + /// x axis value + /// y axis value + public Vector2(float x, float y) { + this.x = x; + this.y = y; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector2 zero = new Vector2(0, 0); + /// + /// A vector with values (1, 1) + /// + public static readonly Vector2 one = new Vector2(1, 1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2 up = new Vector2(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2 down = new Vector2(0, -1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2 forward = new Vector2(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2 back = new Vector2(0, -1); + /// + /// A vector3 with values (-1, 0) + /// + public static readonly Vector2 left = new Vector2(-1, 0); + /// + /// A vector with values (1, 0) + /// + public static readonly Vector2 right = new Vector2(1, 0); + + /// + /// The squared length of this vector + /// + /// The squared length + /// The squared length is computationally simpler than the real length. + /// Think of Pythagoras A^2 + B^2 = C^2. + /// This leaves out the calculation of the squared root of C. + public float sqrMagnitude { + get { + float d = x * x + y * y; + return d; + } + } + + /// + /// The length of this vector + /// + /// The length of this vector + public float magnitude { + get { + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + } + + /// + /// Convert the vector to a length of a 1 + /// + /// The vector with length 1 + public Vector2 normalized { + get { + float l = magnitude; + Vector2 v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + /// + /// Add two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2 operator +(Vector2 v1, Vector2 v2) { + Vector2 v = new Vector2(v1.x + v2.x, v1.y + v2.y); + return v; + } + + /// + /// Subtract two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2 operator -(Vector2 v1, Vector2 v2) { + Vector2 v = new Vector2(v1.x - v2.x, v1.y - v2.y); + return v; + } + + /// + /// Negate the vector + /// + /// The vector to negate + /// The negated vector + /// This will result in a vector pointing in the opposite direction + public static Vector2 operator -(Vector2 v1) { + Vector2 v = new Vector2(-v1.x, -v1.y); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The vector to scale + /// The scaling factor + /// The scaled vector + /// Each component of the vector will be multipled with the same factor. + public static Vector2 operator *(Vector2 v1, float f) { + Vector2 v = new Vector2(v1.x * f, v1.y * f); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The scaling factor + /// The vector to scale + /// The scaled vector + /// Each component of the vector will be multipled with the same factor. + public static Vector2 operator *(float f, Vector2 v1) { + Vector2 v = new Vector2(f * v1.x, f * v1.y); + return v; + } + + /// + /// Scale a vector uniformly down + /// + /// The vector to scale + /// The scaling factor + /// The scaled vector + /// Each component of the vector will be devided by the same factor. + public static Vector2 operator /(Vector2 v1, float f) { + Vector2 v = new Vector2(v1.x / f, v1.y / f); + return v; + } + + /// + /// Tests if the vector has equal values as the given vector + /// + /// The vector to compare to + /// true if the vector values are equal + public bool Equals(Vector2 v1) => x == v1.x && y == v1.y; + + /// + /// Tests if the vector is equal to the given object + /// + /// The object to compare to + /// false when the object is not a Vector2 or does not have equal values + public override bool Equals(object obj) { + if (!(obj is Vector2 v)) + return false; + + return (x == v.x && y == v.y); + } + + /// + /// Tests if the two vectors have equal values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have equal values + /// Note that this uses a Float equality check which cannot be not exact in all cases. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator ==(Vector2 v1, Vector2 v2) { + return (v1.x == v2.x && v1.y == v2.y); + } + + /// + /// Tests if two vectors have different values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have different values + /// Note that this uses a Float equality check which cannot be not exact in all case. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon. + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator !=(Vector2 v1, Vector2 v2) { + return (v1.x != v2.x || v1.y != v2.y); + } + + /// + /// Get an hash code for the vector + /// + /// The hash code + public override int GetHashCode() { + return (x, y).GetHashCode(); + } + + /// + /// Get the distance between two vectors + /// + /// The first vector + /// The second vector + /// The distance between the two vectors + public static float Distance(Vector2 v1, Vector2 v2) { + float x = v1.x - v2.x; + float y = v1.y - v2.y; + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + + /// + /// The dot product of two vectors + /// + /// The first vector + /// The second vector + /// The dot product of the two vectors + public static float Dot(Vector2 v1, Vector2 v2) { + return v1.x * v2.x + v1.y * v2.y; + } + + /// + /// Lerp between two vectors + /// + /// The from vector + /// The to vector + /// The interpolation distance [0..1] + /// The lerped vector + /// The factor f is unclamped. Value 0 matches the *v1* vector, Value 1 + /// matches the *v2* vector Value -1 is *v1* vector minus the difference + /// between *v1* and *v2* etc. + public static Vector2 Lerp(Vector2 v1, Vector2 v2, float f) { + Vector2 v = v1 + (v2 - v1) * f; + return v; + } + + /// + /// Calculate the signed angle between two vectors. + /// + /// The starting vector + /// The ending vector + /// The axis to rotate around + /// The signed angle in degrees + public static float SignedAngle(Vector2 from, Vector2 to) { + //float sign = Math.Sign(v1.y * v2.x - v1.x * v2.y); + //return Vector2.Angle(v1, v2) * sign; + + float sqrMagFrom = from.sqrMagnitude; + float sqrMagTo = to.sqrMagnitude; + + if (sqrMagFrom == 0 || sqrMagTo == 0) + return 0; + //if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo)) + // return nanf(""); + + float angleFrom = (float)Math.Atan2(from.y, from.x); + float angleTo = (float)Math.Atan2(to.y, to.x); + return (angleTo - angleFrom) * Angle.Rad2Deg; + } + + /// + /// Rotates the vector with the given angle + /// + /// The vector to rotate + /// The angle in degrees + /// + public static Vector2 Rotate(Vector2 v1, float angle) { + float sin = (float)Math.Sin(angle * Angle.Deg2Rad); + float cos = (float)Math.Cos(angle * Angle.Deg2Rad); + + float tx = v1.x; + float ty = v1.y; + Vector2 v = new Vector2() { + x = (cos * tx) - (sin * ty), + y = (sin * tx) + (cos * ty) + }; + return v; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// The second vector + /// The resulting factor in interval [0..1] + /// Vectors a and b must be normalized + public static float ToFactor(Vector2 v1, Vector2 v2) { + return (1 - Vector2.Dot(v1, v2)) / 2; + } + } +} \ No newline at end of file diff --git a/LinearAlgebra/src/Vector3.cs b/LinearAlgebra/src/Vector3.cs new file mode 100644 index 0000000..3b17b06 --- /dev/null +++ b/LinearAlgebra/src/Vector3.cs @@ -0,0 +1,179 @@ +#if !UNITY_5_3_OR_NEWER +using System; + +namespace Passer.LinearAlgebra { + public class Vector3Of { + public T x; + public T y; + public T z; + + public Vector3Of(T x, T y, T z) { + this.x = x; + this.y = y; + this.z = z; + } + + // public uint magnitude { + // get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + // } + } + + public class Vector3Int : Vector3Of { + public Vector3Int(int x, int y, int z) : base(x, y, z) { } + } + public class Vector3Float : Vector3Of { + public Vector3Float(float x, float y, float z) : base(x, y, z) { } + + public float magnitude { + get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + } + } + + /// + /// 3-dimensional vectors + /// + /// This uses the right-handed coordinate system. + public struct Vector3 : IEquatable { + + /// + /// The right axis of the vector + /// + public float x; //> left/right + /// + /// The upward axis of the vector + /// + public float y; //> up/down + /// + /// The forward axis of the vector + /// + public float z; //> forward/backward + + /// + /// Create a new 3-dimensional vector + /// + /// x axis value + /// y axis value + /// z axis value + public Vector3(float x, float y, float z) { + this.x = x; + this.y = y; + this.z = z; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector3 zero = new Vector3(0, 0, 0); + /// + /// A vector with one for all axis + /// + public static readonly Vector3 one = new Vector3(1, 1, 1); + /// + /// A vector3 with values (-1, 0, 0) + /// + public static readonly Vector3 left = new Vector3(-1, 0, 0); + /// + /// A vector with values (1, 0, 0) + /// + public static readonly Vector3 right = new Vector3(1, 0, 0); + /// + /// A vector with values (0, -1, 0) + /// + public static readonly Vector3 down = new Vector3(0, -1, 0); + /// + /// A vector with values (0, 1, 0) + /// + public static readonly Vector3 up = new Vector3(0, 1, 0); + /// + /// A vector with values (0, 0, -1) + /// + public static readonly Vector3 back = new Vector3(0, -1, 0); + /// + /// A vector with values (0, 0, 1) + /// + public static readonly Vector3 forward = new Vector3(0, 1, 0); + + public float magnitude { + get { + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + } + + public Vector3 normalized { + get { + float l = magnitude; + Vector3 v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + public static Vector3 operator +(Vector3 v1, Vector3 v2) { + Vector3 v = new Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z); + return v; + } + + public static Vector3 operator -(Vector3 v1, Vector3 v2) { + Vector3 v = new Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); + return v; + } + + public static Vector3 operator -(Vector3 v1) { + Vector3 v = new Vector3(-v1.x, -v1.y, -v1.z); + return v; + } + + public static Vector3 operator *(Vector3 v1, float d) { + Vector3 v = new Vector3(v1.x * d, v1.y * d, v1.z * d); + return v; + } + + public static Vector3 operator *(float d, Vector3 v1) { + Vector3 v = new Vector3(d * v1.x, d * v1.y, d * v1.z); + return v; + } + + public static Vector3 operator /(Vector3 v1, float d) { + Vector3 v = new Vector3(v1.x / d, v1.y / d, v1.z / d); + return v; + } + + public bool Equals(Vector3 v) => (x == v.x && y == v.y && z == v.z); + + public override bool Equals(object obj) { + if (!(obj is Vector3 v)) + return false; + + return (x == v.x && y == v.y && z == v.z); + } + + public static bool operator ==(Vector3 v1, Vector3 v2) { + return (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z); + } + + public static bool operator !=(Vector3 v1, Vector3 v2) { + return (v1.x != v2.x || v1.y != v2.y || v1.z != v2.z); + } + + public override int GetHashCode() { + return (x, y, z).GetHashCode(); + } + + public static float Distance(Vector3 v1, Vector3 v2) { + return (v2 - v1).magnitude; + } + + public static float Dot(Vector3 v1, Vector3 v2) { + return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; + } + + public static Vector3 Lerp(Vector3 v1, Vector3 v2, float f) { + Vector3 v = v1 + (v2 - v1) * f; + return v; + } + + } +} +#endif \ No newline at end of file diff --git a/src/LinearAlgebra/float16.cs b/LinearAlgebra/src/float16.cs similarity index 100% rename from src/LinearAlgebra/float16.cs rename to LinearAlgebra/src/float16.cs diff --git a/LinearAlgebra/test/AngleTest.cs b/LinearAlgebra/test/AngleTest.cs new file mode 100644 index 0000000..85f0cc0 --- /dev/null +++ b/LinearAlgebra/test/AngleTest.cs @@ -0,0 +1,162 @@ +#if NUNIT +using NUnit.Framework; +using Passer.LinearAlgebra; + +namespace LinearAlgebraTest { + public class AngleTest { + + [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() { + float r = 0; + + r = Angle.Clamp(1, 0, 2); + Assert.AreEqual(r, 1, "Clamp 1 0 2"); + + r = Angle.Clamp(-1, 0, 2); + Assert.AreEqual(r, 0, "Clamp -1 0 2"); + + r = Angle.Clamp(3, 0, 2); + Assert.AreEqual(r, 2, "Clamp 3 0 2"); + + r = Angle.Clamp(1, 0, 0); + Assert.AreEqual(r, 0, "Clamp 1 0 0"); + + r = Angle.Clamp(0, 0, 0); + Assert.AreEqual(r, 0, "Clamp 0 0 0"); + + r = Angle.Clamp(0, 1, -1); + Assert.AreEqual(r, 1, "Clamp 0 1 -1"); + + r = Angle.Clamp(1, 0, float.PositiveInfinity); + Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY"); + + r = Angle.Clamp(1, float.NegativeInfinity, 1); + Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1"); + } + + [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, 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(0, 0); + Assert.AreEqual(r, 0, "Difference 0 0"); + + 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.NegativeInfinity); + Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); + + r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); + } + + [Test] + public void MoveTowards() { + float r = 0; + + r = Angle.MoveTowards(0, 90, 30); + Assert.AreEqual(r, 30, "MoveTowards 0 90 30"); + + r = Angle.MoveTowards(0, 90, 90); + Assert.AreEqual(r, 90, "MoveTowards 0 90 90"); + + r = Angle.MoveTowards(0, 90, 180); + Assert.AreEqual(r, 90, "MoveTowards 0 90 180"); + + r = Angle.MoveTowards(0, 90, 270); + Assert.AreEqual(r, 90, "MoveTowrads 0 90 270"); + + r = Angle.MoveTowards(0, 90, -30); + Assert.AreEqual(r, -30, "MoveTowards 0 90 -30"); + + r = Angle.MoveTowards(0, -90, -30); + Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30"); + + r = Angle.MoveTowards(0, -90, -90); + Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90"); + + r = Angle.MoveTowards(0, -90, -180); + Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180"); + + r = Angle.MoveTowards(0, -90, -270); + Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270"); + + r = Angle.MoveTowards(0, 90, 0); + Assert.AreEqual(r, 0, "MoveTowards 0 90 0"); + + r = Angle.MoveTowards(0, 0, 0); + Assert.AreEqual(r, 0, "MoveTowards 0 0 0"); + + r = Angle.MoveTowards(0, 0, 30); + Assert.AreEqual(r, 0, "MoveTowrads 0 0 30"); + + r = Angle.MoveTowards(0, 90, float.PositiveInfinity); + Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY"); + + r = Angle.MoveTowards(0, float.PositiveInfinity, 30); + Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30"); + + r = Angle.MoveTowards(0, -90, float.NegativeInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY"); + + r = Angle.MoveTowards(0, float.NegativeInfinity, -30); + Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30"); + + } + } +} +#endif \ No newline at end of file diff --git a/LinearAlgebra/test/LinearAlgebra_Test.csproj b/LinearAlgebra/test/LinearAlgebra_Test.csproj new file mode 100644 index 0000000..3ee2230 --- /dev/null +++ b/LinearAlgebra/test/LinearAlgebra_Test.csproj @@ -0,0 +1,19 @@ + + + + net5.0 + false + true + + + + + + + + + + + + + diff --git a/RoboidControl.sln b/RoboidControl.sln index daf2834..94abce5 100644 --- a/RoboidControl.sln +++ b/RoboidControl.sln @@ -3,7 +3,7 @@ Microsoft Visual Studio Solution File, Format Version 12.00 # Visual Studio Version 17 VisualStudioVersion = 17.0.31903.59 MinimumVisualStudioVersion = 10.0.40219.1 -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "test", "test\test.csproj", "{B65BB41E-5A93-46FC-BA68-B865F50D57BD}" +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RoboidControl_Test", "test\RoboidControl_Test.csproj", "{B65BB41E-5A93-46FC-BA68-B865F50D57BD}" EndProject Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Examples", "Examples", "{D6086F71-404B-4D18-BBE9-45947ED33DB2}" EndProject @@ -11,14 +11,17 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "BB2B", "Examples\BB2B\BB2B. EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RoboidControl", "src\RoboidControl.csproj", "{72BFCD03-FA78-4D0B-8B36-32301D60D6DD}" EndProject +Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "LinearAlgebra", "LinearAlgebra", "{3B5ED006-C6FC-4776-A6DC-FEE4E6909C03}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LinearAlgebra", "LinearAlgebra\src\LinearAlgebra.csproj", "{CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LinearAlgebra_Test", "LinearAlgebra\test\LinearAlgebra_Test.csproj", "{8F286220-E70C-4E77-BDA6-6F28B726E320}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU Release|Any CPU = Release|Any CPU EndGlobalSection - GlobalSection(SolutionProperties) = preSolution - HideSolutionNode = FALSE - EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution {B65BB41E-5A93-46FC-BA68-B865F50D57BD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {B65BB41E-5A93-46FC-BA68-B865F50D57BD}.Debug|Any CPU.Build.0 = Debug|Any CPU @@ -32,8 +35,21 @@ Global {72BFCD03-FA78-4D0B-8B36-32301D60D6DD}.Debug|Any CPU.Build.0 = Debug|Any CPU {72BFCD03-FA78-4D0B-8B36-32301D60D6DD}.Release|Any CPU.ActiveCfg = Release|Any CPU {72BFCD03-FA78-4D0B-8B36-32301D60D6DD}.Release|Any CPU.Build.0 = Release|Any CPU + {CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}.Debug|Any CPU.Build.0 = Debug|Any CPU + {CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}.Release|Any CPU.ActiveCfg = Release|Any CPU + {CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}.Release|Any CPU.Build.0 = Release|Any CPU + {8F286220-E70C-4E77-BDA6-6F28B726E320}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {8F286220-E70C-4E77-BDA6-6F28B726E320}.Debug|Any CPU.Build.0 = Debug|Any CPU + {8F286220-E70C-4E77-BDA6-6F28B726E320}.Release|Any CPU.ActiveCfg = Release|Any CPU + {8F286220-E70C-4E77-BDA6-6F28B726E320}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE EndGlobalSection GlobalSection(NestedProjects) = preSolution {488F28B2-A2CC-4E32-8D3B-7DB4EB1485F9} = {D6086F71-404B-4D18-BBE9-45947ED33DB2} + {CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3} = {3B5ED006-C6FC-4776-A6DC-FEE4E6909C03} + {8F286220-E70C-4E77-BDA6-6F28B726E320} = {3B5ED006-C6FC-4776-A6DC-FEE4E6909C03} EndGlobalSection EndGlobal diff --git a/src/LinearAlgebra/Angle.cs b/src/LinearAlgebra/Angle.cs deleted file mode 100644 index 0c23840..0000000 --- a/src/LinearAlgebra/Angle.cs +++ /dev/null @@ -1,11 +0,0 @@ -using System; - -namespace Passer.LinearAlgebra { - - public 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; - } - -} \ No newline at end of file diff --git a/src/LinearAlgebra/Vector2.cs b/src/LinearAlgebra/Vector2.cs deleted file mode 100644 index e76d12a..0000000 --- a/src/LinearAlgebra/Vector2.cs +++ /dev/null @@ -1,51 +0,0 @@ -using System; -using System.Numerics; - -namespace Passer.LinearAlgebra { - - public class Vector2Of where T : IComparable { - public T x; - public T y; - - public Vector2Of(T x, T y) { - this.x = x; - this.y = y; - } - } - - public class Vector2Int : Vector2Of { - public Vector2Int(int x, int y) : base(x, y) { } - - public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) { - return new Vector2Int(v1.x - v2.x, v1.y - v2.y); - } - - public float magnitude { - get { - return (float)Math.Sqrt(this.x * this.x + this.y * this.y); - } - } - - public static float Distance(Vector2Int v1, Vector2Int v2) { - return (v1 - v2).magnitude; - } - } - - public class Vector2Float : Vector2Of { - public Vector2Float(float x, float y) : base(x, y) { } - - public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) { - return new Vector2Float(v1.x - v2.x, v1.y - v2.y); - } - - public float magnitude { - get { - return (float)Math.Sqrt(this.x * this.x + this.y * this.y); - } - } - - public static float Distance(Vector2Float v1, Vector2Float v2) { - return (v1 - v2).magnitude; - } - } -} \ No newline at end of file diff --git a/src/LinearAlgebra/Vector3.cs b/src/LinearAlgebra/Vector3.cs deleted file mode 100644 index 0cc4d7b..0000000 --- a/src/LinearAlgebra/Vector3.cs +++ /dev/null @@ -1,33 +0,0 @@ -#if !UNITY_5_3_OR_NEWER -using System; - -namespace Passer.LinearAlgebra { - public class Vector3Of { - public T x; - public T y; - public T z; - - public Vector3Of(T x, T y, T z) { - this.x = x; - this.y = y; - this.z = z; - } - - // public uint magnitude { - // get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); - // } - } - - public class Vector3Int : Vector3Of { - public Vector3Int(int x, int y, int z) : base(x, y, z) { } - } - public class Vector3Float : Vector3Of { - public Vector3Float(float x, float y, float z) : base(x, y, z) { } - - public float magnitude { - get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); - } - } -} - -#endif \ No newline at end of file diff --git a/src/RoboidControl.csproj b/src/RoboidControl.csproj index feb0b62..a45ac67 100644 --- a/src/RoboidControl.csproj +++ b/src/RoboidControl.csproj @@ -9,6 +9,6 @@ + - diff --git a/test/test.csproj b/test/RoboidControl_Test.csproj similarity index 100% rename from test/test.csproj rename to test/RoboidControl_Test.csproj