JoltPhysics/UnitTests/Math/QuatTests.cpp

// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT

#include "UnitTestFramework.h"
#include <Math/Mat44.h>
#include <Math/Quat.h>
#include <random>

TEST_SUITE("QuatTests")
{
	TEST_CASE("TestQuatIdentity")
	{
		Quat identity = Quat::sIdentity();

		CHECK_APPROX_EQUAL(identity.GetX(), 0.0f);
		CHECK_APPROX_EQUAL(identity.GetY(), 0.0f);
		CHECK_APPROX_EQUAL(identity.GetZ(), 0.0f);
		CHECK_APPROX_EQUAL(identity.GetW(), 1.0f);
	}

	TEST_CASE("TestQuatConvertMatrix")
	{
		UnitTestRandom random;
		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
		for (int i = 0; i < 1000; ++i)
		{
			Vec3 axis = Vec3::sRandom(random);
			float angle = zero_to_two_pi(random);

			Mat44 m1 = Mat44::sRotation(axis, angle);
			Quat q1 = m1.GetQuaternion();
			Quat q2 = Quat::sRotation(axis, angle);
			CHECK_APPROX_EQUAL(q1, q2);
			Mat44 m2 = Mat44::sRotation(q2);
			CHECK_APPROX_EQUAL(m1, m2);
		}
	}

	TEST_CASE("TestQuatMultiplyVec3")
	{
		UnitTestRandom random;
		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
		for (int i = 0; i < 1000; ++i)
		{
			Vec3 axis = Vec3::sRandom(random);
			float angle = zero_to_two_pi(random);
			Mat44 m1 = Mat44::sRotation(axis, angle);
			Quat q1 = Quat::sRotation(axis, angle);

			Vec3 rv = 10.0f * Vec3::sRandom(random);
			Vec3 r1 = m1 * rv;
			Vec3 r2 = q1 * rv;
			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);
		}
	}

	TEST_CASE("TestQuatRotateAxisXYZ")
	{
		UnitTestRandom random;
		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
		for (int i = 0; i < 1000; ++i)
		{
			Vec3 axis = Vec3::sRandom(random);
			float angle = zero_to_two_pi(random);
			Quat q1 = Quat::sRotation(axis, angle);

			Vec3 r1 = q1 * Vec3::sAxisX();
			Vec3 r2 = q1.RotateAxisX();
			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);

			r1 = q1 * Vec3::sAxisY();
			r2 = q1.RotateAxisY();
			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);

			r1 = q1 * Vec3::sAxisZ();
			r2 = q1.RotateAxisZ();
			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);
		}
	}

	TEST_CASE("TestQuatMultiplyQuat")
	{
		{
			// We use a right handed system, so test that: i * j = k
			Quat r1 = Quat(1, 0, 0, 0) * Quat(0, 1, 0, 0);
			Quat r2 = Quat(0, 0, 1, 0);
			CHECK(r1.IsClose(r2));
		}

		{
			// Test: j * i = -k
			Quat r1 = Quat(0, 1, 0, 0) * Quat(1, 0, 0, 0);
			Quat r2 = Quat(0, 0, -1, 0);
			CHECK(r1.IsClose(r2));
		}

		{
			// Test predefined multiplication
			Quat r1 = Quat(2, 3, 4, 1) * Quat(6, 7, 8, 5);
			Quat r2 = Quat(12, 30, 24, -60);
			CHECK(r1.IsClose(r2));
		}

		// Compare random matrix multiplications with quaternion multiplications
		UnitTestRandom random;
		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
		for (int i = 0; i < 1000; ++i)
		{
			Vec3 axis1 = Vec3::sRandom(random);
			float angle1 = zero_to_two_pi(random);
			Quat q1 = Quat::sRotation(axis1, angle1);
			Mat44 m1 = Mat44::sRotation(axis1, angle1);

			Vec3 axis2 = Vec3::sRandom(random);
			float angle2 = zero_to_two_pi(random);
			Quat q2 = Quat::sRotation(axis2, angle2);
			Mat44 m2 = Mat44::sRotation(axis2, angle2);

			Quat r1 = q1 * q2;
			Quat r2 = (m1 * m2).GetQuaternion();

			CHECK_APPROX_EQUAL(r1, r2);
		}
	}

	TEST_CASE("TestQuatRotationAxisAngle")
	{
		Mat44 r1 = Mat44::sRotation(Vec3(1, 0, 0), 0.1f * JPH_PI);
		Mat44 r2 = Mat44::sRotation(Quat::sRotation(Vec3(1, 0, 0), 0.1f * JPH_PI));
		CHECK_APPROX_EQUAL(r1, r2);

		r1 = Mat44::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);
		r2 = Mat44::sRotation(Quat::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI));
		CHECK_APPROX_EQUAL(r1, r2);

		r1 = Mat44::sRotation(Vec3(0, 0, 1), 0.3f * JPH_PI);
		r2 = Mat44::sRotation(Quat::sRotation(Vec3(0, 0, 1), 0.3f * JPH_PI));
		CHECK_APPROX_EQUAL(r1, r2);
	}

	TEST_CASE("TestQuatGetAxisAngle")
	{
		// Test identity rotation
		{
			Vec3 axis;
			float angle;
			Quat::sIdentity().GetAxisAngle(axis, angle);
			CHECK_APPROX_EQUAL(Vec3::sZero(), axis);
			CHECK_APPROX_EQUAL(0.0f, angle);
		}

		{
			Vec3 axis;
			float angle;
			(-Quat::sIdentity()).GetAxisAngle(axis, angle);
			CHECK_APPROX_EQUAL(Vec3::sZero(), axis);
			CHECK_APPROX_EQUAL(0.0f, angle);
		}

		// Test positive rotation
		Quat q1 = Quat::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);

		{
			Vec3 axis;
			float angle;
			q1.GetAxisAngle(axis, angle);
			CHECK_APPROX_EQUAL(Vec3(0, 1, 0), axis);
			CHECK_APPROX_EQUAL(0.2f * JPH_PI, angle, 1.0e-5f);
		}

		{
			Vec3 axis;
			float angle;
			(-q1).GetAxisAngle(axis, angle);
			CHECK_APPROX_EQUAL(Vec3(0, 1, 0), axis);
			CHECK_APPROX_EQUAL(0.2f * JPH_PI, angle, 1.0e-5f);
		}

		// Test negative rotation
		Quat q2 = Quat::sRotation(Vec3(0, 1, 0), -0.2f * JPH_PI);

		{
			Vec3 axis;
			float angle;
			q2.GetAxisAngle(axis, angle);
			CHECK_APPROX_EQUAL(Vec3(0, -1, 0), axis);
			CHECK_APPROX_EQUAL(0.2f * JPH_PI, angle, 1.0e-5f);
		}

		{
			Vec3 axis;
			float angle;
			(-q2).GetAxisAngle(axis, angle);
			CHECK_APPROX_EQUAL(Vec3(0, -1, 0), axis);
			CHECK_APPROX_EQUAL(0.2f * JPH_PI, angle, 1.0e-5f);
		}

		// Test keeping range between [0, PI]
		Quat q3 = Quat::sRotation(Vec3(0, 1, 0), 1.1f * JPH_PI);

		{
			Vec3 axis;
			float angle;
			q3.GetAxisAngle(axis, angle);
			CHECK_APPROX_EQUAL(Vec3(0, -1, 0), axis);
			CHECK_APPROX_EQUAL(0.9f * JPH_PI, angle, 1.0e-5f);
		}

		{
			Vec3 axis;
			float angle;
			(-q3).GetAxisAngle(axis, angle);
			CHECK_APPROX_EQUAL(Vec3(0, -1, 0), axis);
			CHECK_APPROX_EQUAL(0.9f * JPH_PI, angle, 1.0e-5f);
		}
	}

	TEST_CASE("TestQuatInverse")
	{
		UnitTestRandom random;
		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
		for (int i = 0; i < 1000; ++i)
		{
			Vec3 axis = Vec3::sRandom(random);
			float angle = zero_to_two_pi(random);

			Quat q1 = Quat::sRotation(axis, angle);
			Quat q2 = q1.Inversed();

			CHECK_APPROX_EQUAL(Quat::sIdentity(), q1 * q2);
		}
	}

	TEST_CASE("TestQuatGetTwistAxis")
	{
		Quat q1 = Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(-10.0f));
		Quat q2 = Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(20.0f));
		Quat q = q1 * q2;

		Quat twist1 = q.GetTwist(Vec3::sAxisX());
		CHECK_APPROX_EQUAL(twist1, q1);
		Quat swing1 = twist1.Inversed() * q;
		CHECK_APPROX_EQUAL(swing1, q2);
		Quat twist2 = swing1.GetTwist(Vec3::sAxisY());
		CHECK_APPROX_EQUAL(twist2, q2);
		Quat swing2 = twist2.Inversed() * swing1;
		CHECK_APPROX_EQUAL(swing2, Quat::sIdentity());
	}

	TEST_CASE("TestQuatGetRotationAngle")
	{
		Quat q1 = Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(-10.0f));
		Quat q2 = Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(20.0f));
		Quat q3 = Quat::sRotation(Vec3::sAxisZ(), DegreesToRadians(-95.0f));

		float a = q1.GetRotationAngle(Vec3::sAxisX());
		CHECK_APPROX_EQUAL(a, DegreesToRadians(-10.0f), 1.0e-5f);

		a = q2.GetRotationAngle(Vec3::sAxisY());
		CHECK_APPROX_EQUAL(a, DegreesToRadians(20.0f), 1.0e-5f);

		a = q3.GetRotationAngle(Vec3::sAxisZ());
		CHECK_APPROX_EQUAL(a, DegreesToRadians(-95.0f), 1.0e-5f);

		a = (q1 * q2).GetRotationAngle(Vec3::sAxisX());
		CHECK_APPROX_EQUAL(a, DegreesToRadians(-10.0f), 1.0e-5f);

		a = (q3 * q1).GetRotationAngle(Vec3::sAxisX());
		CHECK_APPROX_EQUAL(a, DegreesToRadians(-10.0f), 1.0e-5f);
	}

	TEST_CASE("TestQuatGetEulerAngles")
	{
		Vec3 input(DegreesToRadians(-10.0f), DegreesToRadians(20.0f), DegreesToRadians(-95.0f));

		Quat qx = Quat::sRotation(Vec3::sAxisX(), input.GetX());
		Quat qy = Quat::sRotation(Vec3::sAxisY(), input.GetY());
		Quat qz = Quat::sRotation(Vec3::sAxisZ(), input.GetZ());
		Quat q = qz * qy * qx;

		Quat q2 = Quat::sEulerAngles(input);
		CHECK_APPROX_EQUAL(q, q2);

		Vec3 angles = q2.GetEulerAngles();
		CHECK_APPROX_EQUAL(angles, input);
	}

	TEST_CASE("TestQuatRotationFromTo")
	{
		{
			Vec3 v1(10, 0, 0);
			Vec3 v2(20, 0, 0);
			Quat q = Quat::sFromTo(v1, v2);
			CHECK_APPROX_EQUAL(q, Quat::sIdentity());
		}

		{
			Vec3 v1(10, 0, 0);
			Vec3 v2(0, 20, 0);
			Quat q = Quat::sFromTo(v1, v2);
			CHECK_APPROX_EQUAL(v2.Normalized(), (q * v1).Normalized());
		}

		{
			Vec3 v1(10, 0, 0);
			Vec3 v2(-20, 0, 0);
			Quat q = Quat::sFromTo(v1, v2);
			CHECK_APPROX_EQUAL(v2.Normalized(), (q * v1).Normalized());
		}
	}

	TEST_CASE("TestQuatRotationFromToRandom")
	{
		UnitTestRandom random;
		uniform_real_distribution<float> one_to_ten(1.0f, 10.0f);
		for (int i = 0; i < 1000; ++i)
		{
			Vec3 v1 = one_to_ten(random) * Vec3::sRandom(random);
			Vec3 v2 = one_to_ten(random) * Vec3::sRandom(random);

			Quat q = Quat::sFromTo(v1, v2);

			Vec3 v1t = (q * v1).Normalized();
			Vec3 v2t = v2.Normalized();
			CHECK_APPROX_EQUAL(v2t, v1t, 1.0e-5f);
		}
	}
}