solveTOIPositionConstraints method
bool
solveTOIPositionConstraints(
- int toiIndexA,
- int toiIndexB
)
Implementation
bool solveTOIPositionConstraints(int toiIndexA, int toiIndexB) {
var minSeparation = 0.0;
for (final contact in _contacts) {
final pc = contact.positionConstraint;
final indexA = pc.indexA;
final indexB = pc.indexB;
final localCenterA = pc.localCenterA;
final localCenterB = pc.localCenterB;
final localCenterAx = localCenterA.x;
final localCenterAy = localCenterA.y;
final localCenterBx = localCenterB.x;
final localCenterBy = localCenterB.y;
final pointCount = pc.pointCount;
var mA = 0.0;
var iA = 0.0;
if (indexA == toiIndexA || indexA == toiIndexB) {
mA = pc.invMassA;
iA = pc.invIA;
}
var mB = 0.0;
var iB = 0.0;
if (indexB == toiIndexA || indexB == toiIndexB) {
mB = pc.invMassB;
iB = pc.invIB;
}
final cA = _positions[indexA].c;
var aA = _positions[indexA].a;
final cB = _positions[indexB].c;
var aB = _positions[indexB].a;
// Solve normal constraints
for (var j = 0; j < pointCount; ++j) {
final xfAq = _xfA.q;
final xfBq = _xfB.q;
xfAq.setAngle(aA);
xfBq.setAngle(aB);
_xfA.p.x = cA.x - xfAq.cos * localCenterAx + xfAq.sin * localCenterAy;
_xfA.p.y = cA.y - xfAq.sin * localCenterAx - xfAq.cos * localCenterAy;
_xfB.p.x = cB.x - xfBq.cos * localCenterBx + xfBq.sin * localCenterBy;
_xfB.p.y = cB.y - xfBq.sin * localCenterBx - xfBq.cos * localCenterBy;
final psm = _pSolver;
psm.initialize(pc, _xfA, _xfB, j);
final normal = psm.normal;
final point = psm.point;
final separation = psm.separation;
final rAx = point.x - cA.x;
final rAy = point.y - cA.y;
final rBx = point.x - cB.x;
final rBy = point.y - cB.y;
// Track max constraint error.
minSeparation = min(minSeparation, separation);
// Prevent large corrections and allow slop.
final C = (settings.baumgarte * (separation + settings.linearSlop))
.clamp(-settings.maxLinearCorrection, 0.0);
// Compute the effective mass.
final rnA = rAx * normal.y - rAy * normal.x;
final rnB = rBx * normal.y - rBy * normal.x;
final k = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
final impulse = k > 0.0 ? -C / k : 0.0;
final pX = normal.x * impulse;
final pY = normal.y * impulse;
cA.x -= pX * mA;
cA.y -= pY * mA;
aA -= iA * (rAx * pY - rAy * pX);
cB.x += pX * mB;
cB.y += pY * mB;
aB += iB * (rBx * pY - rBy * pX);
}
_positions[indexA].a = aA;
_positions[indexB].a = aB;
}
// We can't expect minSeparation >= -_linearSlop because we don't
// push the separation above -_linearSlop.
return minSeparation >= -1.5 * settings.linearSlop;
}