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();
  final temp = Vector2.zero();
  final k = Matrix2.zero();

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

  // Compute the effective mass matrix.
  // _rA = b2Mul(qA, -_localCenterA);
  // _rB = b2Mul(qB, -_localCenterB);
  _rA.x = qA.cos * -_localCenterA.x - qA.sin * -_localCenterA.y;
  _rA.y = qA.sin * -_localCenterA.x + qA.cos * -_localCenterA.y;
  _rB.x = qB.cos * -_localCenterB.x - qB.sin * -_localCenterB.y;
  _rB.y = qB.sin * -_localCenterB.x + qB.cos * -_localCenterB.y;

  // 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 a11 = mA + mB + iA * _rA.y * _rA.y + iB * _rB.y * _rB.y;
  final a21 = -iA * _rA.x * _rA.y - iB * _rB.x * _rB.y;
  final a12 = a21;
  final a22 = mA + mB + iA * _rA.x * _rA.x + iB * _rB.x * _rB.x;

  k.setValues(a11, a21, a12, a22);
  _linearMass.setFrom(k);
  _linearMass.invert();

  _angularMass = iA + iB;
  if (_angularMass > 0.0) {
    _angularMass = 1.0 / _angularMass;
  }

  temp.setFrom(Rot.mulVec2(qA, _linearOffset));
  _linearError.x = cB.x + _rB.x - cA.x - _rA.x - temp.x;
  _linearError.y = cB.y + _rB.y - cA.y - _rA.y - temp.y;
  _angularError = aB - aA - _angularOffset;

  if (data.step.warmStarting) {
    // Scale impulses to support a variable time step.
    _linearImpulse.x *= data.step.dtRatio;
    _linearImpulse.y *= data.step.dtRatio;
    _angularImpulse *= data.step.dtRatio;

    final P = _linearImpulse;
    vA.x -= mA * P.x;
    vA.y -= mA * P.y;
    wA -= iA * (_rA.x * P.y - _rA.y * P.x + _angularImpulse);
    vB.x += mB * P.x;
    vB.y += mB * P.y;
    wB += iB * (_rB.x * P.y - _rB.y * P.x + _angularImpulse);
  } else {
    _linearImpulse.setZero();
    _angularImpulse = 0.0;
  }

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