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 dt = data.step.dt;

  // Solve angular friction
  {
    final cDot = wB - wA;
    var impulse = -_angularMass * cDot;

    final oldImpulse = _angularImpulse;
    final maxImpulse = dt * _maxTorque;
    _angularImpulse =
        (_angularImpulse + impulse).clamp(-maxImpulse, maxImpulse);
    impulse = _angularImpulse - oldImpulse;

    wA -= iA * impulse;
    wB += iB * impulse;
  }

  // Solve linear friction
  {
    final cDot = Vector2.zero();
    final temp = Vector2.zero();

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

    final impulse = Vector2.zero();
    _linearMass.transformed(cDot, impulse);
    impulse.negate();

    final oldImpulse = Vector2.zero();
    oldImpulse.setFrom(_linearImpulse);
    _linearImpulse.add(impulse);

    final maxImpulse = dt * _maxForce;

    if (_linearImpulse.length2 > maxImpulse * maxImpulse) {
      _linearImpulse.normalize();
      _linearImpulse.scale(maxImpulse);
    }

    impulse
      ..setFrom(_linearImpulse)
      ..sub(oldImpulse);

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

    temp
      ..setFrom(impulse)
      ..scale(mB);
    vB.add(temp);
    wB += iB * _rB.cross(impulse);
  }

  if (data.velocities[_indexA].w != wA) {
    assert(data.velocities[_indexA].w != wA);
  }
  data.velocities[_indexA].w = wA;
  data.velocities[_indexB].w = wB;
}