evaluate method

double evaluate(
  1. int indexA,
  2. int indexB,
  3. double t
)

Implementation

double evaluate(int indexA, int indexB, double t) {
  sweepA.getTransform(_xfa, t);
  sweepB.getTransform(_xfb, t);

  switch (type) {
    case SeparationFunctionType.points:
      _localPointA.setFrom(proxyA.getVertex(indexA));
      _localPointB.setFrom(proxyB.getVertex(indexB));

      _pointA.setFrom(Transform.mulVec2(_xfa, _localPointA));
      _pointB.setFrom(Transform.mulVec2(_xfb, _localPointB));

      return (_pointB..sub(_pointA)).dot(axis);
    case SeparationFunctionType.faceA:
      _normal.setFrom(Rot.mulVec2(_xfa.q, axis));
      _pointA.setFrom(Transform.mulVec2(_xfa, localPoint));

      _localPointB.setFrom(proxyB.getVertex(indexB));
      _pointB.setFrom(Transform.mulVec2(_xfb, _localPointB));
      return (_pointB..sub(_pointA)).dot(_normal);
    case SeparationFunctionType.faceB:
      _normal.setFrom(Rot.mulVec2(_xfb.q, axis));
      _pointB.setFrom(Transform.mulVec2(_xfb, localPoint));

      _localPointA.setFrom(proxyA.getVertex(indexA));
      _pointA.setFrom(Transform.mulVec2(_xfa, _localPointA));

      return (_pointA..sub(_pointB)).dot(_normal);
    default:
      assert(false);
      return 0.0;
  }
}