collidePolygonAndCircle method

void collidePolygonAndCircle(
  1. Manifold manifold,
  2. PolygonShape polygon,
  3. Transform xfA,
  4. CircleShape circle,
  5. Transform xfB
)

Compute the collision manifold between a polygon and a circle.

Implementation

void collidePolygonAndCircle(Manifold manifold, final PolygonShape polygon,
    final Transform xfA, final CircleShape circle, final 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 Vector2 circlep = circle.p;
  final Rot xfBq = xfB.q;
  final Rot xfAq = xfA.q;
  final double cx = (xfBq.c * circlep.x - xfBq.s * circlep.y) + xfB.p.x;
  final double cy = (xfBq.s * circlep.x + xfBq.c * circlep.y) + xfB.p.y;
  final double px = cx - xfA.p.x;
  final double py = cy - xfA.p.y;
  final double cLocalx = (xfAq.c * px + xfAq.s * py);
  final double cLocaly = (-xfAq.s * px + xfAq.c * py);
  // end inline

  // Find the min separating edge.
  int normalIndex = 0;
  double separation = -double.maxFinite;
  final double radius = polygon.radius + circle.radius;
  final int vertexCount = polygon.count;
  double s;
  final List<Vector2> vertices = polygon.vertices;
  final List<Vector2> normals = polygon.normals;

  for (int i = 0; i < vertexCount; i++) {
    // before inline
    // temp.set(cLocal).subLocal(vertices[i]);
    // double s = Vec2.dot(normals[i], temp);
    // after inline
    final Vector2 vertex = vertices[i];
    final double tempx = cLocalx - vertex.x;
    final double 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 int vertIndex1 = normalIndex;
  final int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
  final Vector2 v1 = vertices[vertIndex1];
  final Vector2 v2 = vertices[vertIndex2];

  // If the center is inside the polygon ...
  if (separation < Settings.EPSILON) {
    manifold.pointCount = 1;
    manifold.type = ManifoldType.FACE_A;

    // before inline:
    // manifold._localNormal.set(normals[normalIndex]);
    // manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
    // manifold.points[0].localPoint.set(circle.p);
    // after inline:
    final Vector2 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 ManifoldPoint mpoint = manifold.points[0];
    mpoint.localPoint.x = circlep.x;
    mpoint.localPoint.y = circlep.y;
    mpoint.id.zero();
    // end inline

    return;
  }

  // Compute barycentric coordinates
  // before inline:
  // temp.set(cLocal).subLocal(v1);
  // temp2.set(v2).subLocal(v1);
  // double u1 = Vec2.dot(temp, temp2);
  // temp.set(cLocal).subLocal(v2);
  // temp2.set(v1).subLocal(v2);
  // double u2 = Vec2.dot(temp, temp2);
  // after inline:
  final double tempX = cLocalx - v1.x;
  final double tempY = cLocaly - v1.y;
  final double temp2X = v2.x - v1.x;
  final double temp2Y = v2.y - v1.y;
  final double u1 = tempX * temp2X + tempY * temp2Y;

  final double temp3X = cLocalx - v2.x;
  final double temp3Y = cLocaly - v2.y;
  final double temp4X = v1.x - v2.x;
  final double temp4Y = v1.y - v2.y;
  final double u2 = temp3X * temp4X + temp3Y * temp4Y;
  // end inline

  if (u1 <= 0.0) {
    // inlined
    final double dx = cLocalx - v1.x;
    final double dy = cLocaly - v1.y;
    if (dx * dx + dy * dy > radius * radius) {
      return;
    }

    manifold.pointCount = 1;
    manifold.type = ManifoldType.FACE_A;
    // before inline:
    // manifold._localNormal.set(cLocal).subLocal(v1);
    // after inline:
    manifold.localNormal.x = cLocalx - v1.x;
    manifold.localNormal.y = cLocaly - v1.y;
    // end inline
    manifold.localNormal.normalize();
    manifold.localPoint.setFrom(v1);
    manifold.points[0].localPoint.setFrom(circlep);
    manifold.points[0].id.zero();
  } else if (u2 <= 0.0) {
    // inlined
    final double dx = cLocalx - v2.x;
    final double dy = cLocaly - v2.y;
    if (dx * dx + dy * dy > radius * radius) {
      return;
    }

    manifold.pointCount = 1;
    manifold.type = ManifoldType.FACE_A;
    // before inline:
    // manifold._localNormal.set(cLocal).subLocal(v2);
    // after inline:
    manifold.localNormal.x = cLocalx - v2.x;
    manifold.localNormal.y = cLocaly - v2.y;
    // end inline
    manifold.localNormal.normalize();
    manifold.localPoint.setFrom(v2);
    manifold.points[0].localPoint.setFrom(circlep);
    manifold.points[0].id.zero();
  } else {
    // Vec2 faceCenter = 0.5f * (v1 + v2);
    // (temp is faceCenter)
    // before inline:
    // temp.set(v1).addLocal(v2).mulLocal(.5f);
    //
    // temp2.set(cLocal).subLocal(temp);
    // separation = Vec2.dot(temp2, normals[vertIndex1]);
    // if (separation > radius) {
    // return;
    // }
    // after inline:
    final double fcx = (v1.x + v2.x) * .5;
    final double fcy = (v1.y + v2.y) * .5;

    final double tx = cLocalx - fcx;
    final double ty = cLocaly - fcy;
    final Vector2 normal = normals[vertIndex1];
    separation = tx * normal.x + ty * normal.y;
    if (separation > radius) {
      return;
    }
    // end inline

    manifold.pointCount = 1;
    manifold.type = ManifoldType.FACE_A;
    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();
  }
}