/// <summary>
/// Construct a world object.
/// </summary>
/// <param name="gravity">the world gravity vector.</param>
/// <param name="doSleep">improve performance by not simulating inactive bodies.</param>
public World(Vec2 gravity, IWorldPool argPool)
{
contactStacks = new ContactRegister[ShapeTypesCount][];
for (int i = 0; i < ShapeTypesCount; i++)
{
contactStacks[i] = new ContactRegister[ShapeTypesCount];
}
pool = argPool;
m_destructionListener = null;
m_debugDraw = null;
m_bodyList = null;
m_jointList = null;
m_bodyCount = 0;
m_jointCount = 0;
m_warmStarting = true;
m_continuousPhysics = true;
m_subStepping = false;
m_stepComplete = true;
m_allowSleep = true;
m_gravity.set_Renamed(gravity);
m_flags = CLEAR_FORCES;
m_inv_dt0 = 0f;
m_contactManager = new ContactManager(this);
m_profile = new Profile();
initializeRegisters();
}
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;
}
}
}
}
/// <summary>
/// Construct a world object.
/// </summary>
/// <param name="gravity">the world gravity vector.</param>
/// <param name="argPool"> </param>
public World(Vec2 gravity, IWorldPool argPool)
{
contactStacks = new ContactRegister[ShapeTypesCount][];
for (int i = 0; i < ShapeTypesCount; i++)
{
contactStacks[i] = new ContactRegister[ShapeTypesCount];
}
Pool = argPool;
DestructionListener = null;
DebugDraw = null;
BodyList = null;
JointList = null;
BodyCount = 0;
JointCount = 0;
WarmStarting = true;
ContinuousPhysics = true;
m_subStepping = false;
m_stepComplete = true;
SleepingAllowed = true;
m_gravity.Set(gravity);
Flags = CLEAR_FORCES;
invDt0 = 0f;
ContactManager = new ContactManager(this);
Profile = new Profile();
InitializeRegisters();
}
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;
}
}
}
}