solvePositionConstraints method

bool solvePositionConstraints (SolverData data)
override

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

Implementation

bool solvePositionConstraints(final SolverData data) {
  if (_frequencyHz > 0.0) {
    return true;
  }
  final Rot qA = pool.popRot();
  final Rot qB = pool.popRot();
  final Vector2 rA = pool.popVec2();
  final Vector2 rB = pool.popVec2();
  final Vector2 u = pool.popVec2();

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

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

  Rot.mulToOutUnsafe(
      qA,
      u
        ..setFrom(_localAnchorA)
        ..sub(_localCenterA),
      rA);
  Rot.mulToOutUnsafe(
      qB,
      u
        ..setFrom(_localAnchorB)
        ..sub(_localCenterB),
      rB);
  u
    ..setFrom(cB)
    ..add(rB)
    ..sub(cA)
    ..sub(rA);

  double length = u.normalize();
  double C = length - _length;
  C = MathUtils.clampDouble(
      C, -Settings.maxLinearCorrection, 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);

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

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

  return C.abs() < Settings.linearSlop;
}