solvePositionConstraints method

@override bool solvePositionConstraints(SolverData data)

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

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 cC = data.positions[_indexC].c;
  var aC = data.positions[_indexC].a;
  final cD = data.positions[_indexD].c;
  var aD = data.positions[_indexD].a;

  final qA = Rot();
  final qB = Rot();
  final qC = Rot();
  final qD = Rot();
  qA.setAngle(aA);
  qB.setAngle(aB);
  qC.setAngle(aC);
  qD.setAngle(aD);

  // TODO(spydon): Is this really needed?
  const linearError = 0.0;

  double coordinateA;
  double coordinateB;

  final temp = Vector2.zero();
  final jvBC = Vector2.zero();
  final jvBD = Vector2.zero();
  double jwA;
  double jwB;
  double jwC;
  double jwD;
  var mass = 0.0;

  if (_joint1 is RevoluteJoint) {
    jvBC.setZero();
    jwA = 1.0;
    jwC = 1.0;
    mass += _iA + _iC;

    coordinateA = aA - aC - _referenceAngleA;
  } else {
    final rC = Vector2.zero();
    final rA = Vector2.zero();
    final pC = Vector2.zero();
    final pA = Vector2.zero();
    jvBC.setFrom(Rot.mulVec2(qC, _localAxisC));
    temp
      ..setFrom(_localAnchorC)
      ..sub(_lcC);
    rC.setFrom(Rot.mulVec2(qC, temp));
    temp
      ..setFrom(localAnchorA)
      ..sub(_lcA);
    rA.setFrom(Rot.mulVec2(qA, temp));
    jwC = rC.cross(jvBC);
    jwA = rA.cross(jvBC);
    mass += _mC + _mA + _iC * jwC * jwC + _iA * jwA * jwA;

    pC
      ..setFrom(_localAnchorC)
      ..sub(_lcC);
    temp
      ..setFrom(rA)
      ..add(cA)
      ..sub(cC);
    pA.setFrom(Rot.mulTransVec2(qC, temp));
    coordinateA = (pA..sub(pC)).dot(_localAxisC);
  }

  if (_joint2 is RevoluteJoint) {
    jvBD.setZero();
    jwB = ratio;
    jwD = ratio;
    mass += ratio * ratio * (_iB + _iD);

    coordinateB = aB - aD - _referenceAngleB;
  } else {
    final u = Vector2.zero();
    final rD = Vector2.zero();
    final rB = Vector2.zero();
    final pD = Vector2.zero();
    final pB = Vector2.zero();
    u.setFrom(Rot.mulVec2(qD, _localAxisD));
    temp
      ..setFrom(_localAnchorD)
      ..sub(_lcD);
    rD.setFrom(Rot.mulVec2(qD, temp));
    temp
      ..setFrom(localAnchorB)
      ..sub(_lcB);
    rB.setFrom(Rot.mulVec2(qB, temp));
    jvBD
      ..setFrom(u)
      ..scale(ratio);
    jwD = rD.cross(u);
    jwB = rB.cross(u);
    mass += ratio * ratio * (_mD + _mB) + _iD * jwD * jwD + _iB * jwB * jwB;

    pD
      ..setFrom(_localAnchorD)
      ..sub(_lcD);
    temp
      ..setFrom(rB)
      ..add(cB)
      ..sub(cD);
    pB.setFrom(Rot.mulTransVec2(qD, pB));
    coordinateB = (pB..sub(pD)).dot(_localAxisD);
  }

  final c = (coordinateA + ratio * coordinateB) - _constant;

  var impulse = 0.0;
  if (mass > 0.0) {
    impulse = -c / mass;
  }

  cA.x += (_mA * impulse) * jvBC.x;
  cA.y += (_mA * impulse) * jvBC.y;
  aA += _iA * impulse * jwA;

  cB.x += (_mB * impulse) * jvBD.x;
  cB.y += (_mB * impulse) * jvBD.y;
  aB += _iB * impulse * jwB;

  cC.x -= (_mC * impulse) * jvBC.x;
  cC.y -= (_mC * impulse) * jvBC.y;
  aC -= _iC * impulse * jwC;

  cD.x -= (_mD * impulse) * jvBD.x;
  cD.y -= (_mD * impulse) * jvBD.y;
  aD -= _iD * impulse * jwD;

  // data.positions[_indexA].c = cA;
  data.positions[_indexA].a = aA;
  // data.positions[_indexB].c = cB;
  data.positions[_indexB].a = aB;
  // data.positions[_indexC].c = cC;
  data.positions[_indexC].a = aC;
  // data.positions[_indexD].c = cD;
  data.positions[_indexD].a = aD;

  // TODO_ERIN not implemented
  return linearError < settings.linearSlop;
}