public void SolveToi(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.Set(b.Sweep.C); Positions[i].A = b.Sweep.A; Velocities[i].V.Set(b.LinearVelocity); Velocities[i].W = b.AngularVelocity; } toiSolverDef.Contacts = Contacts; toiSolverDef.Count = ContactCount; toiSolverDef.Step = subStep; toiSolverDef.Positions = Positions; toiSolverDef.Velocities = Velocities; toiContactSolver.Init(toiSolverDef); // Solve position constraints. for (int i = 0; i < subStep.PositionIterations; ++i) { bool contactsOkay = toiContactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // #if 0 // // Is the new position really safe? // for (int i = 0; i < m_contactCount; ++i) // { // Contact* c = m_contacts[i]; // Fixture* fA = c.GetFixtureA(); // Fixture* fB = c.GetFixtureB(); // // Body bA = fA.GetBody(); // Body bB = fB.GetBody(); // // int indexA = c.GetChildIndexA(); // int indexB = c.GetChildIndexB(); // // DistanceInput input; // input.proxyA.Set(fA.GetShape(), indexA); // input.proxyB.Set(fB.GetShape(), indexB); // input.transformA = bA.GetTransform(); // input.transformB = bB.GetTransform(); // input.useRadii = false; // // DistanceOutput output; // SimplexCache cache; // cache.count = 0; // Distance(&output, &cache, &input); // // if (output.distance == 0 || cache.count == 3) // { // cache.count += 0; // } // } // #endif // Leap of faith to new safe state. Bodies[toiIndexA].Sweep.C0.Set(Positions[toiIndexA].C); Bodies[toiIndexA].Sweep.A0 = Positions[toiIndexA].A; Bodies[toiIndexB].Sweep.C0.Set(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. toiContactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < subStep.VelocityIterations; ++i) { toiContactSolver.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) { Vec2 c = Positions[i].C; float a = Positions[i].A; Vec2 v = Velocities[i].V; float w = Velocities[i].W; // Check for large velocities translation.Set(v).MulLocal(h); if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED) { float ratio = Settings.MAX_TRANSLATION / translation.Length(); v.MulLocal(ratio); } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation); w *= ratio; } // Integrate c.X += v.X * h; c.Y += v.Y * h; a += h * w; Positions[i].C.Set(c); Positions[i].A = a; Velocities[i].V.Set(v); Velocities[i].W = w; // Sync bodies Body body = Bodies[i]; body.Sweep.C.Set(c); body.Sweep.A = a; body.LinearVelocity.Set(v); body.AngularVelocity = w; body.SynchronizeTransform(); } Report(toiContactSolver.VelocityConstraints); }
public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep) { // Console.WriteLine("Solving Island"); float h = step.Dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; Vec2 c = b.Sweep.C; float a = b.Sweep.A; Vec2 v = b.LinearVelocity; float w = b.AngularVelocity; // Store positions for continuous collision. b.Sweep.C0.Set(b.Sweep.C); b.Sweep.A0 = b.Sweep.A; if (b.Type == BodyType.Dynamic) { // Integrate velocities. // v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force); v.X += h * (b.GravityScale * gravity.X + b.InvMass * b.Force.X); v.Y += h * (b.GravityScale * gravity.Y + b.InvMass * b.Force.Y); 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.MulLocal(MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f)); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } //Debug.Assert (v.x == 0); Positions[i].C.Set(c); Positions[i].A = a; Velocities[i].V.Set(v); Velocities[i].W = w; } timer.Reset(); // Solver data solverData.Step = step; solverData.Positions = Positions; solverData.Velocities = Velocities; // Initialize velocity constraints. solverDef.Step = step; solverDef.Contacts = Contacts; solverDef.Count = ContactCount; solverDef.Positions = Positions; solverDef.Velocities = Velocities; contactSolver.Init(solverDef); //Console.WriteLine("island init vel"); contactSolver.InitializeVelocityConstraints(); if (step.WarmStarting) { //Console.WriteLine("island warm start"); contactSolver.WarmStart(); } for (int i = 0; i < JointCount; ++i) { Joints[i].InitVelocityConstraints(solverData); } profile.SolveInit = timer.Milliseconds; // Solve velocity constraints timer.Reset(); //Console.WriteLine("island solving velocities"); for (int i = 0; i < step.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joints[j].SolveVelocityConstraints(solverData); } contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting contactSolver.StoreImpulses(); profile.SolveVelocity = timer.Milliseconds; // Integrate positions for (int i = 0; i < BodyCount; ++i) { Vec2 c = Positions[i].C; float a = Positions[i].A; Vec2 v = Velocities[i].V; float w = Velocities[i].W; // Check for large velocities translation.X = v.X * h; translation.Y = v.Y * h; if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED) { float ratio = Settings.MAX_TRANSLATION / translation.Length(); v.X *= ratio; v.Y *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation); w *= ratio; } // Integrate c.X += h * v.X; c.Y += h * v.Y; a += h * w; Positions[i].A = a; Velocities[i].W = w; } // Solve position constraints timer.Reset(); bool positionSolved = false; for (int i = 0; i < step.PositionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { bool jointOkay = Joints[j].SolvePositionConstraints(solverData); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body.Sweep.C.Set(Positions[i].C); body.Sweep.A = Positions[i].A; body.LinearVelocity.Set(Velocities[i].V); body.AngularVelocity = Velocities[i].W; body.SynchronizeTransform(); } profile.SolvePosition = timer.Milliseconds; Report(contactSolver.VelocityConstraints); if (allowSleep) { float minSleepTime = Single.MaxValue; const float linTolSqr = Settings.LINEAR_SLEEP_TOLERANCE * Settings.LINEAR_SLEEP_TOLERANCE; float angTolSqr = Settings.ANGULAR_SLEEP_TOLERANCE * Settings.ANGULAR_SLEEP_TOLERANCE; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.Type == BodyType.Static) { continue; } if ((b.Flags & Body.TypeFlags.AutoSleep) == 0 || b.AngularVelocity * b.AngularVelocity > angTolSqr || Vec2.Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = MathUtils.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TIME_TO_SLEEP && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }