diff --git a/docs/fase5/3d-math.md b/docs/fase5/3d-math.md index 0634ab0..b7df89e 100644 --- a/docs/fase5/3d-math.md +++ b/docs/fase5/3d-math.md @@ -3,7 +3,7 @@ > **Fase:** 5 — Transição Dimensional > **Namespace:** `Caffeine::Math` > **Arquivo:** `src/math/Quat.hpp` (extensão de `Mat4.hpp`) -> **Status:** 📅 Planejado +> **Status:** ✅ Implementado > **RF:** RF5.1 --- @@ -143,10 +143,10 @@ struct WorldTransform3D { ## Critério de Aceitação -- [ ] `Quat::slerp` produz interpolação correta entre quaisquer duas orientações -- [ ] `quat.toMatrix()` produz mesma matrix que `Mat4::fromAxisAngle` -- [ ] `Quat * Vec3` rota corretamente (comparado com reference impl) -- [ ] Sem gimbal lock em rotações compostas de 3 eixos +- [x] `Quat::slerp` produz interpolação correta entre quaisquer duas orientações +- [x] `quat.toMatrix()` produz mesma matrix que `Mat4::fromAxisAngle` +- [x] `Quat * Vec3` rota corretamente (comparado com reference impl) +- [x] Sem gimbal lock em rotações compostas de 3 eixos - [ ] `Vec4` acessos SIMD-aligned (alinhado a 16 bytes) --- diff --git a/src/Caffeine.hpp b/src/Caffeine.hpp index 4128ba6..2bbca6b 100644 --- a/src/Caffeine.hpp +++ b/src/Caffeine.hpp @@ -25,6 +25,7 @@ #include "math/Vec4.hpp" #include "math/Mat4.hpp" #include "math/Math.hpp" +#include "math/Quat.hpp" // ECS System #include "ecs/World.hpp" diff --git a/src/math/Math.hpp b/src/math/Math.hpp index 86f359a..1f4aa29 100644 --- a/src/math/Math.hpp +++ b/src/math/Math.hpp @@ -12,6 +12,7 @@ namespace Caffeine { namespace Math { constexpr f32 PI = 3.14159265358979323846f; +constexpr f32 PI_HALF = PI * 0.5f; constexpr f32 TAU = 2.0f * PI; constexpr f32 E = 2.71828182845904523536f; diff --git a/src/math/Quat.hpp b/src/math/Quat.hpp new file mode 100644 index 0000000..fade048 --- /dev/null +++ b/src/math/Quat.hpp @@ -0,0 +1,230 @@ +#pragma once + +#include "../core/Types.hpp" +#include "Vec3.hpp" +#include "Vec4.hpp" +#include "Mat4.hpp" +#include "Math.hpp" +#include + +namespace Caffeine { + +struct Quat { + f32 x = 0.0f; + f32 y = 0.0f; + f32 z = 0.0f; + f32 w = 1.0f; + + Quat() = default; + Quat(f32 x, f32 y, f32 z, f32 w) : x(x), y(y), z(z), w(w) {} + + static Quat identity() { return {0.0f, 0.0f, 0.0f, 1.0f}; } + + static Quat fromAxisAngle(Vec3 axis, f32 angleRad); + static Quat fromEuler(f32 pitchRad, f32 yawRad, f32 rollRad); + static Quat lookAt(Vec3 forward, Vec3 up); + static Quat fromMatrix(const Mat4& m); + + static Quat slerp(Quat a, Quat b, f32 t); + static Quat nlerp(Quat a, Quat b, f32 t); + + Vec3 toEuler() const; + Mat4 toMatrix() const; + + Vec3 rotate(Vec3 v) const; + Quat conjugate() const { return {-x, -y, -z, w}; } + Quat inverse() const; + Quat normalized() const; + f32 length() const; + f32 lengthSquared() const { return x*x + y*y + z*z + w*w; } + f32 dot(Quat o) const { return x*o.x + y*o.y + z*o.z + w*o.w; } + + Quat operator*(Quat o) const; + Vec3 operator*(Vec3 v) const { return rotate(v); } + bool operator==(Quat o) const; +}; + +inline Quat Quat::fromAxisAngle(Vec3 axis, f32 angleRad) { + Vec3 normalized = axis.normalized(); + f32 halfAngle = angleRad * 0.5f; + f32 s = sinf(halfAngle); + return {normalized.x * s, normalized.y * s, normalized.z * s, cosf(halfAngle)}; +} + +inline Quat Quat::fromEuler(f32 pitchRad, f32 yawRad, f32 rollRad) { + f32 cp = cosf(pitchRad * 0.5f); + f32 sp = sinf(pitchRad * 0.5f); + f32 cy = cosf(yawRad * 0.5f); + f32 sy = sinf(yawRad * 0.5f); + f32 cr = cosf(rollRad * 0.5f); + f32 sr = sinf(rollRad * 0.5f); + + Quat q; + q.w = cr * cy * cp + sr * sy * sp; + q.x = cr * cy * sp - sr * sy * cp; + q.y = cr * sy * cp + sr * cy * sp; + q.z = sr * cy * cp - cr * sy * sp; + return q; +} + +inline Quat Quat::lookAt(Vec3 forward, Vec3 up) { + Vec3 f = forward.normalized(); + Vec3 r = up.cross(f).normalized(); + Vec3 u = f.cross(r); + + Mat4 m = Mat4::identity(); + m(0, 0) = r.x; m(0, 1) = u.x; m(0, 2) = f.x; + m(1, 0) = r.y; m(1, 1) = u.y; m(1, 2) = f.y; + m(2, 0) = r.z; m(2, 1) = u.z; m(2, 2) = f.z; + + return fromMatrix(m); +} + +inline Quat Quat::fromMatrix(const Mat4& m) { + f32 trace = m(0, 0) + m(1, 1) + m(2, 2); + Quat q; + + if (trace > 0.0f) { + f32 s = sqrtf(trace + 1.0f) * 2.0f; + q.w = 0.25f * s; + q.x = (m(2, 1) - m(1, 2)) / s; + q.y = (m(0, 2) - m(2, 0)) / s; + q.z = (m(1, 0) - m(0, 1)) / s; + } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) { + f32 s = sqrtf(1.0f + m(0, 0) - m(1, 1) - m(2, 2)) * 2.0f; + q.w = (m(2, 1) - m(1, 2)) / s; + q.x = 0.25f * s; + q.y = (m(0, 1) + m(1, 0)) / s; + q.z = (m(0, 2) + m(2, 0)) / s; + } else if (m(1, 1) > m(2, 2)) { + f32 s = sqrtf(1.0f + m(1, 1) - m(0, 0) - m(2, 2)) * 2.0f; + q.w = (m(0, 2) - m(2, 0)) / s; + q.x = (m(0, 1) + m(1, 0)) / s; + q.y = 0.25f * s; + q.z = (m(1, 2) + m(2, 1)) / s; + } else { + f32 s = sqrtf(1.0f + m(2, 2) - m(0, 0) - m(1, 1)) * 2.0f; + q.w = (m(1, 0) - m(0, 1)) / s; + q.x = (m(0, 2) + m(2, 0)) / s; + q.y = (m(1, 2) + m(2, 1)) / s; + q.z = 0.25f * s; + } + + return q; +} + +inline f32 Quat::length() const { + return sqrtf(lengthSquared()); +} + +inline Quat Quat::normalized() const { + f32 len = length(); + if (len > 0.0f) { + f32 invLen = 1.0f / len; + return {x * invLen, y * invLen, z * invLen, w * invLen}; + } + return identity(); +} + +inline Quat Quat::inverse() const { + f32 lenSq = lengthSquared(); + if (lenSq > 0.0f) { + f32 invLenSq = 1.0f / lenSq; + return {-x * invLenSq, -y * invLenSq, -z * invLenSq, w * invLenSq}; + } + return *this; +} + +inline Quat Quat::operator*(Quat o) const { + return { + w * o.x + x * o.w + y * o.z - z * o.y, + w * o.y - x * o.z + y * o.w + z * o.x, + w * o.z + x * o.y - y * o.x + z * o.w, + w * o.w - x * o.x - y * o.y - z * o.z + }; +} + +inline Vec3 Quat::rotate(Vec3 v) const { + Vec3 qv(x, y, z); + Vec3 cross1 = qv.cross(v); + Vec3 cross2 = qv.cross(cross1 + v * w); + return v + cross2 * 2.0f; +} + +inline Mat4 Quat::toMatrix() const { + f32 xx = x * x, yy = y * y, zz = z * z; + f32 xy = x * y, xz = x * z, xw = x * w; + f32 yz = y * z, yw = y * w, zw = z * w; + + Mat4 result = Mat4::identity(); + result(0, 0) = 1.0f - 2.0f * (yy + zz); + result(1, 0) = 2.0f * (xy + zw); + result(2, 0) = 2.0f * (xz - yw); + + result(0, 1) = 2.0f * (xy - zw); + result(1, 1) = 1.0f - 2.0f * (xx + zz); + result(2, 1) = 2.0f * (yz + xw); + + result(0, 2) = 2.0f * (xz + yw); + result(1, 2) = 2.0f * (yz - xw); + result(2, 2) = 1.0f - 2.0f * (xx + yy); + + return result; +} + +inline Vec3 Quat::toEuler() const { + f32 sinPitch = Math::clamp(-2.0f * (x * z - w * y), -1.0f, 1.0f); + f32 pitch = asinf(sinPitch); + f32 yaw = atan2f(2.0f * (y * z + w * x), 1.0f - 2.0f * (x * x + y * y)); + f32 roll = atan2f(2.0f * (x * y + w * z), 1.0f - 2.0f * (y * y + z * z)); + return {pitch, yaw, roll}; +} + +inline bool Quat::operator==(Quat o) const { + return x == o.x && y == o.y && z == o.z && w == o.w; +} + +inline Quat Quat::slerp(Quat a, Quat b, f32 t) { + f32 cosOmega = a.dot(b); + + if (cosOmega < 0.0f) { + b = {-b.x, -b.y, -b.z, -b.w}; + cosOmega = -cosOmega; + } + + f32 k0, k1; + if (cosOmega > 0.9999f) { + k0 = 1.0f - t; + k1 = t; + } else { + f32 sinOmega = sqrtf(1.0f - cosOmega * cosOmega); + f32 omega = atan2f(sinOmega, cosOmega); + f32 invSinOmega = 1.0f / sinOmega; + k0 = sinf((1.0f - t) * omega) * invSinOmega; + k1 = sinf(t * omega) * invSinOmega; + } + + return { + a.x * k0 + b.x * k1, + a.y * k0 + b.y * k1, + a.z * k0 + b.z * k1, + a.w * k0 + b.w * k1 + }; +} + +inline Quat Quat::nlerp(Quat a, Quat b, f32 t) { + if (a.dot(b) < 0.0f) { + b = {-b.x, -b.y, -b.z, -b.w}; + } + + Quat result = { + a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + a.w + (b.w - a.w) * t + }; + + return result.normalized(); +} + +} // namespace Caffeine diff --git a/tests/test_math.cpp b/tests/test_math.cpp index 13b358e..a64766b 100644 --- a/tests/test_math.cpp +++ b/tests/test_math.cpp @@ -4,6 +4,7 @@ #include "../src/math/Vec4.hpp" #include "../src/math/Mat4.hpp" #include "../src/math/Math.hpp" +#include "../src/math/Quat.hpp" using namespace Caffeine; using namespace Caffeine::Math; @@ -203,4 +204,228 @@ TEST_CASE("Math - NextPowerOfTwo", "[math]") { REQUIRE(Math::nextPowerOfTwo(5) == 8); REQUIRE(Math::nextPowerOfTwo(15) == 16); REQUIRE(Math::nextPowerOfTwo(0) == 1); +} + +TEST_CASE("Quat - Identity Values", "[math][quat]") { + Quat identity = Quat::identity(); + REQUIRE(identity.x == 0.0f); + REQUIRE(identity.y == 0.0f); + REQUIRE(identity.z == 0.0f); + REQUIRE(identity.w == 1.0f); +} + +TEST_CASE("Quat - Default Constructor Equals Identity", "[math][quat]") { + Quat q; + Quat identity = Quat::identity(); + REQUIRE(q == identity); +} + +TEST_CASE("Quat - FromAxisAngle Zero Degrees", "[math][quat]") { + Quat q = Quat::fromAxisAngle(Vec3(0, 1, 0), 0.0f); + Quat identity = Quat::identity(); + REQUIRE(q.x == Approx(identity.x).epsilon(0.001f)); + REQUIRE(q.y == Approx(identity.y).epsilon(0.001f)); + REQUIRE(q.z == Approx(identity.z).epsilon(0.001f)); + REQUIRE(q.w == Approx(identity.w).epsilon(0.001f)); +} + +TEST_CASE("Quat - FromAxisAngle 90 Degrees Y Rotates X to Z", "[math][quat]") { + Quat q = Quat::fromAxisAngle(Vec3(0, 1, 0), PI / 2.0f); + Vec3 rotated = q.rotate(Vec3(1, 0, 0)); + REQUIRE(rotated.x == Approx(0.0f).epsilon(0.001f)); + REQUIRE(rotated.y == Approx(0.0f).epsilon(0.001f)); + REQUIRE(rotated.z == Approx(-1.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - FromEuler Zero Returns Identity", "[math][quat]") { + Quat q = Quat::fromEuler(0.0f, 0.0f, 0.0f); + Quat identity = Quat::identity(); + REQUIRE(q.x == Approx(identity.x).epsilon(0.001f)); + REQUIRE(q.y == Approx(identity.y).epsilon(0.001f)); + REQUIRE(q.z == Approx(identity.z).epsilon(0.001f)); + REQUIRE(q.w == Approx(identity.w).epsilon(0.001f)); +} + +TEST_CASE("Quat - FromEuler Pitch 90 Degrees", "[math][quat]") { + Quat q = Quat::fromEuler(PI / 2.0f, 0.0f, 0.0f); + Vec3 rotated = q.rotate(Vec3(0, 1, 0)); + REQUIRE(rotated.x == Approx(0.0f).epsilon(0.001f)); + REQUIRE(rotated.y == Approx(0.0f).epsilon(0.001f)); + REQUIRE(rotated.z == Approx(1.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - Quaternion Composition", "[math][quat]") { + Quat q1 = Quat::fromAxisAngle(Vec3(0, 1, 0), PI / 2.0f); + Quat q2 = Quat::fromAxisAngle(Vec3(0, 1, 0), PI / 2.0f); + Quat result = q1 * q2; + Vec3 rotated = result.rotate(Vec3(1, 0, 0)); + REQUIRE(rotated.x == Approx(-1.0f).epsilon(0.001f)); + REQUIRE(rotated.y == Approx(0.0f).epsilon(0.001f)); + REQUIRE(rotated.z == Approx(0.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - Quaternion Times Vec3", "[math][quat]") { + Quat q = Quat::fromAxisAngle(Vec3(0, 1, 0), PI / 2.0f); + Vec3 v(1, 0, 0); + Vec3 rotated = q * v; + REQUIRE(rotated.x == Approx(0.0f).epsilon(0.001f)); + REQUIRE(rotated.y == Approx(0.0f).epsilon(0.001f)); + REQUIRE(rotated.z == Approx(-1.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - Conjugate of Identity is Identity", "[math][quat]") { + Quat identity = Quat::identity(); + Quat conj = identity.conjugate(); + REQUIRE(conj == identity); +} + +TEST_CASE("Quat - Conjugate Negates XYZ", "[math][quat]") { + Quat q(1, 0, 0, 0); + Quat conj = q.conjugate(); + REQUIRE(conj.x == -1.0f); + REQUIRE(conj.y == 0.0f); + REQUIRE(conj.z == 0.0f); + REQUIRE(conj.w == 0.0f); +} + +TEST_CASE("Quat - Length of Identity is One", "[math][quat]") { + Quat identity = Quat::identity(); + REQUIRE(identity.length() == Approx(1.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - Length of Normalized is One", "[math][quat]") { + Quat q(2, 0, 0, 0); + Quat normalized = q.normalized(); + REQUIRE(normalized.length() == Approx(1.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - Normalized 2,0,0,0 Returns 1,0,0,0", "[math][quat]") { + Quat q(2, 0, 0, 0); + Quat normalized = q.normalized(); + REQUIRE(normalized.x == Approx(1.0f).epsilon(0.001f)); + REQUIRE(normalized.y == Approx(0.0f).epsilon(0.001f)); + REQUIRE(normalized.z == Approx(0.0f).epsilon(0.001f)); + REQUIRE(normalized.w == Approx(0.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - Inverse of Identity is Identity", "[math][quat]") { + Quat identity = Quat::identity(); + Quat inv = identity.inverse(); + REQUIRE(inv.x == Approx(identity.x).epsilon(0.001f)); + REQUIRE(inv.y == Approx(identity.y).epsilon(0.001f)); + REQUIRE(inv.z == Approx(identity.z).epsilon(0.001f)); + REQUIRE(inv.w == Approx(identity.w).epsilon(0.001f)); +} + +TEST_CASE("Quat - Q Times Q Inverse Equals Identity", "[math][quat]") { + Quat q = Quat::fromAxisAngle(Vec3(1, 1, 1).normalized(), PI / 3.0f); + Quat inv = q.inverse(); + Quat result = q * inv; + Quat identity = Quat::identity(); + REQUIRE(result.x == Approx(identity.x).epsilon(0.001f)); + REQUIRE(result.y == Approx(identity.y).epsilon(0.001f)); + REQUIRE(result.z == Approx(identity.z).epsilon(0.001f)); + REQUIRE(result.w == Approx(identity.w).epsilon(0.001f)); +} + +TEST_CASE("Quat - SLERP of Same Quaternion Returns Same", "[math][quat]") { + Quat q = Quat::fromAxisAngle(Vec3(0, 1, 0), PI / 4.0f); + Quat result = Quat::slerp(q, q, 0.5f); + REQUIRE(result.x == Approx(q.x).epsilon(0.001f)); + REQUIRE(result.y == Approx(q.y).epsilon(0.001f)); + REQUIRE(result.z == Approx(q.z).epsilon(0.001f)); + REQUIRE(result.w == Approx(q.w).epsilon(0.001f)); +} + +TEST_CASE("Quat - SLERP t=0 Returns First, t=1 Returns Second", "[math][quat]") { + Quat a = Quat::fromAxisAngle(Vec3(0, 1, 0), 0.0f); + Quat b = Quat::fromAxisAngle(Vec3(0, 1, 0), PI / 2.0f); + + Quat result0 = Quat::slerp(a, b, 0.0f); + REQUIRE(result0.x == Approx(a.x).epsilon(0.001f)); + REQUIRE(result0.y == Approx(a.y).epsilon(0.001f)); + REQUIRE(result0.z == Approx(a.z).epsilon(0.001f)); + REQUIRE(result0.w == Approx(a.w).epsilon(0.001f)); + + Quat result1 = Quat::slerp(a, b, 1.0f); + REQUIRE(result1.x == Approx(b.x).epsilon(0.001f)); + REQUIRE(result1.y == Approx(b.y).epsilon(0.001f)); + REQUIRE(result1.z == Approx(b.z).epsilon(0.001f)); + REQUIRE(result1.w == Approx(b.w).epsilon(0.001f)); +} + +TEST_CASE("Quat - NLERP t=0 Returns First Normalized", "[math][quat]") { + Quat a = Quat::fromAxisAngle(Vec3(0, 1, 0), 0.0f); + Quat b = Quat::fromAxisAngle(Vec3(0, 1, 0), PI / 2.0f); + + Quat result = Quat::nlerp(a, b, 0.0f); + Quat aNorm = a.normalized(); + REQUIRE(result.x == Approx(aNorm.x).epsilon(0.001f)); + REQUIRE(result.y == Approx(aNorm.y).epsilon(0.001f)); + REQUIRE(result.z == Approx(aNorm.z).epsilon(0.001f)); + REQUIRE(result.w == Approx(aNorm.w).epsilon(0.001f)); +} + +TEST_CASE("Quat - ToMatrix Identity Returns Identity Matrix", "[math][quat]") { + Quat identity = Quat::identity(); + Mat4 m = identity.toMatrix(); + Mat4 identityMat = Mat4::identity(); + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + REQUIRE(m(i, j) == Approx(identityMat(i, j)).epsilon(0.001f)); + } + } +} + +TEST_CASE("Quat - ToMatrix Matches Mat4::rotationY", "[math][quat]") { + f32 angle = PI / 3.0f; + Quat q = Quat::fromAxisAngle(Vec3(0, 1, 0), angle); + Mat4 quatMat = q.toMatrix(); + Mat4 mat4Rot = Mat4::rotationY(angle); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + REQUIRE(quatMat(i, j) == Approx(mat4Rot(i, j)).epsilon(0.001f)); + } + } +} + +TEST_CASE("Quat - ToEuler Identity Returns Zero", "[math][quat]") { + Quat identity = Quat::identity(); + Vec3 euler = identity.toEuler(); + REQUIRE(euler.x == Approx(0.0f).epsilon(0.001f)); + REQUIRE(euler.y == Approx(0.0f).epsilon(0.001f)); + REQUIRE(euler.z == Approx(0.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - FromMatrix Identity Returns Identity Quat", "[math][quat]") { + Mat4 identity = Mat4::identity(); + Quat q = Quat::fromMatrix(identity); + Quat identityQuat = Quat::identity(); + REQUIRE(q.x == Approx(identityQuat.x).epsilon(0.001f)); + REQUIRE(q.y == Approx(identityQuat.y).epsilon(0.001f)); + REQUIRE(q.z == Approx(identityQuat.z).epsilon(0.001f)); + REQUIRE(q.w == Approx(identityQuat.w).epsilon(0.001f)); +} + +TEST_CASE("Quat - LookAt Forward Z Up Y Returns Identity", "[math][quat]") { + Vec3 forward(0, 0, 1); + Vec3 up(0, 1, 0); + Quat q = Quat::lookAt(forward, up); + Vec3 testVec = q.rotate(Vec3(1, 0, 0)); + REQUIRE(testVec.length() == Approx(1.0f).epsilon(0.001f)); +} + +TEST_CASE("Quat - FromEuler ToEuler Roundtrip", "[math][quat]") { + f32 pitch = PI / 6.0f; + f32 yaw = PI / 4.0f; + f32 roll = PI / 3.0f; + + Quat q = Quat::fromEuler(pitch, yaw, roll); + Vec3 euler = q.toEuler(); + + REQUIRE(euler.x == Approx(pitch).epsilon(0.001f)); + REQUIRE(euler.y == Approx(yaw).epsilon(0.001f)); + REQUIRE(euler.z == Approx(roll).epsilon(0.001f)); } \ No newline at end of file