// 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; Vector2 localCenterA = pc.localCenterA; 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; } 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; } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -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 || NETSTANDARD2_0 || 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 _locks[orderedIndexA], 1, 0) == 0) { if (Interlocked.CompareExchange(ref _locks[orderedIndexB], 1, 0) == 0) { break; } System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0); } #if NET40 || NET45 || NETSTANDARD2_0 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 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 // Unlock bodies. System.Threading.Interlocked.Exchange(ref _locks[orderedIndexB], 0); System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 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); }
internal void Reset(ref TimeStep step, int count, Contact[] contacts, SolverPosition[] positions, SolverVelocity[] velocities, int[] locks, int velocityConstraintsMultithreadThreshold, int positionConstraintsMultithreadThreshold) { _count = count; _positions = positions; _velocities = velocities; _locks = locks; _contacts = contacts; _velocityConstraintsMultithreadThreshold = velocityConstraintsMultithreadThreshold; _positionConstraintsMultithreadThreshold = positionConstraintsMultithreadThreshold; // grow the array if (_velocityConstraints == null || _velocityConstraints.Length < count) { int newBufferCount = Math.Max(count, 32); newBufferCount = newBufferCount + (newBufferCount * 2 >> 4); // grow by x1.125f newBufferCount = (newBufferCount + 31) & (~31); // grow in chunks of 32. int oldBufferCount = (_velocityConstraints == null) ? 0 : _velocityConstraints.Length; Array.Resize(ref _velocityConstraints, newBufferCount); Array.Resize(ref _positionConstraints, newBufferCount); for (int i = oldBufferCount; i < newBufferCount; i++) { _velocityConstraints[i] = new ContactVelocityConstraint(); _positionConstraints[i] = new ContactPositionConstraint(); } } // Initialize position independent portions of the constraints. for (int i = 0; i < _count; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; int pointCount = manifold.PointCount; Debug.Assert(pointCount > 0); ContactVelocityConstraint vc = _velocityConstraints[i]; vc.friction = contact.Friction; vc.restitution = contact.Restitution; vc.tangentSpeed = contact.TangentSpeed; vc.indexA = bodyA.IslandIndex; vc.indexB = bodyB.IslandIndex; vc.invMassA = bodyA._invMass; vc.invMassB = bodyB._invMass; vc.invIA = bodyA._invI; vc.invIB = bodyB._invI; vc.contactIndex = i; vc.pointCount = pointCount; vc.K.SetZero(); vc.normalMass.SetZero(); ContactPositionConstraint pc = _positionConstraints[i]; pc.indexA = bodyA.IslandIndex; pc.indexB = bodyB.IslandIndex; pc.invMassA = bodyA._invMass; pc.invMassB = bodyB._invMass; pc.localCenterA = bodyA._sweep.LocalCenter; pc.localCenterB = bodyB._sweep.LocalCenter; pc.invIA = bodyA._invI; pc.invIB = bodyB._invI; pc.localNormal = manifold.LocalNormal; pc.localPoint = manifold.LocalPoint; pc.pointCount = pointCount; pc.radiusA = radiusA; pc.radiusB = radiusB; pc.type = manifold.Type; for (int j = 0; j < pointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; VelocityConstraintPoint vcp = vc.points[j]; if (step.warmStarting) { vcp.normalImpulse = step.dtRatio * cp.NormalImpulse; vcp.tangentImpulse = step.dtRatio * cp.TangentImpulse; } else { vcp.normalImpulse = 0.0f; vcp.tangentImpulse = 0.0f; } vcp.rA = Vector2.Zero; vcp.rB = Vector2.Zero; vcp.normalMass = 0.0f; vcp.tangentMass = 0.0f; vcp.velocityBias = 0.0f; pc.localPoints[j] = cp.LocalPoint; } } }
public void InitializeVelocityConstraints() { for (int i = 0; i < _count; ++i) { ContactVelocityConstraint vc = _velocityConstraints[i]; ContactPositionConstraint pc = _positionConstraints[i]; float radiusA = pc.radiusA; float radiusB = pc.radiusB; Manifold manifold = _contacts[vc.contactIndex].Manifold; int indexA = vc.indexA; int indexB = vc.indexB; float mA = vc.invMassA; float mB = vc.invMassB; float iA = vc.invIA; float iB = vc.invIB; Vector2 localCenterA = pc.localCenterA; Vector2 localCenterB = pc.localCenterB; Vector2 cA = _positions[indexA].c; float aA = _positions[indexA].a; Vector2 vA = _velocities[indexA].v; float wA = _velocities[indexA].w; Vector2 cB = _positions[indexB].c; float aB = _positions[indexB].a; Vector2 vB = _velocities[indexB].v; float wB = _velocities[indexB].w; Debug.Assert(manifold.PointCount > 0); 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; FixedArray2 <Vector2> points; WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points); vc.normal = normal; Vector2 tangent = MathUtils.Rot270(ref vc.normal); int pointCount = vc.pointCount; for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = vc.points[j]; vcp.rA = points[j] - cA; vcp.rB = points[j] - cB; float rnA = MathUtils.Cross(ref vcp.rA, ref vc.normal); float rnB = MathUtils.Cross(ref vcp.rB, ref vc.normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; float rtA = MathUtils.Cross(ref vcp.rA, ref tangent); float rtB = MathUtils.Cross(ref vcp.rB, ref tangent); float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // Setup a velocity bias for restitution. vcp.velocityBias = 0.0f; float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA)); if (vRel < -Settings.VelocityThreshold) { vcp.velocityBias = -vc.restitution * vRel; } } // If we have two points, then prepare the block solver. if (vc.pointCount == 2) { VelocityConstraintPoint vcp1 = vc.points[0]; VelocityConstraintPoint vcp2 = vc.points[1]; float rn1A = MathUtils.Cross(ref vcp1.rA, ref vc.normal); float rn1B = MathUtils.Cross(ref vcp1.rB, ref vc.normal); float rn2A = MathUtils.Cross(ref vcp2.rA, ref vc.normal); float rn2B = MathUtils.Cross(ref vcp2.rB, ref vc.normal); float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 1000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. vc.K.ex = new Vector2(k11, k12); vc.K.ey = new Vector2(k12, k22); vc.normalMass = vc.K.Inverse; } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.pointCount = 1; } } } }
public void Reset(ref TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, int velocityConstraintsMultithreadThreshold, int positionConstraintsMultithreadThreshold) { _count = count; _positions = positions; _velocities = velocities; _contacts = contacts; _velocityConstraintsMultithreadThreshold = velocityConstraintsMultithreadThreshold; _positionConstraintsMultithreadThreshold = positionConstraintsMultithreadThreshold; // grow the array if (_velocityConstraints == null || _velocityConstraints.Length < count) { _velocityConstraints = new ContactVelocityConstraint[count * 2]; _positionConstraints = new ContactPositionConstraint[count * 2]; for (int i = 0; i < _velocityConstraints.Length; i++) { _velocityConstraints[i] = new ContactVelocityConstraint(); } for (int i = 0; i < _positionConstraints.Length; i++) { _positionConstraints[i] = new ContactPositionConstraint(); } } // Initialize position independent portions of the constraints. for (int i = 0; i < _count; ++i) { Contact contact = contacts[i]; // Contact may have been destroyed during collision handling. if (contact.FixtureA == null || contact.FixtureB == null) { Debug.Assert(contact.FixtureA == null && contact.FixtureB == null); _count--; Array.Copy(contacts, i + 1, contacts, i, _count - i); i--; continue; } Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; int pointCount = manifold.PointCount; Debug.Assert(pointCount > 0); ContactVelocityConstraint vc = _velocityConstraints[i]; vc.friction = contact.Friction; vc.restitution = contact.Restitution; vc.tangentSpeed = contact.TangentSpeed; vc.indexA = bodyA.IslandIndex; vc.indexB = bodyB.IslandIndex; vc.invMassA = bodyA._invMass; vc.invMassB = bodyB._invMass; vc.invIA = bodyA._invI; vc.invIB = bodyB._invI; vc.contactIndex = i; vc.pointCount = pointCount; vc.K.SetZero(); vc.normalMass.SetZero(); ContactPositionConstraint pc = _positionConstraints[i]; pc.indexA = bodyA.IslandIndex; pc.indexB = bodyB.IslandIndex; pc.invMassA = bodyA._invMass; pc.invMassB = bodyB._invMass; pc.localCenterA = bodyA._sweep.LocalCenter; pc.localCenterB = bodyB._sweep.LocalCenter; pc.invIA = bodyA._invI; pc.invIB = bodyB._invI; pc.localNormal = manifold.LocalNormal; pc.localPoint = manifold.LocalPoint; pc.pointCount = pointCount; pc.radiusA = radiusA; pc.radiusB = radiusB; pc.type = manifold.Type; for (int j = 0; j < pointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; VelocityConstraintPoint vcp = vc.points[j]; if (step.warmStarting) { vcp.normalImpulse = step.dtRatio * cp.NormalImpulse; vcp.tangentImpulse = step.dtRatio * cp.TangentImpulse; } else { vcp.normalImpulse = 0.0f; vcp.tangentImpulse = 0.0f; } vcp.rA = Vector2.Zero; vcp.rB = Vector2.Zero; vcp.normalMass = 0.0f; vcp.tangentMass = 0.0f; vcp.velocityBias = 0.0f; pc.localPoints[j] = cp.LocalPoint; } } }