solveVelocityConstraints method

void solveVelocityConstraints(
  1. 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;

  // Cdot = dot(u, v + cross(w, r))
  Vector2 vpA = pool.popVec2();
  Vector2 vpB = pool.popVec2();
  Vector2 temp = pool.popVec2();

  _rA.scaleOrthogonalInto(wA, vpA);
  vpA.add(vA);
  _rB.scaleOrthogonalInto(wB, vpB);
  vpB.add(vB);

  double C = _length - _maxLength;
  double Cdot = _u.dot(temp
    ..setFrom(vpB)
    ..sub(vpA));

  // Predictive constraint.
  if (C < 0.0) {
    Cdot += data.step.inv_dt * C;
  }

  double impulse = -_mass * Cdot;
  double oldImpulse = _impulse;
  _impulse = Math.min(0.0, _impulse + impulse);
  impulse = _impulse - oldImpulse;

  double Px = impulse * _u.x;
  double Py = impulse * _u.y;
  vA.x -= _invMassA * Px;
  vA.y -= _invMassA * Py;
  wA -= _invIA * (_rA.x * Py - _rA.y * Px);
  vB.x += _invMassB * Px;
  vB.y += _invMassB * Py;
  wB += _invIB * (_rB.x * Py - _rB.y * Px);

  pool.pushVec2(3);

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