public Initialize ( |
||
pc | ||
xfA | ||
xfB | ||
index | int | |
Результат | void |
// Sequential position solver for position constraints. public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) { float minSeparation = 0.0f; for (int i = 0; i < Count; ++i) { ContactPositionConstraint pc = PositionConstraints[i]; int indexA = pc.IndexA; int indexB = pc.IndexB; Vec2 localCenterA = pc.LocalCenterA; Vec2 localCenterB = pc.LocalCenterB; int pointCount = pc.PointCount; float mA = 0.0f; float iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.InvMassA; iA = pc.InvIA; } float mB = pc.InvMassB; float iB = pc.InvIB; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.InvMassB; iB = pc.InvIB; } Vec2 cA = Positions[indexA].C; float aA = Positions[indexA].A; Vec2 cB = Positions[indexB].C; float aB = Positions[indexB].A; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { xfA.Q.Set(aA); xfB.Q.Set(aB); Rot.MulToOutUnsafe(xfA.Q, localCenterA, xfA.P); xfA.P.NegateLocal().AddLocal(cA); Rot.MulToOutUnsafe(xfB.Q, localCenterB, xfB.P); xfB.P.NegateLocal().AddLocal(cB); PositionSolverManifold psm = psolver; psm.Initialize(pc, xfA, xfB, j); Vec2 normal = psm.Normal; Vec2 point = psm.Point; float separation = psm.Separation; rA.Set(point).SubLocal(cA); rB.Set(point).SubLocal(cB); // Track max constraint error. minSeparation = MathUtils.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.TOI_BAUGARTE * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0f); // Compute the effective mass. float rnA = Vec2.Cross(rA, normal); float rnB = Vec2.Cross(rB, normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? (-C) / K : 0.0f; P.Set(normal).MulLocal(impulse); cA.SubLocal(temp.Set(P).MulLocal(mA)); aA -= iA * Vec2.Cross(rA, P); cB.AddLocal(temp.Set(P).MulLocal(mB)); aB += iB * Vec2.Cross(rB, P); } Positions[indexA].C.Set(cA); Positions[indexA].A = aA; Positions[indexB].C.Set(cB); Positions[indexB].A = aB; } // We can't expect minSpeparation >= -_linearSlop because we don't // push the separation above -_linearSlop. return(minSeparation >= (-1.5f) * Settings.LINEAR_SLOP); }