Crafter.Math/tests/Intersection.cpp

/*
Crafter® Build
Copyright (C) 2026 Catcrafts®
Catcrafts.net

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License version 3.0 as published by the Free Software Foundation;

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
*/
#include <cmath>
import Crafter.Math;
import std;
using namespace Crafter;

namespace {

constexpr float kEps = 1e-3f;
constexpr float kMaxF = std::numeric_limits<float>::max();

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);
}

VectorF32<4, 1> Vec4(float x, float y, float z, float w) {
    alignas(16) float buf[4] = { x, y, z, w };
    return VectorF32<4, 1>(buf);
}

VectorF32<1, 4> Vec1x4(float a, float b, float c, float d) {
    alignas(16) float buf[4] = { a, b, c, d };
    return VectorF32<1, 4>(buf);
}

// Möller-Trumbore in this codebase rejects det <= eps, so triangles must be
// wound so their geometric normal opposes the ray direction. For rays going +Z
// that means clockwise from a +Z viewer.
std::string* TestRayTriangle() {
    VectorF32<3, 1> rayOrigin = Vec3(0, 0, -5);
    VectorF32<3, 1> rayDir    = Vec3(0, 0, 1);

    // A: hits at z=0, t=5 (front-facing).
    VectorF32<3, 1> a0 = Vec3(-1, -1, 0), a1 = Vec3(0, 1, 0), a2 = Vec3(1, -1, 0);
    // B: hits at z=10, t=15.
    VectorF32<3, 1> b0 = Vec3(-1, -1, 10), b1 = Vec3(0, 1, 10), b2 = Vec3(1, -1, 10);
    // C: front-facing triangle far off to the side - u or v out of [0,1].
    VectorF32<3, 1> c0 = Vec3(99, -1, 0), c1 = Vec3(100, 1, 0), c2 = Vec3(101, -1, 0);
    // D: triangle parallel to the ray (all vertices share y=2; ray lives in y=0).
    VectorF32<3, 1> d0 = Vec3(-1, 2, -1), d1 = Vec3(1, 2, 1), d2 = Vec3(0, 2, 2);

    VectorF32<1, 4> t = IntersectionTestRayTriangle(rayOrigin, rayDir,
        a0, a1, a2,
        b0, b1, b2,
        c0, c1, c2,
        d0, d1, d2);
    std::array<float, 4> s = t.template Store<float>();

    if (!FloatEquals(s[0], 5.0f))
        return new std::string(std::format("RayTriangle A: expected 5, got {}", s[0]));
    if (!FloatEquals(s[1], 15.0f))
        return new std::string(std::format("RayTriangle B: expected 15, got {}", s[1]));
    if (s[2] != kMaxF)
        return new std::string(std::format("RayTriangle C: expected max (miss), got {}", s[2]));
    if (s[3] != kMaxF)
        return new std::string(std::format("RayTriangle D: expected max (parallel miss), got {}", s[3]));
    return nullptr;
}

std::string* TestRayTriangleBackFacing() {
    // Same A vertices but CCW from +Z viewer -> back-facing for +Z ray -> miss.
    VectorF32<3, 1> rayOrigin = Vec3(0, 0, -5);
    VectorF32<3, 1> rayDir    = Vec3(0, 0, 1);
    VectorF32<3, 1> v0 = Vec3(-1, -1, 0), v1 = Vec3(1, -1, 0), v2 = Vec3(0, 1, 0);

    VectorF32<1, 4> t = IntersectionTestRayTriangle(rayOrigin, rayDir,
        v0, v1, v2,
        v0, v1, v2,
        v0, v1, v2,
        v0, v1, v2);
    std::array<float, 4> s = t.template Store<float>();
    for (std::uint8_t i = 0; i < 4; ++i) {
        if (s[i] != kMaxF)
            return new std::string(std::format("RayTriangle back-facing lane {}: expected max, got {}", i, s[i]));
    }
    return nullptr;
}

