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 d = pool.popVec2();
  final Vector2 temp = pool.popVec2();
  final Vector2 rA = pool.popVec2();
  final Vector2 rB = pool.popVec2();

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

  // Compute the effective masses.
  Rot.mulToOutUnsafe(
      qA,
      d
        ..setFrom(_localAnchorA)
        ..sub(_localCenterA),
      rA);
  Rot.mulToOutUnsafe(
      qB,
      d
        ..setFrom(_localAnchorB)
        ..sub(_localCenterB),
      rB);
  d
    ..setFrom(cB)
    ..sub(cA)
    ..add(rB)
    ..sub(rA);

  double mA = _invMassA, mB = _invMassB;
  double iA = _invIA, iB = _invIB;

  // Compute motor Jacobian and effective mass.
  {
    Rot.mulToOutUnsafe(qA, _localXAxisA, _axis);
    temp
      ..setFrom(d)
      ..add(rA);
    _a1 = temp.cross(_axis);
    _a2 = rB.cross(_axis);

    _motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
    if (_motorMass > 0.0) {
      _motorMass = 1.0 / _motorMass;
    }
  }

  // Prismatic constraint.
  {
    Rot.mulToOutUnsafe(qA, _localYAxisA, _perp);

    temp
      ..setFrom(d)
      ..add(rA);
    _s1 = temp.cross(_perp);
    _s2 = rB.cross(_perp);

    double k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
    double k12 = iA * _s1 + iB * _s2;
    double k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
    double k22 = iA + iB;
    if (k22 == 0.0) {
      // For bodies with fixed rotation.
      k22 = 1.0;
    }
    double k23 = iA * _a1 + iB * _a2;
    double k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;

    _K.setValues(k11, k12, k13, k12, k22, k23, k13, k23, k33);
  }

  // Compute motor and limit terms.
  if (_enableLimit) {
    double jointTranslation = _axis.dot(d);
    if ((_upperTranslation - _lowerTranslation).abs() <
        2.0 * Settings.linearSlop) {
      _limitState = LimitState.EQUAL;
    } else if (jointTranslation <= _lowerTranslation) {
      if (_limitState != LimitState.AT_LOWER) {
        _limitState = LimitState.AT_LOWER;
        _impulse.z = 0.0;
      }
    } else if (jointTranslation >= _upperTranslation) {
      if (_limitState != LimitState.AT_UPPER) {
        _limitState = LimitState.AT_UPPER;
        _impulse.z = 0.0;
      }
    } else {
      _limitState = LimitState.INACTIVE;
      _impulse.z = 0.0;
    }
  } else {
    _limitState = LimitState.INACTIVE;
    _impulse.z = 0.0;
  }

  if (_enableMotor == false) {
    _motorImpulse = 0.0;
  }

  if (data.step.warmStarting) {
    // Account for variable time step.
    _impulse.scale(data.step.dtRatio);
    _motorImpulse *= data.step.dtRatio;

    final Vector2 P = pool.popVec2();
    temp
      ..setFrom(_axis)
      ..scale(_motorImpulse + _impulse.z);
    P
      ..setFrom(_perp)
      ..scale(_impulse.x)
      ..add(temp);

    double LA =
        _impulse.x * _s1 + _impulse.y + (_motorImpulse + _impulse.z) * _a1;
    double LB =
        _impulse.x * _s2 + _impulse.y + (_motorImpulse + _impulse.z) * _a2;

    vA.x -= mA * P.x;
    vA.y -= mA * P.y;
    wA -= iA * LA;

    vB.x += mB * P.x;
    vB.y += mB * P.y;
    wB += iB * LB;

    pool.pushVec2(1);
  } else {
    _impulse.setZero();
    _motorImpulse = 0.0;
  }

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

  pool.pushRot(2);
  pool.pushVec2(4);
}