solvePositionConstraints method

Implementation

bool solvePositionConstraints() {
  var minSeparation = 0.0;

  for (final contact in _contacts) {
    final pc = contact.positionConstraint;

    final indexA = pc.indexA;
    final indexB = pc.indexB;

    final mA = pc.invMassA;
    final iA = pc.invIA;
    final localCenterA = pc.localCenterA;
    final localCenterAx = localCenterA.x;
    final localCenterAy = localCenterA.y;
    final mB = pc.invMassB;
    final iB = pc.invIB;
    final localCenterB = pc.localCenterB;
    final localCenterBx = localCenterB.x;
    final localCenterBy = localCenterB.y;
    final pointCount = pc.pointCount;

    final cA = _positions[indexA].c;
    var aA = _positions[indexA].a;
    final cB = _positions[indexB].c;
    var aB = _positions[indexB].a;

    // Solve normal constraints
    for (var j = 0; j < pointCount; ++j) {
      final xfAq = _xfA.q;
      final xfBq = _xfB.q;
      xfAq.setAngle(aA);
      xfBq.setAngle(aB);
      _xfA.p.x = cA.x - xfAq.cos * localCenterAx + xfAq.sin * localCenterAy;
      _xfA.p.y = cA.y - xfAq.sin * localCenterAx - xfAq.cos * localCenterAy;
      _xfB.p.x = cB.x - xfBq.cos * localCenterBx + xfBq.sin * localCenterBy;
      _xfB.p.y = cB.y - xfBq.sin * localCenterBx - xfBq.cos * localCenterBy;

      final psm = _pSolver;
      psm.initialize(pc, _xfA, _xfB, j);
      final normal = psm.normal;
      final point = psm.point;
      final separation = psm.separation;

      final rAx = point.x - cA.x;
      final rAy = point.y - cA.y;
      final rBx = point.x - cB.x;
      final rBy = point.y - cB.y;

      // Track max constraint error.
      minSeparation = min(minSeparation, separation);

      // Prevent large corrections and allow slop.
      final C = (settings.baumgarte * (separation + settings.linearSlop))
          .clamp(-settings.maxLinearCorrection, 0.0);

      // Compute the effective mass.
      final rnA = rAx * normal.y - rAy * normal.x;
      final rnB = rBx * normal.y - rBy * normal.x;
      final K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

      // Compute normal impulse
      final impulse = K > 0.0 ? -C / K : 0.0;

      final pX = normal.x * impulse;
      final pY = normal.y * impulse;

      cA.x -= pX * mA;
      cA.y -= pY * mA;
      aA -= iA * (rAx * pY - rAy * pX);

      cB.x += pX * mB;
      cB.y += pY * mB;
      aB += iB * (rBx * pY - rBy * pX);
    }

    _positions[indexA].a = aA;
    _positions[indexB].a = aB;
  }

  // We can't expect minSeparation >= -linearSlop because we don't
  // push the separation above -linearSlop.
  return minSeparation >= -3.0 * settings.linearSlop;
}