solve method

void solve(Profile profile, TimeStep step, Vector2 gravity, { required bool allowSleep, })

Implementation

void solve(
  Profile profile,
  TimeStep step,
  Vector2 gravity, {
  required bool allowSleep,
}) {
  final dt = step.dt;

  // Integrate velocities and apply damping. Initialize the body state.
  for (final bodyMeta in bodies) {
    final b = bodyMeta.body;
    final bmSweep = b.sweep;
    final c = bmSweep.c;
    final a = bmSweep.a;
    final v = b.linearVelocity;
    var w = b.angularVelocity;

    // Store positions for continuous collision.
    bmSweep.c0.setFrom(bmSweep.c);
    bmSweep.a0 = bmSweep.a;

    if (b.bodyType == BodyType.dynamic) {
      // Integrate velocities.
      final bodyGravity = b.gravityOverride ?? gravity;
      v.x += dt *
          ((b.gravityScale?.x ?? 1) * bodyGravity.x +
              b.inverseMass * b.force.x);
      v.y += dt *
          ((b.gravityScale?.y ?? 1) * bodyGravity.y +
              b.inverseMass * b.force.y);
      w += dt * b.inverseInertia * b.torque;

      // Apply damping.
      // ODE: dv/dt + c * v = 0
      // Solution: v(t) = v0 * exp(-c * t)
      // Time step:
      //   v(t + dt) = v0 * exp(-c * (t + dt))
      //     = v0 * exp(-c * t) * exp(-c * dt) = v *
      // exp(-c * dt)
      // v2 = exp(-c * dt) * v1
      // Pade approximation:
      // v2 = v1 * 1 / (1 + c * dt)
      v.x *= 1.0 / (1.0 + dt * b.linearDamping);
      v.y *= 1.0 / (1.0 + dt * b.linearDamping);
      w *= 1.0 / (1.0 + dt * b.angularDamping);
    }

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

  // Solver data
  _solverData.step = step;
  _solverData.positions = _positions;
  _solverData.velocities = _velocities;

  // Initialize velocity constraints.
  _solverDef.step = step;
  _solverDef.contacts = _contacts;
  _solverDef.positions = _positions;
  _solverDef.velocities = _velocities;

  _contactSolver.init(_solverDef);
  _contactSolver.initializeVelocityConstraints();

  if (step.warmStarting) {
    _contactSolver.warmStart();
  }

  for (final joint in _joints) {
    joint.initVelocityConstraints(_solverData);
  }

  for (var i = 0; i < step.velocityIterations; ++i) {
    for (final joint in _joints) {
      joint.solveVelocityConstraints(_solverData);
    }

    _contactSolver.solveVelocityConstraints();
  }

  // Store impulses for warm starting
  _contactSolver.storeImpulses();

  // Integrate positions
  for (final bodyMeta in bodies) {
    final c = bodyMeta.position.c;
    var a = bodyMeta.position.a;
    final v = bodyMeta.velocity.v;
    var w = bodyMeta.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.x *= ratio;
      v.y *= ratio;
    }

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

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

    bodyMeta.position.a = a;
    bodyMeta.velocity.w = w;
  }

  // Solve position constraints
  var positionSolved = false;
  for (var i = 0; i < step.positionIterations; ++i) {
    final contactsOkay = _contactSolver.solvePositionConstraints();

    var jointsOkay = true;
    for (final joint in _joints) {
      final jointOkay = joint.solvePositionConstraints(_solverData);
      jointsOkay = jointsOkay && jointOkay;
    }

    if (contactsOkay && jointsOkay) {
      // Exit early if the position errors are small.
      positionSolved = true;
      break;
    }
  }

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

  reportVelocityConstraints();

  if (allowSleep) {
    var minSleepTime = double.maxFinite;

    final linTolSqr =
        settings.linearSleepTolerance * settings.linearSleepTolerance;
    final angTolSqr =
        settings.angularSleepTolerance * settings.angularSleepTolerance;

    for (final bodyMeta in bodies) {
      final b = bodyMeta.body;
      if (b.bodyType == BodyType.static) {
        continue;
      }

      if ((b.flags & Body.autoSleepFlag) == 0 ||
          b.angularVelocity * b.angularVelocity > angTolSqr ||
          b.linearVelocity.dot(b.linearVelocity) > linTolSqr) {
        b.sleepTime = 0.0;
        minSleepTime = 0.0;
      } else {
        b.sleepTime += dt;
        minSleepTime = min(minSleepTime, b.sleepTime);
      }
    }

    if (minSleepTime >= settings.timeToSleep && positionSolved) {
      bodies.forEach((b) => b.body.setAwake(false));
    }
  }
}