solveVelocityConstraints method

@override void solveVelocityConstraints(SolverData data)

Implementation

@override
void solveVelocityConstraints(SolverData data) {
  final vA = data.velocities[_indexA].v;
  var wA = data.velocities[_indexA].w;
  final vB = data.velocities[_indexB].v;
  var wB = data.velocities[_indexB].w;

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

  final cDot1 = Vector2.zero();
  final p = Vector2.zero();
  final temp = Vector2.zero();
  if (_frequencyHz > 0.0) {
    final cDot2 = wB - wA;

    final impulse2 =
        -_mass.entry(2, 2) * (cDot2 + _bias + _gamma * _impulse.z);
    _impulse.z += impulse2;

    wA -= iA * impulse2;
    wB += iB * impulse2;

    _rB.scaleOrthogonalInto(wB, cDot1);
    _rA.scaleOrthogonalInto(wA, temp);
    cDot1
      ..add(vB)
      ..sub(vA)
      ..sub(temp);

    final impulse1 = Vector2(
      _mass.entry(1, 0) * cDot1.x + _mass.entry(1, 1) * cDot1.y,
      _mass.entry(0, 0) * cDot1.x + _mass.entry(0, 1) * cDot1.y,
    )..negate();

    _impulse.x += impulse1.x;
    _impulse.y += impulse1.y;

    vA.x -= mA * p.x;
    vA.y -= mA * p.y;
    wA -= iA * _rA.cross(p);

    vB.x += mB * p.x;
    vB.y += mB * p.y;
    wB += iB * _rB.cross(p);
  } else {
    _rA.scaleOrthogonalInto(wA, temp);
    _rB.scaleOrthogonalInto(wB, cDot1);
    cDot1
      ..add(vB)
      ..sub(vA)
      ..sub(temp);
    final cDot2 = wB - wA;

    final impulse = Vector3(cDot1.x, cDot1.y, cDot2)
      ..applyMatrix3(_mass)
      ..negate();
    _impulse.add(impulse);

    p.setValues(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);
  }

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