From 78cb6449202d74b16ccd0b3379a6beac6121fbcb Mon Sep 17 00:00:00 2001 From: orange Date: Sun, 1 Mar 2026 08:23:26 +0300 Subject: [PATCH] added files --- include/omath/linear_algebra/quaternion.hpp | 219 +++++++++++ include/omath/omath.hpp | 3 + tests/general/unit_test_quaternion.cpp | 402 ++++++++++++++++++++ 3 files changed, 624 insertions(+) create mode 100644 include/omath/linear_algebra/quaternion.hpp create mode 100644 tests/general/unit_test_quaternion.cpp diff --git a/include/omath/linear_algebra/quaternion.hpp b/include/omath/linear_algebra/quaternion.hpp new file mode 100644 index 00000000..d943f351 --- /dev/null +++ b/include/omath/linear_algebra/quaternion.hpp @@ -0,0 +1,219 @@ +// +// Created by vlad on 3/1/2026. +// +#pragma once + +#include "omath/linear_algebra/mat.hpp" +#include "omath/linear_algebra/vector3.hpp" +#include +#include +#include + +namespace omath +{ + template + requires std::is_arithmetic_v + class Quaternion + { + public: + using ContainedType = Type; + + Type x = static_cast(0); + Type y = static_cast(0); + Type z = static_cast(0); + Type w = static_cast(1); // identity quaternion + + constexpr Quaternion() noexcept = default; + + constexpr Quaternion(const Type& x, const Type& y, const Type& z, const Type& w) noexcept + : x(x), y(y), z(z), w(w) + { + } + + // Factory: build from a normalized axis and an angle in radians + [[nodiscard]] + static Quaternion from_axis_angle(const Vector3& axis, const Type& angle_rad) noexcept + { + const Type half = angle_rad / static_cast(2); + const Type s = std::sin(half); + return {axis.x * s, axis.y * s, axis.z * s, std::cos(half)}; + } + + [[nodiscard]] constexpr bool operator==(const Quaternion& other) const noexcept + { + return x == other.x && y == other.y && z == other.z && w == other.w; + } + + [[nodiscard]] constexpr bool operator!=(const Quaternion& other) const noexcept + { + return !(*this == other); + } + + // Hamilton product: this * other + [[nodiscard]] constexpr Quaternion operator*(const Quaternion& other) const noexcept + { + return { + w * other.x + x * other.w + y * other.z - z * other.y, + w * other.y - x * other.z + y * other.w + z * other.x, + w * other.z + x * other.y - y * other.x + z * other.w, + w * other.w - x * other.x - y * other.y - z * other.z, + }; + } + + constexpr Quaternion& operator*=(const Quaternion& other) noexcept + { + return *this = *this * other; + } + + [[nodiscard]] constexpr Quaternion operator*(const Type& scalar) const noexcept + { + return {x * scalar, y * scalar, z * scalar, w * scalar}; + } + + constexpr Quaternion& operator*=(const Type& scalar) noexcept + { + x *= scalar; + y *= scalar; + z *= scalar; + w *= scalar; + return *this; + } + + [[nodiscard]] constexpr Quaternion operator+(const Quaternion& other) const noexcept + { + return {x + other.x, y + other.y, z + other.z, w + other.w}; + } + + constexpr Quaternion& operator+=(const Quaternion& other) noexcept + { + x += other.x; + y += other.y; + z += other.z; + w += other.w; + return *this; + } + + [[nodiscard]] constexpr Quaternion operator-() const noexcept + { + return {-x, -y, -z, -w}; + } + + // Conjugate: negates the vector part (x, y, z) + [[nodiscard]] constexpr Quaternion conjugate() const noexcept + { + return {-x, -y, -z, w}; + } + + [[nodiscard]] constexpr Type dot(const Quaternion& other) const noexcept + { + return x * other.x + y * other.y + z * other.z + w * other.w; + } + + [[nodiscard]] constexpr Type length_sqr() const noexcept + { + return x * x + y * y + z * z + w * w; + } + +#ifndef _MSC_VER + [[nodiscard]] constexpr Type length() const noexcept + { + return std::sqrt(length_sqr()); + } + + [[nodiscard]] constexpr Quaternion normalized() const noexcept + { + const Type len = length(); + return len != static_cast(0) ? *this * (static_cast(1) / len) : *this; + } +#else + [[nodiscard]] Type length() const noexcept + { + return std::sqrt(length_sqr()); + } + + [[nodiscard]] Quaternion normalized() const noexcept + { + const Type len = length(); + return len != static_cast(0) ? *this * (static_cast(1) / len) : *this; + } +#endif + + // Inverse: q* / |q|^2 (for unit quaternions inverse == conjugate) + [[nodiscard]] constexpr Quaternion inverse() const noexcept + { + return conjugate() * (static_cast(1) / length_sqr()); + } + + // Rotate a 3D vector: v' = q * pure(v) * q^-1 + // Computed via Rodrigues' formula to avoid full quaternion product overhead + [[nodiscard]] constexpr Vector3 rotate(const Vector3& v) const noexcept + { + const Vector3 q_vec{x, y, z}; + const Vector3 cross = q_vec.cross(v); + return v + cross * (static_cast(2) * w) + q_vec.cross(cross) * static_cast(2); + } + + // 3x3 rotation matrix from this (unit) quaternion + [[nodiscard]] constexpr Mat<3, 3, Type> to_rotation_matrix3() const noexcept + { + const Type xx = x * x, yy = y * y, zz = z * z; + const Type xy = x * y, xz = x * z, yz = y * z; + const Type wx = w * x, wy = w * y, wz = w * z; + const Type one = static_cast(1); + const Type two = static_cast(2); + + return { + {one - two * (yy + zz), two * (xy - wz), two * (xz + wy) }, + {two * (xy + wz), one - two * (xx + zz), two * (yz - wx) }, + {two * (xz - wy), two * (yz + wx), one - two * (xx + yy)}, + }; + } + + // 4x4 rotation matrix (with homogeneous row/column) + [[nodiscard]] constexpr Mat<4, 4, Type> to_rotation_matrix4() const noexcept + { + const Type xx = x * x, yy = y * y, zz = z * z; + const Type xy = x * y, xz = x * z, yz = y * z; + const Type wx = w * x, wy = w * y, wz = w * z; + const Type one = static_cast(1); + const Type two = static_cast(2); + const Type zero = static_cast(0); + + return { + {one - two * (yy + zz), two * (xy - wz), two * (xz + wy), zero}, + {two * (xy + wz), one - two * (xx + zz), two * (yz - wx), zero}, + {two * (xz - wy), two * (yz + wx), one - two * (xx + yy), zero}, + {zero, zero, zero, one }, + }; + } + + [[nodiscard]] constexpr std::array as_array() const noexcept + { + return {x, y, z, w}; + } + }; +} // namespace omath + +template +struct std::formatter> // NOLINT(*-dcl58-cpp) +{ + [[nodiscard]] + static constexpr auto parse(std::format_parse_context& ctx) + { + return ctx.begin(); + } + + template + [[nodiscard]] + static auto format(const omath::Quaternion& q, FormatContext& ctx) + { + if constexpr (std::is_same_v) + return std::format_to(ctx.out(), "[{}, {}, {}, {}]", q.x, q.y, q.z, q.w); + + if constexpr (std::is_same_v) + return std::format_to(ctx.out(), L"[{}, {}, {}, {}]", q.x, q.y, q.z, q.w); + + if constexpr (std::is_same_v) + return std::format_to(ctx.out(), u8"[{}, {}, {}, {}]", q.x, q.y, q.z, q.w); + } +}; diff --git a/include/omath/omath.hpp b/include/omath/omath.hpp index a9615f9b..bcf503a3 100644 --- a/include/omath/omath.hpp +++ b/include/omath/omath.hpp @@ -17,6 +17,9 @@ // Matrix classes #include "omath/linear_algebra/mat.hpp" +// Quaternion +#include "omath/linear_algebra/quaternion.hpp" + // Color functionality #include "omath/utility/color.hpp" diff --git a/tests/general/unit_test_quaternion.cpp b/tests/general/unit_test_quaternion.cpp new file mode 100644 index 00000000..3c42d335 --- /dev/null +++ b/tests/general/unit_test_quaternion.cpp @@ -0,0 +1,402 @@ +// +// Created by vlad on 3/1/2026. +// +#include +#include +#include +#include + +using namespace omath; + +static constexpr float kEps = 1e-5f; + +// ── Helpers ────────────────────────────────────────────────────────────────── + +static void expect_quat_near(const Quaternion& a, const Quaternion& 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& a, const Vector3& 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 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 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 q{0.0, 0.0, 0.0, 1.0}; + EXPECT_DOUBLE_EQ(q.w, 1.0); +} + +// ── Equality ───────────────────────────────────────────────────────────────── + +TEST(Quaternion, EqualityOperators) +{ + constexpr Quaternion a{1.f, 2.f, 3.f, 4.f}; + constexpr Quaternion b{1.f, 2.f, 3.f, 4.f}; + constexpr Quaternion 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 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 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 a{1.f, 2.f, 3.f, 4.f}; + constexpr Quaternion 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 a{1.f, 0.f, 0.f, 0.f}; + const Quaternion 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 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 identity; + constexpr Quaternion 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 identity; + Quaternion 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 i{1.f, 0.f, 0.f, 0.f}; + constexpr Quaternion 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 q = Quaternion::from_axis_angle({0.f, 0.f, 1.f}, + std::numbers::pi_v / 3.f); + const auto result = q * q.inverse(); + expect_quat_near(result, Quaternion{}); +} + +// ── Conjugate ──────────────────────────────────────────────────────────────── + +TEST(Quaternion, Conjugate) +{ + constexpr Quaternion 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 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 a{1.f, 0.f, 0.f, 0.f}; + constexpr Quaternion 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 id; + EXPECT_FLOAT_EQ(id.length_sqr(), 1.f); +} + +TEST(Quaternion, LengthSqrGeneral) +{ + constexpr Quaternion q{1.f, 2.f, 3.f, 4.f}; + EXPECT_FLOAT_EQ(q.length_sqr(), 30.f); +} + +TEST(Quaternion, LengthIdentity) +{ + const Quaternion id; + EXPECT_NEAR(id.length(), 1.f, kEps); +} + +TEST(Quaternion, Normalized) +{ + const Quaternion 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 unit; + const auto n = unit.normalized(); + expect_quat_near(n, unit); +} + +// ── Inverse ─────────────────────────────────────────────────────────────────── + +TEST(Quaternion, InverseOfUnitIsConjugate) +{ + const Quaternion q = Quaternion::from_axis_angle({1.f, 0.f, 0.f}, + std::numbers::pi_v / 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::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 / 2.f; + const auto q = Quaternion::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 id; + constexpr Vector3 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::from_axis_angle({0.f, 0.f, 1.f}, std::numbers::pi_v / 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::from_axis_angle({0.f, 1.f, 0.f}, std::numbers::pi_v); + 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::from_axis_angle({1.f, 0.f, 0.f}, std::numbers::pi_v / 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 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::from_axis_angle({0.f, 0.f, 1.f}, std::numbers::pi_v / 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::from_axis_angle({1.f, 1.f, 0.f}, std::numbers::pi_v / 3.f); + const Vector3 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 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::from_axis_angle({1.f, 0.f, 0.f}, std::numbers::pi_v / 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::from_axis_angle({0.f, 1.f, 0.f}, std::numbers::pi_v / 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 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 q{1.f, 2.f, 3.f, 4.f}; + const auto s = std::format("{}", q); + EXPECT_EQ(s, "[1, 2, 3, 4]"); +}