collidePolygons method

void collidePolygons(
  1. Manifold manifold,
  2. PolygonShape polyA,
  3. Transform xfA,
  4. PolygonShape polyB,
  5. Transform xfB
)

Compute the collision manifold between two polygons.

Implementation

void collidePolygons(Manifold manifold, final PolygonShape polyA,
    final Transform xfA, final PolygonShape polyB, final Transform xfB) {
  // Find edge normal of max separation on A - return if separating axis is found
  // Find edge normal of max separation on B - return if separation axis is found
  // Choose reference edge as min(minA, minB)
  // Find incident edge
  // Clip

  // The normal points from 1 to 2

  manifold.pointCount = 0;
  double totalRadius = polyA.radius + polyB.radius;

  findMaxSeparation(_results1, polyA, xfA, polyB, xfB);
  if (_results1.separation > totalRadius) {
    return;
  }

  findMaxSeparation(results2, polyB, xfB, polyA, xfA);
  if (results2.separation > totalRadius) {
    return;
  }

  PolygonShape poly1; // reference polygon
  PolygonShape poly2; // incident polygon
  Transform xf1, xf2;
  int edge1; // reference edge
  bool flip;
  final double k_tol = 0.1 * Settings.linearSlop;

  if (results2.separation > _results1.separation + k_tol) {
    poly1 = polyB;
    poly2 = polyA;
    xf1 = xfB;
    xf2 = xfA;
    edge1 = results2.edgeIndex;
    manifold.type = ManifoldType.FACE_B;
    flip = true;
  } else {
    poly1 = polyA;
    poly2 = polyB;
    xf1 = xfA;
    xf2 = xfB;
    edge1 = _results1.edgeIndex;
    manifold.type = ManifoldType.FACE_A;
    flip = false;
  }
  final Rot xf1q = xf1.q;

  findIncidentEdge(_incidentEdge, poly1, xf1, edge1, poly2, xf2);

  int count1 = poly1.count;
  final List<Vector2> vertices1 = poly1.vertices;

  final int iv1 = edge1;
  final int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
  _v11.setFrom(vertices1[iv1]);
  _v12.setFrom(vertices1[iv2]);
  _localTangent.x = _v12.x - _v11.x;
  _localTangent.y = _v12.y - _v11.y;
  _localTangent.normalize();

  // Vec2 _localNormal = Vec2.cross(dv, 1.0f);
  _localNormal.x = 1.0 * _localTangent.y;
  _localNormal.y = -1.0 * _localTangent.x;

  // Vec2 _planePoint = 0.5f * (_v11+ _v12);
  _planePoint.x = (_v11.x + _v12.x) * .5;
  _planePoint.y = (_v11.y + _v12.y) * .5;

  // Rot.mulToOutUnsafe(xf1.q, _localTangent, _tangent);
  _tangent.x = xf1q.c * _localTangent.x - xf1q.s * _localTangent.y;
  _tangent.y = xf1q.s * _localTangent.x + xf1q.c * _localTangent.y;

  // Vec2.crossToOutUnsafe(_tangent, 1f, normal);
  final double normalx = 1.0 * _tangent.y;
  final double normaly = -1.0 * _tangent.x;

  Transform.mulToOutVec2(xf1, _v11, _v11);
  Transform.mulToOutVec2(xf1, _v12, _v12);
  // _v11 = Mul(xf1, _v11);
  // _v12 = Mul(xf1, _v12);

  // Face offset
  // double frontOffset = Vec2.dot(normal, _v11);
  double frontOffset = normalx * _v11.x + normaly * _v11.y;

  // Side offsets, extended by polytope skin thickness.
  // double sideOffset1 = -Vec2.dot(_tangent, _v11) + totalRadius;
  // double sideOffset2 = Vec2.dot(_tangent, _v12) + totalRadius;
  double sideOffset1 =
      -(_tangent.x * _v11.x + _tangent.y * _v11.y) + totalRadius;
  double sideOffset2 =
      _tangent.x * _v12.x + _tangent.y * _v12.y + totalRadius;

  // Clip incident edge against extruded edge1 side edges.
  // ClipVertex _clipPoints1[2];
  // ClipVertex _clipPoints2[2];
  int np;

  // Clip to box side 1
  // np = ClipSegmentToLine(_clipPoints1, _incidentEdge, -sideNormal, sideOffset1);
  _tangent.negate();
  np = clipSegmentToLine(
      _clipPoints1, _incidentEdge, _tangent, sideOffset1, iv1);
  _tangent.negate();

  if (np < 2) {
    return;
  }

  // Clip to negative box side 1
  np = clipSegmentToLine(
      _clipPoints2, _clipPoints1, _tangent, sideOffset2, iv2);

  if (np < 2) {
    return;
  }

  // Now _clipPoints2 contains the clipped points.
  manifold.localNormal.setFrom(_localNormal);
  manifold.localPoint.setFrom(_planePoint);

  int pointCount = 0;
  for (int i = 0; i < Settings.maxManifoldPoints; ++i) {
    // double separation = Vec2.dot(normal, _clipPoints2[i].v) - frontOffset;
    double separation = normalx * _clipPoints2[i].v.x +
        normaly * _clipPoints2[i].v.y -
        frontOffset;

    if (separation <= totalRadius) {
      ManifoldPoint cp = manifold.points[pointCount];
      // cp.localPoint = MulT(xf2, _clipPoints2[i].v);
      Vector2 out = cp.localPoint;
      final double px = _clipPoints2[i].v.x - xf2.p.x;
      final double py = _clipPoints2[i].v.y - xf2.p.y;
      out.x = (xf2.q.c * px + xf2.q.s * py);
      out.y = (-xf2.q.s * px + xf2.q.c * py);
      cp.id.set(_clipPoints2[i].id);
      if (flip) {
        // Swap features
        cp.id.flip();
      }
      ++pointCount;
    }
  }

  manifold.pointCount = pointCount;
}