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 fixedRotation = iA + iB == 0.0;

  // Solve motor constraint.
  if (_enableMotor &&
      _limitState != LimitState.equal &&
      fixedRotation == false) {
    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;
  }
  final temp = Vector2.zero();

  // Solve limit constraint.
  if (_enableLimit &&
      _limitState != LimitState.inactive &&
      fixedRotation == false) {
    final cDot1 = Vector2.zero();
    final cDot = Vector3.zero();

    // Solve point-to-point constraint
    _rA.scaleOrthogonalInto(wA, temp);
    _rB.scaleOrthogonalInto(wB, cDot1);
    cDot1
      ..add(vB)
      ..sub(vA)
      ..sub(temp);
    final cDot2 = wB - wA;
    cDot.setValues(cDot1.x, cDot1.y, cDot2);

    final impulse = Vector3.zero();
    Matrix3.solve(_mass, impulse, cDot);
    impulse.negate();

    if (_limitState == LimitState.equal) {
      _impulse.add(impulse);
    } else if (_limitState == LimitState.atLower) {
      final newImpulse = _impulse.z + impulse.z;
      if (newImpulse < 0.0) {
        final rhs = Vector2.zero();
        rhs
          ..setValues(_mass.entry(0, 2), _mass.entry(1, 2))
          ..scale(_impulse.z)
          ..sub(cDot1);
        Matrix3.solve2(_mass, temp, rhs);
        impulse.x = temp.x;
        impulse.y = temp.y;
        impulse.z = -_impulse.z;
        _impulse.x += temp.x;
        _impulse.y += temp.y;
        _impulse.z = 0.0;
      } else {
        _impulse.add(impulse);
      }
    } else if (_limitState == LimitState.atUpper) {
      final newImpulse = _impulse.z + impulse.z;
      if (newImpulse > 0.0) {
        final rhs = Vector2.zero();
        rhs
          ..setValues(_mass.entry(0, 2), _mass.entry(1, 2))
          ..scale(_impulse.z)
          ..sub(cDot1);
        Matrix3.solve2(_mass, temp, rhs);
        impulse.x = temp.x;
        impulse.y = temp.y;
        impulse.z = -_impulse.z;
        _impulse.x += temp.x;
        _impulse.y += temp.y;
        _impulse.z = 0.0;
      } else {
        _impulse.add(impulse);
      }
    }
    final p = Vector2.zero();

    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);
  } else {
    // Solve point-to-point constraint
    final cDot = Vector2.zero();
    final impulse = Vector2.zero();

    _rA.scaleOrthogonalInto(wA, temp);
    _rB.scaleOrthogonalInto(wB, cDot);
    cDot
      ..add(vB)
      ..sub(vA)
      ..sub(temp);
    Matrix3.solve2(_mass, impulse, cDot..negate());

    _impulse.x += impulse.x;
    _impulse.y += impulse.y;

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

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

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