Неправильная работа алгоритма обнаружения столкновений GJK

struct Vertex
{
    glm::vec3 pos;
    glm::vec3 color;
    glm::vec2 texCoord;
};
 
struct Mesh
{
    std::vector<Vertex> vertices;
    std::vector<uint32_t> indices;
    std::vector<glm::dvec3> hitBox;
    glm::dmat4 model;
    glm::dmat4 invModel;
 
    void setModel(const glm::dmat4 &model)
    {
        this->model = model;
        this->invModel = glm::inverse(model);
    }
};
 
enum class SimplexType 
{
    Zero,
    Point,
    Line,
    Triangle,
    Tetrahedron
};
 
struct Simplex
{
    std::list<glm::dvec3> points{};
 
    Simplex() = default;
 
    Simplex(std::initializer_list<glm::dvec3> list) 
    {
        for (const auto& v : list) 
        {
            points.push_back(v);
            if (points.size() > 4) 
            {
                points.pop_front();
            }
        }
    }
 
    void push_front(const glm::dvec3 &point) 
    {
        points.push_front(point);
        if (points.size() > 4) 
        {
            points.pop_back();
        }
    }
 
    glm::dvec3 operator[](size_t i) const 
    {
        auto it = points.begin();
        for (size_t k = 0; k < i; k++) 
        {
            ++it;
        }
        return *it;
 
    }
 
    [[nodiscard]] size_t size() const { return points.size(); }
 
    [[nodiscard]] auto begin() const { return points.begin(); }
 
    [[nodiscard]] auto end() const { return points.end(); }
 
    [[nodiscard]] SimplexType type() const { return static_cast<SimplexType>(points.size()); }
};
 
struct NextSimplex
{
    const Simplex newSimplex;
    const glm::dvec3 newDirection;
    const bool finishSearching;
};
 
class RigidBody
{
public:
    Mesh mesh;
 
    glm::dvec3 findFurthestPoint(const glm::dvec3 &direction)
    {
        glm::dvec3 maxPoint = glm::dvec3(0.0, 0.0, 0.0);
        double maxDistance = 0.0;
 
        glm::dvec3 transformedDirection = glm::normalize(this->mesh.invModel * glm::dvec4(direction, 0.0));
 
        for (auto &it : mesh.hitBox)
        {
            double distance = glm::dot(it, transformedDirection);
 
            if (distance > maxDistance)
            {
                maxDistance = distance;
                maxPoint = it;
            }
        }
 
        return mesh.model * glm::dvec4(maxPoint, 1.0);
    }
 
    glm::dvec3 support(RigidBody &obj, const glm::dvec3 &direction)
    {
        glm::dvec3 p1 = findFurthestPoint(direction);
        glm::dvec3 p2 = obj.findFurthestPoint(-direction);
 
        return p1 - p2;
    }
 
    NextSimplex lineCase(const Simplex &points)
    {
        Simplex newPoints(points);
        glm::dvec3 newDirection;
 
        glm::dvec3 a = points[0];
        glm::dvec3 b = points[1];
 
        glm::dvec3 ab = b - a;
        glm::dvec3 ao = -a;
 
        if (glm::dot(ab, ao) > 0)
        {
            newDirection = glm::cross(glm::cross(ab, ao), ab);
        }
        else
        {
            newPoints = Simplex{ a };
            newDirection = ao;
        }
 
        return NextSimplex{ newPoints, newDirection, false };
    }
 
    NextSimplex triangleCase(const Simplex &points)
    {
        Simplex newPoints(points);
        glm::dvec3 newDirection{};
 
        glm::dvec3 a = points[0];
        glm::dvec3 b = points[1];
        glm::dvec3 c = points[2];
 
        glm::dvec3 ab = b - a;
        glm::dvec3 ac = c - a;
        glm::dvec3 ao = -a;
 
        glm::dvec3 abc = glm::cross(ab, ac);
 
        if (glm::dot(glm::cross(abc, ac), ao) > 0)
        {
            if (glm::dot(ac, ao) > 0)
            {
                newPoints = Simplex{ a, c };
                newDirection = glm::cross(glm::cross(ac, ao), ac);
            }
            else
            {
                return lineCase(Simplex{ a, b });
            }
        }
        else
        {
            if (glm::dot(glm::cross(ab, abc), ao) > 0)
            {
                return lineCase(Simplex{ a, b });
            }
            else
            {
                if (glm::dot(abc, ao) > 0)
                {
                    newDirection = abc;
                }
                else
                {
                    newDirection = -abc;
                    newPoints = Simplex{ a, c, b };
                }
            }
        }
 
        return NextSimplex{ newPoints, newDirection, false };
    }
 
    NextSimplex tetrahedronCase(const Simplex &points)
    {
        glm::dvec3 a = points[0];
        glm::dvec3 b = points[1];
        glm::dvec3 c = points[2];
        glm::dvec3 d = points[3];
 
        glm::dvec3 ab = b - a;
        glm::dvec3 ac = c - a;
        glm::dvec3 ad = d - a;
        glm::dvec3 ao = -a;
 
        glm::dvec3 abc = glm::cross(ab, ac);
        glm::dvec3 acd = glm::cross(ac, ad);
        glm::dvec3 adb = glm::cross(ad, ab);
 
        if (glm::dot(abc, ao) > 0)
        {
            return triangleCase(Simplex{ a, b, c });
        }
 
        if (glm::dot(acd, ao) > 0)
        {
            return triangleCase(Simplex{ a, c, d });
        }
 
        if (glm::dot(adb, ao) > 0)
        {
            return triangleCase(Simplex{ a, d, b });
        }
 
        return NextSimplex(points, glm::dvec3(), true);
    }
 
    NextSimplex nextSimplex(const Simplex &points)
    {
        switch (points.type())
        {
        case SimplexType::Line:
            return lineCase(points);
        case SimplexType::Triangle:
            return triangleCase(points);
        case SimplexType::Tetrahedron:
            return tetrahedronCase(points);
 
        default:
            throw std::logic_error("Simplex is not line, triangle or tetrahedron");
        }
    }
 
    std::pair<bool, Simplex> checkGJKCollision(RigidBody &obj)
    {
        glm::dvec3 support = this->support(obj, glm::dvec3(1.0, 0.0, 0.0));
 
        Simplex points{};
        points.push_front(support);
 
        glm::dvec3 direction = -support;
 
        size_t iters = 0;
        while (++iters < mesh.vertices.size() + obj.mesh.vertices.size())
        {
            support = this->support(obj, direction);
 
            if (glm::dot(support, direction) <= 0)
                return std::make_pair(false, points);
 
            points.push_front(support);
 
            NextSimplex nextSimplex = this->nextSimplex(points);
            direction = nextSimplex.newDirection;
            points = nextSimplex.newSimplex;
 
            if (nextSimplex.finishSearching)
            {
                return std::make_pair(true, points);
            }
        }
 
        return std::make_pair(false, points);
    }
};

glm::dvec3 findFurthestPoint(const glm::dvec3 &direction)
{
    glm::dvec3 maxPoint = glm::dvec3(0.0, 0.0, 0.0);
    double maxDistance = 0.0;
 
    glm::dvec3 transformedDirection = glm::normalize(this->mesh.invModel * glm::dvec4(direction, 0.0));
 
    for (auto &it : mesh.hitBox)
    {
        double distance = glm::dot(it, transformedDirection);
 
        if (distance > maxDistance)
        {
            maxDistance = distance;
            maxPoint = it;
        }
    }
 
    return mesh.model * glm::dvec4(maxPoint, 1.0);
}