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