//Calculates the closest edge involved in the collision
Edge PolygonBody::calculateCollisionEdge(const Vector2f& collisionNormal) const {
float maxProjection = collisionNormal.dotProduct(_verticesWorldSpace[0]);
int vertexIndex = 0;
//We are looking for the furthest projected vertex
for (size_t i = 1; i < _verticesWorldSpace.size(); ++i) {
float projection = collisionNormal.dotProduct(_verticesWorldSpace[i]);
if (projection > maxProjection) {
maxProjection = projection;
vertexIndex = i;
}
}
Edge edge;
edge.vertex = _verticesWorldSpace[vertexIndex];
const Vector2f& previousVertex = _verticesWorldSpace[(vertexIndex - 1) % _verticesWorldSpace.size()];
const Vector2f& nextVertex = _verticesWorldSpace[(vertexIndex + 1) % _verticesWorldSpace.size()];
Vector2f edgeA = edge.vertex - nextVertex;
Vector2f edgeB = edge.vertex - previousVertex;
//We take the dot products and compare them to find the most perpendicular edge
if (fabsf(edgeA.dotProduct(collisionNormal)) <= fabsf(edgeB.dotProduct(collisionNormal))) {
edge.start = edge.vertex;
edge.end = nextVertex;
} else {
edge.start = previousVertex;
edge.end = edge.vertex;
}
return edge;
}
//This is just a simple clipping function for clipping a line(vertexA, vertexB)
//with respect to another line (clippingEdge, offset)
std::vector<Vector2f> PolygonBody::clipPoints(const Vector2f& vertexA, const Vector2f& vertexB, const Vector2f& clippingEdge, float offset) const {
std::vector<Vector2f> clippedPoints;
//We start by checking if each vertex is within the clipping region defined by the clippingEdge and offset along it
//If a vertex is outside of the clipping region, we add it to our list of clipped points
float distanceA = clippingEdge.dotProduct(vertexA) - offset;
if (distanceA >= 0.0f)
clippedPoints.push_back(vertexA);
float distanceB = clippingEdge.dotProduct(vertexB) - offset;
if (distanceB >= 0.0f)
clippedPoints.push_back(vertexB);
//If the points are on opposing sides of the clipping plane,
//we need to compute a new point at the clipping limit
if (distanceA * distanceB < 0.0f) {
Vector2f edge = vertexB - vertexA;
float d = distanceA / (distanceA - distanceB);
edge *= d;
edge += vertexA;
clippedPoints.push_back(edge);
}
return clippedPoints;
}
//Calculates either 1 or 2 contact points for a collision depending on
//how the polygons overlap
std::vector<Vector2f> PolygonBody::calculateContactPoints(PolygonBody& polygon, const Vector2f& collisionNormal) const {
Edge edgeA = calculateCollisionEdge(collisionNormal);
Edge edgeB = polygon.calculateCollisionEdge(-collisionNormal);
const Edge* referenceEdge = 0;
const Edge* incidentEdge = 0;
float dotA = (edgeA.end - edgeA.start).dotProduct(collisionNormal);
float dotB = (edgeB.end - edgeB.start).dotProduct(collisionNormal);
//The reference edge is the most perpendicular to the collision normal (dot product is closest to zero)
if (fabsf(dotA) <= fabsf(dotB)) {
referenceEdge = &edgeA;
incidentEdge = &edgeB;
} else {
referenceEdge = &edgeB;
incidentEdge = &edgeA;
}
Vector2f referenceVector = referenceEdge->end - referenceEdge->start;
referenceVector.normalize();
std::vector<Vector2f> clippedPoints = clipPoints(incidentEdge->start, incidentEdge->end, referenceVector, referenceVector.dotProduct(referenceEdge->start));
if (clippedPoints.size() < 2)
return std::vector<Vector2f>(); //something went wrong, return an empty vector
clippedPoints = clipPoints(clippedPoints[0], clippedPoints[1], -referenceVector, -(referenceVector.dotProduct(referenceEdge->end)));
if (clippedPoints.size() < 2)
return std::vector<Vector2f>(); //something went wrong, return an empty vector
Vector2f referenceNormal = referenceVector.crossProduct(-1.0f);
float max = referenceNormal.dotProduct(referenceEdge->vertex);
if (referenceNormal.dotProduct(clippedPoints[1]) > max)
clippedPoints.erase(clippedPoints.begin() + 1);
if (referenceNormal.dotProduct(clippedPoints[0]) > max)
clippedPoints.erase(clippedPoints.begin());
return clippedPoints;
}
