From 1855aa0705deb54be7886fd6572bf186365559a6 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 7 Jan 2026 14:33:10 +0100 Subject: [PATCH] Squashed 'Assets/NanoBrain/LinearAlgebra/' changes from 672f8bf..a0bbb5b a0bbb5b Improve unit tests f3044db Add more ToString e64eaa2 Fix direction To Unity Vector3 3bcd212 Fixed Spherical.ToVector3 a232385 Merge commit '841d923fed686700610a85aeab6289e44239aa6c' git-subtree-dir: Assets/NanoBrain/LinearAlgebra git-subtree-split: a0bbb5b5dd2e938be6d8a516bb69eb1130c4a51d --- src/Angle.cs | 2 +- src/Direction.cs | 50 +++++++++++++++++++---------- src/Spherical.cs | 75 +++++++++++++++++++++++++------------------ src/Vector3Float.cs | 4 +++ test/DirectionTest.cs | 2 +- test/SphericalTest.cs | 24 ++++++++++++++ 6 files changed, 107 insertions(+), 50 deletions(-) diff --git a/src/Angle.cs b/src/Angle.cs index 70f5b10..7d2fd6b 100644 --- a/src/Angle.cs +++ b/src/Angle.cs @@ -57,7 +57,7 @@ namespace LinearAlgebra { public readonly float inRevolutions => this.value / 360.0f; public override string ToString() { - return $"{this.inDegrees} deg."; + return $"{this.inDegrees}\u00B0"; } public static readonly AngleFloat zero = Degrees(0); diff --git a/src/Direction.cs b/src/Direction.cs index 9293503..a1ff8f3 100644 --- a/src/Direction.cs +++ b/src/Direction.cs @@ -3,7 +3,8 @@ using System; using Vector3Float = UnityEngine.Vector3; #endif -namespace LinearAlgebra { +namespace LinearAlgebra +{ /// /// A direction in 3D space @@ -16,7 +17,8 @@ namespace LinearAlgebra { /// rotation has been applied. /// The angles are automatically normalized to stay within the abovenmentioned /// ranges. - public struct Direction { + public struct Direction + { /// @brief horizontal angle, range = (-180..180] degrees public AngleFloat horizontal; /// @brief vertical angle, range in degrees = (-90..90] degrees @@ -29,7 +31,8 @@ namespace LinearAlgebra { /// The vertical angle /// The direction will be normalized automatically /// to ensure the angles are within the allowed ranges - public Direction(AngleFloat horizontal, AngleFloat vertical) { + public Direction(AngleFloat horizontal, AngleFloat vertical) + { this.horizontal = horizontal; this.vertical = vertical; this.Normalize(); @@ -43,8 +46,10 @@ namespace LinearAlgebra { /// The direction /// The direction will be normalized automatically /// to ensure the angles are within the allowed ranges - public static Direction Degrees(float horizontal, float vertical) { - Direction d = new() { + public static Direction Degrees(float horizontal, float vertical) + { + Direction d = new() + { horizontal = AngleFloat.Degrees(horizontal), vertical = AngleFloat.Degrees(vertical) }; @@ -57,8 +62,10 @@ namespace LinearAlgebra { /// The horizontal angle in radians /// The vertical angle in radians /// The direction - public static Direction Radians(float horizontal, float vertical) { - Direction d = new() { + public static Direction Radians(float horizontal, float vertical) + { + Direction d = new() + { horizontal = AngleFloat.Radians(horizontal), vertical = AngleFloat.Radians(vertical) }; @@ -99,8 +106,10 @@ namespace LinearAlgebra { /// public readonly static Direction right = Degrees(90, 0); - private void Normalize() { - if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90) { + private void Normalize() + { + if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90) + { this.horizontal += AngleFloat.deg180; this.vertical = AngleFloat.Degrees(180 - this.vertical.inDegrees); } @@ -118,9 +127,11 @@ namespace LinearAlgebra { // Calculate Vector float cosV = MathF.Cos(radV); - float x = cosV * MathF.Cos(radH); - float y = MathF.Sin(radV); - float z = cosV * MathF.Sin(radH); + float sinV = MathF.Sin(radV); + + float x = cosV * MathF.Sin(radH); + float y = sinV; + float z = cosV * MathF.Cos(radH); return new UnityEngine.Vector3(x, y, z); } @@ -168,7 +179,8 @@ namespace LinearAlgebra { /// The direction /// Information about the length of the carthesian vector is not /// included in this transformation - public static Direction FromVector3(Vector3Float v) { + public static Direction FromVector3(Vector3Float v) + { AngleFloat horizontal = AngleFloat.Atan2(v.horizontal, v.depth); AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.vertical); Direction d = new(horizontal, vertical); @@ -183,7 +195,8 @@ namespace LinearAlgebra { /// /// /// True when the direction angles are equal, false otherwise. - public static bool operator ==(Direction d1, Direction d2) { + public static bool operator ==(Direction d1, Direction d2) + { bool horizontalEq = d1.horizontal == d2.horizontal; bool verticalEq = d1.vertical == d2.vertical; return horizontalEq && verticalEq; @@ -195,13 +208,15 @@ namespace LinearAlgebra { /// /// /// True when the direction angles are not equal, false otherwise. - public static bool operator !=(Direction d1, Direction d2) { + public static bool operator !=(Direction d1, Direction d2) + { bool horizontalNEq = d1.horizontal != d2.horizontal; bool verticalNEq = d1.vertical != d2.vertical; return horizontalNEq || verticalNEq; } - public override readonly bool Equals(object obj) { + public override readonly bool Equals(object obj) + { if (obj is not Direction d) return false; @@ -211,7 +226,8 @@ namespace LinearAlgebra { } - public override readonly int GetHashCode() { + public override readonly int GetHashCode() + { return HashCode.Combine(horizontal, vertical); } diff --git a/src/Spherical.cs b/src/Spherical.cs index 3a76085..2f3f801 100644 --- a/src/Spherical.cs +++ b/src/Spherical.cs @@ -62,28 +62,34 @@ namespace LinearAlgebra { #if UNITY_5_3_OR_NEWER public static Spherical FromVector3(Vector3 v) { float distance = v.magnitude; - if (distance == 0.0f) - return Spherical.zero; - 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); - } + // if (distance == 0.0f) + // return Spherical.zero; + // 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); + // } + Direction direction = Direction.FromVector3(v.normalized); + return new Spherical(distance, direction); } public readonly Vector3 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 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; + // float x = this.distance * sinVertical * sinHorizontal; + // float y = this.distance * cosVertical; + // float z = this.distance * sinVertical * cosHorizontal; - Vector3 v = new(x, y, z); + // Vector3 v = new(x, y, z); + // return v; + + Vector3 v = this.direction.ToVector3(); + v *= this.distance; return v; } #else @@ -99,22 +105,29 @@ namespace LinearAlgebra { } 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 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; + // float x = this.distance * sinVertical * sinHorizontal; + // float y = this.distance * cosVertical; + // float z = this.distance * sinVertical * cosHorizontal; - Vector3Float v = new(x, y, z); + // Vector3Float v = new(x, y, z); + Vector3Float v = this.direction.ToVector3(); + v *= this.distance; return v; } #endif + public override readonly string ToString() { + return $"Spherical({this.distance}, h: {this.direction.horizontal}, v: {this.direction.vertical})"; + } + + public readonly float magnitude => this.distance; public Spherical normalized { @@ -228,7 +241,7 @@ namespace LinearAlgebra { return Spherical.Radians(rAvg, azAvgRad, elAvgRad); } - + /* public static Spherical Average(List vectors) { // float sumSinPhiCosTheta = 0.0f; // float sumSinPhiSinTheta = 0.0f; @@ -302,9 +315,9 @@ namespace LinearAlgebra { 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)); @@ -371,7 +384,7 @@ namespace LinearAlgebra { return Spherical.Radians(rAvgFinal, azAvg, elAvg); } -*/ + } } \ No newline at end of file diff --git a/src/Vector3Float.cs b/src/Vector3Float.cs index d8208d3..bcf8626 100644 --- a/src/Vector3Float.cs +++ b/src/Vector3Float.cs @@ -119,6 +119,10 @@ namespace LinearAlgebra { return new Vector3Float(horizontal, vertical, depth); } + public override string ToString() { + return $"({this.horizontal}, {this.vertical}, {this.depth})"; + } + /// /// A vector with zero for all axis /// diff --git a/test/DirectionTest.cs b/test/DirectionTest.cs index 8fe3b93..0eb9882 100644 --- a/test/DirectionTest.cs +++ b/test/DirectionTest.cs @@ -103,7 +103,7 @@ namespace LinearAlgebra.Test { [Test] public void ToVector3AndBack2() { - Direction d1 = Direction.Degrees(135, 85); + 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); diff --git a/test/SphericalTest.cs b/test/SphericalTest.cs index d7553dd..48db8d9 100644 --- a/test/SphericalTest.cs +++ b/test/SphericalTest.cs @@ -234,6 +234,30 @@ namespace LinearAlgebra.Test { Assert.AreEqual(MathF.Cos(alpha), avg.distance, 1e-4f); } + [Test] + public void Average_CompareWithVector3() { + // 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 + List dirs = new List { + new(1f, Direction.Radians(0f, elevation)), + new(2f, Direction.Radians(MathF.PI/2, elevation+1)), + new(3f, Direction.Radians(MathF.PI, elevation+2)), + new(4f, Direction.Radians(3*MathF.PI/2, elevation+3)) + }; + + Spherical avg = Spherical.Average(dirs); + + Vector3Float r = Vector3Float.zero; + foreach (Spherical dir in dirs) { + r += dir.ToVector3(); + } + r = r / 4; + Spherical avg2 = Spherical.FromVector3(r); + + Assert.AreEqual(avg, avg2); + } + } } #endif \ No newline at end of file