public void Reset(FSTimeStep 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]; FSFixture fixtureA = contact.FixtureA; FSFixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; FSBody bodyA = fixtureA.Body; FSBody 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 (FSSettings.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 void Solve(ref FSTimeStep step) { // Size the island for the worst case. Island.Reset(BodyList.Count, ContactManager.ContactList.Count, JointList.Count, ContactManager); // Clear all the island flags. #if USE_ISLAND_SET Debug.Assert(IslandSet.Count == 0); #else foreach (FSBody b in BodyList) { b.Flags &= ~BodyFlags.Island; } #endif #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { c.Flags &= ~ContactFlags.Island; } #else for (int i = 0; i < ContactManager.ContactList.Count; i++) { Contact c = ContactManager.ContactList[i]; c.Flags &= ~ContactFlags.Island; } #endif foreach (FarseerJoint j in JointList) { j.IslandFlag = false; } // Build and simulate all awake islands. int stackSize = BodyList.Count; if (stackSize > _stack.Length) _stack = new FSBody[Math.Max(_stack.Length * 2, stackSize)]; #if USE_AWAKE_BODY_SET #if (!SILVERLIGHT) // If AwakeBodyList is empty, the Island code will not have a chance // to update the diagnostics timer so reset the timer here. Island.JointUpdateTime = 0; #endif Debug.Assert(AwakeBodyList.Count == 0); AwakeBodyList.AddRange(AwakeBodySet); foreach (var seed in AwakeBodyList) { #else for (int index = BodyList.Count - 1; index >= 0; index--) { FSBody seed = BodyList[index]; #endif if ((seed.Flags & (BodyFlags.Island)) != BodyFlags.None) { continue; } if (seed.Awake == false || seed.Enabled == false) { continue; } // The seed can be dynamic or kinematic. if (seed.BodyType == BodyType.Static) { continue; } // Reset island and stack. Island.Clear(); int stackCount = 0; _stack[stackCount++] = seed; SetIsland(seed); // Perform a depth first search (DFS) on the constraint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. FSBody b = _stack[--stackCount]; Debug.Assert(b.Enabled); Island.Add(b); // Make sure the body is awake. b.Awake = true; // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.BodyType == BodyType.Static) { continue; } // Search all contacts connected to this body. for (ContactEdge ce = b.ContactList; ce != null; ce = ce.Next) { Contact contact = ce.Contact; // Has this contact already been added to an island? if ((contact.Flags & ContactFlags.Island) != ContactFlags.None) { continue; } // Is this contact solid and touching? if (!ce.Contact.Enabled || !ce.Contact.IsTouching()) { continue; } // Skip sensors. bool sensorA = contact.FixtureA.IsSensor; bool sensorB = contact.FixtureB.IsSensor; if (sensorA || sensorB) { continue; } Island.Add(contact); contact.Flags |= ContactFlags.Island; FSBody other = ce.Other; // Was the other body already added to this island? if ((other.Flags & BodyFlags.Island) != BodyFlags.None) { continue; } Debug.Assert(stackCount < stackSize); _stack[stackCount++] = other; SetIsland(other); //other.Flags |= BodyFlags.Island; } // Search all joints connect to this body. for (JointEdge je = b.JointList; je != null; je = je.Next) { if (je.Joint.IslandFlag) { continue; } FSBody other = je.Other; // WIP David //Enter here when it's a non-fixed joint. Non-fixed joints have a other body. if (other != null) { // Don't simulate joints connected to inactive bodies. if (other.Enabled == false) { continue; } Island.Add(je.Joint); je.Joint.IslandFlag = true; if ((other.Flags & BodyFlags.Island) != BodyFlags.None) { continue; } Debug.Assert(stackCount < stackSize); _stack[stackCount++] = other; // other.Flags |= BodyFlags.Island; SetIsland(other); } else { Island.Add(je.Joint); je.Joint.IslandFlag = true; } } } Island.Solve(ref step, ref Gravity); // Post solve cleanup. for (int i = 0; i < Island.BodyCount; ++i) { // Allow static bodies to participate in other islands. FSBody b = Island.Bodies[i]; if (b.BodyType == BodyType.Static) { b.Flags &= ~BodyFlags.Island; } } } // Synchronize fixtures, check for out of range bodies. #if USE_ISLAND_SET foreach (var b in IslandSet) #else foreach (FSBody b in BodyList) #endif { // If a body was not in an island then it did not move. if ((b.Flags & BodyFlags.Island) != BodyFlags.Island) { continue; } #if USE_ISLAND_SET Debug.Assert(b.BodyType != BodyType.Static); #else if (b.BodyType == BodyType.Static) { continue; } #endif // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } #if OPTIMIZE_TOI foreach (var b in IslandSet) { if (!TOISet.Contains(b)) { TOISet.Add(b); } } #endif #if USE_ISLAND_SET IslandSet.Clear(); #endif // Look for new contacts. ContactManager.FindNewContacts(); #if USE_AWAKE_BODY_SET AwakeBodyList.Clear(); #endif }
/// <summary> /// Find TOI contacts and solve them. /// </summary> /// <param name="step">The step.</param> private void SolveTOI(ref FSTimeStep step) { Island.Reset(2 * FSSettings.MaxTOIContacts, FSSettings.MaxTOIContacts, 0, ContactManager); #if OPTIMIZE_TOI bool wasStepComplete = _stepComplete; #endif if (_stepComplete) { #if OPTIMIZE_TOI foreach (var b in TOISet) { b.Flags &= ~BodyFlags.Island; b.Sweep.Alpha0 = 0.0f; } #else for (int i = 0; i < BodyList.Count; i++) { BodyList[i].Flags &= ~BodyFlags.Island; BodyList[i].Sweep.Alpha0 = 0.0f; } #endif #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { #else for (int i = 0; i < ContactManager.ContactList.Count; i++) { Contact c = ContactManager.ContactList[i]; #endif // Invalidate TOI c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); c.TOICount = 0; c.TOI = 1.0f; } } // Find TOI events and solve them. for (; ; ) { // Find the first TOI. Contact minContact = null; float minAlpha = 1.0f; #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { #else for (int i = 0; i < ContactManager.ContactList.Count; i++) { Contact c = ContactManager.ContactList[i]; #endif // Is this contact disabled? if (c.Enabled == false) { continue; } // Prevent excessive sub-stepping. if (c.TOICount > FSSettings.MaxSubSteps) { continue; } float alpha; if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI) { // This contact has a valid cached TOI. alpha = c.TOI; } else { FSFixture fA = c.FixtureA; FSFixture fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } FSBody bA = fA.Body; FSBody bB = fB.Body; BodyType typeA = bA.BodyType; BodyType typeB = bB.BodyType; Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic); bool activeA = bA.Awake && typeA != BodyType.Static; bool activeB = bB.Awake && typeB != BodyType.Static; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && ((fA.IgnoreCCDWith & fB.CollisionCategories) == 0); bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && ((fB.IgnoreCCDWith & fA.CollisionCategories) == 0); // Are these two non-bullet dynamic bodies? if (collideA == false && collideB == false) { continue; } #if OPTIMIZE_TOI if (_stepComplete) { if (!TOISet.Contains(bA)) { TOISet.Add(bA); bA.Flags &= ~BodyFlags.Island; bA.Sweep.Alpha0 = 0.0f; } if (!TOISet.Contains(bB)) { TOISet.Add(bB); bB.Flags &= ~BodyFlags.Island; bB.Sweep.Alpha0 = 0.0f; } } #endif // Compute the TOI for this contact. // Put the sweeps onto the same time interval. float alpha0 = bA.Sweep.Alpha0; if (bA.Sweep.Alpha0 < bB.Sweep.Alpha0) { alpha0 = bB.Sweep.Alpha0; bA.Sweep.Advance(alpha0); } else if (bB.Sweep.Alpha0 < bA.Sweep.Alpha0) { alpha0 = bA.Sweep.Alpha0; bB.Sweep.Advance(alpha0); } Debug.Assert(alpha0 < 1.0f); // Compute the time of impact in interval [0, minTOI] _input.ProxyA.Set(fA.Shape, c.ChildIndexA); _input.ProxyB.Set(fB.Shape, c.ChildIndexB); _input.SweepA = bA.Sweep; _input.SweepB = bB.Sweep; _input.TMax = 1.0f; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(out output, _input); // Beta is the fraction of the remaining portion of the . float beta = output.T; if (output.State == TOIOutputState.Touching) { alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.TOI = alpha; c.Flags |= ContactFlags.TOI; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * FSSettings.Epsilon < minAlpha) { // No more TOI events. Done! _stepComplete = true; break; } // Advance the bodies to the TOI. FSFixture fA1 = minContact.FixtureA; FSFixture fB1 = minContact.FixtureB; FSBody bA0 = fA1.Body; FSBody bB0 = fB1.Body; Sweep backup1 = bA0.Sweep; Sweep backup2 = bB0.Sweep; bA0.Advance(minAlpha); bB0.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(ContactManager); minContact.Flags &= ~ContactFlags.TOI; ++minContact.TOICount; // Is the contact solid? if (minContact.Enabled == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.Enabled = false; bA0.Sweep = backup1; bB0.Sweep = backup2; bA0.SynchronizeTransform(); bB0.SynchronizeTransform(); continue; } bA0.Awake = true; bB0.Awake = true; // Build the island Island.Clear(); Island.Add(bA0); Island.Add(bB0); Island.Add(minContact); bA0.Flags |= BodyFlags.Island; bB0.Flags |= BodyFlags.Island; minContact.Flags |= ContactFlags.Island; // Get contacts on bodyA and bodyB. FSBody[] bodies = { bA0, bB0 }; for (int i = 0; i < 2; ++i) { FSBody body = bodies[i]; if (body.BodyType == BodyType.Dynamic) { for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { Contact contact = ce.Contact; if (Island.BodyCount == Island.BodyCapacity) { break; } if (Island.ContactCount == Island.ContactCapacity) { break; } // Has this contact already been added to the island? if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island) { continue; } // Only add static, kinematic, or bullet bodies. FSBody other = ce.Other; if (other.BodyType == BodyType.Dynamic && body.IsBullet == false && other.IsBullet == false) { continue; } // Skip sensors. if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor) { continue; } // Tentatively advance the body to the TOI. Sweep backup = other.Sweep; if ((other.Flags & BodyFlags.Island) == 0) { other.Advance(minAlpha); } // Update the contact points contact.Update(ContactManager); // Was the contact disabled by the user? if (contact.Enabled == false) { other.Sweep = backup; other.SynchronizeTransform(); continue; } // Are there contact points? if (contact.IsTouching() == false) { other.Sweep = backup; other.SynchronizeTransform(); continue; } // Add the contact to the island contact.Flags |= ContactFlags.Island; Island.Add(contact); // Has the other body already been added to the island? if ((other.Flags & BodyFlags.Island) == BodyFlags.Island) { continue; } // Add the other body to the island. other.Flags |= BodyFlags.Island; if (other.BodyType != BodyType.Static) { other.Awake = true; } #if OPTIMIZE_TOI if (_stepComplete) { if (!TOISet.Contains(other)) { TOISet.Add(other); other.Sweep.Alpha0 = 0.0f; } } #endif Island.Add(other); } } } FSTimeStep subStep; subStep.dt = (1.0f - minAlpha) * step.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 1.0f; //subStep.positionIterations = 20; //subStep.velocityIterations = step.velocityIterations; //subStep.warmStarting = false; Island.SolveTOI(ref subStep, bA0.IslandIndex, bB0.IslandIndex); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < Island.BodyCount; ++i) { FSBody body = Island.Bodies[i]; body.Flags &= ~BodyFlags.Island; if (body.BodyType != BodyType.Dynamic) { continue; } body.SynchronizeFixtures(); // Invalidate all contact TOIs on this displaced body. for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. ContactManager.FindNewContacts(); if (EnableSubStepping) { _stepComplete = false; break; } } #if OPTIMIZE_TOI if (wasStepComplete) { TOISet.Clear(); } #endif }
internal void SolveTOI(ref FSTimeStep subStep, int toiIndexA, int toiIndexB) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; _positions[i].c = b.Sweep.C; _positions[i].a = b.Sweep.A; _velocities[i].v = b.LinearVelocity; _velocities[i].w = b.AngularVelocity; } //b2ContactSolverDef contactSolverDef; //contactSolverDef.contacts = _contacts; //contactSolverDef.count = _contactCount; //contactSolverDef.allocator = _allocator; //contactSolverDef.step = subStep; //contactSolverDef.positions = _positions; //contactSolverDef.velocities = _velocities; //b2ContactSolver contactSolver(&contactSolverDef); _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities); // Solve position constraints. for (int i = 0; i < FSSettings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // Leap of faith to new safe state. Bodies[toiIndexA].Sweep.C0 = _positions[toiIndexA].c; Bodies[toiIndexA].Sweep.A0 = _positions[toiIndexA].a; Bodies[toiIndexB].Sweep.C0 = _positions[toiIndexB].c; Bodies[toiIndexB].Sweep.A0 = _positions[toiIndexB].a; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. _contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < FSSettings.TOIVelocityIterations; ++i) { _contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. float h = subStep.dt; // Integrate positions. for (int i = 0; i < BodyCount; ++i) { FVector2 c = _positions[i].c; float a = _positions[i].a; FVector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities FVector2 translation = h * v; if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared) { float ratio = FSSettings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > FSSettings.MaxRotationSquared) { float ratio = FSSettings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; // Sync bodies FSBody body = Bodies[i]; body.Sweep.C = c; body.Sweep.A = a; body.LinearVelocity = v; body.AngularVelocity = w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); }
public void Solve(ref FSTimeStep step, ref FVector2 gravity) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; FVector2 c = b.Sweep.C; float a = b.Sweep.A; FVector2 v = b.LinearVelocity; float w = b.AngularVelocity; // Store positions for continuous collision. b.Sweep.C0 = b.Sweep.C; b.Sweep.A0 = b.Sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. v += h * (b.GravityScale * gravity + b.InvMass * b.Force); w += h * b.InvI * b.Torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solver data SolverData solverData = new SolverData(); solverData.step = step; solverData.positions = _positions; solverData.velocities = _velocities; // Initialize velocity constraints. //b2ContactSolverDef contactSolverDef; //contactSolverDef.step = step; //contactSolverDef.contacts = m_contacts; //contactSolverDef.count = m_contactCount; //contactSolverDef.positions = m_positions; //contactSolverDef.velocities = m_velocities; //contactSolverDef.allocator = m_allocator; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (FSSettings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) _joints[i].InitVelocityConstraints(ref solverData); } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif // Solve velocity constraints. for (int i = 0; i < FSSettings.VelocityIterations; ++i) { #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) _watch.Start(); #endif for (int j = 0; j < JointCount; ++j) { FarseerJoint joint = _joints[j]; if (!joint.Enabled) continue; joint.SolveVelocityConstraints(ref solverData); //TODO: Move up before solve? joint.Validate(step.inv_dt); } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { FVector2 c = _positions[i].c; float a = _positions[i].a; FVector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities FVector2 translation = h * v; if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared) { float ratio = FSSettings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > FSSettings.MaxRotationSquared) { float ratio = FSSettings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solve position constraints bool positionSolved = false; for (int i = 0; i < FSSettings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) _watch.Start(); #endif for (int j = 0; j < JointCount; ++j) { FarseerJoint joint = _joints[j]; if (!joint.Enabled) continue; bool jointOkay = joint.SolvePositionConstraints(ref solverData); jointsOkay = jointsOkay && jointOkay; } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { FSBody body = Bodies[i]; body.Sweep.C = _positions[i].c; body.Sweep.A = _positions[i].a; body.LinearVelocity = _velocities[i].v; body.AngularVelocity = _velocities[i].w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); if (FSSettings.AllowSleep) { float minSleepTime = FSSettings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || FVector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= FSSettings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; b.Awake = false; } } } }