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