Странное поведение алгоритма обнаружения столкновений GJK ⇐ C++

[Anonymous]

На 1 изображении коллизии нет, а на 2 есть
У меня ошибка в алгоритме GJK. Я использую библиотеку glm и столкнулся с проблемой: когда я увеличиваю размер модели с помощью матрицы, алгоритм GJK не находит коллизию (некоторая часть объекта может быть погружена в другой объект, но коллизии нет)
Вот код:
struct Vertex
{
glm::vec3 pos;
glm::vec3 color;
glm::vec2 texCoord;
};

struct Mesh
{
std::vector vertices;
std::vector indices;
std::vector 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 points{};

Simplex() = default;

Simplex(std::initializer_list 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(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 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) 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);
}

Следует отметить, что если вы умножите «это» на каждой итерации цикла на матрицу модели и замените glm::dot(..., TransformedDirection) на glm::dot( ..., направление), ошибок не возникает. Возможно проблема в матрице invModel, но я не понимаю в чем проблема. Где я допустил ошибку?
Отредактировано:
Код создания матриц модели (objAngleX, objAngleZ, objX, objZ - имеют динамическое значение):
auto modelMatrix = glm::translate(glm::dmat4(1.0), glm::dvec3(0.0, 0.0, 0.0));
modelMatrix = glm::scale(modelMatrix, glm::dvec3(1.0, 1.5, 1.5));

box2.mesh.setModel(modelMatrix);
auto cameraPos = camera.position;
cameraPos.z -= 2.0;
modelMatrix = glm::translate(glm::dmat4(1.0), glm::dvec3(cameraPos));
modelMatrix = glm::rotate(modelMatrix, objAngleX, glm::dvec3(1.0, 0.0, 0.0));
modelMatrix = glm::rotate(modelMatrix, objAngleZ, glm::dvec3(0.0, 0.0, 1.0));
modelMatrix = glm::scale(modelMatrix, glm::dvec3(0.3 + objX, 1.0, 0.3 + objZ));
box1.mesh.setModel(modelMatrix);

isCollision = box1.checkGJKCollision(box2).first;


Подробнее здесь: https://stackoverflow.com/questions/782 ... -algorithm