solvePositionConstraints method

bool solvePositionConstraints(
  1. SolverData data
)
override

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

Implementation

bool solvePositionConstraints(final SolverData data) {
  Vector2 cA = data.positions[_indexA].c;
  double aA = data.positions[_indexA].a;
  Vector2 cB = data.positions[_indexB].c;
  double aB = data.positions[_indexB].a;

  final Rot qA = pool.popRot();
  final Rot qB = pool.popRot();
  final Vector2 u = pool.popVec2();
  final Vector2 rA = pool.popVec2();
  final Vector2 rB = pool.popVec2();
  final Vector2 temp = pool.popVec2();

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

  // Compute the effective masses.
  Rot.mulToOutUnsafe(
      qA,
      temp
        ..setFrom(_localAnchorA)
        ..sub(_localCenterA),
      rA);
  Rot.mulToOutUnsafe(
      qB,
      temp
        ..setFrom(_localAnchorB)
        ..sub(_localCenterB),
      rB);
  u
    ..setFrom(cB)
    ..add(rB)
    ..sub(cA)
    ..sub(rA);

  double length = u.normalize();
  double C = length - _maxLength;

  C = MathUtils.clampDouble(C, 0.0, Settings.maxLinearCorrection);

  double impulse = -_mass * C;
  double Px = impulse * u.x;
  double Py = impulse * u.y;

  cA.x -= _invMassA * Px;
  cA.y -= _invMassA * Py;
  aA -= _invIA * (rA.x * Py - rA.y * Px);
  cB.x += _invMassB * Px;
  cB.y += _invMassB * Py;
  aB += _invIB * (rB.x * Py - rB.y * Px);

  pool.pushRot(2);
  pool.pushVec2(4);

  // data.positions[_indexA].c = cA;
  data.positions[_indexA].a = aA;
  // data.positions[_indexB].c = cB;
  data.positions[_indexB].a = aB;

  return length - _maxLength < Settings.linearSlop;
}