solveTOI method

void solveTOI(TimeStep subStep, int toiIndexA, int toiIndexB)

Implementation

void solveTOI(TimeStep subStep, int toiIndexA, int toiIndexB) {
  assert(toiIndexA < bodies.length);
  assert(toiIndexB < bodies.length);

  // Initialize the body state.
  for (final bodyMeta in bodies) {
    final body = bodyMeta.body;
    bodyMeta.position.c.x = body.sweep.c.x;
    bodyMeta.position.c.y = body.sweep.c.y;
    bodyMeta.position.a = body.sweep.a;
    bodyMeta.velocity.v.x = body.linearVelocity.x;
    bodyMeta.velocity.v.y = body.linearVelocity.y;
    bodyMeta.velocity.w = body.angularVelocity;
  }

  // TODO(spydon): Is this correct, since it is no longer a fixed list?
  _toiSolverDef.contacts = _contacts;
  _toiSolverDef.step = subStep;
  _toiSolverDef.positions = _positions;
  _toiSolverDef.velocities = _velocities;
  _toiContactSolver.init(_toiSolverDef);

  // Solve position constraints.
  for (var i = 0; i < subStep.positionIterations; ++i) {
    final contactsOkay =
        _toiContactSolver.solveTOIPositionConstraints(toiIndexA, toiIndexB);
    if (contactsOkay) {
      break;
    }
  }

  // Leap of faith to new safe state.
  bodies[toiIndexA].body.sweep.c0.x = _positions[toiIndexA].c.x;
  bodies[toiIndexA].body.sweep.c0.y = _positions[toiIndexA].c.y;
  bodies[toiIndexA].body.sweep.a0 = _positions[toiIndexA].a;
  bodies[toiIndexB].body.sweep.c0.setFrom(_positions[toiIndexB].c);
  bodies[toiIndexB].body.sweep.a0 = _positions[toiIndexB].a;

  // No warm starting is needed for TOI events because warm
  // starting impulses were applied in the discrete solver.
  _toiContactSolver.initializeVelocityConstraints();

  // Solve velocity constraints.
  for (var i = 0; i < subStep.velocityIterations; ++i) {
    _toiContactSolver.solveVelocityConstraints();
  }

  // Don't store the TOI contact forces for warm starting
  // because they can be quite large.

  final dt = subStep.dt;

  // Integrate positions
  for (final bodyMeta in bodies) {
    final position = bodyMeta.position;
    final velocity = bodyMeta.velocity;
    final c = position.c;
    var a = position.a;
    final v = velocity.v;
    var w = velocity.w;

    // Check for large velocities
    final translationX = v.x * dt;
    final translationY = v.y * dt;
    if (translationX * translationX + translationY * translationY >
        settings.maxTranslationSquared) {
      final ratio = settings.maxTranslation /
          sqrt(translationX * translationX + translationY * translationY);
      v.scale(ratio);
    }

    final rotation = dt * w;
    if (rotation * rotation > settings.maxRotationSquared) {
      final ratio = settings.maxRotation / rotation.abs();
      w *= ratio;
    }

    // Integrate
    c.x += v.x * dt;
    c.y += v.y * dt;
    a += dt * w;

    position.c.x = c.x;
    position.c.y = c.y;
    position.a = a;
    velocity.v.x = v.x;
    velocity.v.y = v.y;
    velocity.w = w;

    // Sync bodies
    final body = bodyMeta.body;
    body.sweep.c.x = c.x;
    body.sweep.c.y = c.y;
    body.sweep.a = a;
    body.linearVelocity.x = v.x;
    body.linearVelocity.y = v.y;
    body.angularVelocity = w;
    body.synchronizeTransform();
  }

  reportVelocityConstraints();
}