std::string* TestRaySphere() {
    VectorF32<3, 1> rayOrigin = Vec3(0, 0, -10);
    VectorF32<3, 1> rayDir    = Vec3(0, 0, 1);

    // A: sphere at origin radius 2 - first hit at z=-2, t=8.
    VectorF32<3, 1> posA = Vec3(0, 0, 0);
    // B: sphere at (0,0,20) radius 1 - first hit at z=19, t=29.
    VectorF32<3, 1> posB = Vec3(0, 0, 20);
    // C: sphere off to the side, ray misses.
    VectorF32<3, 1> posC = Vec3(10, 10, 0);
    // D: sphere behind the ray origin.
    VectorF32<3, 1> posD = Vec3(0, 0, -50);

    VectorF32<1, 4> radii = Vec1x4(2.0f, 1.0f, 0.5f, 1.0f);
    VectorF32<1, 4> t = IntersectionTestRaySphere(rayOrigin, rayDir,
        posA, posB, posC, posD, radii);
    std::array<float, 4> s = t.template Store<float>();

    if (!FloatEquals(s[0], 8.0f))
        return new std::string(std::format("RaySphere A: expected 8, got {}", s[0]));
    if (!FloatEquals(s[1], 29.0f))
        return new std::string(std::format("RaySphere B: expected 29, got {}", s[1]));
    if (s[2] != kMaxF)
        return new std::string(std::format("RaySphere C: expected max (miss), got {}", s[2]));
    if (s[3] != kMaxF)
        return new std::string(std::format("RaySphere D: expected max (behind), got {}", s[3]));
    return nullptr;
}

std::string* TestRayOrientedBox() {
    VectorF32<3, 1> rayOrigin = Vec3(0, 0, -5);
    VectorF32<3, 1> rayDir    = Vec3(0, 0, 1);
    // Identity quaternion (axis-aligned).
    VectorF32<4, 1> idQ = Vec4(0, 0, 0, 1);

    // Note: RayOrientedBox treats `size` as the *full* extent (it computes
    // halfExtents = size * 0.5 internally). So size=2 means the box spans
    // [-1, 1] in each axis. (SphereOrientedBox uses the opposite convention.)
    //
    // A: box at origin size 2 (half 1) -> ray enters at z=-1, t=4.
    VectorF32<3, 1> posA = Vec3(0, 0, 0), sizeA = Vec3(2, 2, 2);
    // B: box at (0,0,10) size 2 (half 1) -> ray enters at z=9, t=14.
    VectorF32<3, 1> posB = Vec3(0, 0, 10), sizeB = Vec3(2, 2, 2);
    // C: box off to the side - miss.
    VectorF32<3, 1> posC = Vec3(50, 0, 0), sizeC = Vec3(2, 2, 2);
    // D: box behind ray - miss.
    VectorF32<3, 1> posD = Vec3(0, 0, -50), sizeD = Vec3(2, 2, 2);

    VectorF32<1, 4> t = IntersectionTestRayOrientedBox(rayOrigin, rayDir,
        posA, sizeA, idQ,
        posB, sizeB, idQ,
        posC, sizeC, idQ,
        posD, sizeD, idQ);
    std::array<float, 4> s = t.template Store<float>();

    if (!FloatEquals(s[0], 4.0f))
        return new std::string(std::format("RayOrientedBox A: expected 4, got {}", s[0]));
    if (!FloatEquals(s[1], 14.0f))
        return new std::string(std::format("RayOrientedBox B: expected 14, got {}", s[1]));
    if (s[2] != kMaxF)
        return new std::string(std::format("RayOrientedBox C: expected max (miss), got {}", s[2]));
    if (s[3] != kMaxF)
        return new std::string(std::format("RayOrientedBox D: expected max (behind), got {}", s[3]));
    return nullptr;
}

MatrixRowMajor<float, 4, 3, 1> MakeBoxMatrix(float tx, float ty, float tz) {
    // Box matrix the OBB intersection code expects: rows[i][0..2] is the i-th
    // axis (the existing semantics treat matrix rows as the OBB axes), and
    // rows[i][3] is the translation component along that axis.
    return MatrixRowMajor<float, 4, 3, 1>(
        1, 0, 0, tx,
        0, 1, 0, ty,
        0, 0, 1, tz
    );
}

