collidePolygonAndCircle method
void
collidePolygonAndCircle(
- Manifold manifold,
- PolygonShape polygon,
- Transform xfA,
- CircleShape circle,
- Transform xfB,
Compute the collision manifold between a polygon and a circle.
Implementation
void collidePolygonAndCircle(
Manifold manifold,
PolygonShape polygon,
Transform xfA,
CircleShape circle,
Transform xfB,
) {
manifold.pointCount = 0;
// Vec2 v = circle.p;
// Compute circle position in the frame of the polygon.
// before inline:
// Transform.mulToOutUnsafe(xfB, circle.p, c);
// Transform.mulTransToOut(xfA, c, cLocal);
// final double cLocalx = cLocal.x;
// final double cLocaly = cLocal.y;
// after inline:
final circlep = circle.position;
final xfBq = xfB.q;
final xfAq = xfA.q;
final cx = (xfBq.cos * circlep.x - xfBq.sin * circlep.y) + xfB.p.x;
final cy = (xfBq.sin * circlep.x + xfBq.cos * circlep.y) + xfB.p.y;
final px = cx - xfA.p.x;
final py = cy - xfA.p.y;
final cLocalX = xfAq.cos * px + xfAq.sin * py;
final cLocalY = -xfAq.sin * px + xfAq.cos * py;
// end inline
// Find the min separating edge.
var normalIndex = 0;
var separation = -double.maxFinite;
final radius = polygon.radius + circle.radius;
double s;
final vertices = polygon.vertices;
final normals = polygon.normals;
for (var i = 0; i < vertices.length; i++) {
// before inline
// temp.set(cLocal).subLocal(vertices[i]);
// double s = Vec2.dot(normals[i], temp);
// after inline
final vertex = vertices[i];
final tempX = cLocalX - vertex.x;
final tempY = cLocalY - vertex.y;
s = normals[i].x * tempX + normals[i].y * tempY;
if (s > radius) {
// early out
return;
}
if (s > separation) {
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
final vertIndex1 = normalIndex;
final vertIndex2 = vertIndex1 + 1 < vertices.length ? vertIndex1 + 1 : 0;
final v1 = vertices[vertIndex1];
final v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < settings.epsilon) {
manifold.pointCount = 1;
manifold.type = ManifoldType.faceA;
final normal = normals[normalIndex];
manifold.localNormal.x = normal.x;
manifold.localNormal.y = normal.y;
manifold.localPoint.x = (v1.x + v2.x) * .5;
manifold.localPoint.y = (v1.y + v2.y) * .5;
final mpoint = manifold.points[0];
mpoint.localPoint.x = circlep.x;
mpoint.localPoint.y = circlep.y;
mpoint.id.zero();
// end inline
return;
}
// Compute barycentric coordinates
final tempX = cLocalX - v1.x;
final tempY = cLocalY - v1.y;
final temp2X = v2.x - v1.x;
final temp2Y = v2.y - v1.y;
final u1 = tempX * temp2X + tempY * temp2Y;
final temp3X = cLocalX - v2.x;
final temp3Y = cLocalY - v2.y;
final temp4X = v1.x - v2.x;
final temp4Y = v1.y - v2.y;
final u2 = temp3X * temp4X + temp3Y * temp4Y;
if (u1 <= 0.0) {
final dx = cLocalX - v1.x;
final dy = cLocalY - v1.y;
if (dx * dx + dy * dy > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.faceA;
manifold.localNormal.x = cLocalX - v1.x;
manifold.localNormal.y = cLocalY - v1.y;
manifold.localNormal.normalize();
manifold.localPoint.setFrom(v1);
manifold.points[0].localPoint.setFrom(circlep);
manifold.points[0].id.zero();
} else if (u2 <= 0.0) {
final dx = cLocalX - v2.x;
final dy = cLocalY - v2.y;
if (dx * dx + dy * dy > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.faceA;
manifold.localNormal.x = cLocalX - v2.x;
manifold.localNormal.y = cLocalY - v2.y;
manifold.localNormal.normalize();
manifold.localPoint.setFrom(v2);
manifold.points[0].localPoint.setFrom(circlep);
manifold.points[0].id.zero();
} else {
final fcx = (v1.x + v2.x) * .5;
final fcy = (v1.y + v2.y) * .5;
final tx = cLocalX - fcx;
final ty = cLocalY - fcy;
final normal = normals[vertIndex1];
separation = tx * normal.x + ty * normal.y;
if (separation > radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.faceA;
manifold.localNormal.setFrom(normals[vertIndex1]);
manifold.localPoint.x = fcx; // (faceCenter)
manifold.localPoint.y = fcy;
manifold.points[0].localPoint.setFrom(circlep);
manifold.points[0].id.zero();
}
}