solveVelocityConstraints method

void solveVelocityConstraints (SolverData step)
override

Internal

Implementation

void solveVelocityConstraints(final SolverData step) {
  double crossMassSum = 0.0;
  double dotMassSum = 0.0;

  List<Velocity> velocities = step.velocities;
  List<Position> positions = step.positions;
  final List<Vector2> d = pool.getVec2Array(_bodies.length);

  for (int i = 0; i < _bodies.length; ++i) {
    final int prev = (i == 0) ? _bodies.length - 1 : i - 1;
    final int next = (i == _bodies.length - 1) ? 0 : i + 1;
    d[i].setFrom(positions[_bodies[next]._islandIndex].c);
    d[i].sub(positions[_bodies[prev]._islandIndex].c);
    dotMassSum += (d[i].length2) / _bodies[i].mass;
    crossMassSum += velocities[_bodies[i]._islandIndex].v.cross(d[i]);
  }
  double lambda = -2.0 * crossMassSum / dotMassSum;
  // System.out.println(crossMassSum + " " +dotMassSum);
  // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection,
  // Settings.maxLinearCorrection);
  _impulse += lambda;
  // System.out.println(_impulse);
  for (int i = 0; i < _bodies.length; ++i) {
    velocities[_bodies[i]._islandIndex].v.x +=
        _bodies[i]._invMass * d[i].y * .5 * lambda;
    velocities[_bodies[i]._islandIndex].v.y +=
        _bodies[i]._invMass * -d[i].x * .5 * lambda;
  }
}