initVelocityConstraints method

@override void initVelocityConstraints(SolverData data)

Implementation

@override
void initVelocityConstraints(SolverData data) {
  _indexA = bodyA.islandIndex;
  _indexB = bodyB.islandIndex;
  _indexC = _bodyC.islandIndex;
  _indexD = _bodyD.islandIndex;
  _lcA.setFrom(bodyA.sweep.localCenter);
  _lcB.setFrom(bodyB.sweep.localCenter);
  _lcC.setFrom(_bodyC.sweep.localCenter);
  _lcD.setFrom(_bodyD.sweep.localCenter);
  _mA = bodyA.inverseMass;
  _mB = bodyB.inverseMass;
  _mC = _bodyC.inverseMass;
  _mD = _bodyD.inverseMass;
  _iA = bodyA.inverseInertia;
  _iB = bodyB.inverseInertia;
  _iC = _bodyC.inverseInertia;
  _iD = _bodyD.inverseInertia;

  // Vec2 cA = data.positions[_indexA].c;
  final aA = data.positions[_indexA].a;
  final vA = data.velocities[_indexA].v;
  var wA = data.velocities[_indexA].w;

  // Vec2 cB = data.positions[_indexB].c;
  final aB = data.positions[_indexB].a;
  final vB = data.velocities[_indexB].v;
  var wB = data.velocities[_indexB].w;

  // Vec2 cC = data.positions[_indexC].c;
  final aC = data.positions[_indexC].a;
  final vC = data.velocities[_indexC].v;
  var wC = data.velocities[_indexC].w;

  // Vec2 cD = data.positions[_indexD].c;
  final aD = data.positions[_indexD].a;
  final vD = data.velocities[_indexD].v;
  var wD = data.velocities[_indexD].w;

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

  _mass = 0.0;

  final temp = Vector2.zero();

  if (_joint1 is RevoluteJoint) {
    _jvAC.setZero();
    _jwA = 1.0;
    _jwC = 1.0;
    _mass += _iA + _iC;
  } else {
    final rC = Vector2.zero();
    final rA = Vector2.zero();
    _jvAC.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(_jvAC);
    _jwA = rA.cross(_jvAC);
    _mass += _mC + _mA + _iC * _jwC * _jwC + _iA * _jwA * _jwA;
  }

  if (_joint2 is RevoluteJoint) {
    _jvBD.setZero();
    _jwB = ratio;
    _jwD = ratio;
    _mass += ratio * ratio * (_iB + _iD);
  } else {
    final u = Vector2.zero();
    final rD = Vector2.zero();
    final rB = 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 = ratio * rD.cross(u);
    _jwB = ratio * rB.cross(u);
    _mass +=
        ratio * ratio * (_mD + _mB) + _iD * _jwD * _jwD + _iB * _jwB * _jwB;
  }

  // Compute effective mass.
  _mass = _mass > 0.0 ? 1.0 / _mass : 0.0;

  if (data.step.warmStarting) {
    vA.x += (_mA * _impulse) * _jvAC.x;
    vA.y += (_mA * _impulse) * _jvAC.y;
    wA += _iA * _impulse * _jwA;

    vB.x += (_mB * _impulse) * _jvBD.x;
    vB.y += (_mB * _impulse) * _jvBD.y;
    wB += _iB * _impulse * _jwB;

    vC.x -= (_mC * _impulse) * _jvAC.x;
    vC.y -= (_mC * _impulse) * _jvAC.y;
    wC -= _iC * _impulse * _jwC;

    vD.x -= (_mD * _impulse) * _jvBD.x;
    vD.y -= (_mD * _impulse) * _jvBD.y;
    wD -= _iD * _impulse * _jwD;
  } else {
    _impulse = 0.0;
  }

  data.velocities[_indexA].w = wA;
  data.velocities[_indexB].w = wB;
  data.velocities[_indexC].w = wC;
  data.velocities[_indexD].w = wD;
}