public void Solve(ref TimeStep step, ref FVector2 gravity) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body 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 (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT) if (Settings.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 (Settings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { #if (!SILVERLIGHT) if (Settings.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 (Settings.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) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.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 < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; #if (!SILVERLIGHT) if (Settings.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 (Settings.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 (Settings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body 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 (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body 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 >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
public void Solve(ref TimeStep step, ref Vector2 gravity) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; Vector2 c = b._sweep.C; float a = b._sweep.A; Vector2 v = b._linearVelocity; float w = b._angularVelocity; // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; // Added by Grimelios. int decelerationSign = b.ManuallyControlled ? Math.Sign(v.X) : 0; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. // FPE: Only apply gravity if the body wants it. if (b.IgnoreGravity) { v += h * (b._invMass * b._force); } else { 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); } // Added by Grimelios. bool accelerating = b.Accelerating; bool decelerating = b.Decelerating; // Acceleration and deceleration will never be set at the same time. However, both can be false, indicating that the player // is fully stopped. if (b.ManuallyControlled && (accelerating ^ decelerating)) { if (b.Accelerating) { Vector2 maxSpeed = b.MaximumSpeed; v.X = MathHelper.Clamp(v.X, -maxSpeed.X, maxSpeed.X); v.Y = MathHelper.Clamp(v.Y, -maxSpeed.Y, maxSpeed.Y); } else if (b.Decelerating && decelerationSign != Math.Sign(v.X)) { v.X = 0; b.Decelerating = false; } } _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; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } if (Settings.EnableDiagnostics) { _watch.Start(); } for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } if (Settings.EnableDiagnostics) { _watch.Stop(); } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } joint.SolveVelocityConstraints(ref solverData); joint.Validate(step.inv_dt); if (Settings.EnableDiagnostics) { _watch.Stop(); } } _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { Vector2 c = _positions[i].c; float a = _positions[i].a; Vector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities Vector2 translation = h * v; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.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 < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } bool jointOkay = joint.SolvePositionConstraints(ref solverData); if (Settings.EnableDiagnostics) { _watch.Stop(); } jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } if (Settings.EnableDiagnostics) { JointUpdateTime = _watch.ElapsedTicks; _watch.Reset(); } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body 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 (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += h; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body 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 < Settings.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 < Settings.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) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.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 Body body = Bodies[i]; body.Sweep.C = c; body.Sweep.A = a; body.LinearVelocity = v; body.AngularVelocity = w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); }
internal void SolveTOI(ref TimeStep subStep) { _contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio, false); // Solve position constraints. const float kTOIBaumgarte = 0.75f; for (int i = 0; i < Settings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(kTOIBaumgarte); if (contactsOkay) { break; } if (i == Settings.TOIPositionIterations - 1) { i += 0; } } // Leap of faith to new safe state. for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body.Sweep.A0 = body.Sweep.A; body.Sweep.C0 = body.Sweep.C; } // 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 < Settings.TOIVelocityIterations; ++i) { _contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. float translationx = subStep.dt * b.LinearVelocityInternal.X; float translationy = subStep.dt * b.LinearVelocityInternal.Y; float dot = translationx * translationx + translationy * translationy; if (dot > Settings.MaxTranslationSquared) { float norm = 1f / (float)Math.Sqrt(dot); float value = Settings.MaxTranslation * subStep.inv_dt; b.LinearVelocityInternal.X = value * (translationx * norm); b.LinearVelocityInternal.Y = value * (translationy * norm); } float rotation = subStep.dt * b.AngularVelocity; if (rotation * rotation > Settings.MaxRotationSquared) { if (rotation < 0.0) { b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation; } else { b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation; } } // Integrate b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X; b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y; b.Sweep.A += subStep.dt * b.AngularVelocityInternal; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } Report(_contactSolver.Constraints); }
public void Solve(ref TimeStep step, ref Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType != BodyType.Dynamic) { continue; } // Integrate velocities. // FPE 3 only - Only apply gravity if the body wants it. if (b.IgnoreGravity) { b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X); b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } else { b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X); b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y); b.AngularVelocityInternal += step.dt * 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 b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f); b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f); } // Partition contacts so that contacts with static bodies are solved last. int i1 = -1; for (int i2 = 0; i2 < ContactCount; ++i2) { Fixture fixtureA = _contacts[i2].FixtureA; Fixture fixtureB = _contacts[i2].FixtureB; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static; if (nonStatic) { ++i1; //TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162 Contact tmp = _contacts[i1]; _contacts[i1] = _contacts[i2]; _contacts[i2] = tmp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref step); } } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } joint.SolveVelocityConstraints(ref step); joint.Validate(step.inv_dt); } _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. float translationX = step.dt * b.LinearVelocityInternal.X; float translationY = step.dt * b.LinearVelocityInternal.Y; float result = translationX * translationX + translationY * translationY; if (result > Settings.MaxTranslationSquared) { float sq = (float)Math.Sqrt(result); float ratio = Settings.MaxTranslation / sq; b.LinearVelocityInternal.X *= ratio; b.LinearVelocityInternal.Y *= ratio; } float rotation = step.dt * b.AngularVelocityInternal; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / (float)Math.Abs(rotation); b.AngularVelocityInternal *= ratio; } // Store positions for continuous collision. b.Sweep.C0.X = b.Sweep.C.X; b.Sweep.C0.Y = b.Sweep.C.Y; b.Sweep.A0 = b.Sweep.A; // Integrate b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X; b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y; b.Sweep.A += step.dt * b.AngularVelocityInternal; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } bool jointOkay = joint.SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } Report(_contactSolver.Constraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TimeToSleep) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
public void Solve(ref TimeStep step, ref System.Numerics.Vector2 gravity) { float h = step.Dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { var b = Bodies[i]; var c = b._sweep.C; float a = b._sweep.A; var 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. // FPE: Only apply gravity if the body wants it. if (b.IgnoreGravity) { v += h * (b._invMass * b._force); } else { 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; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } if (Settings.EnableDiagnostics) { _watch.Start(); } for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } if (Settings.EnableDiagnostics) { _watch.Stop(); } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } joint.SolveVelocityConstraints(ref solverData); joint.Validate(step.Inv_dt); if (Settings.EnableDiagnostics) { _watch.Stop(); } } _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { System.Numerics.Vector2 c = _positions[i].C; float a = _positions[i].A; System.Numerics.Vector2 v = _velocities[i].V; float w = _velocities[i].W; // Check for large velocities System.Numerics.Vector2 translation = h * v; if (System.Numerics.Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.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 < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } bool jointOkay = joint.SolvePositionConstraints(ref solverData); if (Settings.EnableDiagnostics) { _watch.Stop(); } jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } if (Settings.EnableDiagnostics) { JointUpdateTime = _watch.ElapsedTicks; _watch.Reset(); } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body 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 (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if (!b.IsSleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || System.Numerics.Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += h; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.IsAwake = false; } } } }
/// <summary> /// Find TOI contacts and solve them. /// </summary> /// <param name="step">The step.</param> private void SolveTOI(ref TimeStep step) { this.Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, this.ContactManager); if (this._stepComplete) { for (int i = 0; i < this.BodyList.Count; i++) { this.BodyList[i].Flags &= ~BodyFlags.Island; this.BodyList[i].Sweep.Alpha0 = 0.0f; } for (int i = 0; i < this.ContactManager.ContactList.Count; i++) { Contact c = this.ContactManager.ContactList[i]; // 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; for (int i = 0; i < this.ContactManager.ContactList.Count; i++) { Contact c = this.ContactManager.ContactList[i]; // Is this contact disabled? if (c.Enabled == false) { continue; } // Prevent excessive sub-stepping. if (c.TOICount > Settings.MaxSubSteps) { continue; } float alpha; if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI) { // This contact has a valid cached TOI. alpha = c.TOI; } else { Fixture fA = c.FixtureA; Fixture fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } Body bA = fA.Body; Body bB = fB.Body; BodyType typeA = bA.BodyType; BodyType typeB = bB.BodyType; Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic); bool awakeA = bA.Awake && typeA != BodyType.Static; bool awakeB = bB.Awake && typeB != BodyType.Static; // Is at least one body awake? if (awakeA == false && awakeB == false) { continue; } bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && !bA.IgnoreCCD; bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD; // Are these two non-bullet dynamic bodies? if (collideA == false && collideB == false) { continue; } // 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] this._input.ProxyA.Set(fA.Shape, c.ChildIndexA); this._input.ProxyB.Set(fB.Shape, c.ChildIndexB); this._input.SweepA = bA.Sweep; this._input.SweepB = bB.Sweep; this._input.TMax = 1.0f; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(out output, this._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 * Settings.Epsilon < minAlpha) { // No more TOI events. Done! this._stepComplete = true; break; } // Advance the bodies to the TOI. Fixture fA1 = minContact.FixtureA; Fixture fB1 = minContact.FixtureB; Body bA1 = fA1.Body; Body bB1 = fB1.Body; Sweep backup1 = bA1.Sweep; Sweep backup2 = bB1.Sweep; bA1.Advance(minAlpha); bB1.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(this.ContactManager); minContact.Flags &= ~ContactFlags.TOI; ++minContact.TOICount; // Is the contact solid? if (minContact.Enabled == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.Enabled = false; bA1.Sweep = backup1; bB1.Sweep = backup2; bA1.SynchronizeTransform(); bB1.SynchronizeTransform(); continue; } bA1.Awake = true; bB1.Awake = true; // Build the island this.Island.Clear(); this.Island.Add(bA1); this.Island.Add(bB1); this.Island.Add(minContact); bA1.Flags |= BodyFlags.Island; bB1.Flags |= BodyFlags.Island; minContact.Flags |= ContactFlags.Island; // Get contacts on bodyA and bodyB. Body[] bodies = { bA1, bB1 }; for (int i = 0; i < 2; ++i) { Body body = bodies[i]; if (body.BodyType == BodyType.Dynamic) { // for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next) for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { Contact contact = ce.Contact; // Has this contact already been added to the island? if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island) { continue; } // Only add static, kinematic, or bullet bodies. Body 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(this.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; this.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; } this.Island.Add(other); } } } TimeStep 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; this.Island.SolveTOI(ref subStep); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < this.Island.BodyCount; ++i) { Body body = this.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. this.ContactManager.FindNewContacts(); if (this.EnableSubStepping) { this._stepComplete = false; break; } } }
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body 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; } _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities); // Solve position constraints. for (int i = 0; i < Settings.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 < Settings.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) { System.Numerics.Vector2 c = _positions[i].C; float a = _positions[i].A; System.Numerics.Vector2 v = _velocities[i].V; float w = _velocities[i].W; // Check for large velocities System.Numerics.Vector2 translation = h * v; if (System.Numerics.Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.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 Body 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 TimeStep step, Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType != BodyType.Dynamic) { continue; } // Integrate velocities. Only apply gravity if the body wants it. if (b.IgnoreGravity) { b.LinearVelocityInternal += step.dt * (b.InvMass * b.Force); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } else { b.LinearVelocityInternal += step.dt * (gravity + b.InvMass * b.Force); b.AngularVelocityInternal += step.dt * 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 b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f); b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f); } // Partition contacts so that contacts with static bodies are solved last. int i1 = -1; for (int i2 = 0; i2 < ContactCount; ++i2) { Fixture fixtureA = _contacts[i2].FixtureA; Fixture fixtureB = _contacts[i2].FixtureB; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static; if (nonStatic) { ++i1; // b2Swap(_contacts[i1], _contacts[i2]); Contact temp = _contacts[i1]; _contacts[i1] = _contacts[i2]; _contacts[i2] = temp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, ContactCount, step.dtRatio); _contactSolver.WarmStart(); for (int i = 0; i < JointCount; ++i) { _joints[i].InitVelocityConstraints(ref step); } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { _joints[j].SolveVelocityConstraints(ref step); } _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b.LinearVelocityInternal; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); b.LinearVelocityInternal *= ratio; } float rotation = step.dt * b.AngularVelocityInternal; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); b.AngularVelocityInternal *= ratio; } // Store positions for continuous collision. b.Sweep.c0 = b.Sweep.c; b.Sweep.a0 = b.Sweep.a; // Integrate b.Sweep.c += step.dt * b.LinearVelocityInternal; b.Sweep.a += step.dt * b.AngularVelocityInternal; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } if (_contactManager.PostSolve != null) { Report(_contactSolver.Constraints); } if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; const float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; const float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > angTolSqr || Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > linTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TimeToSleep) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
public void Solve(ref TimeStep step, ref Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType != BodyType.Dynamic) { continue; } // Integrate velocities. // FPE 3 only - Only apply gravity if the body wants it. if (b.IgnoreGravity) { b.LinearVelocityInternal += step.dt * (b.InvMass * b.Force); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } else { b.LinearVelocityInternal += step.dt * (gravity + b.InvMass * b.Force); b.AngularVelocityInternal += step.dt * 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 b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f); b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f); } // Partition contacts so that contacts with static bodies are solved last. int i1 = -1; for (int i2 = 0; i2 < ContactCount; ++i2) { Fixture fixtureA = _contacts[i2].FixtureA; Fixture fixtureB = _contacts[i2].FixtureB; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static; if (nonStatic) { ++i1; //TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162 //if (i1 != i2) MathUtils.Swap(ref _contacts[i1], ref _contacts[i2]); //Contact temp = _contacts[i1]; //_contacts[i1] = _contacts[i2]; //_contacts[i2] = temp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, ContactCount, step.dtRatio); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { _joints[i].InitVelocityConstraints(ref step); } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { _joints[j].SolveVelocityConstraints(ref step); } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b.LinearVelocityInternal; float result; Vector2.Dot(ref translation, ref translation, out result); if (result > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); b.LinearVelocityInternal *= ratio; } float rotation = step.dt * b.AngularVelocityInternal; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); b.AngularVelocityInternal *= ratio; } // Store positions for continuous collision. b.Sweep.C0 = b.Sweep.C; b.Sweep.A0 = b.Sweep.A; // Integrate b.Sweep.C += step.dt * b.LinearVelocityInternal; b.Sweep.A += step.dt * b.AngularVelocityInternal; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool jointsOkay = true; #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif if (_contactManager.PostSolve != null) { Report(_contactSolver.Constraints); } if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; const float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; const float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > angTolSqr || Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > linTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TimeToSleep) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
internal void SolveTOI(ref TimeStep subStep, Body bodyA, Body bodyB) { _contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio); // Solve position constraints. const float kTOIBaumgarte = 0.75f; for (int i = 0; i < Settings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraintsTOI(kTOIBaumgarte, bodyA, bodyB); if (contactsOkay) { break; } if (i == Settings.TOIPositionIterations - 1) { i += 0; } } /* #if 0 * // Is the new position really safe? * for (int32 i = 0; i < m_contactCount; ++i) * { * b2Contact* c = m_contacts[i]; * b2Fixture* fA = c->GetFixtureA(); * b2Fixture* fB = c->GetFixtureB(); * * b2Body* bA = fA->GetBody(); * b2Body* bB = fB->GetBody(); * * int32 indexA = c->GetChildIndexA(); * int32 indexB = c->GetChildIndexB(); * * b2DistanceInput input; * input.proxyA.Set(fA->GetShape(), indexA); * input.proxyB.Set(fB->GetShape(), indexB); * input.transformA = bA->GetTransform(); * input.transformB = bB->GetTransform(); * input.useRadii = false; * * b2DistanceOutput output; * b2SimplexCache cache; * cache.count = 0; * b2Distance(&output, &cache, &input); * * if (output.distance == 0 || cache.count == 3) * { * cache.count += 0; * } * } #endif */ // Leap of faith to new safe state. for (int i = 0; i < BodyCount; ++i) { Bodies[i].Sweep.A0 = Bodies[i].Sweep.A; Bodies[i].Sweep.C0 = Bodies[i].Sweep.C; } // 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 < Settings.TOIVelocityIterations; ++i) { _contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = subStep.dt * b.LinearVelocity; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { translation.Normalize(); b.LinearVelocity = (Settings.MaxTranslation * subStep.inv_dt) * translation; } float rotation = subStep.dt * b.AngularVelocity; if (rotation * rotation > Settings.MaxRotationSquared) { if (rotation < 0.0) { b.AngularVelocity = -subStep.inv_dt * Settings.MaxRotation; } else { b.AngularVelocity = subStep.inv_dt * Settings.MaxRotation; } } // Integrate b.Sweep.C += subStep.dt * b.LinearVelocity; b.Sweep.A += subStep.dt * b.AngularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } if (_contactManager.PostSolve != null) { Report(_contactSolver.Constraints); } }
internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _body1; Body b2 = _body2; Vector2 s1 = _ground.GetXForm().Position + _groundAnchor1; Vector2 s2 = _ground.GetXForm().Position + _groundAnchor2; float linearError = 0.0f; if (_state == LimitState.AtUpperLimit) { Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vector2 p1 = b1._sweep.C + r1; Vector2 p2 = b2._sweep.C + r2; // Get the pulley axes. _u1 = p1 - s1; _u2 = p2 - s2; float length1 = _u1.Length(); float length2 = _u2.Length(); if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.Zero; } if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.Zero; } float C = _constant - length1 - _ratio * length2; linearError = CommonMath.Max(linearError, -C); C = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_pulleyMass * C; Vector2 P1 = -impulse * _u1; Vector2 P2 = -_ratio * impulse * _u2; b1._sweep.C += b1._invMass * P1; b1._sweep.A += b1._invI * CommonMath.Cross(ref r1, ref P1); b2._sweep.C += b2._invMass * P2; b2._sweep.A += b2._invI * CommonMath.Cross(ref r2, ref P2); b1.SynchronizeTransform(); b2.SynchronizeTransform(); } if (_limitState1 == LimitState.AtUpperLimit) { Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vector2 p1 = b1._sweep.C + r1; _u1 = p1 - s1; float length1 = _u1.Length(); if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.Zero; } float C = _maxLength1 - length1; linearError = CommonMath.Max(linearError, -C); C = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass1 * C; Vector2 P1 = -impulse * _u1; b1._sweep.C += b1._invMass * P1; b1._sweep.A += b1._invI * CommonMath.Cross(ref r1, ref P1); b1.SynchronizeTransform(); } if (_limitState2 == LimitState.AtUpperLimit) { Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vector2 p2 = b2._sweep.C + r2; _u2 = p2 - s2; float length2 = _u2.Length(); if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.Zero; } float C = _maxLength2 - length2; linearError = CommonMath.Max(linearError, -C); C = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass2 * C; Vector2 P2 = -impulse * _u2; b2._sweep.C += b2._invMass * P2; b2._sweep.A += b2._invI * CommonMath.Cross(ref r2, ref P2); b2.SynchronizeTransform(); } return(linearError < Settings.LinearSlop); }