diff --git a/include/omath/collision/line_tracer.hpp b/include/omath/collision/line_tracer.hpp index 324f80b..f959f3c 100644 --- a/include/omath/collision/line_tracer.hpp +++ b/include/omath/collision/line_tracer.hpp @@ -4,6 +4,7 @@ #pragma once #include "omath/3d_primitives/aabb.hpp" +#include "omath/3d_primitives/obb.hpp" #include "omath/linear_algebra/triangle.hpp" #include "omath/linear_algebra/vector3.hpp" @@ -36,6 +37,7 @@ namespace omath::collision { using TriangleType = Triangle; using AABBType = primitives::Aabb; + using OBBType = primitives::Obb; public: LineTracer() = delete; @@ -137,6 +139,61 @@ namespace omath::collision return ray.start + dir * t_hit; } + // Slab method ray-OBB intersection. Project the ray into the OBB's local frame + // (axes are orthonormal, so the inverse rotation is just a transpose / dot products), + // then run the standard slab test against the local box [-half_extents, +half_extents]. + // The ray parameter t is invariant under rigid transform, so the hit point is recovered + // in world space as ray.start + dir * t_hit. + [[nodiscard]] + constexpr static auto get_ray_hit_point(const RayType& ray, const OBBType& obb) noexcept + { + using T = typename RayType::VectorType::ContainedType; + + const auto offset = ray.start - obb.center; + const auto dir = ray.direction_vector(); + + const T local_start[3] = {offset.dot(obb.axis_x), offset.dot(obb.axis_y), offset.dot(obb.axis_z)}; + const T local_dir[3] = {dir.dot(obb.axis_x), dir.dot(obb.axis_y), dir.dot(obb.axis_z)}; + const T half[3] = {obb.half_extents.x, obb.half_extents.y, obb.half_extents.z}; + + auto t_min = -std::numeric_limits::infinity(); + auto t_max = std::numeric_limits::infinity(); + + const auto process_axis = [&](const T& d, const T& origin, const T& h) -> bool + { + constexpr T k_epsilon = std::numeric_limits::epsilon(); + if (std::abs(d) < k_epsilon) + return origin >= -h && origin <= h; + + const T inv = T(1) / d; + T t0 = (-h - origin) * inv; + T t1 = (h - origin) * inv; + if (t0 > t1) + std::swap(t0, t1); + + t_min = std::max(t_min, t0); + t_max = std::min(t_max, t1); + return t_min <= t_max; + }; + + if (!process_axis(local_dir[0], local_start[0], half[0])) + return ray.end; + if (!process_axis(local_dir[1], local_start[1], half[1])) + return ray.end; + if (!process_axis(local_dir[2], local_start[2], half[2])) + return ray.end; + + const T t_hit = std::max(T(0), t_min); + + if (t_max < T(0)) + return ray.end; // box entirely behind origin + + if (!ray.infinite_length && t_hit > T(1)) + return ray.end; // box beyond ray endpoint + + return ray.start + dir * t_hit; + } + template [[nodiscard]] constexpr static auto get_ray_hit_point(const RayType& ray, const MeshType& mesh) noexcept diff --git a/tests/general/unit_test_line_tracer_obb.cpp b/tests/general/unit_test_line_tracer_obb.cpp new file mode 100644 index 0000000..5331f76 --- /dev/null +++ b/tests/general/unit_test_line_tracer_obb.cpp @@ -0,0 +1,225 @@ +// +// Created by Vladislav on 07.05.2026. +// +#include "omath/3d_primitives/obb.hpp" +#include "omath/collision/line_tracer.hpp" +#include +#include +#include + +using Vec3 = omath::Vector3; +using Ray = omath::collision::Ray<>; +using LineTracer = omath::collision::LineTracer<>; +using OBB = omath::primitives::Obb; + +namespace +{ + Ray make_ray(const Vec3 start, const Vec3 end, const bool infinite = false) + { + Ray r; + r.start = start; + r.end = end; + r.infinite_length = infinite; + return r; + } + + constexpr OBB axis_aligned_obb(const Vec3& center, const Vec3& half_extents) noexcept + { + return OBB{center, {1.f, 0.f, 0.f}, {0.f, 1.f, 0.f}, {0.f, 0.f, 1.f}, half_extents}; + } + + OBB rotated_around_z(const Vec3& center, const Vec3& half_extents, const float radians) noexcept + { + const auto c = std::cos(radians); + const auto s = std::sin(radians); + return OBB{center, {c, s, 0.f}, {-s, c, 0.f}, {0.f, 0.f, 1.f}, half_extents}; + } +} // namespace + +// --- axis-aligned OBB behaves like AABB --- + +TEST(LineTracerOBBTests, AxisAlignedHitAlongZ) +{ + const auto box = axis_aligned_obb({0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({0.f, 0.f, -5.f}, {0.f, 0.f, 5.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_NE(hit, ray.end); + EXPECT_NEAR(hit.x, 0.f, 1e-4f); + EXPECT_NEAR(hit.y, 0.f, 1e-4f); + EXPECT_NEAR(hit.z, -1.f, 1e-4f); +} + +TEST(LineTracerOBBTests, AxisAlignedHitAlongX) +{ + const auto box = axis_aligned_obb({0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({-5.f, 0.f, 0.f}, {5.f, 0.f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_NE(hit, ray.end); + EXPECT_NEAR(hit.x, -1.f, 1e-4f); +} + +TEST(LineTracerOBBTests, MissReturnsEnd) +{ + const auto box = axis_aligned_obb({0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({0.f, 5.f, -5.f}, {0.f, 5.f, 5.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_EQ(hit, ray.end); +} + +TEST(LineTracerOBBTests, RayTooShortReturnsEnd) +{ + const auto box = axis_aligned_obb({4.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({0.f, 0.f, 0.f}, {2.f, 0.f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_EQ(hit, ray.end); +} + +TEST(LineTracerOBBTests, InfiniteRayHits) +{ + const auto box = axis_aligned_obb({4.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({0.f, 0.f, 0.f}, {2.f, 0.f, 0.f}, true); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_NE(hit, ray.end); + EXPECT_NEAR(hit.x, 3.f, 1e-4f); +} + +TEST(LineTracerOBBTests, RayStartsInsideBox) +{ + const auto box = axis_aligned_obb({0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({0.f, 0.f, 0.f}, {5.f, 0.f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_NE(hit, ray.end); + EXPECT_NEAR(hit.x, 0.f, 1e-4f); + EXPECT_NEAR(hit.y, 0.f, 1e-4f); + EXPECT_NEAR(hit.z, 0.f, 1e-4f); +} + +TEST(LineTracerOBBTests, RayBehindBoxReturnsEnd) +{ + const auto box = axis_aligned_obb({4.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({10.f, 0.f, 0.f}, {20.f, 0.f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_EQ(hit, ray.end); +} + +// --- rotated OBB --- + +TEST(LineTracerOBBTests, RotatedBoxHitOnRotatedFace) +{ + // Box centred at the origin, rotated 45° around Z. After rotation, the box's "near" face + // (originally x=-1) is now perpendicular to the (1, 1, 0)/√2 direction. A ray approaching + // from +X (along world -X) first hits the box at the rotated face — at x = √2 ≈ 1.414. + const auto box = rotated_around_z({0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}, std::numbers::pi_v / 4.f); + const auto ray = make_ray({5.f, 0.f, 0.f}, {-5.f, 0.f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_NE(hit, ray.end); + EXPECT_NEAR(hit.x, std::numbers::sqrt2_v, 1e-4f); + EXPECT_NEAR(hit.y, 0.f, 1e-4f); + EXPECT_NEAR(hit.z, 0.f, 1e-4f); +} + +TEST(LineTracerOBBTests, RotatedBoxMissesWhereAabbWouldHit) +{ + // A unit cube rotated 45° around Z has an XY footprint that is a diamond reaching + // (±√2, 0) and (0, ±√2). The AABB envelope spans x,y ∈ [-√2, √2], but at y just below √2 + // the diamond is essentially a point. A ray at y = 1.43 is outside the diamond entirely + // (|x| + |y| ≤ √2 ⇒ |x| ≤ √2 - 1.43 < 0), yet it would still pass through the AABB + // envelope of the rotated box. + const auto box = rotated_around_z({0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}, std::numbers::pi_v / 4.f); + const auto ray = make_ray({-5.f, 1.43f, 0.f}, {5.f, 1.43f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_EQ(hit, ray.end); +} + +TEST(LineTracerOBBTests, RotatedThinBoxHitFromTheSide) +{ + // Long, thin axis-aligned slab along X, rotated 90° around Z so it now points along Y. + // A ray from +X straight back along -X must miss (the slab is thin in X), but a ray along + // -Y from +Y must hit. + const auto box = rotated_around_z({0.f, 0.f, 0.f}, {5.f, 0.5f, 1.f}, std::numbers::pi_v / 2.f); + + const auto ray_along_x = make_ray({10.f, 0.f, 0.f}, {-10.f, 0.f, 0.f}); + const auto hit_x = LineTracer::get_ray_hit_point(ray_along_x, box); + EXPECT_NE(hit_x, ray_along_x.end); + EXPECT_NEAR(hit_x.x, 0.5f, 1e-4f); // hit on the rotated slab's narrow side + + const auto ray_along_y = make_ray({0.f, 10.f, 0.f}, {0.f, -10.f, 0.f}); + const auto hit_y = LineTracer::get_ray_hit_point(ray_along_y, box); + EXPECT_NE(hit_y, ray_along_y.end); + EXPECT_NEAR(hit_y.y, 5.f, 1e-4f); // hit on the long end at y=+5 +} + +TEST(LineTracerOBBTests, RotatedAndTranslatedBoxHit) +{ + const auto box = rotated_around_z({10.f, 5.f, 0.f}, {1.f, 1.f, 1.f}, std::numbers::pi_v / 4.f); + // Ray approaches the rotated box from +X. + const auto ray = make_ray({20.f, 5.f, 0.f}, {0.f, 5.f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_NE(hit, ray.end); + EXPECT_NEAR(hit.x, 10.f + std::numbers::sqrt2_v, 1e-4f); + EXPECT_NEAR(hit.y, 5.f, 1e-4f); +} + +TEST(LineTracerOBBTests, ParallelRayOutsideMisses) +{ + // Ray runs parallel to a slab face, completely outside the slab. + const auto box = axis_aligned_obb({0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({-5.f, 2.f, 0.f}, {5.f, 2.f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_EQ(hit, ray.end); +} + +TEST(LineTracerOBBTests, ParallelRayInsideHits) +{ + // Ray runs parallel to a slab face but inside the slab — should still hit the entry plane. + const auto box = axis_aligned_obb({0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}); + const auto ray = make_ray({-5.f, 0.5f, 0.f}, {5.f, 0.5f, 0.f}); + + const auto hit = LineTracer::get_ray_hit_point(ray, box); + EXPECT_NE(hit, ray.end); + EXPECT_NEAR(hit.x, -1.f, 1e-4f); +} + +TEST(LineTracerOBBTests, MatchesAabbForAxisAlignedBox) +{ + using AABB = omath::primitives::Aabb; + + struct + { + Vec3 center; + Vec3 half; + Vec3 ray_start; + Vec3 ray_end; + } cases[] = { + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}, {-5.f, 0.f, 0.f}, {5.f, 0.f, 0.f}}, + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}, {0.f, -5.f, 0.f}, {0.f, 5.f, 0.f}}, + {{4.f, 0.f, 0.f}, {1.f, 1.f, 1.f}, {0.f, 0.f, 0.f}, {2.f, 0.f, 0.f}}, // too short + {{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}, {-5.f, 5.f, 0.f}, {5.f, 5.f, 0.f}}, // miss + {{2.f, 3.f, -1.f}, {0.5f, 0.5f, 0.5f}, {0.f, 0.f, 0.f}, {10.f, 15.f, -5.f}}, // diagonal + }; + + for (const auto& [center, half, start, end]: cases) + { + const AABB aabb{center - half, center + half}; + const auto obb = axis_aligned_obb(center, half); + const auto ray = make_ray(start, end); + + const auto aabb_hit = LineTracer::get_ray_hit_point(ray, aabb); + const auto obb_hit = LineTracer::get_ray_hit_point(ray, obb); + + EXPECT_NEAR(aabb_hit.x, obb_hit.x, 1e-4f); + EXPECT_NEAR(aabb_hit.y, obb_hit.y, 1e-4f); + EXPECT_NEAR(aabb_hit.z, obb_hit.z, 1e-4f); + } +}