initVelocityConstraints method

Implementation

@override
void initVelocityConstraints(SolverData data) {
  _indexB = bodyB.islandIndex;
  _localCenterB.setFrom(bodyB.sweep.localCenter);
  _invMassB = bodyB.inverseMass;
  _invIB = bodyB.inverseInertia;

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

  final qB = Rot();

  qB.setAngle(aB);

  final mass = bodyB.mass;

  // Frequency
  final omega = 2.0 * pi * _frequencyHz;

  // Damping coefficient
  final d = 2.0 * mass * _dampingRatio * omega;

  final springStiffness = mass * (omega * omega);

  // magic formulas
  // gamma has units of inverse mass.
  // beta has units of inverse time.
  final dt = data.step.dt;
  assert(d + dt * springStiffness > settings.epsilon);
  _gamma = dt * (d + dt * springStiffness);
  if (_gamma != 0.0) {
    _gamma = 1.0 / _gamma;
  }
  _beta = dt * springStiffness * _gamma;

  // Compute the effective mass matrix.
  final effectiveMassMatrix = Vector2.copy(localAnchorB)..sub(_localCenterB);

  _rB.setFrom(Rot.mulVec2(qB, effectiveMassMatrix));

  // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
  // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
  // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
  final k = Matrix2.zero();
  final a11 = _invMassB + _invIB * _rB.y * _rB.y + _gamma;
  final a21 = -_invIB * _rB.x * _rB.y;
  final a12 = a21;
  final a22 = _invMassB + _invIB * _rB.x * _rB.x + _gamma;

  k.setValues(a11, a21, a12, a22);
  _mass.setFrom(k);
  _mass.invert();

  _c
    ..setFrom(cB)
    ..add(_rB)
    ..sub(_targetA);
  _c.scale(_beta);

  // Cheat with some damping
  wB *= 0.98;

  if (data.step.warmStarting) {
    _impulse.scale(data.step.dtRatio);
    vB.x += _invMassB * _impulse.x;
    vB.y += _invMassB * _impulse.y;
    wB += _invIB * _rB.cross(_impulse);
  } else {
    _impulse.setZero();
  }

  data.velocities[_indexB].w = wB;
}