initializeVelocityConstraints method

void initializeVelocityConstraints()

Implementation

void initializeVelocityConstraints() {
  // Warm start.
  for (final contact in _contacts) {
    final velocityConstraint = contact.velocityConstraint;
    final positionConstraint = contact.positionConstraint;

    final radiusA = positionConstraint.radiusA;
    final radiusB = positionConstraint.radiusB;
    final manifold = _contacts[velocityConstraint.contactIndex].manifold;

    final indexA = velocityConstraint.indexA;
    final indexB = velocityConstraint.indexB;

    final mA = velocityConstraint.invMassA;
    final mB = velocityConstraint.invMassB;
    final iA = velocityConstraint.invIA;
    final iB = velocityConstraint.invIB;
    final localCenterA = positionConstraint.localCenterA;
    final localCenterB = positionConstraint.localCenterB;

    final cA = _positions[indexA].c;
    final aA = _positions[indexA].a;
    final vA = _velocities[indexA].v;
    final wA = _velocities[indexA].w;

    final cB = _positions[indexB].c;
    final aB = _positions[indexB].a;
    final vB = _velocities[indexB].v;
    final wB = _velocities[indexB].w;

    assert(manifold.pointCount > 0);

    final xfAq = _xfA.q;
    final xfBq = _xfB.q;
    xfAq.setAngle(aA);
    xfBq.setAngle(aB);
    _xfA.p.x = cA.x - (xfAq.cos * localCenterA.x - xfAq.sin * localCenterA.y);
    _xfA.p.y = cA.y - (xfAq.sin * localCenterA.x + xfAq.cos * localCenterA.y);
    _xfB.p.x = cB.x - (xfBq.cos * localCenterB.x - xfBq.sin * localCenterB.y);
    _xfB.p.y = cB.y - (xfBq.sin * localCenterB.x + xfBq.cos * localCenterB.y);

    _worldManifold.initialize(manifold, _xfA, radiusA, _xfB, radiusB);

    final vcNormal = velocityConstraint.normal;
    vcNormal.x = _worldManifold.normal.x;
    vcNormal.y = _worldManifold.normal.y;

    final pointCount = velocityConstraint.pointCount;
    for (var j = 0; j < pointCount; ++j) {
      final vcp = velocityConstraint.points[j];
      final wmPj = _worldManifold.points[j];
      final vcprA = vcp.rA;
      final vcprB = vcp.rB;
      vcprA.x = wmPj.x - cA.x;
      vcprA.y = wmPj.y - cA.y;
      vcprB.x = wmPj.x - cB.x;
      vcprB.y = wmPj.y - cB.y;

      final rnA = vcprA.x * vcNormal.y - vcprA.y * vcNormal.x;
      final rnB = vcprB.x * vcNormal.y - vcprB.y * vcNormal.x;

      final kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

      vcp.normalMass = kNormal > 0.0 ? 1.0 / kNormal : 0.0;

      final tangentX = 1.0 * vcNormal.y;
      final tangentY = -1.0 * vcNormal.x;

      final rtA = vcprA.x * tangentY - vcprA.y * tangentX;
      final rtB = vcprB.x * tangentY - vcprB.y * tangentX;

      final kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;

      vcp.tangentMass = kTangent > 0.0 ? 1.0 / kTangent : 0.0;

      // Setup a velocity bias for restitution.
      vcp.velocityBias = 0.0;
      final tempX = vB.x + -wB * vcprB.y - vA.x - (-wA * vcprA.y);
      final tempY = vB.y + wB * vcprB.x - vA.y - (wA * vcprA.x);
      final vRel = vcNormal.x * tempX + vcNormal.y * tempY;
      if (vRel < -settings.velocityThreshold) {
        vcp.velocityBias = -velocityConstraint.restitution * vRel;
      }
    }

    // If we have two points, then prepare the block solver.
    if (velocityConstraint.pointCount == 2) {
      final vcp1 = velocityConstraint.points[0];
      final vcp2 = velocityConstraint.points[1];
      final rn1A = vcp1.rA.x * vcNormal.y - vcp1.rA.y * vcNormal.x;
      final rn1B = vcp1.rB.x * vcNormal.y - vcp1.rB.y * vcNormal.x;
      final rn2A = vcp2.rA.x * vcNormal.y - vcp2.rA.y * vcNormal.x;
      final rn2B = vcp2.rB.x * vcNormal.y - vcp2.rB.y * vcNormal.x;

      final k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
      final k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
      final k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
      if (k11 * k11 < maxConditionNumber * (k11 * k22 - k12 * k12)) {
        // K is safe to invert.
        velocityConstraint.K.setValues(k11, k12, k12, k22);
        velocityConstraint.normalMass.setFrom(velocityConstraint.K);
        velocityConstraint.normalMass.invert();
      } else {
        // The constraints are redundant, just use one.
        // TODO_ERIN use deepest?
        velocityConstraint.pointCount = 1;
      }
    }
  }
}