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;

  final Vector2 vpA = pool.popVec2();
  final Vector2 vpB = pool.popVec2();

  // Cdot = dot(u, v + cross(w, r))
  _rA.scaleOrthogonalInto(wA, vpA);
  vpA.add(vA);
  _rB.scaleOrthogonalInto(wB, vpB);
  vpB.add(vB);
  double Cdot = _u.dot(vpB..sub(vpA));

  double impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
  _impulse += impulse;

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

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

  pool.pushVec2(2);
}