collide method

void collide(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB2, Transform xfB,)

Implementation

void collide(
  Manifold manifold,
  EdgeShape edgeA,
  Transform xfA,
  PolygonShape polygonB2,
  Transform xfB,
) {
  xf.setFrom(Transform.mulTrans(xfA, xfB));
  centroidB.setFrom(Transform.mulVec2(xf, polygonB2.centroid));

  v0 = edgeA.vertex0;
  v1 = edgeA.vertex1;
  v2 = edgeA.vertex2;
  v3 = edgeA.vertex3;

  final hasVertex0 = edgeA.hasVertex0;
  final hasVertex3 = edgeA.hasVertex3;

  _edge1
    ..setFrom(v2)
    ..sub(v1);
  _edge1.normalize();
  normal1.setValues(_edge1.y, -_edge1.x);
  final offset1 = normal1.dot(
    _temp
      ..setFrom(centroidB)
      ..sub(v1),
  );
  var offset0 = 0.0;
  var offset2 = 0.0;
  var convex1 = false;
  var convex2 = false;

  // Is there a preceding edge?
  if (hasVertex0) {
    _edge0
      ..setFrom(v1)
      ..sub(v0);
    _edge0.normalize();
    normal0.setValues(_edge0.y, -_edge0.x);
    convex1 = _edge0.cross(_edge1) >= 0.0;
    offset0 = normal0.dot(
      _temp
        ..setFrom(centroidB)
        ..sub(v0),
    );
  }

  // Is there a following edge?
  if (hasVertex3) {
    _edge2
      ..setFrom(v3)
      ..sub(v2);
    _edge2.normalize();
    normal2.setValues(_edge2.y, -_edge2.x);
    convex2 = _edge1.cross(_edge2) > 0.0;
    offset2 = normal2.dot(
      _temp
        ..setFrom(centroidB)
        ..sub(v2),
    );
  }

  // Determine front or back collision. Determine collision normal limits.
  if (hasVertex0 && hasVertex3) {
    if (convex1 && convex2) {
      front = offset0 >= 0.0 || offset1 >= 0.0 || offset2 >= 0.0;
      if (front) {
        normal.x = normal1.x;
        normal.y = normal1.y;
        lowerLimit.x = normal0.x;
        lowerLimit.y = normal0.y;
        upperLimit.x = normal2.x;
        upperLimit.y = normal2.y;
      } else {
        normal.x = -normal1.x;
        normal.y = -normal1.y;
        lowerLimit.x = -normal1.x;
        lowerLimit.y = -normal1.y;
        upperLimit.x = -normal1.x;
        upperLimit.y = -normal1.y;
      }
    } else if (convex1) {
      front = offset0 >= 0.0 || (offset1 >= 0.0 && offset2 >= 0.0);
      if (front) {
        normal.x = normal1.x;
        normal.y = normal1.y;
        lowerLimit.x = normal0.x;
        lowerLimit.y = normal0.y;
        upperLimit.x = normal1.x;
        upperLimit.y = normal1.y;
      } else {
        normal.x = -normal1.x;
        normal.y = -normal1.y;
        lowerLimit.x = -normal2.x;
        lowerLimit.y = -normal2.y;
        upperLimit.x = -normal1.x;
        upperLimit.y = -normal1.y;
      }
    } else if (convex2) {
      front = offset2 >= 0.0 || (offset0 >= 0.0 && offset1 >= 0.0);
      if (front) {
        normal.x = normal1.x;
        normal.y = normal1.y;
        lowerLimit.x = normal1.x;
        lowerLimit.y = normal1.y;
        upperLimit.x = normal2.x;
        upperLimit.y = normal2.y;
      } else {
        normal.x = -normal1.x;
        normal.y = -normal1.y;
        lowerLimit.x = -normal1.x;
        lowerLimit.y = -normal1.y;
        upperLimit.x = -normal0.x;
        upperLimit.y = -normal0.y;
      }
    } else {
      front = offset0 >= 0.0 && offset1 >= 0.0 && offset2 >= 0.0;
      if (front) {
        normal.x = normal1.x;
        normal.y = normal1.y;
        lowerLimit.x = normal1.x;
        lowerLimit.y = normal1.y;
        upperLimit.x = normal1.x;
        upperLimit.y = normal1.y;
      } else {
        normal.x = -normal1.x;
        normal.y = -normal1.y;
        lowerLimit.x = -normal2.x;
        lowerLimit.y = -normal2.y;
        upperLimit.x = -normal0.x;
        upperLimit.y = -normal0.y;
      }
    }
  } else if (hasVertex0) {
    if (convex1) {
      front = offset0 >= 0.0 || offset1 >= 0.0;
      if (front) {
        normal.x = normal1.x;
        normal.y = normal1.y;
        lowerLimit.x = normal0.x;
        lowerLimit.y = normal0.y;
        upperLimit.x = -normal1.x;
        upperLimit.y = -normal1.y;
      } else {
        normal.x = -normal1.x;
        normal.y = -normal1.y;
        lowerLimit.x = normal1.x;
        lowerLimit.y = normal1.y;
        upperLimit.x = -normal1.x;
        upperLimit.y = -normal1.y;
      }
    } else {
      front = offset0 >= 0.0 && offset1 >= 0.0;
      if (front) {
        normal.x = normal1.x;
        normal.y = normal1.y;
        lowerLimit.x = normal1.x;
        lowerLimit.y = normal1.y;
        upperLimit.x = -normal1.x;
        upperLimit.y = -normal1.y;
      } else {
        normal.x = -normal1.x;
        normal.y = -normal1.y;
        lowerLimit.x = normal1.x;
        lowerLimit.y = normal1.y;
        upperLimit.x = -normal0.x;
        upperLimit.y = -normal0.y;
      }
    }
  } else if (hasVertex3) {
    if (convex2) {
      front = offset1 >= 0.0 || offset2 >= 0.0;
      if (front) {
        normal.x = normal1.x;
        normal.y = normal1.y;
        lowerLimit.x = -normal1.x;
        lowerLimit.y = -normal1.y;
        upperLimit.x = normal2.x;
        upperLimit.y = normal2.y;
      } else {
        normal.x = -normal1.x;
        normal.y = -normal1.y;
        lowerLimit.x = -normal1.x;
        lowerLimit.y = -normal1.y;
        upperLimit.x = normal1.x;
        upperLimit.y = normal1.y;
      }
    } else {
      front = offset1 >= 0.0 && offset2 >= 0.0;
      if (front) {
        normal.x = normal1.x;
        normal.y = normal1.y;
        lowerLimit.x = -normal1.x;
        lowerLimit.y = -normal1.y;
        upperLimit.x = normal1.x;
        upperLimit.y = normal1.y;
      } else {
        normal.x = -normal1.x;
        normal.y = -normal1.y;
        lowerLimit.x = -normal2.x;
        lowerLimit.y = -normal2.y;
        upperLimit.x = normal1.x;
        upperLimit.y = normal1.y;
      }
    }
  } else {
    front = offset1 >= 0.0;
    if (front) {
      normal.x = normal1.x;
      normal.y = normal1.y;
      lowerLimit.x = -normal1.x;
      lowerLimit.y = -normal1.y;
      upperLimit.x = -normal1.x;
      upperLimit.y = -normal1.y;
    } else {
      normal.x = -normal1.x;
      normal.y = -normal1.y;
      lowerLimit.x = normal1.x;
      lowerLimit.y = normal1.y;
      upperLimit.x = normal1.x;
      upperLimit.y = normal1.y;
    }
  }

  // Get polygonB in frameA
  polygonB.count = polygonB2.vertices.length;
  for (var i = 0; i < polygonB2.vertices.length; ++i) {
    polygonB.vertices[i]
        .setFrom(Transform.mulVec2(xf, polygonB2.vertices[i]));
    polygonB.normals[i].setFrom(Rot.mulVec2(xf.q, polygonB2.normals[i]));
  }

  radius = 2.0 * settings.polygonRadius;

  manifold.pointCount = 0;

  computeEdgeSeparation(_edgeAxis);

  // If no valid normal can be found than this edge should not collide.
  if (_edgeAxis.type == EPAxisType.unknown) {
    return;
  }

  if (_edgeAxis.separation > radius) {
    return;
  }

  computePolygonSeparation(_polygonAxis);
  if (_polygonAxis.type != EPAxisType.unknown &&
      _polygonAxis.separation > radius) {
    return;
  }

  // Use hysteresis for jitter reduction.
  const relativeTol = 0.98;
  const kAbsoluteTol = 0.001;

  EPAxis primaryAxis;
  if (_polygonAxis.type == EPAxisType.unknown) {
    primaryAxis = _edgeAxis;
  } else if (_polygonAxis.separation >
      relativeTol * _edgeAxis.separation + kAbsoluteTol) {
    primaryAxis = _polygonAxis;
  } else {
    primaryAxis = _edgeAxis;
  }

  final ie0 = _incidentEdge[0];
  final ie1 = _incidentEdge[1];

  if (primaryAxis.type == EPAxisType.edgeA) {
    manifold.type = ManifoldType.faceA;

    // Search for the polygon normal that is most anti-parallel to the edge
    // normal.
    var bestIndex = 0;
    var bestValue = normal.dot(polygonB.normals[0]);
    for (var i = 1; i < polygonB.count; ++i) {
      final value = normal.dot(polygonB.normals[i]);
      if (value < bestValue) {
        bestValue = value;
        bestIndex = i;
      }
    }

    final i1 = bestIndex;
    final i2 = i1 + 1 < polygonB.count ? i1 + 1 : 0;

    ie0.v.setFrom(polygonB.vertices[i1]);
    ie0.id.indexA = 0;
    ie0.id.indexB = i1 & 0xFF;
    ie0.id.typeA = ContactIDType.face.index & 0xFF;
    ie0.id.typeB = ContactIDType.vertex.index & 0xFF;

    ie1.v.setFrom(polygonB.vertices[i2]);
    ie1.id.indexA = 0;
    ie1.id.indexB = i2 & 0xFF;
    ie1.id.typeA = ContactIDType.face.index & 0xFF;
    ie1.id.typeB = ContactIDType.vertex.index & 0xFF;

    if (front) {
      _rf.i1 = 0;
      _rf.i2 = 1;
      _rf.v1.setFrom(v1);
      _rf.v2.setFrom(v2);
      _rf.normal.setFrom(normal1);
    } else {
      _rf.i1 = 1;
      _rf.i2 = 0;
      _rf.v1.setFrom(v2);
      _rf.v2.setFrom(v1);
      _rf.normal
        ..setFrom(normal1)
        ..negate();
    }
  } else {
    manifold.type = ManifoldType.faceB;

    ie0.v.setFrom(v1);
    ie0.id.indexA = 0;
    ie0.id.indexB = primaryAxis.index & 0xFF;
    ie0.id.typeA = ContactIDType.vertex.index & 0xFF;
    ie0.id.typeB = ContactIDType.face.index & 0xFF;

    ie1.v.setFrom(v2);
    ie1.id.indexA = 0;
    ie1.id.indexB = primaryAxis.index & 0xFF;
    ie1.id.typeA = ContactIDType.vertex.index & 0xFF;
    ie1.id.typeB = ContactIDType.face.index & 0xFF;

    _rf.i1 = primaryAxis.index;
    _rf.i2 = _rf.i1 + 1 < polygonB.count ? _rf.i1 + 1 : 0;
    _rf.v1.setFrom(polygonB.vertices[_rf.i1]);
    _rf.v2.setFrom(polygonB.vertices[_rf.i2]);
    _rf.normal.setFrom(polygonB.normals[_rf.i1]);
  }

  _rf.sideNormal1.setValues(_rf.normal.y, -_rf.normal.x);
  _rf.sideNormal2
    ..setFrom(_rf.sideNormal1)
    ..negate();
  _rf.sideOffset1 = _rf.sideNormal1.dot(_rf.v1);
  _rf.sideOffset2 = _rf.sideNormal2.dot(_rf.v2);

  // Clip to box side 1
  if (Collision.clipSegmentToLine(
        _clipPoints1,
        _incidentEdge,
        _rf.sideNormal1,
        _rf.sideOffset1,
        _rf.i1,
      ) <
      settings.maxManifoldPoints) {
    return;
  }

  // Clip to negative box side 1
  if (Collision.clipSegmentToLine(
        _clipPoints2,
        _clipPoints1,
        _rf.sideNormal2,
        _rf.sideOffset2,
        _rf.i2,
      ) <
      settings.maxManifoldPoints) {
    return;
  }

  // Now _clipPoints2 contains the clipped points.
  if (primaryAxis.type == EPAxisType.edgeA) {
    manifold.localNormal.setFrom(_rf.normal);
    manifold.localPoint.setFrom(_rf.v1);
  } else {
    manifold.localNormal.setFrom(polygonB2.normals[_rf.i1]);
    manifold.localPoint.setFrom(polygonB2.vertices[_rf.i1]);
  }

  var pointCount = 0;
  for (var i = 0; i < settings.maxManifoldPoints; ++i) {
    double separation;

    separation = _rf.normal.dot(
      _temp
        ..setFrom(_clipPoints2[i].v)
        ..sub(_rf.v1),
    );

    if (separation <= radius) {
      final cp = manifold.points[pointCount];

      if (primaryAxis.type == EPAxisType.edgeA) {
        cp.localPoint.setFrom(Transform.mulTransVec2(xf, _clipPoints2[i].v));
        cp.id.set(_clipPoints2[i].id);
      } else {
        cp.localPoint.setFrom(_clipPoints2[i].v);
        cp.id.typeA = _clipPoints2[i].id.typeB;
        cp.id.typeB = _clipPoints2[i].id.typeA;
        cp.id.indexA = _clipPoints2[i].id.indexB;
        cp.id.indexB = _clipPoints2[i].id.indexA;
      }

      ++pointCount;
    }
  }

  manifold.pointCount = pointCount;
}