public Arbiter(SequentialImpulsesSolver parent, Body body1, Body body2)
{
if (body1.ID < body2.ID)
{
this.body1 = body1;
this.body2 = body2;
}
else
{
this.body1 = body2;
this.body2 = body1;
}
this.tag1 = (SequentialImpulsesTag)this.body1.SolverTag;
this.tag2 = (SequentialImpulsesTag)this.body2.SolverTag;
this.circle1 = this.body1.Shape as CircleShape;
this.circle2 = this.body2.Shape as CircleShape;
this.friction = MathHelper.Sqrt(
this.body1.Coefficients.DynamicFriction *
this.body2.Coefficients.DynamicFriction);
this.restitution = Math.Min(body1.Coefficients.Restitution, body2.Coefficients.Restitution);
this.parent = parent;
this.contacts = new LinkedList <ContactPoint>();
this.lastUpdate = -1;
this.state = ContactState.New;
this.ignoresCollisionResponse = body1.IgnoresCollisionResponse || body2.IgnoresCollisionResponse;
}
protected internal override void Solve(TimeStep step)
{
foreach (Arbiter arb in arbiters.Values)
{
arb.Updated = false;
}
Detect(step);
RemoveEmpty();
this.Engine.RunLogic(step);
for (int index = 0; index < tags.Count; ++index)
{
SequentialImpulsesTag tag = tags[index];
tag.biasVelocity = ALVector2D.Zero;
tag.body.UpdateVelocity(step);
tag.body.ClearForces();
}
Arbiter[] arbs = new Arbiter[arbiters.Count];
arbiters.Values.CopyTo(arbs, 0);
for (int index = 0; index < arbs.Length; ++index)
{
arbs[index].PreApply(step.DtInv);
}
for (int index = 0; index < siJoints.Count; ++index)
{
siJoints[index].PreStep(step);
}
for (int i = 0; i < iterations; ++i)
{
for (int index = 0; index < arbs.Length; ++index)
{
arbs[index].Apply();
}
for (int index = 0; index < siJoints.Count; ++index)
{
siJoints[index].ApplyImpulse();
}
}
for (int index = 0; index < tags.Count; ++index)
{
SequentialImpulsesTag tag = tags[index];
if (splitImpulse)
{
tag.body.UpdatePosition(step, ref tag.biasVelocity);
}
else
{
tag.body.UpdatePosition(step);
}
}
}
protected internal override void AddBodyRange(List <Body> collection)
{
foreach (Body item in collection)
{
if (item.SolverTag == null)
{
SequentialImpulsesTag tag = new SequentialImpulsesTag(item);
SetTag(item, tag);
tags.Add(tag);
}
else
{
tags.Add((SequentialImpulsesTag)item.SolverTag);
}
}
}
public Arbiter(SequentialImpulsesSolver parent, Body body1, Body body2)
{
if (body1.ID < body2.ID)
{
this.body1 = body1;
this.body2 = body2;
}
else
{
this.body1 = body2;
this.body2 = body1;
}
this.tag1 = (SequentialImpulsesTag)this.body1.SolverTag;
this.tag2 = (SequentialImpulsesTag)this.body2.SolverTag;
this.circle1 = this.body1.Shape as CircleShape;
this.circle2 = this.body2.Shape as CircleShape;
this.friction = MathHelper.Sqrt(
this.body1.Coefficients.DynamicFriction *
this.body2.Coefficients.DynamicFriction);
this.restitution = Math.Min(body1.Coefficients.Restitution, body2.Coefficients.Restitution);
this.parent = parent;
this.contacts = new LinkedList <Contact>();
}
protected internal override void Solve(TimeStep step)
{
Detect(step);
Arbiter[] arbs = RemoveEmpty(step);
this.Engine.RunLogic(step);
if (freezing)
{
for (int index = 0; index < siJoints.Count; ++index)
{
siJoints[index].CheckFrozen();
}
}
for (int index = 0; index < tags.Count; ++index)
{
SequentialImpulsesTag tag = tags[index];
tag.biasVelocity = ALVector2D.Zero;
if (freezing)
{
bool accelSame = tag.body.State.Acceleration == tag.lastAccel;
ALVector2D vel = tag.body.State.Velocity;
ALVector2D force = tag.body.State.ForceAccumulator;
bool isVelZero =
Math.Abs(vel.X) < freezeVelocityTolerance.X &&
Math.Abs(vel.Y) < freezeVelocityTolerance.Y &&
Math.Abs(vel.Angular) < freezeVelocityTolerance.Angular;
bool isForceZero = tag.body.State.ForceAccumulator == ALVector2D.Zero;
if (accelSame && isVelZero && isForceZero)
{
if (tag.body.Joints.Count == 0)
{
tag.body.idleCount++;
}
if (tag.body.idleCount > freezeTimeout)
{
tag.body.idleCount = freezeTimeout;
tag.body.IsFrozen = true;
tag.body.State.Velocity = ALVector2D.Zero;
}
}
else
{
tag.body.IsFrozen = false;
tag.body.idleCount = 0;
}
tag.lastAccel = tag.body.State.Acceleration;
if (tag.body.IsFrozen)
{
tag.body.State.ForceAccumulator = ALVector2D.Zero;
tag.body.State.Acceleration = ALVector2D.Zero;
}
}
tag.body.UpdateVelocity(step);
tag.body.ClearForces();
}
for (int index = 0; index < arbs.Length; ++index)
{
arbs[index].PreApply(step.DtInv);
}
for (int index = 0; index < siJoints.Count; ++index)
{
siJoints[index].PreStep(step);
}
for (int i = 0; i < iterations; ++i)
{
for (int index = 0; index < arbs.Length; ++index)
{
arbs[index].Apply();
}
for (int index = 0; index < siJoints.Count; ++index)
{
siJoints[index].ApplyImpulse();
}
}
for (int index = 0; index < tags.Count; ++index)
{
SequentialImpulsesTag tag = tags[index];
if (splitImpulse)
{
tag.body.UpdatePosition(step, ref tag.biasVelocity);
}
else
{
tag.body.UpdatePosition(step);
}
tag.body.ApplyPosition();
}
}
static bool IsTagRemoved(SequentialImpulsesTag tag)
{
return(!tag.body.IsAdded);
}
protected internal override void AddBodyRange(List<Body> collection)
{
foreach (Body item in collection)
{
if (item.SolverTag == null)
{
SequentialImpulsesTag tag = new SequentialImpulsesTag(item);
SetTag(item, tag);
tags.Add(tag);
}
else
{
tags.Add((SequentialImpulsesTag)item.SolverTag);
}
}
}
static bool IsTagRemoved(SequentialImpulsesTag tag)
{
return !tag.body.IsAdded;
}
public Arbiter(SequentialImpulsesSolver parent, Body body1, Body body2)
{
if (body1.ID < body2.ID)
{
this.body1 = body1;
this.body2 = body2;
}
else
{
this.body1 = body2;
this.body2 = body1;
}
this.tag1 = (SequentialImpulsesTag)this.body1.SolverTag;
this.tag2 = (SequentialImpulsesTag)this.body2.SolverTag;
this.circle1 = this.body1.Shape as CircleShape;
this.circle2 = this.body2.Shape as CircleShape;
this.friction = MathHelper.Sqrt(
this.body1.Coefficients.DynamicFriction *
this.body2.Coefficients.DynamicFriction);
this.restitution = Math.Min(body1.Coefficients.Restitution, body2.Coefficients.Restitution);
this.parent = parent;
this.contacts = new LinkedList<ContactPoint>();
this.lastUpdate = -1;
this.state = ContactState.New;
this.ignoresCollisionResponse = body1.IgnoresCollisionResponse || body2.IgnoresCollisionResponse;
}