Merge commit '841d923fed686700610a85aeab6289e44239aa6c'
This commit is contained in:
Pascal Serrarens
commit
f8fc9dabe6
@ -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
|
||||
/// </summary>
|
||||
/// <returns>The carthesian vector corresponding to this direction.</returns>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -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;
|
||||
|
||||
@ -180,11 +180,7 @@ namespace LinearAlgebra {
|
||||
/// <returns>The resulting quaternion</returns>
|
||||
/// 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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@ -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<Spherical> 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);
|
||||
|
||||
// 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<Spherical> 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<Spherical> 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);
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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<Spherical> { 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<Spherical> {
|
||||
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<Spherical> {
|
||||
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
|
||||
Loading…
x
Reference in New Issue
Block a user