solvePositionConstraints method

Implementation

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

  final qA = Rot();
  final qB = Rot();
  final temp = Vector2.zero();

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

  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)
    ..sub(cA)
    ..add(rB)
    ..sub(rA);

  final ay = Vector2.copy(Rot.mulVec2(qA, _localYAxisA));
  final sAy = (temp
        ..setFrom(d)
        ..add(rA))
      .cross(ay);
  final sBy = rB.cross(ay);

  final c = d.dot(ay);

  final k =
      _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy;

  final impulse = k != 0.0 ? -c / k : 0.0;
  final p = Vector2.zero();
  p.x = impulse * ay.x;
  p.y = impulse * ay.y;
  final lA = impulse * sAy;
  final lB = impulse * sBy;

  cA.x -= _invMassA * p.x;
  cA.y -= _invMassA * p.y;
  aA -= _invIA * lA;
  cB.x += _invMassB * p.x;
  cB.y += _invMassB * p.y;
  aB += _invIB * lB;

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

  return c.abs() <= settings.linearSlop;
}