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();

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

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

  _uA
    ..setFrom(cA)
    ..add(_rA)
    ..sub(_groundAnchorA);
  _uB
    ..setFrom(cB)
    ..add(_rB)
    ..sub(_groundAnchorB);

  final lengthA = _uA.length;
  final lengthB = _uB.length;

  if (lengthA > 10.0 * settings.linearSlop) {
    _uA.scale(1.0 / lengthA);
  } else {
    _uA.setZero();
  }

  if (lengthB > 10.0 * settings.linearSlop) {
    _uB.scale(1.0 / lengthB);
  } else {
    _uB.setZero();
  }

  // Compute effective mass.
  final ruA = _rA.cross(_uA);
  final ruB = _rB.cross(_uB);

  final mA = _invMassA + _invIA * ruA * ruA;
  final mB = _invMassB + _invIB * ruB * ruB;

  _mass = mA + _ratio * _ratio * mB;

  if (_mass > 0.0) {
    _mass = 1.0 / _mass;
  }

  if (data.step.warmStarting) {
    // Scale impulses to support variable time steps.
    _impulse *= data.step.dtRatio;

    // Warm starting.
    final pA = Vector2.zero();
    final pB = Vector2.zero();

    pA
      ..setFrom(_uA)
      ..scale(-_impulse);
    pB
      ..setFrom(_uB)
      ..scale(-_ratio * _impulse);

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