public void Solve(ref b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep)
{
b2Timer timer = new b2Timer();
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
b2Vec2 c = b.Sweep.c;
float a = b.Sweep.a;
b2Vec2 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 == b2BodyType.b2_dynamicBody)
{
// Integrate velocities.
v += h * (b.GravityScale * gravity + b.InvertedMass * b.Force);
w += h * b.InvertedI * 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 *= b2Math.b2Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= b2Math.b2Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
timer.Reset();
// Solver data
b2SolverData solverData = new b2SolverData();
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_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;
b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_jointCount; ++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_jointCount; ++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_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
{
float ratio = b2Settings.b2_maxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
float ratio = b2Settings.b2_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 i2 = 0; i2 < m_jointCount; ++i2)
{
bool jointOkay = m_joints[i2].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)
{
b2Body body = m_bodies[i];
body.Sweep.c = m_positions[i].c;
body.Sweep.a = m_positions[i].a;
body.LinearVelocity = m_velocities[i].v;
body.AngularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
profile.solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = b2Settings.b2_maxFloat;
float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance;
float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance;
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
if (b.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
if (!(b.BodyFlags.HasFlag(b2BodyFlags.e_autoSleepFlag)) ||
b.AngularVelocity * b.AngularVelocity > angTolSqr ||
b2Math.b2Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= b2Settings.b2_timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
// Find islands, integrate and solveraints, solve positionraints
public void Solve(b2TimeStep step)
{
m_profile.solveInit = 0.0f;
m_profile.solveVelocity = 0.0f;
m_profile.solvePosition = 0.0f;
// Size the island for the worst case.
b2Island island = new b2Island(m_bodyCount,
m_contactManager.ContactCount,
m_jointCount,
m_contactManager.ContactListener);
// Clear all the island flags.
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
}
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
c.ContactFlags &= ~b2ContactFlags.e_islandFlag;
}
for (b2Joint j = m_jointList; j; j = j.Next)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = m_bodyCount;
b2Body[] stack = new b2Body[stackSize];
for (b2Body seed = m_bodyList; seed != null; seed = seed.Next)
{
if (seed.BodyFlags & b2BodyFlags.e_islandFlag)
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed.BodyFlags |= b2BodyFlags.e_islandFlag;
// Perform a depth first search (DFS) on theraint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
b2Body b = stack[--stackCount];
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.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
// Search all contacts connected to this body.
for (b2ContactEdge ce = b.ContactList; ce != null; ce = ce.next)
{
b2Contact contact = ce.contact;
// Has this contact already been added to an island?
if (contact.ContactFlags & b2ContactFlags.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.ContactFlags |= b2ContactType.e_islandFlag;
b2Body other = ce.other;
// Was the other body already added to this island?
if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
{
continue;
}
stack[stackCount++] = other;
other.BodyFlags |= b2BodyFlags.e_islandFlag;
}
// Search all joints connect to this body.
for (b2JointEdge je = b.JointList; je; je = je.next)
{
if (je.joint.IslandFlag == true)
{
continue;
}
b2Body 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.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
{
continue;
}
stack[stackCount++] = other;
other.BodyFlags |= b2BodyFlags.e_islandFlag;
}
}
b2Profile profile = island.Solve(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_bodyCount; ++i)
{
// Allow static bodies to participate in other islands.
b2Body b = island.m_bodies[i];
if (b.BodyType == b2BodyType.b2_staticBody)
{
b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
}
}
}
{
b2Timer timer;
// Synchronize fixtures, check for out of range bodies.
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
// If a body was not in an island then it did not move.
if ((b.BodyFlags & b2BodyType.e_islandFlag) == 0)
{
continue;
}
if (b.GetBodyType() == b2BodyType.b2_staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
m_profile.broadphase = timer.GetMilliseconds();
}
}
protected virtual void Draw(Settings settings)
{
m_world.DrawDebugData();
if (settings.drawStats)
{
int bodyCount = m_world.BodyCount;
int contactCount = m_world.ContactCount;
int jointCount = m_world.JointCount;
m_debugDraw.DrawString(5, m_textLine, "bodies/contacts/joints = {0}/{1}/{2}", bodyCount, contactCount,
jointCount);
m_textLine += 15;
int proxyCount = m_world.GetProxyCount();
int height = m_world.GetTreeHeight();
int balance = m_world.GetTreeBalance();
float quality = m_world.GetTreeQuality();
m_debugDraw.DrawString(5, m_textLine, "proxies/height/balance/quality = {0}/{1}/{2}/{3}", proxyCount,
height, balance, quality);
m_textLine += 15;
}
#if PROFILING
// Track maximum profile times
{
b2Profile p = m_world.Profile;
m_maxProfile.step = Math.Max(m_maxProfile.step, p.step);
m_maxProfile.collide = Math.Max(m_maxProfile.collide, p.collide);
m_maxProfile.solve = Math.Max(m_maxProfile.solve, p.solve);
m_maxProfile.solveInit = Math.Max(m_maxProfile.solveInit, p.solveInit);
m_maxProfile.solveVelocity = Math.Max(m_maxProfile.solveVelocity, p.solveVelocity);
m_maxProfile.solvePosition = Math.Max(m_maxProfile.solvePosition, p.solvePosition);
m_maxProfile.solveTOI = Math.Max(m_maxProfile.solveTOI, p.solveTOI);
m_maxProfile.broadphase = Math.Max(m_maxProfile.broadphase, p.broadphase);
m_totalProfile.step += p.step;
m_totalProfile.collide += p.collide;
m_totalProfile.solve += p.solve;
m_totalProfile.solveInit += p.solveInit;
m_totalProfile.solveVelocity += p.solveVelocity;
m_totalProfile.solvePosition += p.solvePosition;
m_totalProfile.solveTOI += p.solveTOI;
m_totalProfile.broadphase += p.broadphase;
}
if (settings.drawProfile)
{
b2Profile p = m_world.Profile;
b2Profile aveProfile = new b2Profile();
if (m_stepCount > 0)
{
float scale = 1.0f / m_stepCount;
aveProfile.step = scale * m_totalProfile.step;
aveProfile.collide = scale * m_totalProfile.collide;
aveProfile.solve = scale * m_totalProfile.solve;
aveProfile.solveInit = scale * m_totalProfile.solveInit;
aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity;
aveProfile.solvePosition = scale * m_totalProfile.solvePosition;
aveProfile.solveTOI = scale * m_totalProfile.solveTOI;
aveProfile.broadphase = scale * m_totalProfile.broadphase;
}
m_debugDraw.DrawString(5, m_textLine, "step [ave] (max) = {0:00000.00} [{1:000000.00}] ({2:000000.00})", p.step,
aveProfile.step, m_maxProfile.step);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "collide [ave] (max) = {0:00000.00} [{1:000000.00}] ({2:000000.00})", p.collide,
aveProfile.collide, m_maxProfile.collide);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "solve [ave] (max) = {0:00000.00} [{1:000000.00}] ({2:000000.00})", p.solve,
aveProfile.solve, m_maxProfile.solve);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "solve init [ave] (max) = {0:00000.00} [{1:000000.00}] ({2:000000.00})", p.solveInit,
aveProfile.solveInit, m_maxProfile.solveInit);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "solve velocity [ave] (max) = {0:00000.00} [{1:000000.00}] ({2:000000.00})",
p.solveVelocity, aveProfile.solveVelocity, m_maxProfile.solveVelocity);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "solve position [ave] (max) = {0:00000.00} [{1:000000.00}] ({2:000000.00})",
p.solvePosition, aveProfile.solvePosition, m_maxProfile.solvePosition);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "solveTOI [ave] (max) = {0:00000.00} [{1:000000.00}] ({2:000000.00})", p.solveTOI,
aveProfile.solveTOI, m_maxProfile.solveTOI);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "broad-phase [ave] (max) = {0:00000.00} [{1:000000.00}] ({2:000000.00})", p.broadphase,
aveProfile.broadphase, m_maxProfile.broadphase);
m_textLine += 15;
}
#endif
if (m_mouseJoint != null)
{
b2Vec2 p1 = m_mouseJoint.GetAnchorB();
b2Vec2 p2 = m_mouseJoint.GetTarget();
b2Color c = new b2Color();
c.Set(0.0f, 1.0f, 0.0f);
m_debugDraw.DrawPoint(p1, 4.0f, c);
m_debugDraw.DrawPoint(p2, 4.0f, c);
c.Set(0.8f, 0.8f, 0.8f);
m_debugDraw.DrawSegment(p1, p2, c);
}
if (m_bombSpawning)
{
b2Color c = new b2Color();
c.Set(0.0f, 0.0f, 1.0f);
m_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c);
c.Set(0.8f, 0.8f, 0.8f);
m_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c);
}
if (settings.drawContactPoints)
{
//const float32 k_impulseScale = 0.1f;
float k_axisScale = 0.3f;
for (int i = 0; i < m_pointCount; ++i)
{
ContactPoint point = m_points[i];
if (point.state == b2PointState.b2_addState)
{
// Add
m_debugDraw.DrawPoint(point.position, 10.0f, new b2Color(0.3f, 0.95f, 0.3f));
}
else if (point.state == b2PointState.b2_persistState)
{
// Persist
m_debugDraw.DrawPoint(point.position, 5.0f, new b2Color(0.3f, 0.3f, 0.95f));
}
if (settings.drawContactNormals == 1)
{
b2Vec2 p1 = point.position;
b2Vec2 p2 = p1 + k_axisScale * point.normal;
m_debugDraw.DrawSegment(p1, p2, new b2Color(0.9f, 0.9f, 0.9f));
}
else if (settings.drawContactForces == 1)
{
//b2Vec2 p1 = point->position;
//b2Vec2 p2 = p1 + k_forceScale * point->normalForce * point->normal;
//DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
}
if (settings.drawFrictionForces == 1)
{
//b2Vec2 tangent = b2Cross(point->normal, 1.0f);
//b2Vec2 p1 = point->position;
//b2Vec2 p2 = p1 + k_forceScale * point->tangentForce * tangent;
//DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
}
}
}
}
public void Solve(ref b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep)
static void Run(Action iterCallback)
{
int iter = 0;
long span = 0L;
float step = (float)(TimeSpan.FromTicks(333333).TotalMilliseconds) / 1000f;
Console.WriteLine("Cycle step = {0:F3} which is {1} fps", step, (int)(1f / step));
int interval = 0;
#if PROFILING
b2Profile m_maxProfile = new b2Profile();
b2Profile m_totalProfile = new b2Profile();
b2Profile aveProfile = new b2Profile();
#endif
for (float dt = 0f; dt < simulat_time; )
{
long dtStart = DateTime.Now.Ticks;
Update(_world, step);
long duration = DateTime.Now.Ticks - dtStart;
span += duration;
dt += step;
iter++;
bool bdump = false;
if (iterCallback != null)
{
iterCallback();
}
if (iter == 30)
{
interval++;
bdump = true;
//Dump(_world);
TimeSpan ts = new TimeSpan(span);
float fs = (float)ts.TotalMilliseconds / (float)iter;
Console.WriteLine("{2}: iteration time is {0:F3} ms avg. and is {1:F3} cycles", fs, fs / step, interval);
iter = 0;
span = 0L;
int bodyCount = _world.BodyCount;
int contactCount = _world.ContactManager.ContactCount;
int jointCount = _world.JointCount;
Console.WriteLine("{3}:bodies/contacts/joints = {0}/{1}/{2}", bodyCount, contactCount, jointCount, interval);
int proxyCount = _world.GetProxyCount();
int treeheight = _world.GetTreeHeight();
int balance = _world.GetTreeBalance();
float quality = _world.GetTreeQuality();
Console.WriteLine("{4}:proxies/height/balance/quality = {0}/{1}/{2}/{3:F3}", proxyCount, height, balance, quality, interval);
for (b2Body b = _world.BodyList; b != null; b = b.Next)
{
Console.WriteLine("Body: p={0:F3},{1:F3} v={2:F3},{3:F3}, w={4:F3}", b.Position.x, b.Position.y, b.LinearVelocity.x, b.LinearVelocity.y, b.AngularVelocity);
}
}
#if PROFILING
b2Profile p = _world.Profile;
// Track maximum profile times
{
m_maxProfile.step = Math.Max(m_maxProfile.step, p.step);
m_maxProfile.collide = Math.Max(m_maxProfile.collide, p.collide);
m_maxProfile.solve = Math.Max(m_maxProfile.solve, p.solve);
m_maxProfile.solveInit = Math.Max(m_maxProfile.solveInit, p.solveInit);
m_maxProfile.solveVelocity = Math.Max(m_maxProfile.solveVelocity, p.solveVelocity);
m_maxProfile.solvePosition = Math.Max(m_maxProfile.solvePosition, p.solvePosition);
m_maxProfile.solveTOI = Math.Max(m_maxProfile.solveTOI, p.solveTOI);
m_maxProfile.broadphase = Math.Max(m_maxProfile.broadphase, p.broadphase);
m_totalProfile.step += p.step;
m_totalProfile.collide += p.collide;
m_totalProfile.solve += p.solve;
m_totalProfile.solveInit += p.solveInit;
m_totalProfile.solveVelocity += p.solveVelocity;
m_totalProfile.solvePosition += p.solvePosition;
m_totalProfile.solveTOI += p.solveTOI;
m_totalProfile.broadphase += p.broadphase;
}
if (interval > 0)
{
float scale = 1.0f / (float)interval;
aveProfile.step = scale * m_totalProfile.step;
aveProfile.collide = scale * m_totalProfile.collide;
aveProfile.solve = scale * m_totalProfile.solve;
aveProfile.solveInit = scale * m_totalProfile.solveInit;
aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity;
aveProfile.solvePosition = scale * m_totalProfile.solvePosition;
aveProfile.solveTOI = scale * m_totalProfile.solveTOI;
aveProfile.broadphase = scale * m_totalProfile.broadphase;
}
if (bdump)
{
Console.WriteLine("{3}:step [ave] (max) = {0:F2} [{1:F2}] ({2:F2})", p.step, aveProfile.step, m_maxProfile.step,interval);
Console.WriteLine("{3}:collide [ave] (max) = {0:F2} [{1:F2}] ({2:F2})", p.collide, aveProfile.collide, m_maxProfile.collide, interval);
Console.WriteLine("{3}:solve [ave] (max) = {0:F2} [{1:F2}] ({2:F2})", p.solve, aveProfile.solve, m_maxProfile.solve, interval);
Console.WriteLine("{3}:solve init [ave] (max) = {0:F2} [{1:F2}] ({2:F2})", p.solveInit, aveProfile.solveInit, m_maxProfile.solveInit, interval);
Console.WriteLine("{3}:solve velocity [ave] (max) = {0:F2} [{1:F2}] ({2:F2})", p.solveVelocity, aveProfile.solveVelocity, m_maxProfile.solveVelocity, interval);
Console.WriteLine("{3}:solve position [ave] (max) = {0:F2} [{1:F2}] ({2:F2})", p.solvePosition, aveProfile.solvePosition, m_maxProfile.solvePosition, interval);
Console.WriteLine("{3}:solveTOI [ave] (max) = {0:F2} [{1:F2}] ({2:F2})", p.solveTOI, aveProfile.solveTOI, m_maxProfile.solveTOI, interval);
Console.WriteLine("{3}:broad-phase [ave] (max) = {0:F2} [{1:F2}] ({2:F2})", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase, interval);
}
#endif
}
#if PROFILING
Dump(_world);
#endif
Console.WriteLine("hit <enter> to exit");
Console.ReadLine();
}
public void Solve(ref b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep)
public void Solve(ref b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep)
{
b2Timer timer = new b2Timer();
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
b2Vec2 c = b.Sweep.c;
float a = b.Sweep.a;
b2Vec2 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 == b2BodyType.b2_dynamicBody)
{
// Integrate velocities.
v += h * (b.GravityScale * gravity + b.InvertedMass * b.Force);
w += h * b.InvertedI * 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 *= b2Math.b2Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= b2Math.b2Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
timer.Reset();
// Solver data
b2SolverData solverData = new b2SolverData();
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_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;
b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_jointCount; ++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_jointCount; ++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_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
{
float ratio = b2Settings.b2_maxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
float ratio = b2Settings.b2_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 i2 = 0; i2 < m_jointCount; ++i2)
{
bool jointOkay = m_joints[i2].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)
{
b2Body body = m_bodies[i];
body.Sweep.c = m_positions[i].c;
body.Sweep.a = m_positions[i].a;
body.LinearVelocity = m_velocities[i].v;
body.AngularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
profile.solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = b2Settings.b2_maxFloat;
float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance;
float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance;
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
if (b.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
if (!(b.BodyFlags.HasFlag(b2BodyFlags.e_autoSleepFlag)) ||
b.AngularVelocity * b.AngularVelocity > angTolSqr ||
b2Math.b2Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= b2Settings.b2_timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
