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;

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

  // Vec2 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);

  // Compute the effective masses.
  final temp = Vector2.copy(localAnchorA)..sub(_localCenterA);
  _rA.setFrom(Rot.mulVec2(qA, temp));
  temp
    ..setFrom(localAnchorB)
    ..sub(_localCenterB);
  _rB.setFrom(Rot.mulVec2(qB, temp));

  // J = [-I -r1_skew I r2_skew]
  // [ 0 -1 0 1]
  // r_skew = [-ry; rx]

  // Matlab
  // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
  // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
  // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]

  final mA = _invMassA;
  final mB = _invMassB;
  final iA = _invIA;
  final iB = _invIB;

  final K = Matrix3.zero();

  final exX = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
  final eyX = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
  final ezX = -_rA.y * iA - _rB.y * iB;
  final exY = K.entry(0, 1);
  final eyY = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
  final ezY = _rA.x * iA + _rB.x * iB;
  final exZ = K.entry(0, 2);
  final eyZ = K.entry(1, 2);
  final ezZ = iA + iB;

  K.setValues(exX, exY, exZ, eyX, eyY, eyZ, ezX, ezY, ezZ);

  if (_frequencyHz > 0.0) {
    _mass.setFrom(_matrix3GetInverse22(K));

    var invM = iA + iB;
    final m = invM > 0.0 ? 1.0 / invM : 0.0;

    final c = aB - aA - _referenceAngle;

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

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

    // Spring stiffness
    final k = m * 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;

    invM += _gamma;
    _mass.setEntry(2, 2, invM != 0.0 ? 1.0 / invM : 0.0);
  } else {
    _mass.setFrom(_matrix3GetSymInverse33(K, _mass));
    _gamma = 0.0;
    _bias = 0.0;
  }

  if (data.step.warmStarting) {
    // Scale impulses to support a variable time step.
    _impulse.scale(data.step.dtRatio);

    final P = Vector2(_impulse.x, _impulse.y);

    vA.x -= mA * P.x;
    vA.y -= mA * P.y;
    wA -= iA * (_rA.cross(P) + _impulse.z);

    vB.x += mB * P.x;
    vB.y += mB * P.y;
    wB += iB * (_rB.cross(P) + _impulse.z);
  } else {
    _impulse.setZero();
  }

  data.velocities[_indexA].w = wA;
  data.velocities[_indexB].w = wB;
}