void PolygonBody::calculateNormals(std::vector<Vector2f>& outputNormals) const {
for (size_t i = 0; i < _verticesWorldSpace.size(); ++i) {
Vector2f edge = _verticesWorldSpace[i] - _verticesWorldSpace[(i + 1) % _verticesWorldSpace.size()];
edge.normalize();
outputNormals.push_back(edge.crossProduct(-1.0f));
}
}
//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;
}
