diff --git a/Assets/NanoBrain/LinearAlgebra/src/Direction.cs b/Assets/NanoBrain/LinearAlgebra/src/Direction.cs
index 95b8d6a..7047da6 100644
--- a/Assets/NanoBrain/LinearAlgebra/src/Direction.cs
+++ b/Assets/NanoBrain/LinearAlgebra/src/Direction.cs
@@ -73,8 +73,7 @@ namespace LinearAlgebra
return d;
}
- public override readonly string ToString()
- {
+ public override readonly string ToString() {
return $"Direction(h: {this.horizontal}, v: {this.vertical})";
}
@@ -112,7 +111,7 @@ namespace LinearAlgebra
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);
}
}
@@ -127,9 +126,10 @@ namespace LinearAlgebra
float radV = this.vertical.inRadians;
// Calculate Vector
- float x = MathF.Cos(radV) * MathF.Cos(radH);
+ float cosV = MathF.Cos(radV);
+ float x = cosV * MathF.Cos(radH);
float y = MathF.Sin(radV);
- float z = MathF.Cos(radV) * MathF.Sin(radH);
+ float z = cosV * MathF.Sin(radH);
return new UnityEngine.Vector3(x, y, z);
}
@@ -152,22 +152,22 @@ namespace LinearAlgebra
/// Convert the direction into a carthesian vector
///
/// The carthesian vector corresponding to this direction.
- public Vector3Float ToVector3()
- {
+ public readonly Vector3Float ToVector3() {
// Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0);
// Vector3Float v = q * Vector3Float.forward;
// return v;
- // Convert degrees to radians
+
float radH = this.horizontal.inRadians;
float radV = this.vertical.inRadians;
- // Calculate Vector
- float x = MathF.Cos(radV) * MathF.Cos(radH);
- float y = MathF.Sin(radV);
- float z = MathF.Cos(radV) * MathF.Sin(radH);
+ float cosV = MathF.Cos(radV);
+ float sinV = MathF.Sin(radV);
- return new Vector3Float(x, y, z);
+ float horizontal = cosV * MathF.Sin(radH);
+ float vertical = sinV;
+ float depth = cosV * MathF.Cos(radH);
+ return new Vector3Float(horizontal, vertical, depth);
}
///
@@ -229,8 +229,7 @@ namespace LinearAlgebra
return HashCode.Combine(horizontal, vertical);
}
- public static AngleFloat UnsignedAngle(Direction d1, Direction d2)
- {
+ public static AngleFloat UnsignedAngle(Direction d1, Direction d2) {
// Convert angles from degrees to radians
float horizontal1Rad = d1.horizontal.inRadians;
float vertical1Rad = d1.vertical.inRadians;
diff --git a/Assets/NanoBrain/LinearAlgebra/src/Quaternion.cs b/Assets/NanoBrain/LinearAlgebra/src/Quaternion.cs
index 670c0a4..7936843 100644
--- a/Assets/NanoBrain/LinearAlgebra/src/Quaternion.cs
+++ b/Assets/NanoBrain/LinearAlgebra/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/Assets/NanoBrain/LinearAlgebra/src/Spherical.cs b/Assets/NanoBrain/LinearAlgebra/src/Spherical.cs
index db829dc..3a76085 100644
--- a/Assets/NanoBrain/LinearAlgebra/src/Spherical.cs
+++ b/Assets/NanoBrain/LinearAlgebra/src/Spherical.cs
@@ -87,13 +87,11 @@ namespace LinearAlgebra {
return v;
}
#else
- public static Spherical FromVector3(Vector3Float v)
- {
+ public static Spherical FromVector3(Vector3Float v) {
float distance = v.magnitude;
if (distance == 0.0f)
return Spherical.zero;
- else
- {
+ 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);
@@ -178,36 +176,202 @@ namespace LinearAlgebra {
return distance;
}
- public static Spherical Average(List vectors) {
- float sumSinPhiCosTheta = 0.0f;
- float sumSinPhiSinTheta = 0.0f;
- float sumCosPhi = 0.0f;
+ public static Spherical Average(Spherical v1, Spherical v2) {
+ const float EPS = 1e-6f;
- int n = vectors.Count;
+ // 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;
- // 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);
+ // 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);
}
- // Step 2: Calculate average components
- float avgSinPhiCosTheta = sumSinPhiCosTheta / n;
- float avgSinPhiSinTheta = sumSinPhiSinTheta / n;
- float avgCosPhi = sumCosPhi / n;
+ // Horizontal unit-circle sum
+ float cx = MathF.Cos(a1) + MathF.Cos(a2);
+ float cy = MathF.Sin(a1) + MathF.Sin(a2);
- // Step 3: Calculate the magnitude of the average vector
- float rAvg = MathF.Sqrt(avgSinPhiCosTheta * avgSinPhiCosTheta +
- avgSinPhiSinTheta * avgSinPhiSinTheta +
- avgCosPhi * avgCosPhi);
+ // Vertical as z = sin(el)
+ float z1 = MathF.Sin(e1);
+ float z2 = MathF.Sin(e2);
+ float cz = z1 + z2;
- // Step 4: Calculate average angles
- AngleFloat horizontalAvg = AngleFloat.Acos(avgCosPhi / rAvg); // Handle rAvg != 0 case
- AngleFloat verticalAvg = AngleFloat.Atan2(avgSinPhiSinTheta, avgSinPhiCosTheta);
+ // 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);
- return new Spherical(rAvg, new Direction(horizontalAvg, verticalAvg));
+ // 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/Assets/NanoBrain/LinearAlgebra/test/AngleTest.cs b/Assets/NanoBrain/LinearAlgebra/test/AngleTest.cs
index 787130d..8362d82 100644
--- a/Assets/NanoBrain/LinearAlgebra/test/AngleTest.cs
+++ b/Assets/NanoBrain/LinearAlgebra/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/Assets/NanoBrain/LinearAlgebra/test/DirectionTest.cs b/Assets/NanoBrain/LinearAlgebra/test/DirectionTest.cs
index 146c9df..8fe3b93 100644
--- a/Assets/NanoBrain/LinearAlgebra/test/DirectionTest.cs
+++ b/Assets/NanoBrain/LinearAlgebra/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/Assets/NanoBrain/LinearAlgebra/test/SphericalTest.cs b/Assets/NanoBrain/LinearAlgebra/test/SphericalTest.cs
index 125cdb1..d7553dd 100644
--- a/Assets/NanoBrain/LinearAlgebra/test/SphericalTest.cs
+++ b/Assets/NanoBrain/LinearAlgebra/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