NanoBrain-unitypackage/LinearAlgebra/test/QuaternionTest.cs

#if !UNITY_5_6_OR_NEWER
using NUnit.Framework;

namespace LinearAlgebra.Test {

    public class QuaternionTest {

        [SetUp]
        public void Setup() {
        }

        [Test]
        public void Normalize() {
            Quaternion q1 = new(0, 0, 0, 1);
            Quaternion r = Quaternion.identity;

            r = q1.normalized;
            Assert.AreEqual(r, q1, "q.normalized 0 0 0 1");

            r = Quaternion.Normalize(q1);
            Assert.AreEqual(r, q1, "q.normalized 0 0 0 1");
        }

        [Test]
        public void ToAngles() {
            Quaternion q1 = new(0, 0, 0, 1);
            Vector3Float v = Vector3Float.zero;

            v = Quaternion.ToAngles(q1);
            Assert.AreEqual(v, new Vector3Float(0, 0, 0), "ToAngles 0 0 0 1");

            q1 = new(1, 0, 0, 0);
            v = Quaternion.ToAngles(q1);
            Assert.AreEqual(0, v.horizontal, "1 0 0 0  H");
            Assert.AreEqual(180, v.vertical, "1 0 0 0  V");
            Assert.AreEqual(180, v.depth, "1 0 0 0  D");

        }

        [Test]
        public void Multiplication() {
            Quaternion q1 = new(0, 0, 0, 1);
            Quaternion q2 = new(1, 0, 0, 0);
            Quaternion r;

            r = q1 * q2;
            Assert.AreEqual(r, new Quaternion(1, 0, 0, 0), "0 0 0 1 * 1 0 0 0 ");
        }

        [Test]
        public void MultiplicationVector() {
            Quaternion q1 = new(0, 0, 0, 1);
            Vector3Float v1 = new(0, 1, 0);
            Vector3Float r;

            r = q1 * v1;
            Assert.AreEqual(r, new Vector3Float(0, 1, 0), "0 0 0 1 * Vector 0 1 0");

            q1 = new(1, 0, 0, 0);
            r = q1 * v1;
            Assert.AreEqual(r, new Vector3Float(0, -1, 0), "1 0 0 0 * Vector 0 1 0");
        }

        [Test]
        public void Equality() {
            Quaternion q1 = new(0, 0, 0, 1);
            Quaternion q2 = new(1, 0, 0, 0);
            Assert.AreNotEqual(q1, q2, "0 0 0 1 == 1 0 0 0");

            q2 = new(0, 0, 0, 1);
            Assert.AreEqual(q1, q2, "0 0 0 1 == 1 0 0 0");
        }

        [Test, Ignore("ToDo")]
        public void Inverse() { }

        [Test, Ignore("ToDo")]
        public void LookRotation() { }

        [Test, Ignore("ToDo")]
        public void FromToRotation() { }

        [Test, Ignore("ToDo")]
        public void RotateTowards() { }

        [Test, Ignore("ToDo")]
        public void AngleAxis() { }

        [Test, Ignore("ToDo")]
        public void Angle() { }

        [Test, Ignore("ToDo")]
        public void Slerp() { }

        [Test, Ignore("ToDo")]
        public void SlerpUnclamped() { }

        [Test]
        public void Euler() {
            Vector3Float v1 = new(0, 0, 0);
            Quaternion q;

            q = Quaternion.Euler(v1);
            Assert.AreEqual(q, Quaternion.identity, "Euler Vector 0 0 0");

            q = Quaternion.Euler(0, 0, 0);
            Assert.AreEqual(q, Quaternion.identity, "Euler 0 0 0");

            v1 = new(90, 90, -90);
            q = Quaternion.Euler(v1);
            Assert.AreEqual(q, new Quaternion(0, 0.707106709F, -0.707106709F, 0), "Euler Vector 90 90 -90");

            q = Quaternion.Euler(90, 90, -90);
            Assert.AreEqual(q, new Quaternion(0, 0.707106709F, -0.707106709F, 0), "Euler 90 90 -90");
        }

        [Test]
        public void EulerToAngles() {
            Vector3Float v;
            Quaternion q;
            Quaternion r;

            //v = new(0, 0, 0);
            q = Quaternion.Euler(0, 0 , 0);
            v = Quaternion.ToAngles(q);
            r = Quaternion.Euler(v);
            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "0 0 0");

            q = Quaternion.Euler(-45, -30, -15);
            v = Quaternion.ToAngles(q);
            r = Quaternion.Euler(v);
            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "-45, -30, -15");

            // Gimball lock
            // q = Quaternion.Euler(90, 90, -90);
            // v = Quaternion.ToAngles(q);
            // r = Quaternion.Euler(v);
            // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "0 0 0");

        }

        [Test]
        public void GetAngleAround() {
            Vector3Float v1 = new(0, 1, 0);
            Quaternion q1 = new(0, 0, 0, 1);

            float f = Quaternion.GetAngleAround(v1, q1);
            Assert.AreEqual(f, 0, "GetAngleAround 0 1 0 , 0 0 0 1");

            q1 = new(0, 0.707106709F, -0.707106709F, 0);
            f = Quaternion.GetAngleAround(v1, q1);
            Assert.AreEqual(f, 180, "GetAngleAround 0 1 0 , 0 0.7 -0.7 0");

            v1 = new(0, 0, 0);
            f = Quaternion.GetAngleAround(v1, q1);
            Assert.IsTrue(float.IsNaN(f), "GetAngleAround 0 0 0 , 0 0.7 -0.7 0");
        }

        [Test]
        public void GetRotationAround() {
            Vector3Float v1 = new(0, 1, 0);
            Quaternion q1 = new(0, 0, 0, 1);

            Quaternion q = Quaternion.GetRotationAround(v1, q1);
            Assert.AreEqual(q, new Quaternion(0, 0, 0, 1), "GetRotationAround 0 1 0 , 0 0 0 1");

            q1 = new(0, 0.707106709F, -0.707106709F, 0);
            q = Quaternion.GetRotationAround(v1, q1);
            Assert.AreEqual(q, new Quaternion(0, 1, 0, 0), "GetRotationAround 0 1 0 , 0 0.7 -0.7 0");

            v1 = new(0, 0, 0);
            q = Quaternion.GetRotationAround(v1, q1);
            bool r = float.IsNaN(q.x) && float.IsNaN(q.y) && float.IsNaN(q.z) && float.IsNaN(q.w);
            Assert.IsTrue(r, "GetRotationAround 0 0 0 , 0 0.7 -0.7 0");
        }

        [Test, Ignore("ToDo")]
        public void GetSwingTwist() { }

        [Test, Ignore("ToDo")]
        public void Dot() { }

    }
}
#endif