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) {
  final Rot qA = pool.popRot();
  final Rot qB = pool.popRot();
  final Vector2 rA = pool.popVec2();
  final Vector2 rB = pool.popVec2();
  final Vector2 d = pool.popVec2();
  final Vector2 axis = pool.popVec2();
  final Vector2 perp = pool.popVec2();
  final Vector2 temp = pool.popVec2();
  final Vector2 C1 = pool.popVec2();

  final Vector3 impulse = pool.popVec3();

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

  double mA = _invMassA, mB = _invMassB;
  double iA = _invIA, iB = _invIB;

  // Compute fresh Jacobians
  Rot.mulToOutUnsafe(
      qA,
      temp
        ..setFrom(_localAnchorA)
        ..sub(_localCenterA),
      rA);
  Rot.mulToOutUnsafe(
      qB,
      temp
        ..setFrom(_localAnchorB)
        ..sub(_localCenterB),
      rB);
  d
    ..setFrom(cB)
    ..add(rB)
    ..sub(cA)
    ..sub(rA);

  Rot.mulToOutUnsafe(qA, _localXAxisA, axis);
  double a1 = (temp
        ..setFrom(d)
        ..add(rA))
      .cross(axis);
  double a2 = rB.cross(axis);
  Rot.mulToOutUnsafe(qA, _localYAxisA, perp);

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

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

  double linearError = C1.x.abs();
  double angularError = C1.y.abs();

  bool active = false;
  double C2 = 0.0;
  if (_enableLimit) {
    double translation = axis.dot(d);
    if ((_upperTranslation - _lowerTranslation).abs() <
        2.0 * Settings.linearSlop) {
      // Prevent large angular corrections
      C2 = MathUtils.clampDouble(translation, -Settings.maxLinearCorrection,
          Settings.maxLinearCorrection);
      linearError = Math.max(linearError, translation.abs());
      active = true;
    } else if (translation <= _lowerTranslation) {
      // Prevent large linear corrections and allow some slop.
      C2 = MathUtils.clampDouble(
          translation - _lowerTranslation + Settings.linearSlop,
          -Settings.maxLinearCorrection,
          0.0);
      linearError = Math.max(linearError, _lowerTranslation - translation);
      active = true;
    } else if (translation >= _upperTranslation) {
      // Prevent large linear corrections and allow some slop.
      C2 = MathUtils.clampDouble(
          translation - _upperTranslation - Settings.linearSlop,
          0.0,
          Settings.maxLinearCorrection);
      linearError = Math.max(linearError, translation - _upperTranslation);
      active = true;
    }
  }

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

    final Matrix3 K = pool.popMat33();
    K.setValues(k11, k12, k13, k12, k22, k23, k13, k23, k33);

    final Vector3 C = pool.popVec3();
    C.x = C1.x;
    C.y = C1.y;
    C.z = C2;

    Matrix3.solve(K, impulse, C..negate());
    pool.pushVec3(1);
    pool.pushMat33(1);
  } else {
    double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
    double k12 = iA * s1 + iB * s2;
    double k22 = iA + iB;
    if (k22 == 0.0) {
      k22 = 1.0;
    }

    final Matrix2 K = pool.popMat22();

    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;

    pool.pushMat22(1);
  }

  double Px = impulse.x * perp.x + impulse.z * axis.x;
  double Py = impulse.x * perp.y + impulse.z * axis.y;
  double LA = impulse.x * s1 + impulse.y + impulse.z * a1;
  double 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].c.set(cA);
  data.positions[_indexA].a = aA;
  // data.positions[_indexB].c.set(cB);
  data.positions[_indexB].a = aB;

  pool.pushVec2(7);
  pool.pushVec3(1);
  pool.pushRot(2);

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