From 841d923fed686700610a85aeab6289e44239aa6c Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 7 Jan 2026 11:33:48 +0100 Subject: [PATCH] Squashed 'Assets/NanoBrain/LinearAlgebra/' changes from 15c08f2..672f8bf 672f8bf Spherical Average a278b7d Fix/Improve ToVector3 09d34d1 Prepare for spherical average b19e504 (A little) Performance improvements 2b0433f Fix normalizing direction 3e115cc Fix Direction.ToVector3 0eeedd2 Vector3 conversion fixes 3024562 Fix Unity warnings aa23d57 Fix roaming boid cdfe039 Improve Unity compatibility git-subtree-dir: Assets/NanoBrain/LinearAlgebra git-subtree-split: 672f8bfca1b1e0bc312df41142fa3c4447ce6dba --- src/Angle.cs | 27 ++- src/Direction.cs | 83 ++++++++-- src/Quaternion.cs | 26 --- src/Spherical.cs | 376 +++++++++++++++++++++++++++++++----------- src/SwingTwist.cs | 66 +++++--- src/Vector2Float.cs | 36 ++-- src/Vector2Int.cs | 31 ++-- src/Vector3Float.cs | 31 ++-- test/AngleTest.cs | 4 + test/DirectionTest.cs | 25 +++ test/SphericalTest.cs | 186 +++++++++++++++++++++ 11 files changed, 682 insertions(+), 209 deletions(-) diff --git a/src/Angle.cs b/src/Angle.cs index 694d1b7..70f5b10 100644 --- a/src/Angle.cs +++ b/src/Angle.cs @@ -32,7 +32,6 @@ namespace LinearAlgebra { } return new AngleFloat(radians * Rad2Deg); - } public static AngleFloat Revolutions(float revolutions) { @@ -51,16 +50,14 @@ namespace LinearAlgebra { return new AngleFloat(revolutions * 360); } - public float inDegrees { - get { return this.value; } - } + public readonly float inDegrees => this.value; - public float inRadians { - get { return this.value * Deg2Rad; } - } + public readonly float inRadians => this.value * Deg2Rad; + + public readonly float inRevolutions => this.value / 360.0f; - public float inRevolutions { - get { return this.value / 360.0f; } + public override string ToString() { + return $"{this.inDegrees} deg."; } public static readonly AngleFloat zero = Degrees(0); @@ -115,6 +112,18 @@ namespace LinearAlgebra { 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(); + } + + /// /// Tests if the first angle is greater than the second /// diff --git a/src/Direction.cs b/src/Direction.cs index ed82901..9293503 100644 --- a/src/Direction.cs +++ b/src/Direction.cs @@ -66,6 +66,10 @@ namespace LinearAlgebra { return d; } + public override readonly string ToString() { + return $"Direction(h: {this.horizontal}, v: {this.vertical})"; + } + /// /// A forward direction with zero for both angles /// @@ -98,31 +102,64 @@ namespace LinearAlgebra { private void Normalize() { if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90) { this.horizontal += AngleFloat.deg180; - this.vertical = AngleFloat.deg180 - this.vertical; + this.vertical = AngleFloat.Degrees(180 - this.vertical.inDegrees); } } -#if !UNITY_5_3_OR_NEWER +#if UNITY_5_3_OR_NEWER /// /// Convert the direction into a carthesian vector /// /// The carthesian vector corresponding to this direction. - public Vector3Float ToVector3Float() { - Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0); - Vector3Float v = q * Vector3Float.forward; - return v; + public readonly UnityEngine.Vector3 ToVector3() { + // Convert degrees to radians + float radH = this.horizontal.inRadians; + float radV = this.vertical.inRadians; + + // Calculate Vector + float cosV = MathF.Cos(radV); + float x = cosV * MathF.Cos(radH); + float y = MathF.Sin(radV); + float z = cosV * MathF.Sin(radH); + + return new UnityEngine.Vector3(x, y, z); } -#else + + /// + /// Convert a carthesian vector into a direction + /// + /// The carthesian vector + /// The direction + /// Information about the length of the carthesian vector is not + /// included in this transformation + public static Direction FromVector3(UnityEngine.Vector3 v) { + AngleFloat horizontal = AngleFloat.Atan2(v.x, v.z); + AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.y); + Direction d = new(horizontal, vertical); + return d; + } +#else /// /// Convert the direction into a carthesian vector /// /// The carthesian vector corresponding to this direction. - public UnityEngine.Vector3 ToVector3() { - UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0); - UnityEngine.Vector3 v = q * UnityEngine.Vector3.forward; - return v; + public readonly Vector3Float ToVector3() { + // Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0); + // Vector3Float v = q * Vector3Float.forward; + // return v; + + float radH = this.horizontal.inRadians; + float radV = this.vertical.inRadians; + + float cosV = MathF.Cos(radV); + float sinV = MathF.Sin(radV); + + float horizontal = cosV * MathF.Sin(radH); + float vertical = sinV; + float depth = cosV * MathF.Cos(radH); + + return new Vector3Float(horizontal, vertical, depth); } -#endif /// /// Convert a carthesian vector into a direction @@ -137,6 +174,8 @@ namespace LinearAlgebra { Direction d = new(horizontal, vertical); return d; } +#endif + /// /// Tests the equality of two directions @@ -176,6 +215,26 @@ namespace LinearAlgebra { return HashCode.Combine(horizontal, vertical); } + public static AngleFloat UnsignedAngle(Direction d1, Direction d2) { + // Convert angles from degrees to radians + float horizontal1Rad = d1.horizontal.inRadians; + float vertical1Rad = d1.vertical.inRadians; + + float horizontal2Rad = d2.horizontal.inRadians; + float vertical2Rad = d2.vertical.inRadians; + + // Calculate the cosine of the angle using the spherical law of cosines + float cosTheta = MathF.Sin(vertical1Rad) * MathF.Sin(vertical2Rad) + + MathF.Cos(vertical1Rad) * MathF.Cos(vertical2Rad) * + MathF.Cos(horizontal1Rad - horizontal2Rad); + + // Clip cosTheta to the valid range for acos + cosTheta = Float.Clamp(cosTheta, -1.0f, 1.0f); + + // Calculate the angle + AngleFloat angle = AngleFloat.Acos(cosTheta); + return angle; + } } } \ No newline at end of file diff --git a/src/Quaternion.cs b/src/Quaternion.cs index 670c0a4..7936843 100644 --- a/src/Quaternion.cs +++ b/src/Quaternion.cs @@ -180,11 +180,7 @@ namespace LinearAlgebra { /// The resulting quaternion /// Rotation are appied in the order Z, X, Y. public static Quaternion Euler(Vector3Float angles) { - // return Quaternion.FromEulerRad(angles * AngleFloat.Deg2Rad); - // } - // private static Quaternion FromEulerRad(Vector3Float euler) { Vector3Float euler = angles * AngleFloat.Deg2Rad; - // euler.x = pitch, euler.y = yaw, euler.z = roll (radians) float cx = MathF.Cos(euler.horizontal * 0.5f); float sx = MathF.Sin(euler.horizontal * 0.5f); float cy = MathF.Cos(euler.vertical * 0.5f); @@ -199,28 +195,6 @@ namespace LinearAlgebra { q.y = sy * cx * cz - cy * sx * sz; q.z = cy * cx * sz - sy * sx * cz; return q; - // float yaw = euler.horizontal; - // float pitch = euler.vertical; - // float roll = euler.depth; - // float rollOver2 = roll * 0.5f; - // float sinRollOver2 = MathF.Sin(rollOver2); - // float cosRollOver2 = MathF.Cos(rollOver2); - // float pitchOver2 = pitch * 0.5f; - // float sinPitchOver2 = MathF.Sin(pitchOver2); - // float cosPitchOver2 = MathF.Cos(pitchOver2); - // float yawOver2 = yaw * 0.5f; - // float sinYawOver2 = MathF.Sin(yawOver2); - // float cosYawOver2 = MathF.Cos(yawOver2); - // Quaternion result; - // result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + - // sinYawOver2 * sinPitchOver2 * sinRollOver2; - // result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + - // cosYawOver2 * sinPitchOver2 * sinRollOver2; - // result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - - // sinYawOver2 * cosPitchOver2 * sinRollOver2; - // result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - - // sinYawOver2 * sinPitchOver2 * cosRollOver2; - // return result; } /// diff --git a/src/Spherical.cs b/src/Spherical.cs index de06d24..3a76085 100644 --- a/src/Spherical.cs +++ b/src/Spherical.cs @@ -1,23 +1,22 @@ using System; +using System.Collections.Generic; + #if UNITY_5_3_OR_NEWER using Vector3 = UnityEngine.Vector3; #endif -namespace LinearAlgebra -{ +namespace LinearAlgebra { /// /// A spherical vector /// /// This is a struct such that it is a value type and cannot be null - public struct Spherical - { + public struct Spherical { /// /// Create a spherical vector /// /// The distance in meters /// The direction of the vector - public Spherical(float distance, Direction direction) - { + public Spherical(float distance, Direction direction) { this.distance = distance; this.direction = direction; } @@ -29,15 +28,13 @@ namespace LinearAlgebra /// The horizontal angle in degrees /// The vertical angle in degrees /// - public static Spherical Degrees(float distance, float horizontal, float vertical) - { + public static Spherical Degrees(float distance, float horizontal, float vertical) { Direction direction = Direction.Degrees(horizontal, vertical); Spherical s = new(distance, direction); return s; } - public static Spherical Radians(float distance, float horizontal, float vertical) - { + public static Spherical Radians(float distance, float horizontal, float vertical) { Direction direction = Direction.Radians(horizontal, vertical); Spherical s = new(distance, direction); return s; @@ -62,79 +59,19 @@ namespace LinearAlgebra /// public readonly static Spherical forward = new(1, Direction.forward); - - // public static Spherical FromVector3Float(Vector3Float v) { - // float distance = v.magnitude; - // if (distance == 0.0f) - // return Spherical.zero; - // else { - // float verticalAngle = (float)((Angle.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); - // } - // } - - public static Spherical FromVector3Float(Vector3Float v) - { - float distance = v.magnitude; - if (distance == 0.0f) - return Spherical.zero; - else - { - float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.vertical / distance)) * AngleFloat.Rad2Deg; - float horizontalAngle = (float)Math.Atan2(v.horizontal, v.depth) * AngleFloat.Rad2Deg; - return Degrees(distance, horizontalAngle, verticalAngle); - } - } - - // public Vector3Float ToVector3Float() { - // float verticalRad = (Angle.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); - // float cosHorizontal = (float)Math.Cos(horizontalRad); - // float sinHorizontal = (float)Math.Sin(horizontalRad); - - // float x = this.distance * sinVertical * sinHorizontal; - // float y = this.distance * cosVertical; - // float z = this.distance * sinVertical * cosHorizontal; - - // Vector3Float v = new(x, y, z); - // return v; - // } - - public Vector3Float ToVector3Float() - { - float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians; - float horizontalRad = this.direction.horizontal.inRadians; - float cosVertical = (float)Math.Cos(verticalRad); - float sinVertical = (float)Math.Sin(verticalRad); - float cosHorizontal = (float)Math.Cos(horizontalRad); - float sinHorizontal = (float)Math.Sin(horizontalRad); - - float x = this.distance * sinVertical * sinHorizontal; - float y = this.distance * cosVertical; - float z = this.distance * sinVertical * cosHorizontal; - - Vector3Float v = new(x, y, z); - return v; - } #if UNITY_5_3_OR_NEWER - public static Spherical FromVector3(Vector3 v) - { + public static Spherical FromVector3(Vector3 v) { float distance = v.magnitude; if (distance == 0.0f) return Spherical.zero; - else - { + else { float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.y / distance)) * AngleFloat.Rad2Deg; float horizontalAngle = (float)Math.Atan2(v.x, v.z) * AngleFloat.Rad2Deg; return Degrees(distance, horizontalAngle, verticalAngle); } } - public Vector3 ToVector3() - { + public readonly Vector3 ToVector3() { float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians; float horizontalRad = this.direction.horizontal.inRadians; float cosVertical = (float)Math.Cos(verticalRad); @@ -149,21 +86,40 @@ namespace LinearAlgebra Vector3 v = new(x, y, z); return v; } -#endif - - public float magnitude - { - get - { - return this.distance; +#else + public static Spherical FromVector3(Vector3Float v) { + float distance = v.magnitude; + if (distance == 0.0f) + return Spherical.zero; + else { + float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.vertical / distance)) * AngleFloat.Rad2Deg; + float horizontalAngle = (float)Math.Atan2(v.horizontal, v.depth) * AngleFloat.Rad2Deg; + return Degrees(distance, horizontalAngle, verticalAngle); } } - public Spherical normalized - { - get - { - Spherical r = new() - { + + public readonly Vector3Float ToVector3() { + float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians; + float horizontalRad = this.direction.horizontal.inRadians; + float cosVertical = (float)Math.Cos(verticalRad); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.Sin(horizontalRad); + + float x = this.distance * sinVertical * sinHorizontal; + float y = this.distance * cosVertical; + float z = this.distance * sinVertical * cosHorizontal; + + Vector3Float v = new(x, y, z); + return v; + } +#endif + + public readonly float magnitude => this.distance; + + public Spherical normalized { + get { + Spherical r = new() { distance = 1, direction = this.direction }; @@ -171,15 +127,251 @@ namespace LinearAlgebra } } - public static Spherical operator +(Spherical s1, Spherical s2) - { + public static Spherical operator +(Spherical s1, Spherical s2) { // let's do it the easy way... - Vector3Float v1 = s1.ToVector3Float(); - Vector3Float v2 = s2.ToVector3Float(); - Vector3Float v = v1 + v2; - Spherical r = FromVector3Float(v); + // using vars to be compatible with both unity (Vector3) and native (Vector3Float) + var v1 = s1.ToVector3(); + var v2 = s2.ToVector3(); + var v = v1 + v2; + Spherical r = FromVector3(v); return r; } + public static Spherical operator *(Spherical v, float d) { + Spherical r = new(v.distance * d, v.direction); + return r; + } + + public static bool operator ==(Spherical v1, Spherical v2) { + return (v1.distance == v2.distance && v1.direction == v2.direction); + } + + public static bool operator !=(Spherical v1, Spherical v2) { + return (v1.distance != v2.distance || v1.direction != v2.direction); + } + + public static float Distance(Spherical v1, Spherical v2) { + // Convert degrees to radians + float thetaARadians = v1.direction.horizontal.inRadians; + float phiARadians = v1.direction.vertical.inRadians;// DegreesToRadians(phiA); + float thetaBRadians = v2.direction.horizontal.inRadians; // DegreesToRadians(thetaB); + float phiBRadians = v2.direction.vertical.inRadians; // DegreesToRadians(phiB); + + // Calculate sine and cosine values + float sinPhiA = MathF.Sin(phiARadians); + float cosPhiA = MathF.Cos(phiARadians); + float sinPhiB = MathF.Sin(phiBRadians); + float cosPhiB = MathF.Cos(phiBRadians); + + // Calculate the cosine of the difference in azimuthal angles + float cosThetaDifference = MathF.Cos(thetaARadians - thetaBRadians); + + // Apply the spherical law of cosines + float distance = MathF.Sqrt( + v1.distance * v1.distance + + v2.distance * v2.distance - + 2 * v1.distance * v2.distance * (sinPhiA * sinPhiB * cosThetaDifference + cosPhiA * cosPhiB) + ); + + return distance; + } + + public static Spherical Average(Spherical v1, Spherical v2) { + const float EPS = 1e-6f; + + // Angles in radians + float a1 = v1.direction.horizontal.inRadians; + float a2 = v2.direction.horizontal.inRadians; + float e1 = v1.direction.vertical.inRadians; + float e2 = v2.direction.vertical.inRadians; + + // Fast path: exactly same direction (allowing wrap for azimuth) -> preserve exact angles + bool sameAz = MathF.Abs(MathF.IEEERemainder(a1 - a2, MathF.PI * 2f)) < EPS; + bool sameEl = MathF.Abs(e1 - e2) < EPS; + if (sameAz && sameEl) { + // Distances may differ; average distance but keep exact angles from v1 + float rAvgExact = 0.5f * (v1.distance + v2.distance); + return new Spherical(rAvgExact, v1.direction); + } + + // Horizontal unit-circle sum + float cx = MathF.Cos(a1) + MathF.Cos(a2); + float cy = MathF.Sin(a1) + MathF.Sin(a2); + + // Vertical as z = sin(el) + float z1 = MathF.Sin(e1); + float z2 = MathF.Sin(e2); + float cz = z1 + z2; + + // Magnitude of summed unit-direction vectors + float sumX = cx; + float sumY = cy; + float sumZ = cz; + float magSum = MathF.Sqrt(sumX * sumX + sumY * sumY + sumZ * sumZ); + + // If the two direction unit-vectors cancel (or nearly), return zero distance. + if (magSum < EPS) { + return Spherical.Radians(0f, 0f, 0f); + } + + // Normalized averaged direction components + float ux = sumX / magSum; + float uy = sumY / magSum; + float uz = sumZ / magSum; + + // Compute averaged angles from normalized vector + float azAvgRad = MathF.Atan2(uy, ux); + float elAvgRad = MathF.Asin(Float.Clamp(uz, -1f, 1f)); + + // Average distance (arithmetic mean) + float rAvg = 0.5f * (v1.distance + v2.distance); + + return Spherical.Radians(rAvg, azAvgRad, elAvgRad); + } + + public static Spherical Average(List vectors) { + // float sumSinPhiCosTheta = 0.0f; + // float sumSinPhiSinTheta = 0.0f; + // float sumCosPhi = 0.0f; + + // int n = vectors.Count; + + // // Step 1: Accumulate sine and cosine components + // foreach(Spherical v in vectors) { + // float sinHorizontal = AngleFloat.Sin(v.direction.horizontal); + // sumSinPhiCosTheta += v.distance * sinHorizontal * AngleFloat.Cos(v.direction.vertical); + // sumSinPhiSinTheta += v.distance * sinHorizontal * AngleFloat.Sin(v.direction.vertical); + // sumCosPhi += v.distance * AngleFloat.Cos(v.direction.horizontal); + // } + + // // Step 2: Calculate average components + // float avgSinPhiCosTheta = sumSinPhiCosTheta / n; + // float avgSinPhiSinTheta = sumSinPhiSinTheta / n; + // float avgCosPhi = sumCosPhi / n; + + // // Step 3: Calculate the magnitude of the average vector + // float rAvg = MathF.Sqrt(avgSinPhiCosTheta * avgSinPhiCosTheta + + // avgSinPhiSinTheta * avgSinPhiSinTheta + + // avgCosPhi * avgCosPhi); + + // // Step 4: Calculate average angles + // AngleFloat horizontalAvg = AngleFloat.Acos(avgCosPhi / rAvg); // Handle rAvg != 0 case + // AngleFloat verticalAvg = AngleFloat.Atan2(avgSinPhiSinTheta, avgSinPhiCosTheta); + + // return new Spherical(rAvg, new Direction(horizontalAvg, verticalAvg)); + + if (vectors == null || vectors.Count == 0) + throw new ArgumentException("vectors must contain at least one element", nameof(vectors)); + + float sumX = 0f, sumY = 0f, sumZ = 0f; + int n = vectors.Count; + + foreach (var v in vectors) { + // AngleFloat -> radians; assume AngleFloat provides Radians property + float theta = v.direction.horizontal.inRadians; // azimuth + float phi = v.direction.vertical.inRadians; // elevation + + float cosPhi = MathF.Cos(phi); + float sinPhi = MathF.Sin(phi); + float cosTheta = MathF.Cos(theta); + float sinTheta = MathF.Sin(theta); + + float x = v.distance * cosPhi * cosTheta; + float y = v.distance * cosPhi * sinTheta; + float z = v.distance * sinPhi; + + sumX += x; + sumY += y; + sumZ += z; + } + + float avgX = sumX / n; + float avgY = sumY / n; + float avgZ = sumZ / n; + + float rAvg = MathF.Sqrt(avgX * avgX + avgY * avgY + avgZ * avgZ); + + if (rAvg == 0f) { + return new Spherical(0f, new Direction(AngleFloat.Radians(0f), AngleFloat.Radians(0f))); + } + + // elevation = asin(z / r) + AngleFloat verticalAvg = AngleFloat.Asin(avgZ / rAvg); // -90..90 + // azimuth = atan2(y, x) -> -pi..pi + AngleFloat horizontalAvg = AngleFloat.Atan2(avgY, avgX); // -180..180 + + return new Spherical(rAvg, new Direction(horizontalAvg, verticalAvg)); + } + + +/* + public static Spherical Average(IEnumerable vectors) { + const float EPS = 1e-6f; + if (vectors == null) throw new ArgumentNullException(nameof(vectors)); + + float sumRx = 0f, sumRy = 0f, sumRz = 0f; + float sumDistances = 0f; + int count = 0; + + bool firstSet = false; + float firstAz = 0f, firstEl = 0f; + bool allSameDirection = true; + + foreach (var v in vectors) { + float az = v.direction.horizontal.inRadians; // horizontal (azimuth) + float el = v.direction.vertical.inRadians; // vertical (elevation) + + if (!firstSet) { + firstSet = true; + firstAz = az; + firstEl = el; + } + else { + if (MathF.Abs(MathF.IEEERemainder(az - firstAz, MathF.PI * 2f)) >= EPS || + MathF.Abs(el - firstEl) >= EPS) { + allSameDirection = false; + } + } + + float cosEl = MathF.Cos(el); + float ux = cosEl * MathF.Cos(az); // x + float uy = cosEl * MathF.Sin(az); // y + float uz = MathF.Sin(el); // z + + sumRx += v.distance * ux; + sumRy += v.distance * uy; + sumRz += v.distance * uz; + + sumDistances += v.distance; + count++; + } + + if (count == 0) throw new ArgumentException("Sequence contains no elements", nameof(vectors)); + + // All directions equal -> preserve exact angles, average distance + if (allSameDirection) { + float rAvg = sumDistances / count; + return new Spherical(rAvg, Direction.Radians(firstAz, firstEl)); + } + + // Total vector sum V + float Vx = sumRx; + float Vy = sumRy; + float Vz = sumRz; + float Vmag = MathF.Sqrt(Vx * Vx + Vy * Vy + Vz * Vz); + + if (Vmag < EPS) { + // Directions cancel out -> zero distance, angles arbitrary + return Spherical.Radians(0f, 0f, 0f); + } + + float azAvg = MathF.Atan2(Vy, Vx); + float elAvg = MathF.Asin(Float.Clamp(Vz / Vmag, -1f, 1f)); + float rAvgFinal = Vmag / count; + + return Spherical.Radians(rAvgFinal, azAvg, elAvg); + } +*/ + } } \ No newline at end of file diff --git a/src/SwingTwist.cs b/src/SwingTwist.cs index 4437c4f..df6e048 100644 --- a/src/SwingTwist.cs +++ b/src/SwingTwist.cs @@ -1,6 +1,6 @@ -#if !UNITY_5_3_OR_NEWER -using UnityEngine; -#endif +// #if !UNITY_5_3_OR_NEWER +// using UnityEngine; +// #endif namespace LinearAlgebra { @@ -46,7 +46,45 @@ namespace LinearAlgebra { return s; } -#if !UNITY_5_3_OR_NEWER +#if UNITY_5_3_OR_NEWER + /// + /// A zero angle rotation + /// + public static readonly SwingTwist zero = Degrees(0, 0, 0); + + public Spherical ToAngleAxis() { + UnityEngine.Quaternion q = this.ToQuaternion(); + q.ToAngleAxis(out float angle, out UnityEngine.Vector3 axis); + Direction direction = Direction.FromVector3(axis); + + Spherical r = new(angle, direction); + return r; + } + + public static SwingTwist FromAngleAxis(Spherical r) { + UnityEngine.Vector3 vectorAxis = r.direction.ToVector3(); + UnityEngine.Quaternion q = UnityEngine.Quaternion.AngleAxis(r.distance, vectorAxis); + return FromQuaternion(q); + } + + /// + /// Convert a quaternion in a swing/twist rotation + /// + /// The quaternion to convert + /// The swing/twist rotation + public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) { + UnityEngine.Vector3 angles = q.eulerAngles; + SwingTwist r = Degrees(angles.y, -angles.x, -angles.z); + return r; + } + + public UnityEngine.Quaternion ToQuaternion() { + UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(this.swing.vertical.inDegrees, + this.swing.horizontal.inDegrees, + this.twist.inDegrees); + return q; + } +#else /// /// A zero angle rotation /// @@ -92,25 +130,7 @@ namespace LinearAlgebra { return r; } #endif -#if UNITY_5_3_OR_NEWER - /// - /// Convert a quaternion in a swing/twist rotation - /// - /// The quaternion to convert - /// The swing/twist rotation - public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) { - UnityEngine.Vector3 angles = q.eulerAngles; - SwingTwist r = Degrees(angles.y, -angles.x, -angles.z); - return r; - } - - public UnityEngine.Quaternion ToUnityQuaternion() { - UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(this.swing.vertical.inDegrees, - this.swing.horizontal.inDegrees, - this.twist.inDegrees); - return q; - } -#endif + } } \ No newline at end of file diff --git a/src/Vector2Float.cs b/src/Vector2Float.cs index e0418a8..ac1867c 100644 --- a/src/Vector2Float.cs +++ b/src/Vector2Float.cs @@ -324,27 +324,13 @@ namespace LinearAlgebra { public static Vector2Float Scale(Vector2Float v1, Vector2Float v2) { return new Vector2Float(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical); } - - /* + /// /// 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(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical; - - /// - /// 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 Vector2Float v)) - return false; - - return (horizontal == v.horizontal && vertical == v.vertical); - } - */ + //public readonly bool Equals(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical; /// /// Tests if the two vectors have equal values @@ -372,15 +358,25 @@ namespace LinearAlgebra { return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical); } - /* + /// + /// 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 readonly bool Equals(object obj) { + if (obj is not Vector2Float v) + return false; + + return (horizontal == v.horizontal && vertical == v.vertical); + } + /// /// Get an hash code for the vector /// /// The hash code - public override int GetHashCode() { - return (horizontal, vertical).GetHashCode(); + public override readonly int GetHashCode() { + return HashCode.Combine(horizontal, vertical); } - */ /// /// Get the distance between two vectors diff --git a/src/Vector2Int.cs b/src/Vector2Int.cs index 0eca7dc..ed68e8b 100644 --- a/src/Vector2Int.cs +++ b/src/Vector2Int.cs @@ -60,17 +60,6 @@ namespace LinearAlgebra { /// true if the vector values are equal public readonly bool Equals(Vector2Int v) => this.horizontal == v.horizontal && vertical == v.vertical; - /// - /// 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 readonly bool Equals(object obj) { - if (obj is not Vector2Int v) - return false; - - return (this.horizontal == v.horizontal && this.vertical == v.vertical); - } */ /// @@ -98,6 +87,26 @@ namespace LinearAlgebra { return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical); } + /// + /// 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 readonly bool Equals(object obj) { + if (obj is not Vector2Int v) + return false; + + return (this.horizontal == v.horizontal && this.vertical == v.vertical); + } + + /// + /// Get an hash code for the vector + /// + /// The hash code + public override readonly int GetHashCode() { + return HashCode.Combine(horizontal, vertical); + } + public readonly float sqrMagnitude => this.horizontal * this.horizontal + this.vertical * this.vertical; public static float SqrMagnitudeOf(Vector2Int v) { diff --git a/src/Vector3Float.cs b/src/Vector3Float.cs index bff0936..d8208d3 100644 --- a/src/Vector3Float.cs +++ b/src/Vector3Float.cs @@ -250,30 +250,22 @@ namespace LinearAlgebra { public static Vector3Float operator *(Vector3Float v1, float d) { - Vector3Float v = new Vector3Float(v1.horizontal * d, v1.vertical * d, v1.depth * d); + Vector3Float v = new(v1.horizontal * d, v1.vertical * d, v1.depth * d); return v; } public static Vector3Float operator *(float d, Vector3Float v1) { - Vector3Float v = new Vector3Float(d * v1.horizontal, d * v1.vertical, d * v1.depth); + Vector3Float v = new(d * v1.horizontal, d * v1.vertical, d * v1.depth); return v; } public static Vector3Float operator /(Vector3Float v1, float d) { - Vector3Float v = new Vector3Float(v1.horizontal / d, v1.vertical / d, v1.depth / d); + Vector3Float v = new(v1.horizontal / d, v1.vertical / d, v1.depth / d); return v; } - /* - public bool Equals(Vector3Float v) => (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth); - - public override bool Equals(object obj) { - if (!(obj is Vector3Float v)) - return false; - - return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth); - } - */ + + //public bool Equals(Vector3Float v) => (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth); public static bool operator ==(Vector3Float v1, Vector3Float v2) { return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical && v1.depth == v2.depth); @@ -283,9 +275,16 @@ namespace LinearAlgebra { return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical || v1.depth != v2.depth); } - // public override int GetHashCode() { - // return (horizontal, vertical, depth).GetHashCode(); - // } + public override readonly bool Equals(object obj) { + if (obj is not Vector3Float v) + return false; + + return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth); + } + + public override readonly int GetHashCode() { + return HashCode.Combine(horizontal, vertical, depth); + } /// @brief The distance between two vectors /// @param v1 The first vector diff --git a/test/AngleTest.cs b/test/AngleTest.cs index 787130d..8362d82 100644 --- a/test/AngleTest.cs +++ b/test/AngleTest.cs @@ -24,6 +24,10 @@ namespace LinearAlgebra.Test { a = AngleFloat.Degrees(angle); Assert.AreEqual(-90, a.inDegrees); + angle = -270.0f; + a = AngleFloat.Degrees(angle); + Assert.AreEqual(90, a.inDegrees); + // Radians angle = 0.0f; a = AngleFloat.Radians(angle); diff --git a/test/DirectionTest.cs b/test/DirectionTest.cs index 146c9df..8fe3b93 100644 --- a/test/DirectionTest.cs +++ b/test/DirectionTest.cs @@ -33,6 +33,13 @@ namespace LinearAlgebra.Test { Assert.AreEqual(30, d.vertical.inDegrees, 0.0001f); } + [Test] + public void DegreesNormalize1() { + Direction d = Direction.Degrees(112, 91); + Assert.AreEqual(-68, d.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(89, d.vertical.inDegrees, 0.0001f); + } + [Test] public void RadiansEquivalentToDegreesConversion() { Direction d1 = Direction.Radians((float)Math.PI / 3, (float)Math.PI / 4); @@ -68,6 +75,15 @@ namespace LinearAlgebra.Test { Assert.AreEqual(0, v.depth, 0.0001f); } + [Test] + public void ToVector3Left() { + Direction d = Direction.left; + Vector3Float v = d.ToVector3(); + Assert.AreEqual(-1, v.horizontal, 0.0001f); + Assert.AreEqual(0, v.vertical, 0.0001f); + Assert.AreEqual(0, v.depth, 0.0001f); + } + [Test] public void FromVector3Forward() { Vector3Float v = new(0, 0, 1); @@ -85,6 +101,15 @@ namespace LinearAlgebra.Test { Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f); } + [Test] + public void ToVector3AndBack2() { + Direction d1 = Direction.Degrees(135, 85); + Vector3Float v = d1.ToVector3(); + Direction d2 = Direction.FromVector3(v); + Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f); + } + [Test] public void Compare() { Direction d1 = Direction.Degrees(45, 135); diff --git a/test/SphericalTest.cs b/test/SphericalTest.cs index 125cdb1..d7553dd 100644 --- a/test/SphericalTest.cs +++ b/test/SphericalTest.cs @@ -1,5 +1,6 @@ #if !UNITY_5_6_OR_NEWER using System; +using System.Collections.Generic; using NUnit.Framework; namespace LinearAlgebra.Test { @@ -48,6 +49,191 @@ namespace LinearAlgebra.Test { Assert.AreEqual(45.0f, r.direction.horizontal.inDegrees, "Addition(1 0 90)"); Assert.AreEqual(45.0f, r.direction.vertical.inDegrees, 1.0E-05F, "Addition(1 0 90)"); } + + [Test] + public void Average2_IdenticalVectors() { + Direction dir = Direction.Radians(MathF.PI / 4f, MathF.PI / 6f); + Spherical v = new(2.5f, dir); + + Spherical avg = Spherical.Average(v, v); + + Assert.AreEqual(2.5f, avg.distance, 1e-5f); + Assert.AreEqual(dir.horizontal, avg.direction.horizontal); + Assert.AreEqual(dir.vertical, avg.direction.vertical); + } + + [Test] + public void Average2_OppositeUnitVectors() { + // Two opposite vectors: same distance, horizontal opposite (pi apart), same vertical + Spherical v1 = Spherical.Radians(1f, 0f, 0f); + Spherical v2 = Spherical.Radians(1f, MathF.PI, 0f); + Spherical avg = Spherical.Average(v1, v2); + + Assert.AreEqual(0f, avg.distance, 1e-4f); + // When distance is zero, angles may be undefined; allow any angle but ensure near-zero magnitude + } + + [Test] + public void Average2_WeightedByDistance() { + // Two vectors same direction but different distances -> weighted average distance + Direction dir = Direction.Radians(MathF.PI / 3f, MathF.PI / 4f); + Spherical a = new(1f, dir); + Spherical b = new(3f, dir); + Spherical avg = Spherical.Average(a, b); + + // average distance should be (1+3)/2 = 2 + Assert.AreEqual(2f, avg.distance, 1e-5f); + Assert.AreEqual(dir.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f); + Assert.AreEqual(dir.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f); + } + + [Test] + public void Average2_OppositeButNotExact_NotZero() { + // Nearly opposite but not exact; expect a valid averaged direction and averaged distance + Direction d1 = Direction.Radians(0f, 0f); + Direction d2 = Direction.Radians(MathF.PI - 1e-3f, 0.0f); // slight offset + Spherical v1 = new(2.0f, d1); + Spherical v2 = new(4.0f, d2); + + Spherical avg = Spherical.Average(v1, v2); + + // Distance is arithmetic mean + Assert.AreEqual(3.0f, avg.distance, 1e-5f); + + // Averaged azimuth should be near +pi/2 or -pi/2? we can check it's not NaN and unit-vector properties hold + float ux = MathF.Cos(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians); + float uy = MathF.Sin(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians); + float uz = MathF.Sin(avg.direction.vertical.inRadians); + float mag = MathF.Sqrt(ux * ux + uy * uy + uz * uz); + Assert.IsTrue(mag > 0.999f && mag < 1.001f); + + } + + [Test] + public void Average2_BasicAverageDirectionAndDistance() { + // Two different directions not cancelling: expect vector-average result + Direction d1 = Direction.Radians(MathF.PI / 6f, MathF.PI / 12f); // 30°, 15° + Direction d2 = Direction.Radians(MathF.PI / 3f, MathF.PI / 18f); // 60°, 10° + Spherical v1 = new(2.0f, d1); + Spherical v2 = new(4.0f, d2); + + Spherical avg = Spherical.Average(v1, v2); + + // Distance is arithmetic mean + Assert.AreEqual(3.0f, avg.distance, 1e-5f); + + // Check averaged unit-vector equals normalized sum of unit vectors computed here + float a1 = d1.horizontal.inRadians; + float a2 = d2.horizontal.inRadians; + float e1 = d1.vertical.inRadians; + float e2 = d2.vertical.inRadians; + + float cx = MathF.Cos(a1) + MathF.Cos(a2); + float cy = MathF.Sin(a1) + MathF.Sin(a2); + float z1 = MathF.Sin(e1); + float z2 = MathF.Sin(e2); + float cz = z1 + z2; + float mag = MathF.Sqrt(cx * cx + cy * cy + cz * cz); + Assert.IsTrue(mag > 1e-6f); + + float ux = cx / mag; + float uy = cy / mag; + float uz = cz / mag; + + // Reconstruct direction from avg result + float uxAvg = MathF.Cos(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians); + float uyAvg = MathF.Sin(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians); + float uzAvg = MathF.Sin(avg.direction.vertical.inRadians); + + Assert.AreEqual(ux, uxAvg, 1e-4f); + Assert.AreEqual(uy, uyAvg, 1e-4f); + Assert.AreEqual(uz, uzAvg, 1e-4f); + } + + [Test] + public void Average_IdenticalVectors() { + var dir = Direction.Radians(MathF.PI / 4f, MathF.PI / 6f); + var v = new Spherical(2.5f, dir); + var list = new List { v, v, v }; + + var avg = Spherical.Average(list); + + Assert.AreEqual(2.5f, avg.distance, 1e-5f); + Assert.AreEqual(dir.horizontal, avg.direction.horizontal); + Assert.AreEqual(dir.vertical, avg.direction.vertical); + } + + [Test] + public void Average_SingleElement() { + Spherical s = Spherical.Radians(1.234f, 0.3f, -0.7f); + Spherical avg = Spherical.Average([s]); + + Assert.AreEqual(s.distance, avg.distance, 1e-5f); + Assert.AreEqual(s.direction.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f); + Assert.AreEqual(s.direction.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f); + } + + [Test] + public void Average_OppositeUnitVectors() { + // Two opposite vectors: same distance, horizontal opposite (pi apart), same vertical + Spherical v1 = Spherical.Radians(1f, 0f, 0f); + Spherical v2 = Spherical.Radians(1f, MathF.PI, 0f); + Spherical avg = Spherical.Average([v1, v2]); + + Assert.AreEqual(0f, avg.distance, 1e-4f); + // When distance is zero, angles may be undefined; allow any angle but ensure near-zero magnitude + } + + [Test] + public void Average_WeightedByDistance() { + // Two vectors same direction but different distances -> weighted average distance + Direction dir = Direction.Radians(MathF.PI / 3f, MathF.PI / 4f); + Spherical a = new(1f, dir); + Spherical b = new(3f, dir); + Spherical avg = Spherical.Average([a, b]); + + // average distance should be (1+3)/2 = 2 + Assert.AreEqual(2f, avg.distance, 1e-5f); + Assert.AreEqual(dir.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f); + Assert.AreEqual(dir.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f); + } + + [Test] + public void Average_AxisSymmetricAroundVertical() { + // Four vectors around azimuth 0, pi/2, pi, 3pi/2 at same elevation (vertical) angle phi + float phi = MathF.PI / 6f; // elevation from horizontal plane + var dirs = new List { + new(1f, Direction.Radians(0f, phi)), + new(1f, Direction.Radians(MathF.PI/2, phi)), + new(1f, Direction.Radians(MathF.PI, phi)), + new(1f, Direction.Radians(3*MathF.PI/2, phi)) + }; + + Spherical avg = Spherical.Average(dirs); + + // rAvg should equal r * sin(elevation) = sin(phi) + Assert.AreEqual(MathF.Sin(phi), avg.distance, 1e-4f); + // vertical angle undefined when horizontal xy components cancel; allow any angle but ensure r matches + } + + [Test] + public void Average_AxisSymmetricAroundVertical2() { + // Four vectors around azimuth 0, pi/2, pi, 3pi/2 at same polar angle from vertical (alpha) + float alpha = MathF.PI / 6f; // polar angle from vertical + float elevation = MathF.PI / 2f - alpha; // convert polar-from-vertical to elevation + var dirs = new List { + new(1f, Direction.Radians(0f, elevation)), + new(1f, Direction.Radians(MathF.PI/2, elevation)), + new(1f, Direction.Radians(MathF.PI, elevation)), + new(1f, Direction.Radians(3*MathF.PI/2, elevation)) + }; + + Spherical avg = Spherical.Average(dirs); + + // rAvg should equal r * sin(elevation) which equals cos(alpha) + Assert.AreEqual(MathF.Cos(alpha), avg.distance, 1e-4f); + } + } } #endif \ No newline at end of file