Separating axis theorem: rotation around center of mass

Question

The problem is in 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 is an object of type Matrix22 which is a 2x2 matrix.
polygon.points is std::vector<Vector2D> filled with vectors.
polygon.body is a pointer to an Object instance. In this case it's only used to get position.
polygon.body->position is an instance of Vector2D containing X and Y coordinates.
Vector2D polygon.body->GetPosition() returns position vector of a body.

It works fine, except that the rotation is done around the [0, 0] point but it's supposed to rotate around the center of mass.

I know that rotation around a point can be done like this:

rotationMatrix * (vertex - point) + point

And it works fine when rendering polygons. But not in collision detection.

How do I rotate vectors around a certain point in this case?

EDIT: Here's what I have so far

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;
}

Right now I don't really care about optimizations. There's sill the same problem. When rotating around the center collisions aren't being detected properly. However, if the center is [0, 0] or it's not used, then collision detection works properly, but again rotation is being performed wrong.

Edit: Even when rotating before collision detection, I get the same problem. The best approach so far was to translate polygon so that its center would be at [0, 0], but at some angles collisions aren't being detected. Have no idea what to do now.

Edit: Screenshots (polygons are being translated so that their centers of mass are always at [0, 0], polygons are rectangles in this case) Collision detection didn't work well here

Collision detection didn't work well here too

Collision detection worked well here

Edit: I added the Rectangle class.

Answer 1

This should work whether or not polygon origin is aligned to center of gravity. I'll start with the most important stuff, and end with supporting methods that have changed.

Edit: Revised implementation.

struct Response {
        Response()
            : overlap(std::numeric_limits<double>::max()) {}
        Vector2D axis;
        double overlap;
};

bool FindAxisLeastPenetration(const Polygon& a, const Polygon& b,
        Response* response)
{
        for ( unsigned long i = 0; i < a.points.size(); i++ )
         {
            Vector2D axis = a.normals[i];
            Vector2D support = b.GetSupport(-axis);

            double overlap = axis.DotProduct(a.points[i] - support);
            if (overlap <= 0.0)
                return false;
            if (overlap < response->overlap)
             {
                response->overlap = overlap;
                response->axis = axis;
             }
         }
        return true;
}

bool CheckCollisionLocal(const Polygon& a, const Polygon& b,
        Vector2D* min_translation)
// @note assumes untransformed polygons.
{
        Polygon worldA = a.ToWorld();
        Polygon worldB = b.ToWorld();

        Response responseA;
        Response responseB;

        if (!FindAxisLeastPenetration(worldA, worldB, &responseA))
            return false;
        if (!FindAxisLeastPenetration(worldB, worldA, &responseB))
            return false;

        if (responseA.overlap <= responseB.overlap)
            *min_translation =  responseA.axis * responseA.overlap;
        else
            *min_translation = -responseB.axis * responseB.overlap;
        return true;
}

Use case,

bool HandleCollisionLocal(Polygon& a, Polygon& b)
{
        Vector2D translation;
        if (!CheckCollisionLocal(a, b, &translation))
            return false;
        if (MOVE_POLYGON_A)
            a.body.SetPosition(a.body.GetPosition() - translation);
        else
            b.body.SetPosition(b.body.GetPosition() + translation);
        return true;
}

Support,

Polygon Polygon::ToWorld() const
{
        Polygon result = *this;
        for ( auto& point : result.points )
         {
            point = result.rotationMatrix * point;
            point.Add(result.body.GetPosition());
         }
        for ( auto& normal : result.normals )
            normal = result.rotationMatrix * normal;
        return result;
}

Vector2D Polygon::GetSupport(const Vector2D& direction) const
{
        double best_projection = -std::numeric_limits<double>::max();
        Vector2D best_point;

        for ( auto point : points )
         {
            double projection = point.DotProduct(direction);
            if (projection > best_projection)
             {
                best_projection = projection;
                best_point = point;
             }
         }
        return best_point;
}

Note: This version inverts the translation vector to conform with - what seems to be - the standard.