solveVelocityConstraints method

void solveVelocityConstraints (SolverData data)
override

Internal

Implementation

void solveVelocityConstraints(final SolverData data) {
  Vector2 vA = data.velocities[_indexA].v;
  double wA = data.velocities[_indexA].w;
  Vector2 vB = data.velocities[_indexB].v;
  double wB = data.velocities[_indexB].w;

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

  double h = data.step.dt;

  // Solve angular friction
  {
    double Cdot = wB - wA;
    double impulse = -_angularMass * Cdot;

    double oldImpulse = _angularImpulse;
    double maxImpulse = h * _maxTorque;
    _angularImpulse = MathUtils.clampDouble(
        _angularImpulse + impulse, -maxImpulse, maxImpulse);
    impulse = _angularImpulse - oldImpulse;

    wA -= iA * impulse;
    wB += iB * impulse;
  }

  // Solve linear friction
  {
    final Vector2 Cdot = pool.popVec2();
    final Vector2 temp = pool.popVec2();

    _rA.scaleOrthogonalInto(wA, temp);
    _rB.scaleOrthogonalInto(wB, Cdot);
    Cdot
      ..add(vB)
      ..sub(vA)
      ..sub(temp);

    final Vector2 impulse = pool.popVec2();
    _linearMass.transformed(Cdot, impulse);
    impulse.negate();

    final Vector2 oldImpulse = pool.popVec2();
    oldImpulse.setFrom(_linearImpulse);
    _linearImpulse.add(impulse);

    double maxImpulse = h * _maxForce;

    if (_linearImpulse.length2 > maxImpulse * maxImpulse) {
      _linearImpulse.normalize();
      _linearImpulse.scale(maxImpulse);
    }

    impulse
      ..setFrom(_linearImpulse)
      ..sub(oldImpulse);

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

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

//    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.pushVec2(4);
}