initVelocityConstraints method

@override void initVelocityConstraints(SolverData data)

Implementation

@override
void initVelocityConstraints(SolverData data) {
  _indexA = bodyA.islandIndex;
  _indexB = bodyB.islandIndex;
  _localCenterA.setFrom(bodyA.sweep.localCenter);
  _localCenterB.setFrom(bodyB.sweep.localCenter);
  _invMassA = bodyA.inverseMass;
  _invMassB = bodyB.inverseMass;
  _invIA = bodyA.inverseInertia;
  _invIB = bodyB.inverseInertia;

  final cA = data.positions[_indexA].c;
  final aA = data.positions[_indexA].a;
  final vA = data.velocities[_indexA].v;
  var wA = data.velocities[_indexA].w;

  final cB = data.positions[_indexB].c;
  final aB = data.positions[_indexB].a;
  final vB = data.velocities[_indexB].v;
  var wB = data.velocities[_indexB].w;

  final qA = Rot();
  final qB = Rot();

  qA.setAngle(aA);
  qB.setAngle(aB);

  // use _u as temporary variable
  _u
    ..setFrom(localAnchorA)
    ..sub(_localCenterA);
  _rA.setFrom(Rot.mulVec2(qA, _u));
  _u
    ..setFrom(localAnchorB)
    ..sub(_localCenterB);
  _rB.setFrom(Rot.mulVec2(qB, _u));
  _u
    ..setFrom(cB)
    ..add(_rB)
    ..sub(cA)
    ..sub(_rA);

  // Handle singularity.
  final length = _u.length;
  if (length > settings.linearSlop) {
    _u.x *= 1.0 / length;
    _u.y *= 1.0 / length;
  } else {
    _u.setValues(0.0, 0.0);
  }

  final crAu = _rA.cross(_u);
  final crBu = _rB.cross(_u);
  var invMass =
      _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;

  // Compute the effective mass matrix.
  _mass = invMass != 0.0 ? 1.0 / invMass : 0.0;

  if (_frequencyHz > 0.0) {
    final c = length - _length;

    // Frequency
    final omega = 2.0 * pi * _frequencyHz;

    // Damping coefficient
    final d = 2.0 * _mass * _dampingRatio * omega;

    // Spring stiffness
    final k = _mass * omega * omega;

    // magic formulas
    final dt = data.step.dt;
    _gamma = dt * (d + dt * k);
    _gamma = _gamma != 0.0 ? 1.0 / _gamma : 0.0;
    _bias = c * dt * k * _gamma;

    invMass += _gamma;
    _mass = invMass != 0.0 ? 1.0 / invMass : 0.0;
  } else {
    _gamma = 0.0;
    _bias = 0.0;
  }
  if (data.step.warmStarting) {
    // Scale the impulse to support a variable time step.
    _impulse *= data.step.dtRatio;

    final p = Vector2.copy(_u)..scale(_impulse);

    vA.x -= _invMassA * p.x;
    vA.y -= _invMassA * p.y;
    wA -= _invIA * _rA.cross(p);

    vB.x += _invMassB * p.x;
    vB.y += _invMassB * p.y;
    wB += _invIB * _rB.cross(p);
  } else {
    _impulse = 0.0;
  }
  data.velocities[_indexA].w = wA;
  data.velocities[_indexB].w = wB;
}