public bool SolvePositionConstraints(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; Body bodyA = c.BodyA; Body bodyB = c.BodyB; float invMassA = bodyA.Mass * bodyA.InvMass; float invIA = bodyA.Mass * bodyA.InvI; float invMassB = bodyB.Mass * bodyB.InvMass; float invIB = bodyB.Mass * bodyB.InvI; // Solve normal constraints for (int j = 0; j < c.PointCount; ++j) { PositionSolverManifold psm = new PositionSolverManifold(ref c, j); Vector2 normal = psm.Normal; Vector2 point = psm.Point; float separation = psm.Separation; Vector2 rA = point - bodyA.Sweep.c; Vector2 rB = point - bodyB.Sweep.c; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(rA, normal); float rnB = MathUtils.Cross(rB, normal); float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; #if MATH_OVERLOADS Vector2 P = impulse * normal; bodyA.Sweep.c -= invMassA * P; bodyA.Sweep.a -= invIA * MathUtils.Cross(rA, P); bodyB.Sweep.c += invMassB * P; bodyB.Sweep.a += invIB * MathUtils.Cross(rB, P); #else Vector2 P = new Vector2(impulse * normal.X, impulse * normal.Y); bodyA.Sweep.c.X -= invMassA * P.X; bodyA.Sweep.c.Y -= invMassA * P.Y; bodyA.Sweep.a -= invIA * (rA.X * P.Y - rA.Y * P.X); bodyB.Sweep.c.X += invMassB * P.X; bodyB.Sweep.c.Y += invMassB * P.Y; bodyB.Sweep.a += invIB * (rB.X * P.Y - rB.Y * P.X); #endif bodyA.SynchronizeTransform(); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't // push the separation above -Settings.b2_linearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }
// 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; System.Numerics.Vector2 localCenterA = pc.LocalCenterA; System.Numerics.Vector2 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 = 0.0f; float iB = 0.0f; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.InvMassB; iB = pc.InvIB; } System.Numerics.Vector2 cA = _positions[indexA].C; float aA = _positions[indexA].A; System.Numerics.Vector2 cB = _positions[indexB].C; float 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); System.Numerics.Vector2 normal; System.Numerics.Vector2 point; float separation; PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation); System.Numerics.Vector2 rA = point - cA; System.Numerics.Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(rA, normal); float rnB = MathUtils.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; System.Numerics.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); }
public bool solvePositionConstraints() { float 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; float mA = pc.invMassA; float iA = pc.invIA; Vector2 localCenterB = pc.localCenterB; float mB = pc.invMassB; float iB = pc.invIB; int pointCount = pc.pointCount; Vector2 cA = _positions[indexA].c; float aA = _positions[indexA].a; Vector2 cB = _positions[indexB].c; float 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); Vector2 normal; Vector2 point; float separation; PositionSolverManifold.initialize(pc, xfA, xfB, j, out normal, out point, out separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.clamp(Settings.baumgarte * (separation + Settings.linearSlop), -Settings.maxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.cross(rA, normal); float rnB = MathUtils.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; 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 >= -3.0f * Settings.linearSlop); }
public bool SolvePositionConstraints(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; Body bodyA = c.BodyA; Body bodyB = c.BodyB; float invMassA = bodyA.Mass * bodyA.InvMass; float invIA = bodyA.Mass * bodyA.InvI; float invMassB = bodyB.Mass * bodyB.InvMass; float invIB = bodyB.Mass * bodyB.InvI; // Solve normal constraints for (int j = 0; j < c.PointCount; ++j) { PositionSolverManifold psm = new PositionSolverManifold(ref c, j); Vector2 normal = psm.Normal; Vector2 point = psm.Point; float separation = psm.Separation; Vector2 rA = point - bodyA.Sweep.c; Vector2 rB = point - bodyB.Sweep.c; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(rA, normal); float rnB = MathUtils.Cross(rB, normal); float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; #if MATH_OVERLOADS Vector2 P = impulse * normal; bodyA.Sweep.c -= invMassA * P; bodyA.Sweep.a -= invIA * MathUtils.Cross(rA, P); bodyB.Sweep.c += invMassB * P; bodyB.Sweep.a += invIB * MathUtils.Cross(rB, P); #else Vector2 P = new Vector2(impulse * normal.X, impulse * normal.Y); bodyA.Sweep.c.X -= invMassA * P.X; bodyA.Sweep.c.Y -= invMassA * P.Y; bodyA.Sweep.a -= invIA * (rA.X * P.Y - rA.Y * P.X); bodyB.Sweep.c.X += invMassB * P.X; bodyB.Sweep.c.Y += invMassB * P.Y; bodyB.Sweep.a += invIB * (rB.X * P.Y - rB.Y * P.X); #endif bodyA.SynchronizeTransform(); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't // push the separation above -Settings.b2_linearSlop. return minSeparation >= -1.5f * Settings.LinearSlop; }
private bool SolvePositionConstraints(int start, int end) { float minSeparation = 0.0f; for (int i = start; i < end; ++i) { ContactPositionConstraint pc = _positionConstraints[i]; #if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 // Find lower order item. int orderedIndexA = pc.indexA; int orderedIndexB = pc.indexB; if (orderedIndexB < orderedIndexA) { orderedIndexA = pc.indexB; orderedIndexB = pc.indexA; } // Lock bodies. for (; ;) { if (Interlocked.CompareExchange(ref _positions[orderedIndexA].Lock, 1, 0) == 0) { if (Interlocked.CompareExchange(ref _positions[orderedIndexB].Lock, 1, 0) == 0) { break; } System.Threading.Interlocked.Exchange(ref _positions[orderedIndexA].Lock, 0); } #if NET40 || NET45 Thread.Sleep(0); #endif } #endif int indexA = pc.indexA; int indexB = pc.indexB; Vector2 localCenterA = pc.localCenterA; float mA = pc.invMassA; float iA = pc.invIA; Vector2 localCenterB = pc.localCenterB; float mB = pc.invMassB; float iB = pc.invIB; int pointCount = pc.pointCount; Vector2 cA = _positions[indexA].c; float aA = _positions[indexA].a; Vector2 cB = _positions[indexB].c; float aB = _positions[indexB].a; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { Transform xfA = new Transform(Vector2.Zero, aA); Transform xfB = new Transform(Vector2.Zero, aB); xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q); xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q); Vector2 normal; Vector2 point; float separation; PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(ref rA, ref normal); float rnB = MathUtils.Cross(ref rB, ref normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } _positions[indexA].c = cA; _positions[indexA].a = aA; _positions[indexB].c = cB; _positions[indexB].a = aB; #if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 // Unlock bodies. System.Threading.Interlocked.Exchange(ref _positions[orderedIndexB].Lock, 0); System.Threading.Interlocked.Exchange(ref _positions[orderedIndexA].Lock, 0); #endif } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -3.0f * Settings.LinearSlop); }
// Sequential position solver for position constraints. public bool SolvePositionConstraintsTOI(float baumgarte, Body toiBodyA, Body toiBodyB) { float minSeparation = 0.0f; for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; Body bodyA = c.BodyA; Body bodyB = c.BodyB; float massA = 0.0f; if (bodyA == toiBodyA || bodyA == toiBodyB) { massA = bodyA.Mass; } float massB = 0.0f; if (bodyB == toiBodyA || bodyB == toiBodyB) { massB = bodyB.Mass; } float invMassA = bodyA.Mass * bodyA.InvMass; float invIA = bodyA.Mass * bodyA.InvI; float invMassB = bodyB.Mass * bodyB.InvMass; float invIB = bodyB.Mass * bodyB.InvI; // Solve normal constraints for (int j = 0; j < c.PointCount; ++j) { Vector2 normal; Vector2 point; float separation; PositionSolverManifold.Solve(ref c, j, out normal, out point, out separation); Vector2 rA; //= point - bodyA.Sweep.C); Vector2.Subtract(ref point, ref bodyA.Sweep.C, out rA); Vector2 rB; // = point - bodyB.Sweep.C; Vector2.Subtract(ref point, ref bodyB.Sweep.C, out rB); // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = rA.X * normal.Y - rA.Y * normal.X; float rnB = rB.X * normal.Y - rB.Y * normal.X; float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; bodyA.Sweep.C -= invMassA * P; bodyA.Sweep.A -= invIA * (rA.X * P.Y - rA.Y * P.X); bodyA.SynchronizeTransform(); bodyB.Sweep.C += invMassB * P; bodyB.Sweep.A += invIB * (rB.X * P.Y - rB.Y * P.X); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }