diff --git a/include/omath/collision/gjk_algorithm.hpp b/include/omath/collision/gjk_algorithm.hpp new file mode 100644 index 0000000..2aae2a1 --- /dev/null +++ b/include/omath/collision/gjk_algorithm.hpp @@ -0,0 +1,47 @@ +// +// Created by Vlad on 11/9/2025. +// + +#pragma once +#include "mesh_collider.hpp" +#include "omath/linear_algebra/vector3.hpp" +#include "simplex.hpp" + +namespace omath::collision +{ + class GjkAlgorithm final + { + public: + [[nodiscard]] + static Vector3 find_support_vertex(const MeshCollider& collider_a, const MeshCollider& collider_b, + const Vector3& direction) + { + return collider_a.find_abs_furthest_vertex(direction) - collider_b.find_abs_furthest_vertex(-direction); + } + + [[nodiscard]] + static bool check_collision(const MeshCollider& collider_a, const MeshCollider& collider_b) + { + // Get initial support point in any direction + auto support = find_support_vertex(collider_a, collider_b, {1, 0, 0}); + + Simplex points; + points.push_front(support); + + auto direction = -support; + + while (true) + { + support = find_support_vertex(collider_a, collider_b, direction); + + if (support.dot(direction) <= 0.f) + return false; + + points.push_front(support); + + if (handle_simplex(points, direction)) + return true; + } + } + }; +}// namespace omath::collision \ No newline at end of file diff --git a/include/omath/collision/mesh_collider.hpp b/include/omath/collision/mesh_collider.hpp new file mode 100644 index 0000000..354ce52 --- /dev/null +++ b/include/omath/collision/mesh_collider.hpp @@ -0,0 +1,50 @@ +// +// Created by Vlad on 11/9/2025. +// + +#pragma once +#include "omath/engines/source_engine/traits/pred_engine_trait.hpp" +#include "omath/linear_algebra/vector3.hpp" +#include + +namespace omath::collision +{ + class MeshCollider + { + public: + MeshCollider(const std::vector>& vertexes, const Vector3 origin) + : m_vertexes(vertexes), m_origin(origin) + { + if (m_vertexes.empty()) + throw std::runtime_error("Collider cannot have 0 vertexes"); + } + std::vector> m_vertexes; + Vector3 m_origin; + source_engine::ViewAngles m_rotation; + + [[nodiscard]] + source_engine::Mat4X4 to_world() const + { + return mat_translation(m_origin) * source_engine::rotation_matrix(m_rotation); + } + + [[nodiscard]] + const Vector3& find_furthest_vertex(const Vector3& direction) const + { + return *std::ranges::max_element(m_vertexes, [&direction](const auto& first, const auto& second) + { return first.dot(direction) < second.dot(direction); }); + } + [[nodiscard]] + Vector3 find_abs_furthest_vertex(const Vector3& direction) const + { + return vertex_to_world_space(find_furthest_vertex(direction)); + + } + [[nodiscard]] Vector3 vertex_to_world_space( const Vector3& local_vertex) const + { + auto abs_vec = to_world() * mat_column_from_vector(local_vertex); + + return {abs_vec.at(0, 0), abs_vec.at(1, 0), abs_vec.at(2, 0)}; + } + }; +} // namespace omath::collision \ No newline at end of file diff --git a/include/omath/collision/simplex.hpp b/include/omath/collision/simplex.hpp new file mode 100644 index 0000000..23c9b25 --- /dev/null +++ b/include/omath/collision/simplex.hpp @@ -0,0 +1,160 @@ +// +// Created by Vlad on 11/9/2025. +// + +#pragma once +#include "omath/linear_algebra/vector3.hpp" +#include + +namespace omath::collision +{ + class Simplex + { + std::array, 4> m_points; + int m_size; + + public: + Simplex(): m_size(0) + { + } + + Simplex& operator=(const std::initializer_list> list) + { + m_size = 0; + + for (const Vector3& point : list) + m_points[m_size++] = point; + + return *this; + } + + void push_front(const Vector3& point) + { + m_points = {point, m_points[0], m_points[1], m_points[2]}; + m_size = std::min(m_size + 1, 4); + } + + Vector3& operator[](const int i) + { + return m_points[i]; + } + size_t size() const + { + return m_size; + } + + auto begin() const + { + return m_points.begin(); + } + auto end() const + { + return m_points.end() - (4 - m_size); + } + }; + + bool handle_line(Simplex& points, Vector3& direction) + { + Vector3 a = points[0]; + const Vector3 b = points[1]; + + Vector3 ab = b - a; + const Vector3 ao = -a; + + if (ab.point_to_same_direction(ao)) + direction = ab.cross(ao).cross(ab); + else + { + points = {a}; + direction = ao; + } + + return false; + } + + bool handle_triangle(Simplex& points, Vector3& direction) + { + Vector3 a = points[0]; + Vector3 b = points[1]; + Vector3 c = points[2]; + + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 ao = -a; + + Vector3 abc = ab.cross(ac); + + if (abc.cross(ac).point_to_same_direction(ao)) + { + if (ac.point_to_same_direction(ao)) + { + points = {a, c}; + direction = ac.cross(ao).cross(ac); + + return false; + } + return handle_line(points = {a, b}, direction); + } + + if (ab.cross(abc).point_to_same_direction(ao)) + return handle_line(points = {a, b}, direction); + + + if (abc.point_to_same_direction(ao)) + { + direction = abc; + } + else + { + points = {a, c, b}; + direction = -abc; + } + + return false; + } + + bool handle_tetrahedron(Simplex& points, Vector3& direction) + { + Vector3 a = points[0]; + Vector3 b = points[1]; + Vector3 c = points[2]; + Vector3 d = points[3]; + + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 ad = d - a; + Vector3 ao = -a; + + Vector3 abc = ab.cross(ac); + Vector3 acd = ac.cross(ad); + Vector3 adb = ad.cross(ab); + + if (abc.point_to_same_direction(ao)) + return handle_triangle(points = {a, b, c}, direction); + + if (acd.point_to_same_direction(ao)) + return handle_triangle(points = {a, c, d}, direction); + + if (adb.point_to_same_direction(ao)) + return handle_triangle(points = {a, d, b}, direction); + + + return true; + } + + [[nodiscard]] + bool handle_simplex(Simplex& points, Vector3& direction) + { + switch (points.size()) + { + case 2: + return handle_line(points, direction); + case 3: + return handle_triangle(points, direction); + case 4: + return handle_tetrahedron(points, direction); + default: + return false; + } + } +} // namespace omath::collision \ No newline at end of file diff --git a/include/omath/linear_algebra/vector3.hpp b/include/omath/linear_algebra/vector3.hpp index 77d60cf..5432bb0 100644 --- a/include/omath/linear_algebra/vector3.hpp +++ b/include/omath/linear_algebra/vector3.hpp @@ -216,6 +216,11 @@ namespace omath return sum_2d() + z; } + [[nodiscard]] + bool point_to_same_direction(const Vector3& other) + { + return dot(other) > static_cast(0); + } [[nodiscard]] std::expected, Vector3Error> angle_between(const Vector3& other) const noexcept { diff --git a/tests/general/unit_test_colider.cpp b/tests/general/unit_test_colider.cpp new file mode 100644 index 0000000..97f0eff --- /dev/null +++ b/tests/general/unit_test_colider.cpp @@ -0,0 +1,29 @@ +// +// Created by Vlad on 11/9/2025. +// +#include +#include + + + +TEST(UnitTestColider, CheckToWorld) +{ + const std::vector> mesh = {{1.f, 1.f, 1.f}, {-1.f, -1.f, -1.f}}; + + const omath::collision::MeshCollider collider(mesh, {0.f, 2.f, 0.f}); + + const auto vertex = collider.find_abs_furthest_vertex({1.f, 0.f, 0.f}); + + EXPECT_EQ(vertex, omath::Vector3(1.f, 3.f, 1.f)); +} + +TEST(UnitTestColider, FindFurthestVertex) +{ + const std::vector> mesh = {{1.f, 1.f, 1.f}, {-1.f, -1.f, -1.f}}; + const omath::collision::MeshCollider collider(mesh, {0.f, 0.f, 0.f}); + const auto vertex = collider.find_furthest_vertex({1.f, 0.f, 0.f}); + EXPECT_EQ(vertex, omath::Vector3(1.f, 1.f, 1.f)); +} + + + diff --git a/tests/general/unit_test_gjk.cpp b/tests/general/unit_test_gjk.cpp new file mode 100644 index 0000000..9ec5f42 --- /dev/null +++ b/tests/general/unit_test_gjk.cpp @@ -0,0 +1,24 @@ +// +// Created by Vlad on 11/9/2025. +// +#include +#include + +TEST(UnitTestGjk, TestCollision) +{ + const std::vector> mesh = { + {-1.f, -1.f, -1.f}, + {-1.f, -1.f, 1.f}, + {-1.f, 1.f, -1.f}, + {-1.f, 1.f, 1.f}, + { 1.f, 1.f, 1.f}, // x = +1 vertices (put {1,1,1} first in case your support breaks ties by first-hit) + { 1.f, 1.f, -1.f}, + { 1.f, -1.f, 1.f}, + { 1.f, -1.f, -1.f} + }; + + const omath::collision::MeshCollider collider_a(mesh, {0.f, 0.f, 0.f}); + const omath::collision::MeshCollider collider_b(mesh, {0.f, 3.f, 0.f}); + + const auto result = omath::collision::GjkAlgorithm::check_collision(collider_a, collider_b); +} \ No newline at end of file