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 aA = data.positions[_indexA].a;
  final vA = data.velocities[_indexA].v;
  var wA = data.velocities[_indexA].w;

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

  final temp = Vector2.zero();
  final qA = Rot();
  final qB = Rot();

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

  // Compute the effective mass matrix.
  temp
    ..setFrom(localAnchorA)
    ..sub(_localCenterA);
  _rA.setFrom(Rot.mulVec2(qA, temp));
  temp
    ..setFrom(localAnchorB)
    ..sub(_localCenterB);
  _rB.setFrom(Rot.mulVec2(qB, temp));

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

  final K = Matrix2.zero();
  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, a12, a21, a22);
  _linearMass.setFrom(K);
  _linearMass.invert();

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

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

    final P = Vector2.zero();
    P.setFrom(_linearImpulse);

    temp
      ..setFrom(P)
      ..scale(mA);
    vA.sub(temp);
    wA -= iA * (_rA.cross(P) + _angularImpulse);

    temp
      ..setFrom(P)
      ..scale(mB);
    vB.add(temp);
    wB += iB * (_rB.cross(P) + _angularImpulse);
  } else {
    _linearImpulse.setZero();
    _angularImpulse = 0.0;
  }
  if (data.velocities[_indexA].w != wA) {
    assert(data.velocities[_indexA].w != wA);
  }
  data.velocities[_indexA].w = wA;
  data.velocities[_indexB].w = wB;
}