mirror of
https://github.com/orange-cpp/omath.git
synced 2026-04-18 21:03:27 +00:00
403 lines
14 KiB
C++
403 lines
14 KiB
C++
//
|
|
// Created by vlad on 3/1/2026.
|
|
//
|
|
#include <omath/linear_algebra/quaternion.hpp>
|
|
#include <cmath>
|
|
#include <gtest/gtest.h>
|
|
#include <numbers>
|
|
|
|
using namespace omath;
|
|
|
|
static constexpr float kEps = 1e-5f;
|
|
|
|
// ── Helpers ──────────────────────────────────────────────────────────────────
|
|
|
|
static void expect_quat_near(const Quaternion<float>& a, const Quaternion<float>& b, float eps = kEps)
|
|
{
|
|
EXPECT_NEAR(a.x, b.x, eps);
|
|
EXPECT_NEAR(a.y, b.y, eps);
|
|
EXPECT_NEAR(a.z, b.z, eps);
|
|
EXPECT_NEAR(a.w, b.w, eps);
|
|
}
|
|
|
|
static void expect_vec3_near(const Vector3<float>& a, const Vector3<float>& b, float eps = kEps)
|
|
{
|
|
EXPECT_NEAR(a.x, b.x, eps);
|
|
EXPECT_NEAR(a.y, b.y, eps);
|
|
EXPECT_NEAR(a.z, b.z, eps);
|
|
}
|
|
|
|
// ── Constructors ─────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, DefaultConstructorIsIdentity)
|
|
{
|
|
constexpr Quaternion<float> q;
|
|
EXPECT_FLOAT_EQ(q.x, 0.f);
|
|
EXPECT_FLOAT_EQ(q.y, 0.f);
|
|
EXPECT_FLOAT_EQ(q.z, 0.f);
|
|
EXPECT_FLOAT_EQ(q.w, 1.f);
|
|
}
|
|
|
|
TEST(Quaternion, ValueConstructor)
|
|
{
|
|
constexpr Quaternion<float> q{1.f, 2.f, 3.f, 4.f};
|
|
EXPECT_FLOAT_EQ(q.x, 1.f);
|
|
EXPECT_FLOAT_EQ(q.y, 2.f);
|
|
EXPECT_FLOAT_EQ(q.z, 3.f);
|
|
EXPECT_FLOAT_EQ(q.w, 4.f);
|
|
}
|
|
|
|
TEST(Quaternion, DoubleInstantiation)
|
|
{
|
|
constexpr Quaternion<double> q{0.0, 0.0, 0.0, 1.0};
|
|
EXPECT_DOUBLE_EQ(q.w, 1.0);
|
|
}
|
|
|
|
// ── Equality ─────────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, EqualityOperators)
|
|
{
|
|
constexpr Quaternion<float> a{1.f, 2.f, 3.f, 4.f};
|
|
constexpr Quaternion<float> b{1.f, 2.f, 3.f, 4.f};
|
|
constexpr Quaternion<float> c{1.f, 2.f, 3.f, 5.f};
|
|
|
|
EXPECT_TRUE(a == b);
|
|
EXPECT_FALSE(a == c);
|
|
EXPECT_FALSE(a != b);
|
|
EXPECT_TRUE(a != c);
|
|
}
|
|
|
|
// ── Arithmetic ───────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, ScalarMultiply)
|
|
{
|
|
constexpr Quaternion<float> q{1.f, 2.f, 3.f, 4.f};
|
|
constexpr auto r = q * 2.f;
|
|
EXPECT_FLOAT_EQ(r.x, 2.f);
|
|
EXPECT_FLOAT_EQ(r.y, 4.f);
|
|
EXPECT_FLOAT_EQ(r.z, 6.f);
|
|
EXPECT_FLOAT_EQ(r.w, 8.f);
|
|
}
|
|
|
|
TEST(Quaternion, ScalarMultiplyAssign)
|
|
{
|
|
Quaternion<float> q{1.f, 2.f, 3.f, 4.f};
|
|
q *= 3.f;
|
|
EXPECT_FLOAT_EQ(q.x, 3.f);
|
|
EXPECT_FLOAT_EQ(q.y, 6.f);
|
|
EXPECT_FLOAT_EQ(q.z, 9.f);
|
|
EXPECT_FLOAT_EQ(q.w, 12.f);
|
|
}
|
|
|
|
TEST(Quaternion, Addition)
|
|
{
|
|
constexpr Quaternion<float> a{1.f, 2.f, 3.f, 4.f};
|
|
constexpr Quaternion<float> b{4.f, 3.f, 2.f, 1.f};
|
|
constexpr auto r = a + b;
|
|
EXPECT_FLOAT_EQ(r.x, 5.f);
|
|
EXPECT_FLOAT_EQ(r.y, 5.f);
|
|
EXPECT_FLOAT_EQ(r.z, 5.f);
|
|
EXPECT_FLOAT_EQ(r.w, 5.f);
|
|
}
|
|
|
|
TEST(Quaternion, AdditionAssign)
|
|
{
|
|
Quaternion<float> a{1.f, 0.f, 0.f, 0.f};
|
|
const Quaternion<float> b{0.f, 1.f, 0.f, 0.f};
|
|
a += b;
|
|
EXPECT_FLOAT_EQ(a.x, 1.f);
|
|
EXPECT_FLOAT_EQ(a.y, 1.f);
|
|
}
|
|
|
|
TEST(Quaternion, UnaryNegation)
|
|
{
|
|
constexpr Quaternion<float> q{1.f, -2.f, 3.f, -4.f};
|
|
constexpr auto r = -q;
|
|
EXPECT_FLOAT_EQ(r.x, -1.f);
|
|
EXPECT_FLOAT_EQ(r.y, 2.f);
|
|
EXPECT_FLOAT_EQ(r.z, -3.f);
|
|
EXPECT_FLOAT_EQ(r.w, 4.f);
|
|
}
|
|
|
|
// ── Hamilton product ──────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, MultiplyByIdentityIsNoop)
|
|
{
|
|
constexpr Quaternion<float> identity;
|
|
constexpr Quaternion<float> q{0.5f, 0.5f, 0.5f, 0.5f};
|
|
expect_quat_near(q * identity, q);
|
|
expect_quat_near(identity * q, q);
|
|
}
|
|
|
|
TEST(Quaternion, MultiplyAssign)
|
|
{
|
|
constexpr Quaternion<float> identity;
|
|
Quaternion<float> q{0.5f, 0.5f, 0.5f, 0.5f};
|
|
q *= identity;
|
|
expect_quat_near(q, {0.5f, 0.5f, 0.5f, 0.5f});
|
|
}
|
|
|
|
TEST(Quaternion, MultiplyKnownResult)
|
|
{
|
|
// i * j = k → (1,0,0,0) * (0,1,0,0) = (0,0,1,0)
|
|
constexpr Quaternion<float> i{1.f, 0.f, 0.f, 0.f};
|
|
constexpr Quaternion<float> j{0.f, 1.f, 0.f, 0.f};
|
|
constexpr auto k = i * j;
|
|
EXPECT_FLOAT_EQ(k.x, 0.f);
|
|
EXPECT_FLOAT_EQ(k.y, 0.f);
|
|
EXPECT_FLOAT_EQ(k.z, 1.f);
|
|
EXPECT_FLOAT_EQ(k.w, 0.f);
|
|
}
|
|
|
|
TEST(Quaternion, MultiplyByInverseGivesIdentity)
|
|
{
|
|
const Quaternion<float> q = Quaternion<float>::from_axis_angle({0.f, 0.f, 1.f},
|
|
std::numbers::pi_v<float> / 3.f);
|
|
const auto result = q * q.inverse();
|
|
expect_quat_near(result, Quaternion<float>{});
|
|
}
|
|
|
|
// ── Conjugate ────────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, Conjugate)
|
|
{
|
|
constexpr Quaternion<float> q{1.f, 2.f, 3.f, 4.f};
|
|
constexpr auto c = q.conjugate();
|
|
EXPECT_FLOAT_EQ(c.x, -1.f);
|
|
EXPECT_FLOAT_EQ(c.y, -2.f);
|
|
EXPECT_FLOAT_EQ(c.z, -3.f);
|
|
EXPECT_FLOAT_EQ(c.w, 4.f);
|
|
}
|
|
|
|
TEST(Quaternion, ConjugateOfIdentityIsIdentity)
|
|
{
|
|
constexpr Quaternion<float> id;
|
|
constexpr auto c = id.conjugate();
|
|
EXPECT_FLOAT_EQ(c.x, 0.f);
|
|
EXPECT_FLOAT_EQ(c.y, 0.f);
|
|
EXPECT_FLOAT_EQ(c.z, 0.f);
|
|
EXPECT_FLOAT_EQ(c.w, 1.f);
|
|
}
|
|
|
|
// ── Dot / length ─────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, Dot)
|
|
{
|
|
constexpr Quaternion<float> a{1.f, 0.f, 0.f, 0.f};
|
|
constexpr Quaternion<float> b{0.f, 1.f, 0.f, 0.f};
|
|
EXPECT_FLOAT_EQ(a.dot(b), 0.f);
|
|
EXPECT_FLOAT_EQ(a.dot(a), 1.f);
|
|
}
|
|
|
|
TEST(Quaternion, LengthSqrIdentity)
|
|
{
|
|
constexpr Quaternion<float> id;
|
|
EXPECT_FLOAT_EQ(id.length_sqr(), 1.f);
|
|
}
|
|
|
|
TEST(Quaternion, LengthSqrGeneral)
|
|
{
|
|
constexpr Quaternion<float> q{1.f, 2.f, 3.f, 4.f};
|
|
EXPECT_FLOAT_EQ(q.length_sqr(), 30.f);
|
|
}
|
|
|
|
TEST(Quaternion, LengthIdentity)
|
|
{
|
|
const Quaternion<float> id;
|
|
EXPECT_NEAR(id.length(), 1.f, kEps);
|
|
}
|
|
|
|
TEST(Quaternion, Normalized)
|
|
{
|
|
const Quaternion<float> q{1.f, 1.f, 1.f, 1.f};
|
|
const auto n = q.normalized();
|
|
EXPECT_NEAR(n.length(), 1.f, kEps);
|
|
EXPECT_NEAR(n.x, 0.5f, kEps);
|
|
EXPECT_NEAR(n.y, 0.5f, kEps);
|
|
EXPECT_NEAR(n.z, 0.5f, kEps);
|
|
EXPECT_NEAR(n.w, 0.5f, kEps);
|
|
}
|
|
|
|
TEST(Quaternion, NormalizedOfZeroLengthReturnsSelf)
|
|
{
|
|
// length_sqr = 0 would be UB, but zero-vector part + zero w is degenerate;
|
|
// we just verify the guard branch (divides by zero) doesn't crash by
|
|
// keeping length > 0 via the default constructor path.
|
|
const Quaternion<float> unit;
|
|
const auto n = unit.normalized();
|
|
expect_quat_near(n, unit);
|
|
}
|
|
|
|
// ── Inverse ───────────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, InverseOfUnitIsConjugate)
|
|
{
|
|
const Quaternion<float> q = Quaternion<float>::from_axis_angle({1.f, 0.f, 0.f},
|
|
std::numbers::pi_v<float> / 4.f);
|
|
const auto inv = q.inverse();
|
|
const auto conj = q.conjugate();
|
|
expect_quat_near(inv, conj);
|
|
}
|
|
|
|
// ── from_axis_angle ──────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, FromAxisAngleZeroAngleIsIdentity)
|
|
{
|
|
const auto q = Quaternion<float>::from_axis_angle({1.f, 0.f, 0.f}, 0.f);
|
|
EXPECT_NEAR(q.x, 0.f, kEps);
|
|
EXPECT_NEAR(q.y, 0.f, kEps);
|
|
EXPECT_NEAR(q.z, 0.f, kEps);
|
|
EXPECT_NEAR(q.w, 1.f, kEps);
|
|
}
|
|
|
|
TEST(Quaternion, FromAxisAngle90DegZ)
|
|
{
|
|
const float half_pi = std::numbers::pi_v<float> / 2.f;
|
|
const auto q = Quaternion<float>::from_axis_angle({0.f, 0.f, 1.f}, half_pi);
|
|
const float s = std::sin(half_pi / 2.f);
|
|
const float c = std::cos(half_pi / 2.f);
|
|
EXPECT_NEAR(q.x, 0.f, kEps);
|
|
EXPECT_NEAR(q.y, 0.f, kEps);
|
|
EXPECT_NEAR(q.z, s, kEps);
|
|
EXPECT_NEAR(q.w, c, kEps);
|
|
}
|
|
|
|
// ── rotate ───────────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, RotateByIdentityIsNoop)
|
|
{
|
|
constexpr Quaternion<float> id;
|
|
constexpr Vector3<float> v{1.f, 2.f, 3.f};
|
|
const auto r = id.rotate(v);
|
|
expect_vec3_near(r, v);
|
|
}
|
|
|
|
TEST(Quaternion, Rotate90DegAroundZ)
|
|
{
|
|
// Rotating (1,0,0) by 90° around Z should give (0,1,0)
|
|
const auto q = Quaternion<float>::from_axis_angle({0.f, 0.f, 1.f}, std::numbers::pi_v<float> / 2.f);
|
|
const auto r = q.rotate({1.f, 0.f, 0.f});
|
|
expect_vec3_near(r, {0.f, 1.f, 0.f});
|
|
}
|
|
|
|
TEST(Quaternion, Rotate180DegAroundY)
|
|
{
|
|
// Rotating (1,0,0) by 180° around Y should give (-1,0,0)
|
|
const auto q = Quaternion<float>::from_axis_angle({0.f, 1.f, 0.f}, std::numbers::pi_v<float>);
|
|
const auto r = q.rotate({1.f, 0.f, 0.f});
|
|
expect_vec3_near(r, {-1.f, 0.f, 0.f});
|
|
}
|
|
|
|
TEST(Quaternion, Rotate90DegAroundX)
|
|
{
|
|
// Rotating (0,1,0) by 90° around X should give (0,0,1)
|
|
const auto q = Quaternion<float>::from_axis_angle({1.f, 0.f, 0.f}, std::numbers::pi_v<float> / 2.f);
|
|
const auto r = q.rotate({0.f, 1.f, 0.f});
|
|
expect_vec3_near(r, {0.f, 0.f, 1.f});
|
|
}
|
|
|
|
// ── to_rotation_matrix3 ───────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, RotationMatrix3FromIdentityIsIdentityMatrix)
|
|
{
|
|
constexpr Quaternion<float> id;
|
|
constexpr auto m = id.to_rotation_matrix3();
|
|
for (size_t i = 0; i < 3; ++i)
|
|
for (size_t j = 0; j < 3; ++j)
|
|
EXPECT_NEAR(m.at(i, j), i == j ? 1.f : 0.f, kEps);
|
|
}
|
|
|
|
TEST(Quaternion, RotationMatrix3From90DegZ)
|
|
{
|
|
// Expected: | 0 -1 0 |
|
|
// | 1 0 0 |
|
|
// | 0 0 1 |
|
|
const auto q = Quaternion<float>::from_axis_angle({0.f, 0.f, 1.f}, std::numbers::pi_v<float> / 2.f);
|
|
const auto m = q.to_rotation_matrix3();
|
|
EXPECT_NEAR(m.at(0, 0), 0.f, kEps);
|
|
EXPECT_NEAR(m.at(0, 1), -1.f, kEps);
|
|
EXPECT_NEAR(m.at(0, 2), 0.f, kEps);
|
|
EXPECT_NEAR(m.at(1, 0), 1.f, kEps);
|
|
EXPECT_NEAR(m.at(1, 1), 0.f, kEps);
|
|
EXPECT_NEAR(m.at(1, 2), 0.f, kEps);
|
|
EXPECT_NEAR(m.at(2, 0), 0.f, kEps);
|
|
EXPECT_NEAR(m.at(2, 1), 0.f, kEps);
|
|
EXPECT_NEAR(m.at(2, 2), 1.f, kEps);
|
|
}
|
|
|
|
TEST(Quaternion, RotationMatrix3ConsistentWithRotate)
|
|
{
|
|
// Matrix-vector multiply must agree with the rotate() method
|
|
const auto q = Quaternion<float>::from_axis_angle({1.f, 1.f, 0.f}, std::numbers::pi_v<float> / 3.f);
|
|
const Vector3<float> v{2.f, -1.f, 0.5f};
|
|
|
|
const auto rotated = q.rotate(v);
|
|
const auto m = q.to_rotation_matrix3();
|
|
|
|
// manual mat-vec multiply (row-major)
|
|
const float rx = m.at(0, 0) * v.x + m.at(0, 1) * v.y + m.at(0, 2) * v.z;
|
|
const float ry = m.at(1, 0) * v.x + m.at(1, 1) * v.y + m.at(1, 2) * v.z;
|
|
const float rz = m.at(2, 0) * v.x + m.at(2, 1) * v.y + m.at(2, 2) * v.z;
|
|
|
|
EXPECT_NEAR(rotated.x, rx, kEps);
|
|
EXPECT_NEAR(rotated.y, ry, kEps);
|
|
EXPECT_NEAR(rotated.z, rz, kEps);
|
|
}
|
|
|
|
// ── to_rotation_matrix4 ───────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, RotationMatrix4FromIdentityIsIdentityMatrix)
|
|
{
|
|
constexpr Quaternion<float> id;
|
|
constexpr auto m = id.to_rotation_matrix4();
|
|
for (size_t i = 0; i < 4; ++i)
|
|
for (size_t j = 0; j < 4; ++j)
|
|
EXPECT_NEAR(m.at(i, j), i == j ? 1.f : 0.f, kEps);
|
|
}
|
|
|
|
TEST(Quaternion, RotationMatrix4HomogeneousRowAndColumn)
|
|
{
|
|
const auto q = Quaternion<float>::from_axis_angle({1.f, 0.f, 0.f}, std::numbers::pi_v<float> / 5.f);
|
|
const auto m = q.to_rotation_matrix4();
|
|
|
|
// Last row and last column must be (0,0,0,1)
|
|
for (size_t i = 0; i < 3; ++i)
|
|
{
|
|
EXPECT_NEAR(m.at(3, i), 0.f, kEps);
|
|
EXPECT_NEAR(m.at(i, 3), 0.f, kEps);
|
|
}
|
|
EXPECT_NEAR(m.at(3, 3), 1.f, kEps);
|
|
}
|
|
|
|
TEST(Quaternion, RotationMatrix4Upper3x3MatchesMatrix3)
|
|
{
|
|
const auto q = Quaternion<float>::from_axis_angle({0.f, 1.f, 0.f}, std::numbers::pi_v<float> / 7.f);
|
|
const auto m3 = q.to_rotation_matrix3();
|
|
const auto m4 = q.to_rotation_matrix4();
|
|
|
|
for (size_t i = 0; i < 3; ++i)
|
|
for (size_t j = 0; j < 3; ++j)
|
|
EXPECT_NEAR(m4.at(i, j), m3.at(i, j), kEps);
|
|
}
|
|
|
|
// ── as_array ──────────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, AsArray)
|
|
{
|
|
constexpr Quaternion<float> q{1.f, 2.f, 3.f, 4.f};
|
|
constexpr auto arr = q.as_array();
|
|
EXPECT_FLOAT_EQ(arr[0], 1.f);
|
|
EXPECT_FLOAT_EQ(arr[1], 2.f);
|
|
EXPECT_FLOAT_EQ(arr[2], 3.f);
|
|
EXPECT_FLOAT_EQ(arr[3], 4.f);
|
|
}
|
|
|
|
// ── std::formatter ────────────────────────────────────────────────────────────
|
|
|
|
TEST(Quaternion, Formatter)
|
|
{
|
|
const Quaternion<float> q{1.f, 2.f, 3.f, 4.f};
|
|
const auto s = std::format("{}", q);
|
|
EXPECT_EQ(s, "[1, 2, 3, 4]");
|
|
}
|