initialize method

double initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA, DistanceProxy proxyB, Sweep sweepB, double t1,)

Implementation

double initialize(
  SimplexCache cache,
  DistanceProxy proxyA,
  Sweep sweepA,
  DistanceProxy proxyB,
  Sweep sweepB,
  double t1,
) {
  this.proxyA = proxyA;
  this.proxyB = proxyB;
  final count = cache.count;
  assert(0 < count && count < 3);

  this.sweepA = sweepA;
  this.sweepB = sweepB;

  sweepA.getTransform(_xfa, t1);
  sweepB.getTransform(_xfb, t1);

  if (count == 1) {
    type = SeparationFunctionType.points;
    _localPointA.setFrom(proxyA.getVertex(cache.indexA[0]));
    _localPointB.setFrom(proxyB.getVertex(cache.indexB[0]));
    _pointA.setFrom(Transform.mulVec2(_xfa, _localPointA));
    _pointB.setFrom(Transform.mulVec2(_xfb, _localPointB));
    axis
      ..setFrom(_pointB)
      ..sub(_pointA);
    return axis.normalize();
  } else if (cache.indexA[0] == cache.indexA[1]) {
    // Two points on B and one on A.
    type = SeparationFunctionType.faceB;

    _localPointB1.setFrom(proxyB.getVertex(cache.indexB[0]));
    _localPointB2.setFrom(proxyB.getVertex(cache.indexB[1]));

    _temp
      ..setFrom(_localPointB2)
      ..sub(_localPointB1);
    _temp.scaleOrthogonalInto(-1.0, axis);
    axis.normalize();

    _normal.setFrom(Rot.mulVec2(_xfb.q, axis));

    localPoint
      ..setFrom(_localPointB1)
      ..add(_localPointB2)
      ..scale(.5);
    _pointB.setFrom(Transform.mulVec2(_xfb, localPoint));

    _localPointA.setFrom(proxyA.getVertex(cache.indexA[0]));
    _pointA.setFrom(Transform.mulVec2(_xfa, _localPointA));

    _temp
      ..setFrom(_pointA)
      ..sub(_pointB);
    var s = _temp.dot(_normal);
    if (s < 0.0) {
      axis.negate();
      s = -s;
    }
    return s;
  } else {
    // Two points on A and one or two points on B.
    type = SeparationFunctionType.faceA;

    _localPointA1.setFrom(proxyA.getVertex(cache.indexA[0]));
    _localPointA2.setFrom(proxyA.getVertex(cache.indexA[1]));

    _temp
      ..setFrom(_localPointA2)
      ..sub(_localPointA1);
    _temp.scaleOrthogonalInto(-1.0, axis);
    axis.normalize();

    _normal.setFrom(Rot.mulVec2(_xfa.q, axis));

    localPoint
      ..setFrom(_localPointA1)
      ..add(_localPointA2)
      ..scale(.5);
    _pointA.setFrom(Transform.mulVec2(_xfa, localPoint));

    _localPointB.setFrom(proxyB.getVertex(cache.indexB[0]));
    _pointB.setFrom(Transform.mulVec2(_xfb, _localPointB));

    _temp
      ..setFrom(_pointB)
      ..sub(_pointA);
    var s = _temp.dot(_normal);
    if (s < 0.0) {
      axis.negate();
      s = -s;
    }
    return s;
  }
}