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 cA = data.positions[_indexA].c;
  var aA = data.positions[_indexA].a;
  final cB = data.positions[_indexB].c;
  var aB = data.positions[_indexB].a;
  final qA = Rot();
  final qB = Rot();

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

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

  final temp = Vector2.copy(localAnchorA)..sub(_localCenterA);
  final rA = Vector2.copy(Rot.mulVec2(qA, temp));
  temp
    ..setFrom(localAnchorB)
    ..sub(_localCenterB);
  final rB = Vector2.copy(Rot.mulVec2(qB, temp));
  double positionError;
  double angularError;

  final k = Matrix3.zero();
  final c1 = Vector2.zero();
  final p = Vector2.zero();

  final exX = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
  final eyX = -rA.y * rA.x * iA - rB.y * rB.x * iB;
  final ezX = -rA.y * iA - rB.y * iB;
  final exY = k.entry(0, 1);
  final eyY = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
  final ezY = rA.x * iA + rB.x * iB;
  final exZ = k.entry(0, 2);
  final eyZ = k.entry(1, 2);
  final ezZ = iA + iB;

  k.setValues(exX, exY, exZ, eyX, eyY, eyZ, ezX, ezY, ezZ);

  if (_frequencyHz > 0.0) {
    c1
      ..setFrom(cB)
      ..add(rB)
      ..sub(cA)
      ..sub(rA);

    positionError = c1.length;
    angularError = 0.0;

    Matrix3.solve2(k, p, c1);
    p.negate();

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

    cB.x += mB * p.x;
    cB.y += mB * p.y;
    aB += iB * rB.cross(p);
  } else {
    c1
      ..setFrom(cB)
      ..add(rB)
      ..sub(cA)
      ..sub(rA);
    final c2 = aB - aA - _referenceAngle;

    positionError = c1.length;
    angularError = c2.abs();

    final C = Vector3(c1.x, c1.y, c2);
    final impulse = Vector3.zero();

    Matrix3.solve(k, impulse, C);
    impulse.negate();
    p.setValues(impulse.x, impulse.y);

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

    cB.x += mB * p.x;
    cB.y += mB * p.y;
    aB += iB * (rB.cross(p) + impulse.z);
  }

  data.positions[_indexA].a = aA;
  data.positions[_indexB].a = aB;

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