initVelocityConstraints method

@override void initVelocityConstraints(SolverData data)

Implementation

@override
void initVelocityConstraints(SolverData data) {
  _indexA = bodyA.islandIndex;
  _indexB = bodyB.islandIndex;
  _localCenterA.setFrom(bodyA.sweep.localCenter);
  _localCenterB.setFrom(bodyB.sweep.localCenter);
  _invMassA = bodyA.inverseMass;
  _invMassB = bodyB.inverseMass;
  _invIA = bodyA.inverseInertia;
  _invIB = bodyB.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;
  final qA = Rot();
  final qB = Rot();
  final temp = Vector2.zero();

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

  // Compute the effective masses.
  temp
    ..setFrom(localAnchorA)
    ..sub(_localCenterA);
  _rA.setFrom(Rot.mulVec2(qA, temp));
  temp
    ..setFrom(localAnchorB)
    ..sub(_localCenterB);
  _rB.setFrom(Rot.mulVec2(qB, temp));

  final mA = _invMassA;
  final mB = _invMassB;
  final iA = _invIA;
  final iB = _invIB;

  final fixedRotation = iA + iB == 0.0;

  final exX = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
  final eyX = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
  final ezX = -_rA.y * iA - _rB.y * iB;
  final exY = _mass.entry(0, 1);
  final eyY = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
  final ezY = _rA.x * iA + _rB.x * iB;
  final exZ = _mass.entry(0, 2);
  final eyZ = _mass.entry(1, 2);
  final ezZ = iA + iB;

  _mass.setValues(exX, exY, exZ, eyX, eyY, eyZ, ezX, ezY, ezZ);

  _motorMass = iA + iB;
  if (_motorMass > 0.0) {
    _motorMass = 1.0 / _motorMass;
  }

  if (_enableMotor == false || fixedRotation) {
    _motorImpulse = 0.0;
  }

  if (_enableLimit && fixedRotation == false) {
    final jointAngle = aB - aA - _referenceAngle;
    if ((_upperAngle - _lowerAngle).abs() < 2.0 * settings.angularSlop) {
      _limitState = LimitState.equal;
    } else if (jointAngle <= _lowerAngle) {
      if (_limitState != LimitState.atLower) {
        _impulse.z = 0.0;
      }
      _limitState = LimitState.atLower;
    } else if (jointAngle >= _upperAngle) {
      if (_limitState != LimitState.atUpper) {
        _impulse.z = 0.0;
      }
      _limitState = LimitState.atUpper;
    } else {
      _limitState = LimitState.inactive;
      _impulse.z = 0.0;
    }
  } else {
    _limitState = LimitState.inactive;
  }

  if (data.step.warmStarting) {
    final P = Vector2.zero();
    // Scale impulses to support a variable time step.
    _impulse.x *= data.step.dtRatio;
    _impulse.y *= data.step.dtRatio;
    _motorImpulse *= data.step.dtRatio;

    P.x = _impulse.x;
    P.y = _impulse.y;

    vA.x -= mA * P.x;
    vA.y -= mA * P.y;
    wA -= iA * (_rA.cross(P) + _motorImpulse + _impulse.z);

    vB.x += mB * P.x;
    vB.y += mB * P.y;
    wB += iB * (_rB.cross(P) + _motorImpulse + _impulse.z);
  } else {
    _impulse.setZero();
    _motorImpulse = 0.0;
  }
  // data.velocities[_indexA].v.set(vA);
  data.velocities[_indexA].w = wA;
  // data.velocities[_indexB].v.set(vB);
  data.velocities[_indexB].w = wB;
}