solvePositionConstraints method

@override bool solvePositionConstraints(SolverData data)

This returns true if the position errors are within tolerance. Internal.

Implementation

@override
bool solvePositionConstraints(SolverData data) {
  final qA = Rot();
  final qB = Rot();
  final cA = data.positions[_indexA].c;
  var aA = data.positions[_indexA].a;
  final cB = data.positions[_indexB].c;
  var aB = data.positions[_indexB].a;

  qA.setAngle(aA);
  qB.setAngle(aB);

  var angularError = 0.0;
  var positionError = 0.0;

  final fixedRotation = _invIA + _invIB == 0.0;

  // Solve angular limit constraint.
  if (_enableLimit &&
      _limitState != LimitState.inactive &&
      fixedRotation == false) {
    final angle = aB - aA - _referenceAngle;
    var limitImpulse = 0.0;

    if (_limitState == LimitState.equal) {
      // Prevent large angular corrections
      final c = (angle - _lowerAngle).clamp(
        -settings.maxAngularCorrection,
        settings.maxAngularCorrection,
      );
      limitImpulse = -_motorMass * c;
      angularError = c.abs();
    } else if (_limitState == LimitState.atLower) {
      var C = angle - _lowerAngle;
      angularError = -C;

      // Prevent large angular corrections and allow some slop.
      C = (C + settings.angularSlop)
          .clamp(-settings.maxAngularCorrection, 0.0);
      limitImpulse = -_motorMass * C;
    } else if (_limitState == LimitState.atUpper) {
      var C = angle - _upperAngle;
      angularError = C;

      // Prevent large angular corrections and allow some slop.
      C = (C - settings.angularSlop)
          .clamp(0.0, settings.maxAngularCorrection);
      limitImpulse = -_motorMass * C;
    }

    aA -= _invIA * limitImpulse;
    aB += _invIB * limitImpulse;
  }
  // Solve point-to-point constraint.
  {
    qA.setAngle(aA);
    qB.setAngle(aB);

    final rA = Vector2.zero();
    final rB = Vector2.zero();
    final temp = Vector2.zero();
    final impulse = Vector2.zero();

    temp
      ..setFrom(localAnchorA)
      ..sub(_localCenterA);
    rA.setFrom(Rot.mulVec2(qA, temp));
    temp
      ..setFrom(localAnchorB)
      ..sub(_localCenterB);
    rB.setFrom(Rot.mulVec2(qB, temp));

    temp
      ..setFrom(cB)
      ..add(rB)
      ..sub(cA)
      ..sub(rA);
    positionError = temp.length;

    final mA = _invMassA;
    final mB = _invMassB;
    final iA = _invIA;
    final iB = _invIB;

    final K = Matrix2.zero();
    final a11 = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
    final a21 = -iA * rA.x * rA.y - iB * rB.x * rB.y;
    final a12 = a21;
    final a22 = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;

    K.setValues(a11, a21, a12, a22);
    Matrix2.solve(K, impulse, temp);
    impulse.negate();

    cA.x -= mA * impulse.x;
    cA.y -= mA * impulse.y;
    aA -= iA * rA.cross(impulse);

    cB.x += mB * impulse.x;
    cB.y += mB * impulse.y;
    aB += iB * rB.cross(impulse);
  }
  data.positions[_indexA].a = aA;
  data.positions[_indexB].a = aB;

  return positionError <= settings.linearSlop &&
      angularError <= settings.angularSlop;
}