NanoBrain-unitypackage/LinearAlgebra/test/SphericalTest.cs

//#if !UNITY_5_6_OR_NEWER
using System;
using System.Collections.Generic;
using NUnit.Framework;

namespace LinearAlgebra.Test {
    public class SphericalTest {
        [SetUp]
        public void Setup() {
        }

        [Test]
        public void FromVector3() {
#if UNITY_5_6_OR_NEWER            
            UnityEngine.Vector3 v = new(0, 0, 1);
#else            
            Vector3Float v = new(0, 0, 1);
#endif            
            Spherical s = Spherical.FromVector3(v);
            Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1");
            Assert.AreEqual(0.0f, s.direction.horizontal.inDegrees, "s.hor 0 0 1");
            Assert.AreEqual(0.0f, s.direction.vertical.inDegrees, 1.0E-05F, "s.vert 0 0 1");

            v = new(0, 1, 0);
            s = Spherical.FromVector3(v);
            Assert.AreEqual(1.0f, s.distance, "s.distance 0 1 0");
            Assert.AreEqual(0.0f, s.direction.horizontal.inDegrees, "s.hor 0 1 0");
            Assert.AreEqual(90.0f, s.direction.vertical.inDegrees, "s.vert 0 1 0");

            v = new(1, 0, 0);
            s = Spherical.FromVector3(v);
            Assert.AreEqual(1.0f, s.distance, "s.distance 1 0 0");
            Assert.AreEqual(90.0f, s.direction.horizontal.inDegrees, "s.hor 1 0 0");
            Assert.AreEqual(0.0f, s.direction.vertical.inDegrees, 1.0E-05F, "s.vert 1 0 0");
        }

        [Test]
        public void Addition() {
            Spherical v1 = Spherical.Degrees(1, 45, 0);
            Spherical v2 = Spherical.zero;
            Spherical r = Spherical.zero;

            r = v1 + v2;
            Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)");

            r = v1;
            r += v2;
            Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)");

            v2 = Spherical.Degrees(1, 0, 90);
            r = v1 + v2;
            Assert.AreEqual(Math.Sqrt(2), r.distance, 1.0E-05F, "Addition(1 0 90)");
            Assert.AreEqual(45.0f, r.direction.horizontal.inDegrees, 1e-5f, "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(new List<Spherical> { 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(new List<Spherical> { 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(new List<Spherical> { 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);
        }

        [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<Spherical> dirs = new List<Spherical> {
                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);

#if UNITY_5_3_OR_NEWER
            UnityEngine.Vector3 r = UnityEngine.Vector3.zero;
#else
            Vector3Float r = Vector3Float.zero;
#endif            
            foreach (Spherical dir in dirs) {
                r += dir.ToVector3();
            }
            r = r / 4;
            Spherical avg2 = Spherical.FromVector3(r);

            Assert.AreEqual(avg, avg2);
        }
        
    }
}
//#endif