public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep)
{
Timer timer = new Timer();
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodies.Count(); 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 = b.m_sweep.c;
b.m_sweep.a0 = b.m_sweep.a;
if (b.m_type == BodyType._dynamicBody)
{
// Integrate velocities.
v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
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 *= Utilities.Clamp(1.0f - h * b.m_linearDamping, 0.0f, 1.0f);
w *= Utilities.Clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
}
Position pos = new Position();
pos.c = c;
pos.a = a;
m_positions.Add(pos);
Velocity vel = new Velocity();
vel.v = v;
vel.w = w;
m_velocities.Add(vel);
}
timer.Reset();
// Solver data
SolverData solverData;
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
ContactSolver contactSolver = new ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_joints.Count(); ++i)
{
m_joints[i].InitVelocityConstraints(solverData);
}
profile.solveInit = timer.GetMilliseconds();
// Solve velocity constraints
timer.Reset();
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < m_joints.Count(); ++j)
{
m_joints[j].SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile.solveVelocity = timer.GetMilliseconds();
// Integrate positions
for (int i = 0; i < m_bodies.Count(); ++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
Vec2 translation = h * v;
if (Utilities.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;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
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_joints.Count; ++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_bodies.Count(); ++i)
{
Body body = m_bodies[i];
body.m_sweep.c = m_positions[i].c;
body.m_sweep.a = m_positions[i].a;
body.m_linearVelocity = m_velocities[i].v;
body.m_angularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
profile.solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = Single.MaxValue;
const float linTolSqr = Settings._linearSleepTolerance * Settings._linearSleepTolerance;
const float angTolSqr = Settings._angularSleepTolerance * Settings._angularSleepTolerance;
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0 ||
b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
Utilities.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
{
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.m_sleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.m_sleepTime);
}
}
if (minSleepTime >= Settings._timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
public Tiles()
{
m_fixtureCount = 0;
Timer timer = new Timer();
{
float a = 0.5f;
BodyDef bd = new BodyDef();
bd.Position.Y = -a;
Body ground = m_world.CreateBody(bd);
#if true
int N = 200;
int M = 10;
Vec2 position;
position.Y = 0.0f;
for (int j = 0; j < M; ++j)
{
position.X = -N * a;
for (int i = 0; i < N; ++i)
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(a, a, position, 0.0f);
shape.Density = 0;
ground.CreateFixture(shape);
++m_fixtureCount;
position.X += 2.0f * a;
}
position.Y -= 2.0f * a;
}
#else
int N = 200;
int M = 10;
Vec2 position;
position.X = -N * a;
for (int i = 0; i < N; ++i)
{
position.Y = 0.0f;
for (int j = 0; j < M; ++j)
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(a, a, position, 0.0f);
ground.CreateFixture(shape, 0.0f);
position.Y -= 2.0f * a;
}
position.X += 2.0f * a;
}
#endif
}
{
float a = 0.5f;
PolygonShape shape = new PolygonShape();
shape.SetAsBox(a, a);
Vec2 x = new Vec2(-7.0f, 0.75f);
Vec2 y;
Vec2 deltaX = new Vec2(0.5625f, 1.25f);
Vec2 deltaY = new Vec2(1.125f, 0.0f);
for (int i = 0; i < e_count; ++i)
{
y = x;
for (int j = i; j < e_count; ++j)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position = y;
//if (i == 0 && j == 0)
//{
// bd.allowSleep = false;
//}
//else
//{
// bd.allowSleep = true;
//}
Body body = m_world.CreateBody(bd);
shape.Density = 5;
body.CreateFixture(shape);
++m_fixtureCount;
y += deltaY;
}
x += deltaX;
}
}
m_createTime = timer.GetMilliseconds();
}
private void Solve(TimeStep step){
m_profile.solveInit = 0.0f;
m_profile.solveVelocity = 0.0f;
m_profile.solvePosition = 0.0f;
// Size the island for the worst case.
Island island = new Island(m_contactManager.m_contactListener);
// Clear all the island flags.
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
foreach (Contact c in m_contactManager.m_contactList)
{
c.m_flags &= ~ContactFlags.e_islandFlag;
}
foreach (Joint j in m_jointList)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
List<Body> stack = new List<Body>(m_bodyList.Count());
foreach (Body seed in m_bodyList)
{
if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack.Add(seed); stackCount++;
seed.m_flags |= Body.BodyFlags.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];
Utilities.Assert(b.IsActive() == true);
island.Add(b);
// Make sure the body is awake.
b.SetAwake(true);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Search all contacts connected to this body.
foreach (ContactEdge ce in b.m_contactList)
{
Contact contact = ce.contact;
// Has this contact already been added to an island?
if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag))
{
continue;
}
// Is this contact solid and touching?
if (contact.IsEnabled() == false ||
contact.IsTouching() == 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 |= ContactFlags.e_islandFlag;
Body other = ce.other;
// Was the other body already added to this island?
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
Utilities.Assert(stackCount < m_bodyList.Count());
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
// Search all joints connect to this body.
foreach (JointEdge je in b.m_jointList){
if (je.joint.m_islandFlag == true) {
continue;
}
Body other = je.other;
// Don't simulate joints connected to inactive bodies.
if (other.IsActive() == false) {
continue;
}
island.Add(je.joint);
je.joint.m_islandFlag = true;
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) {
continue;
}
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
}
Profile profile = new Profile();
island.Solve(profile, step, m_gravity, m_allowSleep);
m_profile.solveInit += profile.solveInit;
m_profile.solveVelocity += profile.solveVelocity;
m_profile.solvePosition += profile.solvePosition;
// Post solve cleanup.
for (int i = 0; i < island.m_bodies.Count(); ++i) {
// Allow static bodies to participate in other islands.
Body b = island.m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody) {
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
}
}
{
Timer timer = new Timer();
// Synchronize fixtures, check for out of range bodies.
foreach (Body b in m_bodyList)
{
// If a body was not in an island then it did not move.
if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0)
{
continue;
}
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
m_profile.broadphase = timer.GetMilliseconds();
}
}
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
/// non-tunneling collision. If you change the time interval, you should call this function
/// again.
/// Note: use Distance to compute the contact point and normal at the time of impact.
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static void TimeOfImpact(out TOIOutput output, TOIInput input){
Timer timer = new Timer();
++_toiCalls;
output.state = TOIOutput.State.e_unknown;
output.t = input.tMax;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Sweep sweepA = input.sweepA;
Sweep sweepB = input.sweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float tMax = input.tMax;
float totalRadius = proxyA.m_radius + proxyB.m_radius;
float target = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop);
float tolerance = 0.25f * Settings._linearSlop;
Utilities.Assert(target > tolerance);
float t1 = 0.0f;
const int k_maxIterations = 20; // TODO_ERIN Settings
int iter = 0;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceInput distanceInput;
distanceInput.proxyA = input.proxyA;
distanceInput.proxyB = input.proxyB;
distanceInput.useRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for(;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
DistanceOutput distanceOutput;
Utilities.Distance(out distanceOutput, cache, distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.state = TOIOutput.State.e_overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.state = TOIOutput.State.e_touching;
output.t = t1;
break;
}
// Initialize the separating axis.
throw new NotImplementedException();
// SeparationFunction fcn;
// fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
//#if ZERO
// // Dump the curve seen by the root finder
// {
// const int N = 100;
// float dx = 1.0f / N;
// float xs[N+1];
// float fs[N+1];
// float x = 0.0f;
// for (int i = 0; i <= N; ++i)
// {
// sweepA.GetTransform(out xfA, x);
// sweepB.GetTransform(out xfB, x);
// float f = fcn.Evaluate(xfA, xfB) - target;
// printf("%g %g\n", x, f);
// xs[i] = x;
// fs[i] = f;
// x += dx;
// }
// }
//#endif
// // Compute the TOI on the separating axis. We do this by successively
// // resolving the deepest point. This loop is bounded by the number of vertices.
// bool done = false;
// float t2 = tMax;
// int pushBackIter = 0;
// for (;;)
// {
// // Find the deepest point at t2. Store the witness point indices.
// int indexA, indexB;
// float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
// // Is the final configuration separated?
// if (s2 > target + tolerance)
// {
// // Victory!
// output.state = TOIOutput.State.e_separated;
// output.t = tMax;
// done = true;
// break;
// }
// // Has the separation reached tolerance?
// if (s2 > target - tolerance)
// {
// // Advance the sweeps
// t1 = t2;
// break;
// }
// // Compute the initial separation of the witness points.
// float s1 = fcn.Evaluate(indexA, indexB, t1);
// // Check for initial overlap. This might happen if the root finder
// // runs out of iterations.
// if (s1 < target - tolerance)
// {
// output.state = TOIOutput.State.e_failed;
// output.t = t1;
// done = true;
// break;
// }
// // Check for touching
// if (s1 <= target + tolerance)
// {
// // Victory! t1 should hold the TOI (could be 0.0).
// output.state = TOIOutput.State.e_touching;
// output.t = t1;
// done = true;
// break;
// }
// // Compute 1D root of: f(x) - target = 0
// int rootIterCount = 0;
// float a1 = t1, a2 = t2;
// for (;;)
// {
// // Use a mix of the secant rule and bisection.
// float t;
// if (rootIterCount & 1)
// {
// // Secant rule to improve convergence.
// t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
// }
// else
// {
// // Bisection to guarantee progress.
// t = 0.5f * (a1 + a2);
// }
// ++rootIterCount;
// ++_toiRootIters;
// float s = fcn.Evaluate(indexA, indexB, t);
// if (Math.Abs(s - target) < tolerance)
// {
// // t2 holds a tentative value for t1
// t2 = t;
// break;
// }
// // Ensure we continue to bracket the root.
// if (s > target)
// {
// a1 = t;
// s1 = s;
// }
// else
// {
// a2 = t;
// s2 = s;
// }
// if (rootIterCount == 50)
// {
// break;
// }
// }
// _toiMaxRootIters = Math.Max(_toiMaxRootIters, rootIterCount);
// ++pushBackIter;
// if (pushBackIter == Settings._maxPolygonVertices)
// {
// break;
// }
// }
// ++iter;
// ++_toiIters;
// if (done)
// {
// break;
// }
// if (iter == k_maxIterations)
// {
// // Root finder got stuck. Semi-victory.
// output.state = TOIOutput.State.e_failed;
// output.t = t1;
// break;
// }
}
_toiMaxIters = Math.Max(_toiMaxIters, iter);
float time = timer.GetMilliseconds();
_toiMaxTime = Math.Max(_toiMaxTime, time);
_toiTime += time;
}
/// Take a time step. This performs collision detection, integration,
/// and constraint solution.
/// @param timeStep the amount of time to simulate, this should not vary.
/// @param velocityIterations for the velocity constraint solver.
/// @param positionIterations for the position constraint solver.
public void Step(float timeStep, int velocityIterations, int positionIterations){
Timer stepTimer = new Timer();
// If new fixtures were added, we need to find the new contacts.
if (m_flags.HasFlag(WorldFlags.e_newFixture)) {
m_contactManager.FindNewContacts();
m_flags &= ~WorldFlags.e_newFixture;
}
m_flags |= WorldFlags.e_locked;
TimeStep step;
step.dt = timeStep;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
if (timeStep > 0.0f) {
step.inv_dt = 1.0f / timeStep;
} else {
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * timeStep;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
{
Timer timer = new Timer();
m_contactManager.Collide();
m_profile.collide = timer.GetMilliseconds();
}
// Integrate velocities, solve velocity constraints, and integrate positions.
if (m_stepComplete && step.dt > 0.0f) {
Timer timer = new Timer();
Solve(step);
m_profile.solve = timer.GetMilliseconds();
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f) {
Timer timer = new Timer();
SolveTOI(step);
m_profile.solveTOI = timer.GetMilliseconds();
}
if (step.dt > 0.0f) {
m_inv_dt0 = step.inv_dt;
}
if (m_flags.HasFlag(WorldFlags.e_clearForces)) {
ClearForces();
}
m_flags &= ~WorldFlags.e_locked;
m_profile.step = stepTimer.GetMilliseconds();
}
private void Solve(TimeStep step)
{
m_profile.solveInit = 0.0f;
m_profile.solveVelocity = 0.0f;
m_profile.solvePosition = 0.0f;
// Size the island for the worst case.
Island island = new Island(m_contactManager.m_contactListener);
// Clear all the island flags.
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
foreach (Contact c in m_contactManager.m_contactList)
{
c.m_flags &= ~ContactFlags.e_islandFlag;
}
foreach (Joint j in m_jointList)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
List <Body> stack = new List <Body>(m_bodyList.Count());
foreach (Body seed in m_bodyList)
{
if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack.Add(seed); stackCount++;
seed.m_flags |= Body.BodyFlags.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];
Utilities.Assert(b.IsActive() == true);
island.Add(b);
// Make sure the body is awake.
b.SetAwake(true);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Search all contacts connected to this body.
foreach (ContactEdge ce in b.m_contactList)
{
Contact contact = ce.contact;
// Has this contact already been added to an island?
if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag))
{
continue;
}
// Is this contact solid and touching?
if (contact.IsEnabled() == false ||
contact.IsTouching() == 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 |= ContactFlags.e_islandFlag;
Body other = ce.other;
// Was the other body already added to this island?
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
Utilities.Assert(stackCount < m_bodyList.Count());
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
// Search all joints connect to this body.
foreach (JointEdge je in b.m_jointList)
{
if (je.joint.m_islandFlag == true)
{
continue;
}
Body other = je.other;
// Don't simulate joints connected to inactive bodies.
if (other.IsActive() == false)
{
continue;
}
island.Add(je.joint);
je.joint.m_islandFlag = true;
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
}
Profile profile = new Profile();
island.Solve(profile, step, m_gravity, m_allowSleep);
m_profile.solveInit += profile.solveInit;
m_profile.solveVelocity += profile.solveVelocity;
m_profile.solvePosition += profile.solvePosition;
// Post solve cleanup.
for (int i = 0; i < island.m_bodies.Count(); ++i)
{
// Allow static bodies to participate in other islands.
Body b = island.m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
}
}
{
Timer timer = new Timer();
// Synchronize fixtures, check for out of range bodies.
foreach (Body b in m_bodyList)
{
// If a body was not in an island then it did not move.
if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0)
{
continue;
}
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
m_profile.broadphase = timer.GetMilliseconds();
}
}
/// Take a time step. This performs collision detection, integration,
/// and constraint solution.
/// @param timeStep the amount of time to simulate, this should not vary.
/// @param velocityIterations for the velocity constraint solver.
/// @param positionIterations for the position constraint solver.
public void Step(float timeStep, int velocityIterations, int positionIterations)
{
Timer stepTimer = new Timer();
// If new fixtures were added, we need to find the new contacts.
if (m_flags.HasFlag(WorldFlags.e_newFixture))
{
m_contactManager.FindNewContacts();
m_flags &= ~WorldFlags.e_newFixture;
}
m_flags |= WorldFlags.e_locked;
TimeStep step;
step.dt = timeStep;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
if (timeStep > 0.0f)
{
step.inv_dt = 1.0f / timeStep;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * timeStep;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
{
Timer timer = new Timer();
m_contactManager.Collide();
m_profile.collide = timer.GetMilliseconds();
}
// Integrate velocities, solve velocity constraints, and integrate positions.
if (m_stepComplete && step.dt > 0.0f)
{
Timer timer = new Timer();
Solve(step);
m_profile.solve = timer.GetMilliseconds();
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
Timer timer = new Timer();
SolveTOI(step);
m_profile.solveTOI = timer.GetMilliseconds();
}
if (step.dt > 0.0f)
{
m_inv_dt0 = step.inv_dt;
}
if (m_flags.HasFlag(WorldFlags.e_clearForces))
{
ClearForces();
}
m_flags &= ~WorldFlags.e_locked;
m_profile.step = stepTimer.GetMilliseconds();
}
public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep) {
Timer timer = new Timer();
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodies.Count(); 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 = b.m_sweep.c;
b.m_sweep.a0 = b.m_sweep.a;
if (b.m_type == BodyType._dynamicBody)
{
// Integrate velocities.
v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
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 *= Utilities.Clamp(1.0f - h * b.m_linearDamping, 0.0f, 1.0f);
w *= Utilities.Clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
}
Position pos = new Position();
pos.c = c;
pos.a = a;
m_positions.Add(pos);
Velocity vel = new Velocity();
vel.v = v;
vel.w = w;
m_velocities.Add(vel);
}
timer.Reset();
// Solver data
SolverData solverData;
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
ContactSolver contactSolver = new ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_joints.Count(); ++i)
{
m_joints[i].InitVelocityConstraints(solverData);
}
profile.solveInit = timer.GetMilliseconds();
// Solve velocity constraints
timer.Reset();
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < m_joints.Count(); ++j)
{
m_joints[j].SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile.solveVelocity = timer.GetMilliseconds();
// Integrate positions
for (int i = 0; i < m_bodies.Count(); ++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
Vec2 translation = h * v;
if (Utilities.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;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
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_joints.Count; ++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_bodies.Count(); ++i)
{
Body body = m_bodies[i];
body.m_sweep.c = m_positions[i].c;
body.m_sweep.a = m_positions[i].a;
body.m_linearVelocity = m_velocities[i].v;
body.m_angularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
profile.solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = Single.MaxValue;
const float linTolSqr = Settings._linearSleepTolerance * Settings._linearSleepTolerance;
const float angTolSqr = Settings._angularSleepTolerance * Settings._angularSleepTolerance;
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0 ||
b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
Utilities.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
{
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.m_sleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.m_sleepTime);
}
}
if (minSleepTime >= Settings._timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
/// non-tunneling collision. If you change the time interval, you should call this function
/// again.
/// Note: use Distance to compute the contact point and normal at the time of impact.
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static void TimeOfImpact(out TOIOutput output, TOIInput input)
{
Timer timer = new Timer();
++_toiCalls;
output.state = TOIOutput.State.e_unknown;
output.t = input.tMax;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Sweep sweepA = input.sweepA;
Sweep sweepB = input.sweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float tMax = input.tMax;
float totalRadius = proxyA.m_radius + proxyB.m_radius;
float target = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop);
float tolerance = 0.25f * Settings._linearSlop;
Utilities.Assert(target > tolerance);
float t1 = 0.0f;
const int k_maxIterations = 20; // TODO_ERIN Settings
int iter = 0;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceInput distanceInput;
distanceInput.proxyA = input.proxyA;
distanceInput.proxyB = input.proxyB;
distanceInput.useRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
DistanceOutput distanceOutput;
Utilities.Distance(out distanceOutput, cache, distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.state = TOIOutput.State.e_overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.state = TOIOutput.State.e_touching;
output.t = t1;
break;
}
// Initialize the separating axis.
throw new NotImplementedException();
// SeparationFunction fcn;
// fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
//#if ZERO
// // Dump the curve seen by the root finder
// {
// const int N = 100;
// float dx = 1.0f / N;
// float xs[N+1];
// float fs[N+1];
// float x = 0.0f;
// for (int i = 0; i <= N; ++i)
// {
// sweepA.GetTransform(out xfA, x);
// sweepB.GetTransform(out xfB, x);
// float f = fcn.Evaluate(xfA, xfB) - target;
// printf("%g %g\n", x, f);
// xs[i] = x;
// fs[i] = f;
// x += dx;
// }
// }
//#endif
// // Compute the TOI on the separating axis. We do this by successively
// // resolving the deepest point. This loop is bounded by the number of vertices.
// bool done = false;
// float t2 = tMax;
// int pushBackIter = 0;
// for (;;)
// {
// // Find the deepest point at t2. Store the witness point indices.
// int indexA, indexB;
// float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
// // Is the final configuration separated?
// if (s2 > target + tolerance)
// {
// // Victory!
// output.state = TOIOutput.State.e_separated;
// output.t = tMax;
// done = true;
// break;
// }
// // Has the separation reached tolerance?
// if (s2 > target - tolerance)
// {
// // Advance the sweeps
// t1 = t2;
// break;
// }
// // Compute the initial separation of the witness points.
// float s1 = fcn.Evaluate(indexA, indexB, t1);
// // Check for initial overlap. This might happen if the root finder
// // runs out of iterations.
// if (s1 < target - tolerance)
// {
// output.state = TOIOutput.State.e_failed;
// output.t = t1;
// done = true;
// break;
// }
// // Check for touching
// if (s1 <= target + tolerance)
// {
// // Victory! t1 should hold the TOI (could be 0.0).
// output.state = TOIOutput.State.e_touching;
// output.t = t1;
// done = true;
// break;
// }
// // Compute 1D root of: f(x) - target = 0
// int rootIterCount = 0;
// float a1 = t1, a2 = t2;
// for (;;)
// {
// // Use a mix of the secant rule and bisection.
// float t;
// if (rootIterCount & 1)
// {
// // Secant rule to improve convergence.
// t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
// }
// else
// {
// // Bisection to guarantee progress.
// t = 0.5f * (a1 + a2);
// }
// ++rootIterCount;
// ++_toiRootIters;
// float s = fcn.Evaluate(indexA, indexB, t);
// if (Math.Abs(s - target) < tolerance)
// {
// // t2 holds a tentative value for t1
// t2 = t;
// break;
// }
// // Ensure we continue to bracket the root.
// if (s > target)
// {
// a1 = t;
// s1 = s;
// }
// else
// {
// a2 = t;
// s2 = s;
// }
// if (rootIterCount == 50)
// {
// break;
// }
// }
// _toiMaxRootIters = Math.Max(_toiMaxRootIters, rootIterCount);
// ++pushBackIter;
// if (pushBackIter == Settings._maxPolygonVertices)
// {
// break;
// }
// }
// ++iter;
// ++_toiIters;
// if (done)
// {
// break;
// }
// if (iter == k_maxIterations)
// {
// // Root finder got stuck. Semi-victory.
// output.state = TOIOutput.State.e_failed;
// output.t = t1;
// break;
// }
}
_toiMaxIters = Math.Max(_toiMaxIters, iter);
float time = timer.GetMilliseconds();
_toiMaxTime = Math.Max(_toiMaxTime, time);
_toiTime += time;
}