solveVelocityConstraints method

@override void solveVelocityConstraints(SolverData data)

Implementation

@override
void solveVelocityConstraints(SolverData data) {
  final mA = _invMassA;
  final mB = _invMassB;
  final iA = _invIA;
  final iB = _invIB;

  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 temp = Vector2.zero();
  final p = Vector2.zero();

  // Solve spring constraint
  {
    final cDot = _ax.dot(
          temp
            ..setFrom(vB)
            ..sub(vA),
        ) +
        _sBx * wB -
        _sAx * wA;
    final impulse = -_springMass * (cDot + _bias + _gamma * _springImpulse);
    _springImpulse += impulse;

    p.x = impulse * _ax.x;
    p.y = impulse * _ax.y;
    final lA = impulse * _sAx;
    final lB = impulse * _sBx;

    vA.x -= mA * p.x;
    vA.y -= mA * p.y;
    wA -= iA * lA;

    vB.x += mB * p.x;
    vB.y += mB * p.y;
    wB += iB * lB;
  }

  // Solve rotational motor constraint
  {
    final cDot = wB - wA - _motorSpeed;
    var impulse = -_motorMass * cDot;

    final oldImpulse = _motorImpulse;
    final maxImpulse = data.step.dt * _maxMotorTorque;
    _motorImpulse = (_motorImpulse + impulse).clamp(-maxImpulse, maxImpulse);
    impulse = _motorImpulse - oldImpulse;

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

  // Solve point to line constraint
  {
    final cDot = _ay.dot(
          temp
            ..setFrom(vB)
            ..sub(vA),
        ) +
        _sBy * wB -
        _sAy * wA;
    final impulse = -_mass * cDot;
    _impulse += impulse;

    p.x = impulse * _ay.x;
    p.y = impulse * _ay.y;
    final lA = impulse * _sAy;
    final lB = impulse * _sBy;

    vA.x -= mA * p.x;
    vA.y -= mA * p.y;
    wA -= iA * lA;

    vB.x += mB * p.x;
    vB.y += mB * p.y;
    wB += iB * lB;
  }

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