solveVelocityConstraints method

void solveVelocityConstraints(
  1. SolverData data
)
override

Internal

Implementation

void solveVelocityConstraints(SolverData data) {
  final Vector2 vA = data.velocities[_indexA].v;
  double wA = data.velocities[_indexA].w;
  final 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;
  double inv_h = data.step.inv_dt;

  final Vector2 temp = pool.popVec2();

  // Solve angular friction
  {
    double Cdot = wB - wA + inv_h * _correctionFactor * _angularError;
    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;
  }

  final Vector2 Cdot = pool.popVec2();

  // Solve linear friction
  {
    // Cdot = vB + b2Cross(wB, _rB) - vA - b2Cross(wA, _rA) + inv_h * _correctionFactor *
    // _linearError;
    Cdot.x = vB.x +
        -wB * _rB.y -
        vA.x -
        -wA * _rA.y +
        inv_h * _correctionFactor * _linearError.x;
    Cdot.y = vB.y +
        wB * _rB.x -
        vA.y -
        wA * _rA.x +
        inv_h * _correctionFactor * _linearError.y;

    final Vector2 impulse = temp;
    _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.x = _linearImpulse.x - oldImpulse.x;
    impulse.y = _linearImpulse.y - oldImpulse.y;

    vA.x -= mA * impulse.x;
    vA.y -= mA * impulse.y;
    wA -= iA * (_rA.x * impulse.y - _rA.y * impulse.x);

    vB.x += mB * impulse.x;
    vB.y += mB * impulse.y;
    wB += iB * (_rB.x * impulse.y - _rB.y * impulse.x);
  }

  pool.pushVec2(3);

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