timeOfImpact method

void timeOfImpact(TOIOutput output, TOIInput input)

Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction between 0,tMax. This uses a swept separating axis and may miss some intermediate, non-tunneling collision. If you change the time interval, you should call this function again. Note: use Distance to compute the contact point and normal at the time of impact.

Implementation

void timeOfImpact(TOIOutput output, TOIInput input) {
  // CCD via the local separating axis method. This seeks progression
  // by computing the largest time at which separation is maintained.

  ++toiCalls;

  output.state = TOIOutputState.unknown;
  output.t = input.tMax;

  final proxyA = input.proxyA;
  final proxyB = input.proxyB;

  _sweepA.setFrom(input.sweepA);
  _sweepB.setFrom(input.sweepB);

  // Large rotations can make the root finder fail, so we normalize the
  // sweep angles.
  _sweepA.normalize();
  _sweepB.normalize();

  final tMax = input.tMax;

  final totalRadius = proxyA.radius + proxyB.radius;
  // djm: whats with all these constants?
  final target =
      max(settings.linearSlop, totalRadius - 3.0 * settings.linearSlop);
  final tolerance = 0.25 * settings.linearSlop;

  assert(target > tolerance);

  var t1 = 0.0;
  var iteration = 0;

  _cache.count = 0;
  _distanceInput.proxyA = input.proxyA;
  _distanceInput.proxyB = input.proxyB;
  _distanceInput.useRadii = false;

  // The outer loop progressively attempts to compute new separating axes.
  // This loop terminates when an axis is repeated (no progress is made).
  for (;;) {
    _sweepA.getTransform(_xfA, t1);
    _sweepB.getTransform(_xfB, t1);
    // Get the distance between shapes. We can also use the results
    // to get a separating axis
    _distanceInput.transformA = _xfA;
    _distanceInput.transformB = _xfB;
    World.distance.compute(_distanceOutput, _cache, _distanceInput);

    // If the shapes are overlapped, we give up on continuous collision.
    if (_distanceOutput.distance <= 0.0) {
      // Failure!
      output.state = TOIOutputState.overlapped;
      output.t = 0.0;
      break;
    }

    if (_distanceOutput.distance < target + tolerance) {
      // Victory!
      output.state = TOIOutputState.touching;
      output.t = t1;
      break;
    }

    // Initialize the separating axis.
    _fcn.initialize(_cache, proxyA, _sweepA, proxyB, _sweepB, t1);

    // Compute the TOI on the separating axis. We do this by successively
    // resolving the deepest point. This loop is bounded by the number of
    // vertices.
    var done = false;
    var t2 = tMax;
    var pushBackIteration = 0;
    for (;;) {
      // Find the deepest point at t2. Store the witness point indices.
      var s2 = _fcn.findMinSeparation(_indexes, t2);
      // Is the final configuration separated?
      if (s2 > target + tolerance) {
        // Victory!
        output.state = TOIOutputState.separated;
        output.t = tMax;
        done = true;
        break;
      }

      // Has the separation reached tolerance?
      if (s2 > target - tolerance) {
        // Advance the sweeps
        t1 = t2;
        break;
      }

      // Compute the initial separation of the witness points.
      var s1 = _fcn.evaluate(_indexes[0], _indexes[1], t1);
      // Check for initial overlap. This might happen if the root finder
      // runs out of iterations.
      if (s1 < target - tolerance) {
        output.state = TOIOutputState.failed;
        output.t = t1;
        done = true;
        break;
      }

      // Check for touching
      if (s1 <= target + tolerance) {
        // Victory! t1 should hold the TOI (could be 0.0).
        output.state = TOIOutputState.touching;
        output.t = t1;
        done = true;
        break;
      }

      // Compute 1D root of: f(x) - target = 0
      var rootIterationCount = 0;
      var a1 = t1;
      var a2 = t2;
      for (;;) {
        // Use a mix of the secant rule and bisection.
        double t;
        if ((rootIterationCount & 1) == 1) {
          // Secant rule to improve convergence.
          t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
        } else {
          // Bisection to guarantee progress.
          t = 0.5 * (a1 + a2);
        }

        ++rootIterationCount;
        ++toiRootIterations;

        final s = _fcn.evaluate(_indexes[0], _indexes[1], t);

        if ((s - target).abs() < tolerance) {
          // t2 holds a tentative value for t1
          t2 = t;
          break;
        }

        // Ensure we continue to bracket the root.
        if (s > target) {
          a1 = t;
          s1 = s;
        } else {
          a2 = t;
          s2 = s;
        }

        if (rootIterationCount == maxRootIterations) {
          break;
        }
      }

      toiMaxRootIterations = max(toiMaxRootIterations, rootIterationCount);

      ++pushBackIteration;

      if (pushBackIteration == settings.maxPolygonVertices ||
          rootIterationCount == maxRootIterations) {
        break;
      }
    }

    ++iteration;
    ++toiIterations;

    if (done) {
      break;
    }

    if (iteration == maxIterations) {
      // Root finder got stuck. Semi-victory.
      output.state = TOIOutputState.failed;
      output.t = t1;
      break;
    }
  }

  toiMaxIterations = max(toiMaxIterations, iteration);
}