|
public Solve ( |
||
| step | ||
| gravity | System.Vector2 | |
| allowSleep | bool | |
| Результат | void | |
public void Solve(TimeStep step, Vector2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
continue;
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force = Vector2.zero;
b._torque = 0.0f;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b._linearVelocity *= Mathf.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Mathf.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
}
ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
continue;
// Check for large velocities.
Vector2 translation = step.Dt * b._linearVelocity;
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation.normalized;
}
float rotation = step.Dt * b._angularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
}
else
{
b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
}
}
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new Transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity * b._angularVelocity > angTolSqr ||
Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
}
}
// Find islands, integrate and solve constraints, solve position constraints
private void Solve(TimeStep step)
{
// Size the island for the worst case.
Island island = new Island(_bodyCount, _contactCount, _jointCount, _contactListener);
// Clear all the island flags.
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~Body.BodyFlags.Island;
}
for (Contact c = _contactList; c != null; c = c._next)
{
c._flags &= ~Contact.CollisionFlags.Island;
}
for (Joint j = _jointList; j != null; j = j._next)
{
j._islandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = _bodyCount;
{
Body[] stack = new Body[stackSize];
for (Body seed = _bodyList; seed != null; seed = seed._next)
{
if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
{
continue;
}
if (seed.IsStatic())
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed._flags |= Body.BodyFlags.Island;
// 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];
island.Add(b);
// Make sure the body is awake.
b._flags &= ~Body.BodyFlags.Sleep;
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.IsStatic())
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next)
{
// Has this contact already been added to an island?
if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.NonSolid)) != 0)
{
continue;
}
// Is this contact touching?
if (cn.Contact.GetManifoldCount() == 0)
{
continue;
}
island.Add(cn.Contact);
cn.Contact._flags |= Contact.CollisionFlags.Island;
Body other = cn.Other;
// Was the other body already added to this island?
if ((other._flags & Body.BodyFlags.Island) != 0)
{
continue;
}
Box2DXDebug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= Body.BodyFlags.Island;
}
// Search all joints connect to this body.
for (JointEdge jn = b._jointList; jn != null; jn = jn.Next)
{
if (jn.Joint._islandFlag == true)
{
continue;
}
island.Add(jn.Joint);
jn.Joint._islandFlag = true;
Body other = jn.Other;
if ((other._flags & Body.BodyFlags.Island) != 0)
{
continue;
}
Box2DXDebug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= Body.BodyFlags.Island;
}
}
island.Solve(step, _gravity, _allowSleep);
// Post solve cleanup.
for (int i = 0; i < island._bodyCount; ++i)
{
// Allow static bodies to participate in other islands.
Body b = island._bodies[i];
if (b.IsStatic())
{
b._flags &= ~Body.BodyFlags.Island;
}
}
}
stack = null;
}
// Synchronize shapes, check for out of range bodies.
for (Body b = _bodyList; b != null; b = b.GetNext())
{
if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
{
continue;
}
if (b.IsStatic())
{
continue;
}
// Update shapes (for broad-phase). If the shapes go out of
// the world AABB then shapes and contacts may be destroyed,
// including contacts that are
bool inRange = b.SynchronizeShapes();
// Did the body's shapes leave the world?
if (inRange == false && _boundaryListener != null)
{
_boundaryListener.Violation(b);
}
}
// Commit shape proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
_broadPhase.Commit();
}