/**
* Compute the collision manifold between a polygon and a circle.
*
* @param manifold
* @param polygon
* @param xfA
* @param circle
* @param xfB
*/
public final void collidePolygonAndCircle(
Manifold manifold,
final PolygonShape polygon,
final Transform xfA,
final CircleShape circle,
final Transform xfB) {
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
Transform.mulToOut(xfB, circle.m_p, c);
Transform.mulTransToOut(xfA, c, cLocal);
// Find the min separating edge.
int normalIndex = 0;
float separation = Float.MIN_VALUE;
float radius = polygon.m_radius + circle.m_radius;
int vertexCount = polygon.m_vertexCount;
Vec2[] vertices = polygon.m_vertices;
Vec2[] normals = polygon.m_normals;
for (int i = 0; i < vertexCount; i++) {
temp.set(cLocal).subLocal(vertices[i]);
float s = Vec2.dot(normals[i], temp);
if (s > radius) {
// early out
return;
}
if (s > separation) {
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.EPSILON) {
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(normals[normalIndex]);
manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
return;
}
// Compute barycentric coordinates
temp.set(cLocal).subLocal(v1);
temp2.set(v2).subLocal(v1);
float u1 = Vec2.dot(temp, temp2);
temp.set(cLocal).subLocal(v2);
temp2.set(v1).subLocal(v2);
float u2 = Vec2.dot(temp, temp2);
if (u1 <= 0f) {
if (MathUtils.distanceSquared(cLocal, v1) > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(cLocal).subLocal(v1);
manifold.localNormal.normalize();
manifold.localPoint.set(v1);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
} else if (u2 <= 0.0f) {
if (MathUtils.distanceSquared(cLocal, v2) > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(cLocal).subLocal(v2);
manifold.localNormal.normalize();
manifold.localPoint.set(v2);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
} else {
// Vec2 faceCenter = 0.5f * (v1 + v2);
// (temp is faceCenter)
temp.set(v1).addLocal(v2).mulLocal(.5f);
temp2.set(cLocal).subLocal(temp);
separation = Vec2.dot(temp2, normals[vertIndex1]);
if (separation > radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(normals[vertIndex1]);
manifold.localPoint.set(temp); // (faceCenter)
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
}
}
public final void initialize(
final Manifold manifold,
final Transform xfA,
float radiusA,
final Transform xfB,
float radiusB) {
if (manifold.pointCount == 0) {
return;
}
switch (manifold.type) {
case CIRCLES:
{
// final Vec2 pointA = pool3;
// final Vec2 pointB = pool4;
//
// normal.set(1, 0);
// Transform.mulToOut(xfA, manifold.localPoint, pointA);
// Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
//
// if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON)
// {
// normal.set(pointB).subLocal(pointA);
// normal.normalize();
// }
//
// cA.set(normal).mulLocal(radiusA).addLocal(pointA);
// cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal();
// points[0].set(cA).addLocal(cB).mulLocal(0.5f);
final Vec2 pointA = pool3;
final Vec2 pointB = pool4;
normal.x = 1;
normal.y = 0;
pointA.x =
xfA.position.x
+ xfA.R.col1.x * manifold.localPoint.x
+ xfA.R.col2.x * manifold.localPoint.y;
pointA.y =
xfA.position.y
+ xfA.R.col1.y * manifold.localPoint.x
+ xfA.R.col2.y * manifold.localPoint.y;
pointB.x =
xfB.position.x
+ xfB.R.col1.x * manifold.points[0].localPoint.x
+ xfB.R.col2.x * manifold.points[0].localPoint.y;
pointB.y =
xfB.position.y
+ xfB.R.col1.y * manifold.points[0].localPoint.x
+ xfB.R.col2.y * manifold.points[0].localPoint.y;
if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) {
normal.x = pointB.x - pointA.x;
normal.y = pointB.y - pointA.y;
normal.normalize();
}
final float cAx = normal.x * radiusA + pointA.x;
final float cAy = normal.y * radiusA + pointA.y;
final float cBx = -normal.x * radiusB + pointB.x;
final float cBy = -normal.y * radiusB + pointB.y;
points[0].x = (cAx + cBx) * .5f;
points[0].y = (cAy + cBy) * .5f;
}
break;
case FACE_A:
{
// final Vec2 planePoint = pool3;
//
// Mat22.mulToOut(xfA.R, manifold.localNormal, normal);
// Transform.mulToOut(xfA, manifold.localPoint, planePoint);
//
// final Vec2 clipPoint = pool4;
//
// for (int i = 0; i < manifold.pointCount; i++) {
// // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
// // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
// // normal)) * normal;
// // b2Vec2 cB = clipPoint - radiusB * normal;
// // points[i] = 0.5f * (cA + cB);
// Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint);
// // use cA as temporary for now
// cA.set(clipPoint).subLocal(planePoint);
// float scalar = radiusA - Vec2.dot(cA, normal);
// cA.set(normal).mulLocal(scalar).addLocal(clipPoint);
// cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal();
// points[i].set(cA).addLocal(cB).mulLocal(0.5f);
// }
final Vec2 planePoint = pool3;
normal.x = xfA.R.col1.x * manifold.localNormal.x + xfA.R.col2.x * manifold.localNormal.y;
normal.y = xfA.R.col1.y * manifold.localNormal.x + xfA.R.col2.y * manifold.localNormal.y;
planePoint.x =
xfA.position.x
+ xfA.R.col1.x * manifold.localPoint.x
+ xfA.R.col2.x * manifold.localPoint.y;
planePoint.y =
xfA.position.y
+ xfA.R.col1.y * manifold.localPoint.x
+ xfA.R.col2.y * manifold.localPoint.y;
final Vec2 clipPoint = pool4;
for (int i = 0; i < manifold.pointCount; i++) {
// b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
// b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
// normal)) * normal;
// b2Vec2 cB = clipPoint - radiusB * normal;
// points[i] = 0.5f * (cA + cB);
clipPoint.x =
xfB.position.x
+ xfB.R.col1.x * manifold.points[i].localPoint.x
+ xfB.R.col2.x * manifold.points[i].localPoint.y;
clipPoint.y =
xfB.position.y
+ xfB.R.col1.y * manifold.points[i].localPoint.x
+ xfB.R.col2.y * manifold.points[i].localPoint.y;
final float scalar =
radiusA
- ((clipPoint.x - planePoint.x) * normal.x
+ (clipPoint.y - planePoint.y) * normal.y);
final float cAx = normal.x * scalar + clipPoint.x;
final float cAy = normal.y * scalar + clipPoint.y;
final float cBx = -normal.x * radiusB + clipPoint.x;
final float cBy = -normal.y * radiusB + clipPoint.y;
points[i].x = (cAx + cBx) * .5f;
points[i].y = (cAy + cBy) * .5f;
}
}
break;
case FACE_B:
final Vec2 planePoint = pool3;
final Mat22 R = xfB.R;
normal.x = R.col1.x * manifold.localNormal.x + R.col2.x * manifold.localNormal.y;
normal.y = R.col1.y * manifold.localNormal.x + R.col2.y * manifold.localNormal.y;
final Vec2 v = manifold.localPoint;
planePoint.x = xfB.position.x + xfB.R.col1.x * v.x + xfB.R.col2.x * v.y;
planePoint.y = xfB.position.y + xfB.R.col1.y * v.x + xfB.R.col2.y * v.y;
final Vec2 clipPoint = pool4;
for (int i = 0; i < manifold.pointCount; i++) {
// b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
// b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint,
// normal)) * normal;
// b2Vec2 cA = clipPoint - radiusA * normal;
// points[i] = 0.5f * (cA + cB);
// Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint);
// cB.set(clipPoint).subLocal(planePoint);
// float scalar = radiusB - Vec2.dot(cB, normal);
// cB.set(normal).mulLocal(scalar).addLocal(clipPoint);
// cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal();
// points[i].set(cA).addLocal(cB).mulLocal(0.5f);
// points[i] = 0.5f * (cA + cB);
clipPoint.x =
xfA.position.x
+ xfA.R.col1.x * manifold.points[i].localPoint.x
+ xfA.R.col2.x * manifold.points[i].localPoint.y;
clipPoint.y =
xfA.position.y
+ xfA.R.col1.y * manifold.points[i].localPoint.x
+ xfA.R.col2.y * manifold.points[i].localPoint.y;
final float scalar =
radiusB
- ((clipPoint.x - planePoint.x) * normal.x
+ (clipPoint.y - planePoint.y) * normal.y);
final float cBx = normal.x * scalar + clipPoint.x;
final float cBy = normal.y * scalar + clipPoint.y;
final float cAx = -normal.x * radiusA + clipPoint.x;
final float cAy = -normal.y * radiusA + clipPoint.y;
points[i].x = (cAx + cBx) * .5f;
points[i].y = (cAy + cBy) * .5f;
}
// Ensure normal points from A to B.
normal.x = -normal.x;
normal.y = -normal.y;
break;
}
}
