Crafter.Math/tests/Matrix/main.cpp

//SPDX-License-Identifier: LGPL-3.0-only
//SPDX-FileCopyrightText: Copyright (C) 2026 Catcrafts®

#include <cmath>
import Crafter.Math;
import std;
using namespace Crafter;

namespace {

constexpr float kEps = 1e-4f;

constexpr bool FloatEquals(float a, float b, float epsilon = kEps) {
    return std::abs(a - b) < epsilon;
}

VectorF32<3, 1> Vec3(float x, float y, float z) {
    alignas(16) float buf[4] = { x, y, z, 0.0f };
    return VectorF32<3, 1>(buf);
}

std::string* CheckVec3(VectorF32<3, 1> got, float x, float y, float z, std::string_view label) {
    std::array<float, 4> v = got.template Store<float>();
    if (!FloatEquals(v[0], x) || !FloatEquals(v[1], y) || !FloatEquals(v[2], z))
        return new std::string(std::format(
            "{}: expected ({},{},{}), got ({},{},{})",
            label, x, y, z, v[0], v[1], v[2]));
    return nullptr;
}

std::string* CheckMat43(MatrixRowMajor<float, 4, 3, 1> const& m, std::array<float, 12> expected, std::string_view label) {
    std::array<float, 12> got = m.Store();
    for (std::uint8_t i = 0; i < 12; ++i) {
        if (!FloatEquals(got[i], expected[i]))
            return new std::string(std::format(
                "{} element {}: expected {}, got {}", label, i, expected[i], got[i]));
    }
    return nullptr;
}

std::string* CheckMat44(MatrixRowMajor<float, 4, 4, 1> const& m, std::array<float, 16> expected, std::string_view label) {
    std::array<float, 16> got = m.Store();
    for (std::uint8_t i = 0; i < 16; ++i) {
        if (!FloatEquals(got[i], expected[i]))
            return new std::string(std::format(
                "{} element {}: expected {}, got {}", label, i, expected[i], got[i]));
    }
    return nullptr;
}

// ---------- Identity / Store roundtrip ----------

std::string* TestIdentity43() {
    auto m = MatrixRowMajor<float, 4, 3, 1>::Identity();
    return CheckMat43(m, {
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
    }, "Identity 4x3");
}

std::string* TestIdentity44() {
    auto m = MatrixRowMajor<float, 4, 4, 1>::Identity();
    return CheckMat44(m, {
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1,
    }, "Identity 4x4");
}

std::string* TestStoreRoundtrip() {
    MatrixRowMajor<float, 4, 4, 1> m(
        1,  2,  3,  4,
        5,  6,  7,  8,
        9, 10, 11, 12,
       13, 14, 15, 16
    );
    return CheckMat44(m, {
        1, 2, 3, 4,
        5, 6, 7, 8,
        9, 10, 11, 12,
        13, 14, 15, 16,
    }, "Store roundtrip 4x4");
}

// ---------- Factory matrices ----------

std::string* TestTranslation() {
    auto m = MatrixRowMajor<float, 4, 3, 1>::Translation(10, 20, 30);
    if (auto e = CheckMat43(m, {
        1, 0, 0, 10,
        0, 1, 0, 20,
        0, 0, 1, 30,
    }, "Translation 4x3")) return e;

    auto m4 = MatrixRowMajor<float, 4, 4, 1>::Translation(10, 20, 30);
    if (auto e = CheckMat44(m4, {
        1,  0,  0,  0,
        0,  1,  0,  0,
        0,  0,  1,  0,
        10, 20, 30, 1,
    }, "Translation 4x4")) return e;
    return nullptr;
}

std::string* TestScaling() {
    auto m = MatrixRowMajor<float, 4, 3, 1>::Scaling(2, 3, 4);
    if (auto e = CheckMat43(m, {
        2, 0, 0, 0,
        0, 3, 0, 0,
        0, 0, 4, 0,
    }, "Scaling 4x3")) return e;

    auto m4 = MatrixRowMajor<float, 4, 4, 1>::Scaling(2, 3, 4);
    return CheckMat44(m4, {
        2, 0, 0, 0,
        0, 3, 0, 0,
        0, 0, 4, 0,
        0, 0, 0, 1,
    }, "Scaling 4x4");
}

// ---------- Matrix * Vector ----------

std::string* TestMatVecIdentity() {
    auto m = MatrixRowMajor<float, 4, 3, 1>::Identity();
    VectorF32<3, 1> v = Vec3(7, -2, 3);
    return CheckVec3(m * v, 7, -2, 3, "I * v");
}

std::string* TestMatVecTranslation() {
    auto m = MatrixRowMajor<float, 4, 3, 1>::Translation(1, 2, 3);
    // Affine multiply with implicit w=1 -> result = v + (1,2,3).
    VectorF32<3, 1> v = Vec3(10, 20, 30);
    return CheckVec3(m * v, 11, 22, 33, "Translation * v");
}

std::string* TestMatVecScaling() {
    auto m = MatrixRowMajor<float, 4, 3, 1>::Scaling(2, 3, 4);
    VectorF32<3, 1> v = Vec3(1, 1, 1);
    return CheckVec3(m * v, 2, 3, 4, "Scaling * v");
}

std::string* TestMatVecCombined() {
    // Scale then translate: full matrix is [diag(s) | t].
    MatrixRowMajor<float, 4, 3, 1> m(
        2, 0, 0, 10,
        0, 3, 0, 20,
        0, 0, 4, 30
    );
    VectorF32<3, 1> v = Vec3(1, 1, 1);
    return CheckVec3(m * v, 12, 23, 34, "Scale+Translate * v");
}

std::string* TestTransformNormal() {
    // TransformNormal ignores the translation column.
    MatrixRowMajor<float, 4, 3, 1> m(
        2, 0, 0, 100,
        0, 3, 0, 200,
        0, 0, 4, 300
    );
    VectorF32<3, 1> v = Vec3(1, 1, 1);
    return CheckVec3(m.TransformNormal(v), 2, 3, 4, "TransformNormal");
}

// ---------- Matrix * Matrix ----------

std::string* TestMatMulIdentityLeft44() {
    MatrixRowMajor<float, 4, 4, 1> a = MatrixRowMajor<float, 4, 4, 1>::Identity();
    MatrixRowMajor<float, 4, 4, 1> b(
        1,  2,  3,  4,
        5,  6,  7,  8,
        9, 10, 11, 12,
       13, 14, 15, 16
    );
    auto r = a * b;
    return CheckMat44(r, {
        1, 2, 3, 4,
        5, 6, 7, 8,
        9, 10, 11, 12,
        13, 14, 15, 16,
    }, "I * M");
}

std::string* TestMatMulIdentityRight44() {
    MatrixRowMajor<float, 4, 4, 1> a(
        1,  2,  3,  4,
        5,  6,  7,  8,
        9, 10, 11, 12,
       13, 14, 15, 16
    );
    MatrixRowMajor<float, 4, 4, 1> b = MatrixRowMajor<float, 4, 4, 1>::Identity();
    auto r = a * b;
    return CheckMat44(r, {
        1, 2, 3, 4,
        5, 6, 7, 8,
        9, 10, 11, 12,
        13, 14, 15, 16,
    }, "M * I");
}

std::string* TestMatMul44() {
    // Hand-computed: standard row-major (B*A)[i][j] = sum_k B[i][k]*A[k][j].
    MatrixRowMajor<float, 4, 4, 1> a(
        1, 2, 0, 0,
        0, 1, 3, 0,
        4, 0, 1, 0,
        0, 0, 0, 1
    );
    MatrixRowMajor<float, 4, 4, 1> b(
        1, 0, 2, 0,
        0, 1, 0, 3,
        0, 0, 1, 0,
        0, 0, 0, 1
    );
    auto r = a * b;
    // result.m[i][j] = sum_k b.m[i][k] * a.m[k][j]
    // row 0: [1*1+0*0+2*4+0*0, 1*2+0*1+2*0+0*0, 1*0+0*3+2*1+0*0, 1*0+0*0+2*0+0*1]
    //      = [9, 2, 2, 0]
    // row 1: [0*1+1*0+0*4+3*0, 0*2+1*1+0*0+3*0, 0*0+1*3+0*1+3*0, 0*0+1*0+0*0+3*1]
    //      = [0, 1, 3, 3]
    // row 2: [0*1+0*0+1*4+0*0, 0*2+0*1+1*0+0*0, 0*0+0*3+1*1+0*0, 0*0+0*0+1*0+0*1]
    //      = [4, 0, 1, 0]
    // row 3: [0*1+0*0+0*4+1*0, 0*2+0*1+0*0+1*0, 0*0+0*3+0*1+1*0, 0*0+0*0+0*0+1*1]
    //      = [0, 0, 0, 1]
    return CheckMat44(r, {
        9, 2, 2, 0,
        0, 1, 3, 3,
        4, 0, 1, 0,
        0, 0, 0, 1,
    }, "Mat * Mat 4x4");
}

std::string* TestMatMulTranslationCompose() {
    // Translation composition: T(a) * T(b) acting on a vector translates by a+b
    // (and the resulting matrix has the summed translation in its last column).
    auto t1 = MatrixRowMajor<float, 4, 3, 1>::Translation(1, 2, 3);
    auto t2 = MatrixRowMajor<float, 4, 3, 1>::Translation(10, 20, 30);
    auto r = t1 * t2;
    if (auto e = CheckMat43(r, {
        1, 0, 0, 11,
        0, 1, 0, 22,
        0, 0, 1, 33,
    }, "T(1,2,3) * T(10,20,30)")) return e;

    // Sanity check: applying the composed matrix to origin yields (11,22,33).
    return CheckVec3(r * Vec3(0, 0, 0), 11, 22, 33, "Composed translation * origin");
}

std::string* TestMatMulScaleThenTranslate() {
    // The matrix product follows the engine's row-vector / post-multiply
    // convention: a.operator*(b) yields the matrix b composed with a so that
    // applying the result is equivalent to "first apply this, then apply b"
    // when read left-to-right with a row vector (v * (a*b) = (v*a) * b).
    //
    // For column-vector intuition that means s.operator*(t) builds T·S, i.e.
    // "scale, then translate"; applying it to (1,1,1) yields s*v + t.
    auto s = MatrixRowMajor<float, 4, 3, 1>::Scaling(2, 3, 4);
    auto t = MatrixRowMajor<float, 4, 3, 1>::Translation(10, 20, 30);
    auto r = s * t;
    return CheckVec3(r * Vec3(1, 1, 1), 12, 23, 34, "scale-then-translate * (1,1,1)");
}

// ---------- LookTo / LookAt ----------

std::string* TestLookToBasis() {
    // Camera at +Z looking back toward origin. eyeDirection should point from
    // focus to eye, i.e. +Z, so the resulting basis has R2 = +Z.
    VectorF32<3, 1> eye = Vec3(0, 0, 5);
    VectorF32<3, 1> dir = Vec3(0, 0, 1); // already from origin toward eye
    VectorF32<3, 1> up  = Vec3(0, 1, 0);
    auto m = MatrixRowMajor<float, 4, 4, 1>::LookTo(eye, dir, up);
    // R2 normalized = (0,0,1). R0 = up x R2 = (0,1,0) x (0,0,1) = (1,0,0).
    // R1 = R2 x R0 = (0,0,1) x (1,0,0) = (0,1,0).
    // The view matrix stores basis columns in the rotation block, with the
    // translation column = -basis dot eye. So m.row[3] = (D0, D1, D2, 1).
    // D0 = R0 . -eye = -(0*0+0*0+5*0) = 0
    // D1 = R1 . -eye = -(0*0+1*0+0*5) = 0
    // D2 = R2 . -eye = -(0*0+0*0+1*5) = -5
    return CheckMat44(m, {
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, -5, 1,
    }, "LookTo basis");
}


// ---------- Rotation conventions, quaternions, TRS, inverse ----------
//
// The 4x4 is row-vector (v * M, translation in row 3) and the 4x3 is
// column-vector (M * v, translation in column 3, the VkTransformMatrixKHR
// layout). The same call has to mean the same rotation in both.

using Mat43 = MatrixRowMajor<float, 4, 3, 1>;
using Mat44 = MatrixRowMajor<float, 4, 4, 1>;
using Q = Vector<float, 4, 0>;
using V3 = Vector<float, 3, 0>;

// v as a row vector times a 4x4, translation included.
V3 RowTimes44(Mat44 const& m, V3 v) {
    const std::array<float, 16> a = m.Store();
    return V3(v.x * a[0] + v.y * a[4] + v.z * a[8]  + a[12],
              v.x * a[1] + v.y * a[5] + v.z * a[9]  + a[13],
              v.x * a[2] + v.y * a[6] + v.z * a[10] + a[14]);
}

// A 4x3 times v as a column vector, translation included.
V3 Times43(Mat43 const& m, V3 v) {
    const std::array<float, 12> a = m.Store();
    return V3(a[0] * v.x + a[1] * v.y + a[2]  * v.z + a[3],
              a[4] * v.x + a[5] * v.y + a[6]  * v.z + a[7],
              a[8] * v.x + a[9] * v.y + a[10] * v.z + a[11]);
}

std::string* CheckV3(V3 got, V3 expected, std::string_view label) {
    if (!FloatEquals(got.x, expected.x) || !FloatEquals(got.y, expected.y) || !FloatEquals(got.z, expected.z))
        return new std::string(std::format("{}: expected ({},{},{}), got ({},{},{})",
            label, expected.x, expected.y, expected.z, got.x, got.y, got.z));
    return nullptr;
}

Q SampleRotation() {
    return Q::QuanternionFromAxisAngle(V3::Normalize(V3(1, 2, 3)), 0.9f);
}

std::string* TestRotation43MatchesRotation44() {
    // The 4x3 Euler Rotation used to copy the 4x4's row-vector rows, so as a
    // column-vector transform it rotated the other way.
    const V3 v(1, 2, 3);
    const V3 expected = RowTimes44(Mat44::Rotation(0.3f, 0.7f, -0.4f), v);
    return CheckV3(Times43(Mat43::Rotation(0.3f, 0.7f, -0.4f), v), expected, "4x3 Rotation vs 4x4 Rotation");
}

std::string* TestRotationFromQuaternion() {
    const Q q = SampleRotation();
    const V3 v(1, -2, 0.5f);
    const V3 expected = V3::Rotate(v, q);
    if (auto err = CheckV3(Times43(Mat43::Rotation(q), v), expected, "4x3 Rotation(q)")) return err;
    return CheckV3(RowTimes44(Mat44::Rotation(q), v), expected, "4x4 Rotation(q)");
}

std::string* TestTRS() {
    // Scale, then rotate, then translate.
    const V3 t(1, -2, 3), s(2, 0.5f, 1.5f), v(0.3f, 1, -2);
    const Q q = SampleRotation();
    const V3 rotated = V3::Rotate(V3(v.x * s.x, v.y * s.y, v.z * s.z), q);
    const V3 expected(rotated.x + t.x, rotated.y + t.y, rotated.z + t.z);
    if (auto err = CheckV3(Times43(Mat43::TRS(t, q, s), v), expected, "4x3 TRS")) return err;
    return CheckV3(RowTimes44(Mat44::TRS(t, q, s), v), expected, "4x4 TRS");
}

std::string* TestInverseAffine() {
    const V3 t(1, -2, 3), s(2, 0.5f, 1.5f), v(0.3f, 1, -2);
    const Q q = SampleRotation();
    const Mat43 m43 = Mat43::TRS(t, q, s);
    const Mat44 m44 = Mat44::TRS(t, q, s);
    if (auto err = CheckV3(Times43(m43.InverseAffine(), Times43(m43, v)), v, "4x3 inverse undoes the transform")) return err;
    if (auto err = CheckV3(RowTimes44(m44.InverseAffine(), RowTimes44(m44, v)), v, "4x4 inverse undoes the transform")) return err;
    if (auto err = CheckMat43(m43 * m43.InverseAffine(), { 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0 }, "4x3 M * M^-1")) return err;
    return CheckMat44(m44 * m44.InverseAffine(), { 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1 }, "4x4 M * M^-1");
}

std::string* TestTranspose44() {
    const Mat44 m(
         1,  2,  3,  4,
         5,  6,  7,  8,
         9, 10, 11, 12,
        13, 14, 15, 16);
    return CheckMat44(m.Transpose(), {
        1, 5,  9, 13,
        2, 6, 10, 14,
        3, 7, 11, 15,
        4, 8, 12, 16,
    }, "Transpose 4x4");
}

std::string* CheckSameRotation(Q got, Q expected, std::string_view label) {
    const float sign = (got.x * expected.x + got.y * expected.y + got.z * expected.z + got.w * expected.w) < 0 ? -1.0f : 1.0f;
    if (!FloatEquals(got.x * sign, expected.x) || !FloatEquals(got.y * sign, expected.y)
        || !FloatEquals(got.z * sign, expected.z) || !FloatEquals(got.w * sign, expected.w))
        return new std::string(std::format("{}: expected ({},{},{},{}), got ({},{},{},{})",
            label, expected.x, expected.y, expected.z, expected.w, got.x, got.y, got.z, got.w));
    return nullptr;
}

std::string* TestDecompose() {
    const V3 t(1, -2, 3), s(2, 0.5f, 1.5f);
    const Q q = SampleRotation();
    {
        const auto [dt, dq, ds] = Mat44::TRS(t, q, s).Decompose();
        if (auto err = CheckV3(dt, t, "4x4 Decompose translation")) return err;
        if (auto err = CheckV3(ds, s, "4x4 Decompose scale")) return err;
        if (auto err = CheckSameRotation(dq, q, "4x4 Decompose rotation")) return err;
    }
    {
        const auto [dt, dq, ds] = Mat43::TRS(t, q, s).Decompose();
        if (auto err = CheckV3(dt, t, "4x3 Decompose translation")) return err;
        if (auto err = CheckV3(ds, s, "4x3 Decompose scale")) return err;
        if (auto err = CheckSameRotation(dq, q, "4x3 Decompose rotation")) return err;
    }
    // A mirrored matrix has no unique split; recomposing has to give the same
    // transform back.
    const Mat44 mirrored = Mat44::TRS(t, q, V3(-2, 0.5f, 1.5f));
    const auto [mt, mq, ms] = mirrored.Decompose();
    const V3 v(0.3f, 1, -2);
    return CheckV3(RowTimes44(Mat44::TRS(mt, mq, ms), v), RowTimes44(mirrored, v), "mirrored Decompose recomposes");
}

// ---------- Test harness ----------

} // namespace

