initVelocityConstraints method

void initVelocityConstraints (SolverData data)
override

@see org.jbox2d.dynamics.joints.Joint#initVelocityConstraints(org.jbox2d.dynamics.TimeStep)

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;

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

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

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

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

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

  // 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;

  final Matrix2 K = pool.popMat22();
  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, a12, a21, a22);
  _linearMass.setFrom(K);
  _linearMass.invert();

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

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

    final Vector2 P = pool.popVec2();
    P.setFrom(_linearImpulse);

    temp
      ..setFrom(P)
      ..scale(mA);
    vA.sub(temp);
    wA -= iA * (_rA.cross(P) + _angularImpulse);

    temp
      ..setFrom(P)
      ..scale(mB);
    vB.add(temp);
    wB += iB * (_rB.cross(P) + _angularImpulse);

    pool.pushVec2(1);
  } else {
    _linearImpulse.setZero();
    _angularImpulse = 0.0;
  }
//    data.velocities[_indexA].v.set(vA);
  if (data.velocities[_indexA].w != wA) {
    assert(data.velocities[_indexA].w != wA);
  }
  data.velocities[_indexA].w = wA;
//    data.velocities[_indexB].v.set(vB);
  data.velocities[_indexB].w = wB;

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