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 rA = Vector2.zero();
  final rB = Vector2.zero();
  final d = Vector2.zero();
  final axis = Vector2.zero();
  final perp = Vector2.zero();
  final temp = Vector2.zero();
  final c1 = Vector2.zero();

  final impulse = Vector3.zero();

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

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

  // Compute fresh Jacobians
  temp
    ..setFrom(localAnchorA)
    ..sub(_localCenterA);
  rA.setFrom(Rot.mulVec2(qA, temp));
  temp
    ..setFrom(localAnchorB)
    ..sub(_localCenterB);
  rB.setFrom(Rot.mulVec2(qB, temp));
  d
    ..setFrom(cB)
    ..add(rB)
    ..sub(cA)
    ..sub(rA);

  axis.setFrom(Rot.mulVec2(qA, localXAxisA));
  final a1 = (temp
        ..setFrom(d)
        ..add(rA))
      .cross(axis);
  final a2 = rB.cross(axis);
  perp.setFrom(Rot.mulVec2(qA, _localYAxisA));

  final s1 = (temp
        ..setFrom(d)
        ..add(rA))
      .cross(perp);
  final s2 = rB.cross(perp);

  c1.x = perp.dot(d);
  c1.y = aB - aA - _referenceAngle;

  var linearError = c1.x.abs();
  final angularError = c1.y.abs();

  var active = false;
  var c2 = 0.0;
  if (_enableLimit) {
    final translation = axis.dot(d);
    if ((_upperTranslation - _lowerTranslation).abs() <
        2.0 * settings.linearSlop) {
      // Prevent large angular corrections
      c2 = translation.clamp(
        -settings.maxLinearCorrection,
        settings.maxLinearCorrection,
      );
      linearError = max(linearError, translation.abs());
      active = true;
    } else if (translation <= _lowerTranslation) {
      // Prevent large linear corrections and allow some slop.
      c2 = (translation - _lowerTranslation + settings.linearSlop)
          .clamp(-settings.maxLinearCorrection, 0.0);
      linearError = max(linearError, _lowerTranslation - translation);
      active = true;
    } else if (translation >= _upperTranslation) {
      // Prevent large linear corrections and allow some slop.
      c2 = (translation - _upperTranslation - settings.linearSlop)
          .clamp(0.0, settings.maxLinearCorrection);
      linearError = max(linearError, translation - _upperTranslation);
      active = true;
    }
  }

  if (active) {
    final k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
    final k12 = iA * s1 + iB * s2;
    final k13 = iA * s1 * a1 + iB * s2 * a2;
    var k22 = iA + iB;
    if (k22 == 0.0) {
      // For fixed rotation
      k22 = 1.0;
    }
    final k23 = iA * a1 + iB * a2;
    final k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;

    final k = Matrix3.zero();
    k.setValues(k11, k12, k13, k12, k22, k23, k13, k23, k33);

    final c = Vector3.zero();
    c.x = c1.x;
    c.y = c1.y;
    c.z = c2;

    Matrix3.solve(k, impulse, c..negate());
  } else {
    final k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
    final k12 = iA * s1 + iB * s2;
    final k22 = iA + iB == 0.0 ? 1.0 : iA + iB;

    final k = Matrix2.zero();

    k.setValues(k11, k12, k12, k22);
    // temp is impulse1
    Matrix2.solve(k, temp, c1..negate());
    c1.negate();

    impulse.x = temp.x;
    impulse.y = temp.y;
    impulse.z = 0.0;
  }

  final pX = impulse.x * perp.x + impulse.z * axis.x;
  final pY = impulse.x * perp.y + impulse.z * axis.y;
  final lA = impulse.x * s1 + impulse.y + impulse.z * a1;
  final lB = impulse.x * s2 + impulse.y + impulse.z * a2;

  cA.x -= mA * pX;
  cA.y -= mA * pY;
  aA -= iA * lA;
  cB.x += mB * pX;
  cB.y += mB * pY;
  aB += iB * lB;

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

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