int main() {
    using Fn = std::string* (*)();
    constexpr std::array<std::pair<const char*, Fn>, 21> tests = {{
        { "Identity43",          TestIdentity43               },
        { "Identity44",          TestIdentity44               },
        { "StoreRoundtrip",      TestStoreRoundtrip           },
        { "Translation",         TestTranslation              },
        { "Scaling",             TestScaling                  },
        { "MatVecIdentity",      TestMatVecIdentity           },
        { "MatVecTranslation",   TestMatVecTranslation        },
        { "MatVecScaling",       TestMatVecScaling            },
        { "MatVecCombined",      TestMatVecCombined           },
        { "TransformNormal",     TestTransformNormal          },
        { "MatMulIdentityLeft",  TestMatMulIdentityLeft44     },
        { "MatMulIdentityRight", TestMatMulIdentityRight44    },
        { "MatMul44",            TestMatMul44                 },
        { "MatMulTranslation",   TestMatMulTranslationCompose },
        { "MatMulScaleTranslate",TestMatMulScaleThenTranslate },
        { "Rotation43Matches44", TestRotation43MatchesRotation44 },
        { "RotationQuaternion",  TestRotationFromQuaternion   },
        { "TRS",                 TestTRS                      },
        { "InverseAffine",       TestInverseAffine            },
        { "Transpose44",         TestTranspose44              },
        { "Decompose",           TestDecompose                },
    }};

    for (auto const& [name, fn] : tests) {
        if (auto err = std::unique_ptr<std::string>(fn())) {
            std::println(std::cerr, "[{}] {}", name, *err);
            return 1;
        }
    }

    // LookTo is tested separately - its expected result depends on the
    // internal basis convention and a single failure here is informative
    // enough to keep in the matrix suite.
    if (auto err = std::unique_ptr<std::string>(TestLookToBasis())) {
        std::println(std::cerr, "[LookTo] {}", *err);
        return 1;
    }
    return 0;
}