initVelocityConstraints method

void initVelocityConstraints(
  1. SolverData data
)
override

Internal

Implementation

void initVelocityConstraints(final SolverData data) {
  _indexA = _bodyA._islandIndex;
  _indexB = _bodyB._islandIndex;
  _localCenterA.setFrom(_bodyA._sweep.localCenter);
  _localCenterB.setFrom(_bodyB._sweep.localCenter);
  _invMassA = _bodyA._invMass;
  _invMassB = _bodyB._invMass;
  _invIA = _bodyA._invI;
  _invIB = _bodyB._invI;

  Vector2 cA = data.positions[_indexA].c;
  double aA = data.positions[_indexA].a;
  Vector2 vA = data.velocities[_indexA].v;
  double wA = data.velocities[_indexA].w;

  Vector2 cB = data.positions[_indexB].c;
  double aB = data.positions[_indexB].a;
  Vector2 vB = data.velocities[_indexB].v;
  double wB = data.velocities[_indexB].w;

  final Rot qA = pool.popRot();
  final Rot qB = pool.popRot();
  final Vector2 temp = pool.popVec2();

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

  // Compute the effective masses.
  Rot.mulToOutUnsafe(
      qA,
      temp
        ..setFrom(_localAnchorA)
        ..sub(_localCenterA),
      _rA);
  Rot.mulToOutUnsafe(
      qB,
      temp
        ..setFrom(_localAnchorB)
        ..sub(_localCenterB),
      _rB);

  _u
    ..setFrom(cB)
    ..add(_rB)
    ..sub(cA)
    ..sub(_rA);

  _length = _u.length;

  double C = _length - _maxLength;
  if (C > 0.0) {
    _state = LimitState.AT_UPPER;
  } else {
    _state = LimitState.INACTIVE;
  }

  if (_length > Settings.linearSlop) {
    _u.scale(1.0 / _length);
  } else {
    _u.setZero();
    _mass = 0.0;
    _impulse = 0.0;
    pool.pushRot(2);
    pool.pushVec2(1);
    return;
  }

  // Compute effective mass.
  double crA = _rA.cross(_u);
  double crB = _rB.cross(_u);
  double invMass =
      _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;

  _mass = invMass != 0.0 ? 1.0 / invMass : 0.0;

  if (data.step.warmStarting) {
    // Scale the impulse to support a variable time step.
    _impulse *= data.step.dtRatio;

    double Px = _impulse * _u.x;
    double Py = _impulse * _u.y;
    vA.x -= _invMassA * Px;
    vA.y -= _invMassA * Py;
    wA -= _invIA * (_rA.x * Py - _rA.y * Px);

    vB.x += _invMassB * Px;
    vB.y += _invMassB * Py;
    wB += _invIB * (_rB.x * Py - _rB.y * Px);
  } else {
    _impulse = 0.0;
  }

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

  // data.velocities[_indexA].v = vA;
  data.velocities[_indexA].w = wA;
  // data.velocities[_indexB].v = vB;
  data.velocities[_indexB].w = wB;
}