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

  final temp = Vector2.zero();

  // Solve angular friction
  {
    final cDot = wB - wA + invDt * _correctionFactor * _angularError;
    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;
  }

  final cDot = Vector2.zero();

  // Solve linear friction
  {
    // Cdot = vB + b2Cross(wB, _rB) - vA - b2Cross(wA, _rA) + inv_h *
    // _correctionFactor * _linearError;
    cDot.x = vB.x +
        -wB * _rB.y -
        vA.x -
        -wA * _rA.y +
        invDt * _correctionFactor * _linearError.x;
    cDot.y = vB.y +
        wB * _rB.x -
        vA.y -
        wA * _rA.x +
        invDt * _correctionFactor * _linearError.y;

    final impulse = temp;
    _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.x = _linearImpulse.x - oldImpulse.x;
    impulse.y = _linearImpulse.y - oldImpulse.y;

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

    vB.x += mB * impulse.x;
    vB.y += mB * impulse.y;
    wB += iB * (_rB.x * impulse.y - _rB.y * impulse.x);
  }

  // data.velocities[_indexA].v.set(vA);
  data.velocities[_indexA].w = wA;
  // data.velocities[_indexB].v.set(vB);
  data.velocities[_indexB].w = wB;
}