private void solveTOI(TimeStep step)
{
Island island = toiIsland;
island.init(2 * Settings.maxTOIContacts, Settings.maxTOIContacts, 0, m_contactManager.m_contactListener);
if (m_stepComplete)
{
for (Body b = m_bodyList; b != null; b = b.m_next)
{
b.m_flags &= ~Body.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
{
// Invalidate TOI
c.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (; ; )
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
{
// Is this contact disabled?
if (c.Enabled == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > Settings.maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if ((c.m_flags & Contact.TOI_FLAG) != 0)
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
Fixture fA = c.FixtureA;
Fixture fB = c.FixtureB;
// Is there a sensor?
if (fA.Sensor || fB.Sensor)
{
continue;
}
Body bA = fA.Body;
Body bB = fB.Body;
BodyType typeA = bA.m_type;
BodyType typeB = bB.m_type;
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.Bullet || typeA != BodyType.DYNAMIC;
bool collideB = bB.Bullet || typeB != BodyType.DYNAMIC;
// 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.m_sweep.alpha0;
if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0)
{
alpha0 = bB.m_sweep.alpha0;
bA.m_sweep.advance(alpha0);
}
else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0)
{
alpha0 = bA.m_sweep.alpha0;
bB.m_sweep.advance(alpha0);
}
Debug.Assert(alpha0 < 1.0f);
int indexA = c.ChildIndexA;
int indexB = c.ChildIndexB;
// Compute the time of impact in interval [0, minTOI]
TimeOfImpact.TOIInput input = toiInput;
input.proxyA.set_Renamed(fA.Shape, indexA);
input.proxyB.set_Renamed(fB.Shape, indexB);
input.sweepA.set_Renamed(bA.m_sweep);
input.sweepB.set_Renamed(bB.m_sweep);
input.tMax = 1.0f;
pool.getTimeOfImpact().timeOfImpact(toiOutput, input);
// Beta is the fraction of the remaining portion of the .
float beta = toiOutput.t;
if (toiOutput.state == TimeOfImpact.TOIOutputState.TOUCHING)
{
alpha = MathUtils.min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.m_flags |= Contact.TOI_FLAG;
}
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!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
Fixture fA2 = minContact.FixtureA;
Fixture fB2 = minContact.FixtureB;
Body bA2 = fA2.Body;
Body bB2 = fB2.Body;
backup1.set_Renamed(bA2.m_sweep);
backup2.set_Renamed(bB2.m_sweep);
bA2.advance(minAlpha);
bB2.advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.update(m_contactManager.m_contactListener);
minContact.m_flags &= ~Contact.TOI_FLAG;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.Enabled == false || minContact.Touching == false)
{
// Restore the sweeps.
minContact.Enabled = false;
bA2.m_sweep.set_Renamed(backup1);
bB2.m_sweep.set_Renamed(backup2);
bA2.synchronizeTransform();
bB2.synchronizeTransform();
continue;
}
bA2.Awake = true;
bB2.Awake = true;
// Build the island
island.clear();
island.add(bA2);
island.add(bB2);
island.add(minContact);
bA2.m_flags |= Body.e_islandFlag;
bB2.m_flags |= Body.e_islandFlag;
minContact.m_flags |= Contact.ISLAND_FLAG;
// Get contacts on bodyA and bodyB.
tempBodies[0] = bA2;
tempBodies[1] = bB2;
for (int i = 0; i < 2; ++i)
{
Body body = tempBodies[i];
if (body.m_type == BodyType.DYNAMIC)
{
for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next)
{
if (island.m_bodyCount == island.m_bodyCapacity)
{
break;
}
if (island.m_contactCount == island.m_contactCapacity)
{
break;
}
Contact contact = ce.contact;
// Has this contact already been added to the island?
if ((contact.m_flags & Contact.ISLAND_FLAG) != 0)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
Body other = ce.other;
if (other.m_type == BodyType.DYNAMIC && body.Bullet == false && other.Bullet == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
// Tentatively advance the body to the TOI.
backup1.set_Renamed(other.m_sweep);
if ((other.m_flags & Body.e_islandFlag) == 0)
{
other.advance(minAlpha);
}
// Update the contact points
contact.update(m_contactManager.m_contactListener);
// Was the contact disabled by the user?
if (contact.Enabled == false)
{
other.m_sweep.set_Renamed(backup1);
other.synchronizeTransform();
continue;
}
// Are there contact points?
if (contact.Touching == false)
{
other.m_sweep.set_Renamed(backup1);
other.synchronizeTransform();
continue;
}
// Add the contact to the island
contact.m_flags |= Contact.ISLAND_FLAG;
island.add(contact);
// Has the other body already been added to the island?
if ((other.m_flags & Body.e_islandFlag) != 0)
{
continue;
}
// Add the other body to the island.
other.m_flags |= Body.e_islandFlag;
if (other.m_type != BodyType.STATIC)
{
other.Awake = true;
}
island.add(other);
}
}
}
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(subStep, bA2.m_islandIndex, bB2.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodyCount; ++i)
{
Body body = island.m_bodies[i];
body.m_flags &= ~Body.e_islandFlag;
if (body.m_type != BodyType.DYNAMIC)
{
continue;
}
body.synchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next)
{
ce.contact.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.findNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
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);
}
private void solve(TimeStep step)
{
m_profile.solveInit = 0;
m_profile.solveVelocity = 0;
m_profile.solvePosition = 0;
// Size the island for the worst case.
island.init(m_bodyCount, m_contactManager.m_contactCount, m_jointCount, m_contactManager.m_contactListener);
// Clear all the island flags.
for (Body b = m_bodyList; b != null; b = b.m_next)
{
b.m_flags &= ~Body.e_islandFlag;
}
for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
{
c.m_flags &= ~Contact.ISLAND_FLAG;
}
for (Joint j = m_jointList; j != null; j = j.m_next)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = m_bodyCount;
if (stack.Length < stackSize)
{
stack = new Body[stackSize];
}
for (Body seed = m_bodyList; seed != null; seed = seed.m_next)
{
if ((seed.m_flags & Body.e_islandFlag) == Body.e_islandFlag)
{
continue;
}
if (seed.Awake == false || seed.Active == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.Type == BodyType.STATIC)
{
continue;
}
// Reset island and stack.
island.clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed.m_flags |= Body.e_islandFlag;
// 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.
Body b = stack[--stackCount];
Debug.Assert(b.Active == true);
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.Type == BodyType.STATIC)
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge ce = b.m_contactList; ce != null; ce = ce.next)
{
Contact contact = ce.contact;
// Has this contact already been added to an island?
if ((contact.m_flags & Contact.ISLAND_FLAG) == Contact.ISLAND_FLAG)
{
continue;
}
// Is this contact solid and touching?
if (contact.Enabled == false || contact.Touching == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
island.add(contact);
contact.m_flags |= Contact.ISLAND_FLAG;
Body other = ce.other;
// Was the other body already added to this island?
if ((other.m_flags & Body.e_islandFlag) == Body.e_islandFlag)
{
continue;
}
Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other.m_flags |= Body.e_islandFlag;
}
// Search all joints connect to this body.
for (JointEdge je = b.m_jointList; je != null; je = je.next)
{
if (je.joint.m_islandFlag == true)
{
continue;
}
Body other = je.other;
// Don't simulate joints connected to inactive bodies.
if (other.Active == false)
{
continue;
}
island.add(je.joint);
je.joint.m_islandFlag = true;
if ((other.m_flags & Body.e_islandFlag) == Body.e_islandFlag)
{
continue;
}
Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other.m_flags |= Body.e_islandFlag;
}
}
island.solve(islandProfile, step, m_gravity, m_allowSleep);
m_profile.solveInit += islandProfile.solveInit;
m_profile.solveVelocity += islandProfile.solveVelocity;
m_profile.solvePosition += islandProfile.solvePosition;
// Post solve cleanup.
for (int i = 0; i < island.m_bodyCount; ++i)
{
// Allow static bodies to participate in other islands.
Body b = island.m_bodies[i];
if (b.Type == BodyType.STATIC)
{
b.m_flags &= ~Body.e_islandFlag;
}
}
}
broadphaseTimer.reset();
// Synchronize fixtures, check for out of range bodies.
for (Body b = m_bodyList; b != null; b = b.Next)
{
// If a body was not in an island then it did not move.
if ((b.m_flags & Body.e_islandFlag) == 0)
{
continue;
}
if (b.Type == BodyType.STATIC)
{
continue;
}
// Update fixtures (for broad-phase).
b.synchronizeFixtures();
}
// Look for new contacts.
m_contactManager.findNewContacts();
m_profile.broadphase = broadphaseTimer.Milliseconds;
}
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;
}
}
}
}
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;
}
}
}
}
public virtual void solveTOI(TimeStep subStep, int toiIndexA, int toiIndexB)
{
Debug.Assert(toiIndexA < m_bodyCount);
Debug.Assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
Body b = m_bodies[i];
m_positions[i].c.set_Renamed(b.m_sweep.c);
m_positions[i].a = b.m_sweep.a;
m_velocities[i].v.set_Renamed(b.m_linearVelocity);
m_velocities[i].w = b.m_angularVelocity;
}
toiSolverDef.contacts = m_contacts;
toiSolverDef.count = m_contactCount;
toiSolverDef.step = subStep;
toiSolverDef.positions = m_positions;
toiSolverDef.velocities = m_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.
m_bodies[toiIndexA].m_sweep.c0.set_Renamed(m_positions[toiIndexA].c);
m_bodies[toiIndexA].m_sweep.a0 = m_positions[toiIndexA].a;
m_bodies[toiIndexB].m_sweep.c0.set_Renamed(m_positions[toiIndexB].c);
m_bodies[toiIndexB].m_sweep.a0 = m_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 < m_bodyCount; ++i)
{
Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
translation.set_Renamed(v).mulLocal(h);
if (Vec2.dot(translation, translation) > Settings.maxTranslationSquared)
{
float ratio = Settings.maxTranslation / translation.length();
v.mulLocal(ratio);
}
float rotation = h * w;
if (rotation * rotation > Settings.maxRotationSquared)
{
float ratio = Settings.maxRotation / MathUtils.abs(rotation);
w *= ratio;
}
// Integrate
c.x += v.x * h;
c.y += v.y * h;
a += h * w;
m_positions[i].c.set_Renamed(c);
m_positions[i].a = a;
m_velocities[i].v.set_Renamed(v);
m_velocities[i].w = w;
// Sync bodies
Body body = m_bodies[i];
body.m_sweep.c.set_Renamed(c);
body.m_sweep.a = a;
body.m_linearVelocity.set_Renamed(v);
body.m_angularVelocity = w;
body.synchronizeTransform();
}
report(toiContactSolver.m_velocityConstraints);
}
public virtual 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 < m_bodyCount; ++i)
{
Body b = m_bodies[i];
Vec2 c = b.m_sweep.c;
float a = b.m_sweep.a;
Vec2 v = b.m_linearVelocity;
float w = b.m_angularVelocity;
// Store positions for continuous collision.
b.m_sweep.c0.set_Renamed(b.m_sweep.c);
b.m_sweep.a0 = b.m_sweep.a;
if (b.m_type == BodyType.DYNAMIC)
{
// Integrate velocities.
// v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
v.x += h * (b.m_gravityScale * gravity.x + b.m_invMass * b.m_force.x);
v.y += h * (b.m_gravityScale * gravity.y + b.m_invMass * b.m_force.y);
w += h * b.m_invI * b.m_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.m_linearDamping, 0.0f, 1.0f));
w *= MathUtils.clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
}
//Debug.Assert (v.x == 0);
m_positions[i].c.set_Renamed(c);
m_positions[i].a = a;
m_velocities[i].v.set_Renamed(v);
m_velocities[i].w = w;
}
timer.reset();
// Solver data
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
solverDef.step = step;
solverDef.contacts = m_contacts;
solverDef.count = m_contactCount;
solverDef.positions = m_positions;
solverDef.velocities = m_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 < m_jointCount; ++i)
{
m_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 < m_jointCount; ++j)
{
m_joints[j].solveVelocityConstraints(solverData);
}
contactSolver.solveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.storeImpulses();
profile.solveVelocity = timer.Milliseconds;
// Integrate positions
for (int i = 0; i < m_bodyCount; ++i)
{
Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
translation.x = v.x * h;
translation.y = v.y * h;
if (Vec2.dot(translation, translation) > Settings.maxTranslationSquared)
{
float ratio = Settings.maxTranslation / translation.length();
v.x *= ratio;
v.y *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings.maxRotationSquared)
{
float ratio = Settings.maxRotation / MathUtils.abs(rotation);
w *= ratio;
}
// Integrate
c.x += h * v.x;
c.y += h * v.y;
a += h * w;
m_positions[i].a = a;
m_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 < m_jointCount; ++j)
{
bool jointOkay = m_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 < m_bodyCount; ++i)
{
Body body = m_bodies[i];
body.m_sweep.c.set_Renamed(m_positions[i].c);
body.m_sweep.a = m_positions[i].a;
body.m_linearVelocity.set_Renamed(m_velocities[i].v);
body.m_angularVelocity = m_velocities[i].w;
body.synchronizeTransform();
}
profile.solvePosition = timer.Milliseconds;
report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = Single.MaxValue;
float linTolSqr = Settings.linearSleepTolerance * Settings.linearSleepTolerance;
float angTolSqr = Settings.angularSleepTolerance * Settings.angularSleepTolerance;
for (int i = 0; i < m_bodyCount; ++i)
{
Body b = m_bodies[i];
if (b.Type == BodyType.STATIC)
{
continue;
}
if ((b.m_flags & Body.e_autoSleepFlag) == 0 || b.m_angularVelocity * b.m_angularVelocity > angTolSqr || Vec2.dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
{
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.m_sleepTime += h;
minSleepTime = MathUtils.min(minSleepTime, b.m_sleepTime);
}
}
if (minSleepTime >= Settings.timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodyCount; ++i)
{
Body b = m_bodies[i];
b.Awake = false;
}
}
}
}