initVelocityConstraints method

void initVelocityConstraints(
  1. SolverData data
)
override

Internal

Implementation

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

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

  final Vector2 cB = data.positions[_indexB].c;
  double aB = data.positions[_indexB].a;
  final 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();
  Matrix2 K = pool.popMat22();

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

  // Compute the effective mass matrix.
  // _rA = b2Mul(qA, -_localCenterA);
  // _rB = b2Mul(qB, -_localCenterB);
  _rA.x = qA.c * -_localCenterA.x - qA.s * -_localCenterA.y;
  _rA.y = qA.s * -_localCenterA.x + qA.c * -_localCenterA.y;
  _rB.x = qB.c * -_localCenterB.x - qB.s * -_localCenterB.y;
  _rB.y = qB.s * -_localCenterB.x + qB.c * -_localCenterB.y;

  // J = [-I -r1_skew I r2_skew]
  // [ 0 -1 0 1]
  // r_skew = [-ry; rx]

  // Matlab
  // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
  // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
  // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
  double mA = _invMassA, mB = _invMassB;
  double iA = _invIA, iB = _invIB;

  double a11 = mA + mB + iA * _rA.y * _rA.y + iB * _rB.y * _rB.y;
  double a21 = -iA * _rA.x * _rA.y - iB * _rB.x * _rB.y;
  double a12 = a21;
  double a22 = mA + mB + iA * _rA.x * _rA.x + iB * _rB.x * _rB.x;

  K.setValues(a11, a21, a12, a22);
  _linearMass.setFrom(K);
  _linearMass.invert();

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

  // _linearError = cB + _rB - cA - _rA - b2Mul(qA, _linearOffset);
  Rot.mulToOutUnsafe(qA, _linearOffset, temp);
  _linearError.x = cB.x + _rB.x - cA.x - _rA.x - temp.x;
  _linearError.y = cB.y + _rB.y - cA.y - _rA.y - temp.y;
  _angularError = aB - aA - _angularOffset;

  if (data.step.warmStarting) {
    // Scale impulses to support a variable time step.
    _linearImpulse.x *= data.step.dtRatio;
    _linearImpulse.y *= data.step.dtRatio;
    _angularImpulse *= data.step.dtRatio;

    final Vector2 P = _linearImpulse;
    vA.x -= mA * P.x;
    vA.y -= mA * P.y;
    wA -= iA * (_rA.x * P.y - _rA.y * P.x + _angularImpulse);
    vB.x += mB * P.x;
    vB.y += mB * P.y;
    wB += iB * (_rB.x * P.y - _rB.y * P.x + _angularImpulse);
  } else {
    _linearImpulse.setZero();
    _angularImpulse = 0.0;
  }

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

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