// Sequential position solver for position constraints. public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) { GGame.Math.Fix64 minSeparation = 0.0f; for (int i = 0; i < _count; ++i) { ContactPositionConstraint pc = _positionConstraints[i]; int indexA = pc.IndexA; int indexB = pc.IndexB; Vector2 localCenterA = pc.LocalCenterA; Vector2 localCenterB = pc.LocalCenterB; int pointCount = pc.PointCount; GGame.Math.Fix64 mA = 0.0f; GGame.Math.Fix64 iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.InvMassA; iA = pc.InvIA; } GGame.Math.Fix64 mB = 0.0f; GGame.Math.Fix64 iB = 0.0f; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.InvMassB; iB = pc.InvIB; } Vector2 cA = _positions[indexA].C; GGame.Math.Fix64 aA = _positions[indexA].A; Vector2 cB = _positions[indexB].C; GGame.Math.Fix64 aB = _positions[indexB].A; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { Transform xfA = new Transform(); Transform xfB = new Transform(); xfA.q.Set(aA); xfB.q.Set(aB); xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); PositionSolverManifold.Initialize(pc, xfA, xfB, j, out Vector2 normal, out Vector2 point, out GGame.Math.Fix64 separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min((float)minSeparation, (float)separation); // Prevent large corrections and allow slop. GGame.Math.Fix64 C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. GGame.Math.Fix64 rnA = MathUtils.Cross(rA, normal); GGame.Math.Fix64 rnB = MathUtils.Cross(rB, normal); GGame.Math.Fix64 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse GGame.Math.Fix64 impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } _positions[indexA].C = cA; _positions[indexA].A = aA; _positions[indexB].C = cB; _positions[indexB].A = aB; } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }
// Sequential position solver for position constraints. public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) { var minSeparation = 0.0f; for (var i = 0; i < _count; ++i) { var pc = _positionConstraints[i]; var indexA = pc.IndexA; var indexB = pc.IndexB; var localCenterA = pc.LocalCenterA; var localCenterB = pc.LocalCenterB; var pointCount = pc.PointCount; var mA = 0.0f; var iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.InvMassA; iA = pc.InvIA; } var mB = 0.0f; var iB = 0.0f; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.InvMassB; iB = pc.InvIB; } var cA = _positions[indexA].C; var aA = _positions[indexA].A; var cB = _positions[indexB].C; var aB = _positions[indexB].A; // Solve normal constraints for (var j = 0; j < pointCount; ++j) { var xfA = new Transform(); var xfB = new Transform(); xfA.q.Set(aA); xfB.q.Set(aB); xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); PositionSolverManifold.Initialize(pc, xfA, xfB, j, out var normal, out var point, out var separation); var rA = point - cA; var rB = point - cB; // Track max constraint error. minSeparation = Mathf.Min(minSeparation, separation); // Prevent large corrections and allow slop. var C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. var rnA = MathUtils.Cross(rA, normal); var rnB = MathUtils.Cross(rB, normal); var K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse var impulse = K > 0.0f ? -C / K : 0.0f; var P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } _positions[indexA].C = cA; _positions[indexA].A = aA; _positions[indexB].C = cB; _positions[indexB].A = aB; } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }