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.

@uitty400 none. Everything is custom. Are you looking for implementation of some function or class? — ivknv, Jun 17 '15 at 12:47
You need to clarify what you're doing. It does not look like intersection code of two polygons. What is `points` and where does it come from? What is `polygon.GetSupport()`? What is `polygon.body` and its `.position`? — Nico Schertler, Jun 17 '15 at 14:35
I don't see any rotation code. Are you performing rotation on your actual `Polygon` object or just when rendering it? Each of the points in the `Polygon` need to be rotated (it can be via the equation you provided to rotate around a point) before the collision detection. — Kyle, Jun 17 '15 at 15:01
@Kyle The rotation is performed by multiplying `rotationMatrix` by a vector. It is not applied to whole polygon and is not saved after collision detection. — ivknv, Jun 17 '15 at 15:02
@Kyle I've marked lines where rotation is performed with `//ROTATION` comment — ivknv, Jun 17 '15 at 15:07
Is there a reason you can't translate your models to place their center of mass at origin? — Johnny Cage, Jun 17 '15 at 16:33
Ok, so now I translate polygons when they are created so that their center of mass is `[0, 0]`. But now at some angles collision between polygons is not being detected. — ivknv, Jun 17 '15 at 17:14
I'm not really sure why you have the rotations you do (multiplying `polygonA.point[i]` by `polygonB.rotationMatrix.Transpose()`?). I would expect it to be `newPoints[i] = polygonA.rotationMatrix * (polygonA.points[i] - polygonA.centerOfGravity) + polygonA.centerOfGravity;` for every point, then run any logic on the transformed points. That being said, you probably shouldn't be performing rotation logic in your collision functions without a good reason. — Kyle, Jun 17 '15 at 17:20
You're computing the support point for `B` before it has been transformed. I only have a broad understanding of this algorithm, but for SAT shouldn't you check `A` against `B` and `B` against `A`? Maybe try calling twice with the `polygon` arguments exchanged. — Johnny Cage, Jun 18 '15 at 10:27
@JohnnyCage `n = buT * nw` is to rotate the normal and avoid rotating every point in `polygonB`. And in `Polygon::CheckCollision` I'm checking A against B and B against A. — ivknv, Jun 18 '15 at 10:31
Sorry, I was looking at the edited version. My suggestion would be to render the collision geometry to try and get a visual of what's going on. — Johnny Cage, Jun 18 '15 at 10:54

Johnny Cage · Accepted Answer · 2015-06-20T00:07:32.117

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.

Separating axis theorem: rotation around center of mass

1 Answers1