Теорема о разделяющей оси: вращение вокруг центра масс

Проблема в Polygon::FindAxisLeastPenetration:

double Polygon::FindAxisLeastPenetration(unsigned int *faceIndex, const Polygon &polygonA, const Polygon &polygonB) const {
double bestDistance = -std::numeric_limits<double>::infinity();
unsigned int bestIndex;

for (unsigned int i = 0; i < polygonA.points.size(); i++) {
Vector2D n = polygonA.normals[i];
Vector2D nw = polygonA.rotationMatrix * n; //ROTATION
Matrix22 buT = polygonB.rotationMatrix.Transposed();
n = buT * nw; //ROTATION

Vector2D support = polygonB.points[polygonB.GetSupport(-n)];

Vector2D vertex = polygonA.points[i];
vertex = polygonA.rotationMatrix * vertex; //ROTATION
vertex.Add(polygonA.body->GetPosition());
vertex.Subtract(polygonB.body->GetPosition());
vertex = buT * vertex; // ROTATION
double distance = n.DotProduct(support - vertex);
if (distance > bestDistance) {
bestDistance = distance;
bestIndex = i;
}
}
*faceIndex = bestIndex;

return bestDistance;
}

unsigned int Polygon::GetSupport(const Vector2D &dir) const {
double bestProjection = -std::numeric_limits<double>::infinity();
unsigned int bestIndex = 0;

for (unsigned int i = 0; i < points.size(); i++) {
Vector2D vertex = points[i];
double projection = vertex.DotProduct(dir);

if (projection > bestProjection) {
bestProjection = projection;
bestIndex = i;
}
}

return bestIndex;
}

Manifold Polygon::CheckCollision(const Polygon &polygonA, const Polygon &polygonB) const {
Manifold result;
result.objectA = polygonA.body;
result.objectB = polygonB.body;
unsigned int indexA;
double penetrationA = Polygon::FindAxisLeastPenetration(&indexA, polygonA, polygonB);
if (penetrationA >= 0.0) {
result.intersects = false;
return result;
}

unsigned int indexB;
double penetrationB = Polygon::FindAxisLeastPenetration(&indexB, polygonB, polygonA);

if (penetrationB >= 0.0) {
result.intersects = false;
return result;
}

result.intersects = true;
//...
return result;

Rectangle::Rectangle(double width, double height) : Polygon() {
double hw = width / 2.0;
double hh = height / 2.0;
points.push_back(Vector2D(-hw, -hh));
points.push_back(Vector2D(hw, -hh));
points.push_back(Vector2D(hw, hh));
points.push_back(Vector2D(-hw, hh));

//  points.push_back(Vector2D(0, 0));
//  points.push_back(Vector2D(width, 0));
//  points.push_back(Vector2D(width, height));
//  points.push_back(Vector2D(0, height));

normals.push_back(Vector2D(0.0, -1.0));
normals.push_back(Vector2D(1.0, 0.0));
normals.push_back(Vector2D(0.0, 1.0));
normals.push_back(Vector2D(-1.0, 0.0));

center.x = 0;
center.y = 0;

}

polygon.rotationMatrix является объектом типа Matrix22 которая является матрицей 2×2.
polygon.points является std::vector<Vector2D> заполнены векторами.
polygon.body это указатель на Object пример. В этом случае он используется только для получения позиции.
polygon.body->position это пример Vector2D содержащий X а также Y координаты.
Vector2D polygon.body->GetPosition() возвращает вектор положения тела.

Он работает нормально, за исключением того, что вращение осуществляется вокруг [0, 0] точка
но он должен вращаться вокруг центра масс.

Я знаю, что вращение вокруг точки можно сделать так:

rotationMatrix * (vertex - point) + point

И это прекрасно работает при рендеринг полигонов.
Но не в обнаружении столкновений.

Как мне повернуть векторы вокруг определенной точки в этом случае?

РЕДАКТИРОВАТЬ: Вот что у меня есть до сих пор

double Polygon::FindAxisLeastPenetration(unsigned int *faceIndex, const Polygon &polygonA, const Polygon &polygonB) const {
double bestDistance = -std::numeric_limits<double>::infinity();
unsigned int bestIndex;

for (unsigned int i = 0; i < polygonA.points.size(); i++) {
// Calculate normal
unsigned int j = i == points.size() ? 0 : i + 1;
Vector2D n;
// Rotate points
Vector2D p1 = polygonA.rotationMatrix * (polygonA.points[i] - polygonA.Center()) + polygonA.Center();
Vector2D p2 = polygonA.rotationMatrix * (polygonA.points[j] - polygonA.Center()) + polygonA.Center();
n.x = p2.y - p1.y;
n.y = -(p2.x - p1.x);
n.Normalize();

Vector2D support = polygonB.points[polygonB.GetSupport(-n)];
support = polygonB.rotationMatrix * (support - polygonB.Center()) + polygonB.Center();
support.Add(polygonB.body->GetPosition());

Vector2D vertex = polygonA.points[i];
vertex = polygonA.rotationMatrix * (vertex - polygonA.Center()) + polygonA.Center(); //ROTATION
vertex.Add(polygonA.body->GetPosition());

double distance = n.DotProduct(support - vertex);
if (distance > bestDistance) {
bestDistance = distance;
bestIndex = i;
}
}
*faceIndex = bestIndex;

return bestDistance;
}

unsigned int Polygon::GetSupport(const Vector2D &dir) const {
double bestProjection = -std::numeric_limits<double>::infinity();
unsigned int bestIndex = 0;

for (unsigned int i = 0; i < points.size(); i++) {
Vector2D vertex = rotationMatrix * (points[i] - center) + center;
double projection = vertex.DotProduct(dir);

if (projection > bestProjection) {
bestProjection = projection;
bestIndex = i;
}
}

return bestIndex;
}

Сейчас меня не волнуют оптимизации. Там все та же проблема. При вращении вокруг центра столкновения не обнаруживаются должным образом. Тем не менее, если центр [0, 0] или это не используется, тогда обнаружение столкновения работает должным образом, но снова вращение выполняется неправильно.

Изменить: Даже при вращении до обнаружения столкновения, я получаю ту же проблему.
На сегодняшний день лучшим подходом было перевести многоугольник так, чтобы его центр находился в [0, 0], но под некоторыми углами столкновения не обнаруживаются. Понятия не имею, что делать сейчас.

Изменить: скриншоты (полигоны переводятся так, чтобы их центры масс всегда были на [0, 0]в этом случае полигоны являются прямоугольниками)
Обнаружение столкновения не сработало

Обнаружение столкновений здесь тоже не сработало

Обнаружение столкновения хорошо сработало

Изменить: я добавил Rectangle учебный класс.