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 mA = _invMassA;
  final mB = _invMassB;
  final iA = _invIA;
  final iB = _invIB;

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

  d
    ..setFrom(cB)
    ..add(rB)
    ..sub(cA)
    ..sub(rA);

  // Point to line constraint
  {
    _ay.setFrom(Rot.mulVec2(qA, _localYAxisA));
    _sAy = (temp
          ..setFrom(d)
          ..add(rA))
        .cross(_ay);
    _sBy = rB.cross(_ay);

    _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy;

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

  // Spring constraint
  _springMass = 0.0;
  _bias = 0.0;
  _gamma = 0.0;
  if (_frequencyHz > 0.0) {
    _ax.setFrom(Rot.mulVec2(qA, _localXAxisA));
    _sAx = (temp
          ..setFrom(d)
          ..add(rA))
        .cross(_ax);
    _sBx = rB.cross(_ax);

    final invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx;

    if (invMass > 0.0) {
      _springMass = 1.0 / invMass;

      final c = d.dot(_ax);

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

      // Damping coefficient
      final dd = 2.0 * _springMass * _dampingRatio * omega;

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

      // magic formulas
      final dt = data.step.dt;
      _gamma = dt * (dd + dt * k);
      if (_gamma > 0.0) {
        _gamma = 1.0 / _gamma;
      }

      _bias = c * dt * k * _gamma;

      _springMass = invMass + _gamma;
      if (_springMass > 0.0) {
        _springMass = 1.0 / _springMass;
      }
    }
  } else {
    _springImpulse = 0.0;
  }

  // Rotational motor
  if (_enableMotor) {
    _motorMass = iA + iB;
    if (_motorMass > 0.0) {
      _motorMass = 1.0 / _motorMass;
    }
  } else {
    _motorMass = 0.0;
    _motorImpulse = 0.0;
  }

  if (data.step.warmStarting) {
    final p = Vector2.zero();
    // Account for variable time step.
    _impulse *= data.step.dtRatio;
    _springImpulse *= data.step.dtRatio;
    _motorImpulse *= data.step.dtRatio;

    p.x = _impulse * _ay.x + _springImpulse * _ax.x;
    p.y = _impulse * _ay.y + _springImpulse * _ax.y;
    final lA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse;
    final lB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;

    vA.x -= _invMassA * p.x;
    vA.y -= _invMassA * p.y;
    wA -= _invIA * lA;

    vB.x += _invMassB * p.x;
    vB.y += _invMassB * p.y;
    wB += _invIB * lB;
  } else {
    _impulse = 0.0;
    _springImpulse = 0.0;
    _motorImpulse = 0.0;
  }

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