// }
//
// 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.p.x + xfA.q.ex.x * manifold.localPoint.x + xfA.q.ey.x *
// manifold.localPoint.y;
// pointA.y = xfA.p.y + xfA.q.ex.y * manifold.localPoint.x + xfA.q.ey.y *
// manifold.localPoint.y;
// pointB.x = xfB.p.x + xfB.q.ex.x * manifold.points[0].localPoint.x + xfB.q.ey.x *
// manifold.points[0].localPoint.y;
// pointB.y = xfB.p.y + xfB.q.ex.y * manifold.points[0].localPoint.x + xfB.q.ey.y *
// manifold.points[0].localPoint.y;
Transform.mulToOut(xfA, manifold.localPoint, pointA);
Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
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;
Rot.mulToOutUnsafe(xfA.q, 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 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;
Rot.mulToOutUnsafe(xfB.q, manifold.localNormal, normal);
Transform.mulToOut(xfB, manifold.localPoint, planePoint);
// final Mat22 R = xfB.q;
// normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y;
// normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y;
// final Vec2 v = manifold.localPoint;
// planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
// planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.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;