initialize method
Implementation
void initialize(
ContactPositionConstraint pc,
Transform xfA,
Transform xfB,
int index,
) {
assert(pc.pointCount > 0);
final xfAq = xfA.q;
final xfBq = xfB.q;
final pcLocalPointsI = pc.localPoints[index];
switch (pc.type) {
case ManifoldType.circles:
final pLocalPoint = pc.localPoint;
final pLocalPoints0 = pc.localPoints[0];
final pointAx =
(xfAq.cos * pLocalPoint.x - xfAq.sin * pLocalPoint.y) + xfA.p.x;
final pointAy =
(xfAq.sin * pLocalPoint.x + xfAq.cos * pLocalPoint.y) + xfA.p.y;
final pointBx =
(xfBq.cos * pLocalPoints0.x - xfBq.sin * pLocalPoints0.y) + xfB.p.x;
final pointBy =
(xfBq.sin * pLocalPoints0.x + xfBq.cos * pLocalPoints0.y) + xfB.p.y;
normal.x = pointBx - pointAx;
normal.y = pointBy - pointAy;
normal.normalize();
point.x = (pointAx + pointBx) * .5;
point.y = (pointAy + pointBy) * .5;
final tempX = pointBx - pointAx;
final tempY = pointBy - pointAy;
separation =
tempX * normal.x + tempY * normal.y - pc.radiusA - pc.radiusB;
break;
case ManifoldType.faceA:
final pcLocalNormal = pc.localNormal;
final pcLocalPoint = pc.localPoint;
normal.x = xfAq.cos * pcLocalNormal.x - xfAq.sin * pcLocalNormal.y;
normal.y = xfAq.sin * pcLocalNormal.x + xfAq.cos * pcLocalNormal.y;
final planePointX =
(xfAq.cos * pcLocalPoint.x - xfAq.sin * pcLocalPoint.y) + xfA.p.x;
final planePointY =
(xfAq.sin * pcLocalPoint.x + xfAq.cos * pcLocalPoint.y) + xfA.p.y;
final clipPointX =
(xfBq.cos * pcLocalPointsI.x - xfBq.sin * pcLocalPointsI.y) +
xfB.p.x;
final clipPointY =
(xfBq.sin * pcLocalPointsI.x + xfBq.cos * pcLocalPointsI.y) +
xfB.p.y;
final tempX = clipPointX - planePointX;
final tempY = clipPointY - planePointY;
separation =
tempX * normal.x + tempY * normal.y - pc.radiusA - pc.radiusB;
point.x = clipPointX;
point.y = clipPointY;
break;
case ManifoldType.faceB:
final pcLocalNormal = pc.localNormal;
final pcLocalPoint = pc.localPoint;
normal.x = xfBq.cos * pcLocalNormal.x - xfBq.sin * pcLocalNormal.y;
normal.y = xfBq.sin * pcLocalNormal.x + xfBq.cos * pcLocalNormal.y;
final planePointX =
(xfBq.cos * pcLocalPoint.x - xfBq.sin * pcLocalPoint.y) + xfB.p.x;
final planePointY =
(xfBq.sin * pcLocalPoint.x + xfBq.cos * pcLocalPoint.y) + xfB.p.y;
final clipPointX =
(xfAq.cos * pcLocalPointsI.x - xfAq.sin * pcLocalPointsI.y) +
xfA.p.x;
final clipPointY =
(xfAq.sin * pcLocalPointsI.x + xfAq.cos * pcLocalPointsI.y) +
xfA.p.y;
final tempX = clipPointX - planePointX;
final tempY = clipPointY - planePointY;
separation =
tempX * normal.x + tempY * normal.y - pc.radiusA - pc.radiusB;
point.x = clipPointX;
point.y = clipPointY;
normal.x *= -1;
normal.y *= -1;
break;
}
}