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

  // Solve linear motor constraint.
  if (_enableMotor && _limitState != LimitState.equal) {
    temp
      ..setFrom(vB)
      ..sub(vA);
    final cDot = _axis.dot(temp) + _a2 * wB - _a1 * wA;
    var impulse = _motorMass * (_motorSpeed - cDot);
    final oldImpulse = _motorImpulse;
    final maxImpulse = data.step.dt * _maxMotorForce;
    _motorImpulse = (_motorImpulse + impulse).clamp(-maxImpulse, maxImpulse);
    impulse = _motorImpulse - oldImpulse;

    final P = Vector2.zero();
    P
      ..setFrom(_axis)
      ..scale(impulse);
    final lA = impulse * _a1;
    final lB = impulse * _a2;

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

  final cDot1 = Vector2.zero();
  temp
    ..setFrom(vB)
    ..sub(vA);
  cDot1.x = _perp.dot(temp) + _s2 * wB - _s1 * wA;
  cDot1.y = wB - wA;

  if (_enableLimit && _limitState != LimitState.inactive) {
    // Solve prismatic and limit constraint in block form.
    final cDot2 = _axis.dot(vB - vA) + _a2 * wB - _a1 * wA;
    final cDot = Vector3(cDot1.x, cDot1.y, cDot2);
    final f1 = Vector3.zero();
    final df = Vector3.zero();

    f1.setFrom(_impulse);
    Matrix3.solve(_k, df, cDot..negate());

    // Cdot.negateLocal(); not used anymore
    _impulse.add(df);

    if (_limitState == LimitState.atLower) {
      _impulse.z = max(_impulse.z, 0.0);
    } else if (_limitState == LimitState.atUpper) {
      _impulse.z = min(_impulse.z, 0.0);
    }

    // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +
    // f1(1:2)
    final b = Vector2.zero();
    final f2r = Vector2.zero();

    temp
      ..setValues(_k.entry(0, 2), _k.entry(1, 2))
      ..scale(_impulse.z - f1.z);
    b
      ..setFrom(cDot1)
      ..negate()
      ..sub(temp);

    Matrix3.solve2(_k, f2r, b);
    f2r.add(Vector2(f1.x, f1.y));
    _impulse.x = f2r.x;
    _impulse.y = f2r.y;

    df
      ..setFrom(_impulse)
      ..sub(f1);

    final P = Vector2.zero();
    temp
      ..setFrom(_axis)
      ..scale(df.z);
    P
      ..setFrom(_perp)
      ..scale(df.x)
      ..add(temp);

    final lA = df.x * _s1 + df.y + df.z * _a1;
    final lB = df.x * _s2 + df.y + df.z * _a2;

    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;
  } else {
    // Limit is inactive, just solve the prismatic constraint in block form.
    final df = Vector2.zero();
    Matrix3.solve2(_k, df, cDot1..negate());
    cDot1.negate();

    _impulse.x += df.x;
    _impulse.y += df.y;

    final p = Vector2.zero();
    p
      ..setFrom(_perp)
      ..scale(df.x);
    final lA = df.x * _s1 + df.y;
    final lB = df.x * _s2 + df.y;

    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.set(vA);
  data.velocities[_indexA].w = wA;
  // data.velocities[_indexB].v.set(vB);
  data.velocities[_indexB].w = wB;
}