initVelocityConstraints method

void initVelocityConstraints (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();

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

  // use _u as temporary variable
  Rot.mulToOutUnsafe(
      qA,
      _u
        ..setFrom(_localAnchorA)
        ..sub(_localCenterA),
      _rA);
  Rot.mulToOutUnsafe(
      qB,
      _u
        ..setFrom(_localAnchorB)
        ..sub(_localCenterB),
      _rB);
  _u
    ..setFrom(cB)
    ..add(_rB)
    ..sub(cA)
    ..sub(_rA);

  pool.pushRot(2);

  // Handle singularity.
  double length = _u.length;
  if (length > Settings.linearSlop) {
    _u.x *= 1.0 / length;
    _u.y *= 1.0 / length;
  } else {
    _u.setValues(0.0, 0.0);
  }

  double crAu = _rA.cross(_u);
  double crBu = _rB.cross(_u);
  double invMass =
      _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;

  // Compute the effective mass matrix.
  _mass = invMass != 0.0 ? 1.0 / invMass : 0.0;

  if (_frequencyHz > 0.0) {
    double C = length - _length;

    // Frequency
    double omega = 2.0 * Math.PI * _frequencyHz;

    // Damping coefficient
    double d = 2.0 * _mass * _dampingRatio * omega;

    // Spring stiffness
    double k = _mass * omega * omega;

    // magic formulas
    double h = data.step.dt;
    _gamma = h * (d + h * k);
    _gamma = _gamma != 0.0 ? 1.0 / _gamma : 0.0;
    _bias = C * h * k * _gamma;

    invMass += _gamma;
    _mass = invMass != 0.0 ? 1.0 / invMass : 0.0;
  } else {
    _gamma = 0.0;
    _bias = 0.0;
  }
  if (data.step.warmStarting) {
    // Scale the impulse to support a variable time step.
    _impulse *= data.step.dtRatio;

    Vector2 P = pool.popVec2();
    P
      ..setFrom(_u)
      ..scale(_impulse);

    vA.x -= _invMassA * P.x;
    vA.y -= _invMassA * P.y;
    wA -= _invIA * _rA.cross(P);

    vB.x += _invMassB * P.x;
    vB.y += _invMassB * P.y;
    wB += _invIB * _rB.cross(P);

    pool.pushVec2(1);
  } else {
    _impulse = 0.0;
  }
//    data.velocities[_indexA].v.set(vA);
  data.velocities[_indexA].w = wA;
//    data.velocities[_indexB].v.set(vB);
  data.velocities[_indexB].w = wB;
}