findIncidentEdge method

void findIncidentEdge(
  1. List<ClipVertex> c,
  2. PolygonShape poly1,
  3. Transform xf1,
  4. int edge1,
  5. PolygonShape poly2,
  6. Transform xf2
)

Implementation

void findIncidentEdge(
    final List<ClipVertex> c,
    final PolygonShape poly1,
    final Transform xf1,
    int edge1,
    final PolygonShape poly2,
    final Transform xf2) {
  int count1 = poly1.count;
  final List<Vector2> normals1 = poly1.normals;

  int count2 = poly2.count;
  final List<Vector2> vertices2 = poly2.vertices;
  final List<Vector2> normals2 = poly2.normals;

  assert(0 <= edge1 && edge1 < count1);

  final ClipVertex c0 = c[0];
  final ClipVertex c1 = c[1];
  final Rot xf1q = xf1.q;
  final Rot xf2q = xf2.q;

  // Get the normal of the reference edge in poly2's frame.
  // Vec2 normal1 = MulT(xf2.R, Mul(xf1.R, normals1[edge1]));
  // before inline:
  // Rot.mulToOutUnsafe(xf1.q, normals1[edge1], normal1); // temporary
  // Rot.mulTrans(xf2.q, normal1, normal1);
  // after inline:
  final Vector2 v = normals1[edge1];
  final double tempx = xf1q.c * v.x - xf1q.s * v.y;
  final double tempy = xf1q.s * v.x + xf1q.c * v.y;
  final double normal1x = xf2q.c * tempx + xf2q.s * tempy;
  final double normal1y = -xf2q.s * tempx + xf2q.c * tempy;

  // end inline

  // Find the incident edge on poly2.
  int index = 0;
  double minDot = double.maxFinite;
  for (int i = 0; i < count2; ++i) {
    Vector2 b = normals2[i];
    double dot = normal1x * b.x + normal1y * b.y;
    if (dot < minDot) {
      minDot = dot;
      index = i;
    }
  }

  // Build the clip vertices for the incident edge.
  int i1 = index;
  int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

  // c0.v = Mul(xf2, vertices2[i1]);
  Vector2 v1 = vertices2[i1];
  Vector2 out = c0.v;
  out.x = (xf2q.c * v1.x - xf2q.s * v1.y) + xf2.p.x;
  out.y = (xf2q.s * v1.x + xf2q.c * v1.y) + xf2.p.y;
  c0.id.indexA = edge1 & 0xFF;
  c0.id.indexB = i1 & 0xFF;
  c0.id.typeA = ContactIDType.FACE.index & 0xFF;
  c0.id.typeB = ContactIDType.VERTEX.index & 0xFF;

  // c1.v = Mul(xf2, vertices2[i2]);
  Vector2 v2 = vertices2[i2];
  Vector2 out1 = c1.v;
  out1.x = (xf2q.c * v2.x - xf2q.s * v2.y) + xf2.p.x;
  out1.y = (xf2q.s * v2.x + xf2q.c * v2.y) + xf2.p.y;
  c1.id.indexA = edge1 & 0xFF;
  c1.id.indexB = i2 & 0xFF;
  c1.id.typeA = ContactIDType.FACE.index & 0xFF;
  c1.id.typeB = ContactIDType.VERTEX.index & 0xFF;
}