std::string* TestSphereOrientedBox() {
    // `size` is half-extents (the intersection code clamps to ±size).
    VectorF32<3, 1> sphereCenter = Vec3(0, 0, 0);
    VectorF32<1, 4> radii = Vec1x4(1.0f, 0.5f, 0.5f, 1.0f);

    // A: box at origin half-extent 2 -> sphere center inside -> hit.
    VectorF32<3, 1> sizeA = Vec3(2, 2, 2);
    auto boxA = MakeBoxMatrix(0, 0, 0);
    // B: box at (5,0,0) half-extent 1 -> box spans x in [4,6], sphere in [-0.5,0.5] -> miss.
    VectorF32<3, 1> sizeB = Vec3(1, 1, 1);
    auto boxB = MakeBoxMatrix(5, 0, 0);
    // C: box at (3,0,0) half-extent 1 -> box spans [2,4], sphere [-0.5,0.5] -> miss.
    VectorF32<3, 1> sizeC = Vec3(1, 1, 1);
    auto boxC = MakeBoxMatrix(3, 0, 0);
    // D: box at origin half-extent 0.5 -> sphere center inside the box -> hit.
    VectorF32<3, 1> sizeD = Vec3(0.5f, 0.5f, 0.5f);
    auto boxD = MakeBoxMatrix(0, 0, 0);

    VectorF32<1, 4> r = IntersectionTestSphereOrientedBox(sphereCenter, radii,
        sizeA, boxA,
        sizeB, boxB,
        sizeC, boxC,
        sizeD, boxD);
    std::array<float, 4> s = r.template Store<float>();

    if (s[0] != 0.0f)
        return new std::string(std::format("SphereOrientedBox A: expected hit (0), got {}", s[0]));
    if (s[1] != kMaxF)
        return new std::string(std::format("SphereOrientedBox B: expected max (miss), got {}", s[1]));
    if (s[2] != kMaxF)
        return new std::string(std::format("SphereOrientedBox C: expected max (miss), got {}", s[2]));
    if (s[3] != 0.0f)
        return new std::string(std::format("SphereOrientedBox D: expected hit (0), got {}", s[3]));
    return nullptr;
}

std::string* TestGetOBBCorners() {
    // Identity matrix - the 8 corners are exactly ±size on each axis.
    VectorF32<3, 1> size = Vec3(2, 3, 4);
    auto m = MakeBoxMatrix(0, 0, 0);
    std::array<VectorF32<3, 1>, 8> corners = GetOBBCorners(size, m);

    constexpr std::array<std::array<float, 3>, 8> expected = {{
        {-2, -3, -4}, { 2, -3, -4}, {-2,  3, -4}, { 2,  3, -4},
        {-2, -3,  4}, { 2, -3,  4}, {-2,  3,  4}, { 2,  3,  4},
    }};
    for (std::uint8_t i = 0; i < 8; ++i) {
        std::array<float, 4> v = corners[i].template Store<float>();
        for (std::uint8_t j = 0; j < 3; ++j) {
            if (!FloatEquals(v[j], expected[i][j]))
                return new std::string(std::format(
                    "GetOBBCorners corner {} lane {}: expected {}, got {}",
                    i, j, expected[i][j], v[j]));
        }
    }

    // Translated matrix - corners shift by the translation column.
    auto m2 = MakeBoxMatrix(10, 20, 30);
    std::array<VectorF32<3, 1>, 8> corners2 = GetOBBCorners(size, m2);
    for (std::uint8_t i = 0; i < 8; ++i) {
        std::array<float, 4> v = corners2[i].template Store<float>();
        std::array<float, 3> exp = {
            expected[i][0] + 10.0f,
            expected[i][1] + 20.0f,
            expected[i][2] + 30.0f
        };
        for (std::uint8_t j = 0; j < 3; ++j) {
            if (!FloatEquals(v[j], exp[j]))
                return new std::string(std::format(
                    "GetOBBCorners translated corner {} lane {}: expected {}, got {}",
                    i, j, exp[j], v[j]));
        }
    }
    return nullptr;
}

std::string* TestOBBOBBOverlapping() {
    VectorF32<3, 1> size = Vec3(1, 1, 1);
    auto boxA = MakeBoxMatrix(0, 0, 0);
    auto boxB = MakeBoxMatrix(1, 0, 0); // overlap on x in [-1, 1] (B) and [-1, 1] (A) -> overlap
    if (!IntersectionTestOrientedBoxOrientedBox(size, boxA, size, boxB))
        return new std::string("OBB-OBB overlapping: expected true");

    auto boxFar = MakeBoxMatrix(10, 0, 0);
    if (IntersectionTestOrientedBoxOrientedBox(size, boxA, size, boxFar))
        return new std::string("OBB-OBB far apart: expected false");
    return nullptr;
}

} // namespace

int main() {
    using Fn = std::string* (*)();
    constexpr std::array<std::pair<const char*, Fn>, 7> tests = {{
        { "RayTriangle",            TestRayTriangle           },
        { "RayTriangleBackFacing",  TestRayTriangleBackFacing },
        { "RaySphere",              TestRaySphere             },
        { "RayOrientedBox",         TestRayOrientedBox        },
        { "SphereOrientedBox",      TestSphereOrientedBox     },
        { "GetOBBCorners",          TestGetOBBCorners         },
        { "OBBOBB",                 TestOBBOBBOverlapping     },
    }};

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