initialize method

void initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index,)

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