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(); 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; FixedArray2 <Vector2> points; WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points); vc.Normal = 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(vcp.RA, vc.Normal); float rnB = MathUtils.Cross(vcp.RB, vc.Normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; Vector2 tangent = MathUtils.Cross(vc.Normal, 1.0f); float rtA = MathUtils.Cross(vcp.RA, tangent); float rtB = MathUtils.Cross(vcp.RB, 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, vcp.RB) - vA - MathUtils.Cross(wA, 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(vcp1.RA, vc.Normal); float rn1B = MathUtils.Cross(vcp1.RB, vc.Normal); float rn2A = MathUtils.Cross(vcp2.RA, vc.Normal); float rn2B = MathUtils.Cross(vcp2.RB, 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; } } } }
// 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(); 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 >= -1.5f * Settings.linearSlop); }
public void reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities) { _step = step; _count = count; _positions = positions; _velocities = velocities; _contacts = contacts; // 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) { var contact = contacts[i]; var fixtureA = contact.fixtureA; var fixtureB = contact.fixtureB; var shapeA = fixtureA.shape; var shapeB = fixtureB.shape; var radiusA = shapeA.radius; var radiusB = shapeB.radius; var bodyA = fixtureA.body; var bodyB = fixtureB.body; var manifold = contact.manifold; var pointCount = manifold.pointCount; Debug.Assert(pointCount > 0); var 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(); var 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) { var cp = manifold.points[j]; var vcp = vc.points[j]; if (Settings.enableWarmstarting) { 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 static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation) { Debug.Assert(pc.pointCount > 0); switch (pc.type) { case ManifoldType.Circles: { Vector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint); Vector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]); normal = pointB - pointA; normal.Normalize(); point = 0.5f * (pointA + pointB); separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB; } break; case ManifoldType.FaceA: { normal = MathUtils.Mul(xfA.q, pc.localNormal); Vector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint); Vector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]); separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; point = clipPoint; } break; case ManifoldType.FaceB: { normal = MathUtils.Mul(xfB.q, pc.localNormal); Vector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint); Vector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]); separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; point = clipPoint; // Ensure normal points from A to B normal = -normal; } break; default: normal = Vector2.Zero; point = Vector2.Zero; separation = 0; break; } }
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting) { _step = step; _count = count; _positions = positions; _velocities = velocities; _contacts = contacts; // 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]; 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 (Settings.EnableWarmstarting) { 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 Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities) { _step = step; _count = count; _positions = positions; _velocities = velocities; _contacts = contacts; // 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]; 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 (Settings.EnableWarmstarting) { vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse; vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse; } else { vcp.normalImpulse = 0.0f; vcp.tangentImpulse = 0.0f; } vcp.rA = FVector2.Zero; vcp.rB = FVector2.Zero; vcp.normalMass = 0.0f; vcp.tangentMass = 0.0f; vcp.velocityBias = 0.0f; pc.localPoints[j] = cp.LocalPoint; } } }
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); }
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; } } }