public Island()
{
Bodies = new List <Body>();
Velocities = new List <VelocityState>();
Contacts = new List <ContactConstraint>();
ContactStates = new List <ContactConstraintState>();
ContactSolver = new ContactSolver();
}
public void Solve()
{
// Apply gravity
// Integrate velocities and create state buffers, calculate world inertia
for (int i = 0; i < Bodies.Count; ++i)
{
Body body = Bodies[i];
VelocityState v = Velocities[i];
if ((body.Flags & BodyFlags.eDynamic) > 0)
{
body.ApplyLinearForce(Gravity * body.GravityScale);
// Calculate world space intertia tensor
Mat3 r = body.Tx.rotation;
body.InvInertiaWorld = r * body.InvInertiaModel * Mat3.Transpose(r);
// Integrate velocity
body.LinearVelocity += (body.Force * body.InvMass) * Dt;
body.AngularVelocity += (body.InvInertiaWorld * body.Torque) * Dt;
// From Box2D!
// 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
// Pade approximation:
// v2 = v1 * 1 / (1 + c * dt)
body.LinearVelocity *= 1 / (1 + Dt * body.LinearDamping);
body.AngularVelocity *= 1 / (1 + Dt * body.AngularDamping);
}
Velocities[i] = new VelocityState {
v = body.LinearVelocity, w = body.AngularVelocity
};
}
// Create contact solver, pass in state buffers, create buffers for contacts
// Initialize velocity constraint for normal + friction and warm start
ContactSolver.Initialize(this);
ContactSolver.PreSolve(Dt);
// Solve contacts
for (int i = 0; i < Iterations; ++i)
{
ContactSolver.Solve();
}
ContactSolver.ShutDown();
// Copy back state buffers
// Integrate positions
for (int i = 0; i < Bodies.Count; ++i)
{
Body body = Bodies[i];
VelocityState v = Velocities[i];
if ((body.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
body.LinearVelocity = v.v;
body.AngularVelocity = v.w;
// Integrate position
body.WorldCenter += body.LinearVelocity * Dt;
body.Q.Integrate(body.AngularVelocity, Dt);
body.Q = Quaternion.Normalize(body.Q);
body.Tx.rotation = body.Q.ToMat3();
}
if (AllowSleep)
{
// Find minimum sleep time of the entire island
double minSleepTime = double.MaxValue;
for (int i = 0; i < Bodies.Count; ++i)
{
Body body = Bodies[i];
if ((body.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
double sqrLinVel = Vec3.Dot(body.LinearVelocity, body.LinearVelocity);
double cbAngVel = Vec3.Dot(body.AngularVelocity, body.AngularVelocity);
double linTol = Q3_SLEEP_LINEAR;
double angTol = Q3_SLEEP_ANGULAR;
if (sqrLinVel > linTol || cbAngVel > angTol)
{
minSleepTime = 0;
body.SleepTime = 0;
}
else
{
body.SleepTime += Dt;
minSleepTime = Math.Min(minSleepTime, body.SleepTime);
}
}
// Put entire island to sleep so long as the minimum found sleep time
// is below the threshold. If the minimum sleep time reaches below the
// sleeping threshold, the entire island will be reformed next step
// and sleep test will be tried again.
if (minSleepTime > Q3_SLEEP_TIME)
{
for (int i = 0; i < Bodies.Count; ++i)
{
Bodies[i].SetToSleep();
}
}
}
}