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