omath/include/omath/collision/gjk_algorithm.hpp

//
// Created by Vlad on 11/9/2025.
//

#pragma once
#include "simplex.hpp"

namespace omath::collision
{
    template<class VertexType>
    struct GjkHitInfo final
    {
        bool hit{false};
        Simplex<VertexType> simplex; // valid only if hit == true and size==4
    };

    struct GjkSettings final
    {
        float epsilon = 1e-6f;
        std::size_t max_iterations = 64;
    };
    template<class ColliderInterfaceType>
    class GjkAlgorithm final
    {
        using VectorType = ColliderInterfaceType::VectorType;
    public:
        [[nodiscard]]
        static VectorType find_support_vertex(const ColliderInterfaceType& collider_a,
                                              const ColliderInterfaceType& collider_b, const VectorType& direction)
        {
            return collider_a.find_abs_furthest_vertex_position(direction)
                   - collider_b.find_abs_furthest_vertex_position(-direction);
        }

        [[nodiscard]]
        static bool is_collide(const ColliderInterfaceType& collider_a, const ColliderInterfaceType& collider_b)
        {
            return is_collide_with_simplex_info(collider_a, collider_b).hit;
        }

        [[nodiscard]]
        static GjkHitInfo<VectorType> is_collide_with_simplex_info(const ColliderInterfaceType& collider_a,
                                                                   const ColliderInterfaceType& collider_b,
                                                                   const GjkSettings& settings = {})
        {
            // Use centroid difference as initial direction — greatly reduces iterations for separated shapes.
            VectorType initial_dir;
            if constexpr (requires { collider_b.get_origin() - collider_a.get_origin(); })
            {
                initial_dir = collider_b.get_origin() - collider_a.get_origin();
                if (initial_dir.dot(initial_dir) < settings.epsilon * settings.epsilon)
                    initial_dir = VectorType{1, 0, 0};
            }
            else
            {
                initial_dir = VectorType{1, 0, 0};
            }

            auto support = find_support_vertex(collider_a, collider_b, initial_dir);

            Simplex<VectorType> simplex;
            simplex.push_front(support);

            auto direction = -support;

            for (std::size_t iteration = 0; iteration < settings.max_iterations; ++iteration)
            {
                support = find_support_vertex(collider_a, collider_b, direction);

                if (support.dot(direction) <= settings.epsilon)
                    return {false, simplex};

                simplex.push_front(support);

                if (simplex.handle(direction))
                    return {true, simplex};
            }
            return {false, simplex};
        }
    };
} // namespace omath::collision