void Solve(ref TimeStep step)
{
// Size the island for the worst case.
_island.Reset(_bodyCount,
_contactManager._contactCount,
_jointCount,
_contactManager.ContactListener);
// Clear all the island flags.
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~BodyFlags.Island;
}
for (Contact c = _contactManager._contactList; c != null; c = c._next)
{
c._flags &= ~ContactFlags.Island;
}
for (Joint j = _jointList; j != null; j = j._next)
{
j._islandFlag = false;
}
// Build and simulate all awake islands.
//#warning Remove extra allocs
int stackSize = _bodyCount;
Body[] stack = new Body[_bodyCount];
for (Body seed = _bodyList; seed != null; seed = seed._next)
{
if ((seed._flags & (BodyFlags.Island | BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None)
{
continue;
}
if (seed.IsStatic)
{
continue;
}
// Reset island and stack.
_island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed._flags |= BodyFlags.Island;
// Perform a depth first search (DFS) on the raint 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 &= ~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 ce = b._contactList; ce != null; ce = ce.Next)
{
// Has this contact already been added to an island?
// Is this contact non-solid (involves a sensor).
if ((ce.Contact._flags & (ContactFlags.Island | ContactFlags.NonSolid)) != ContactFlags.None)
{
continue;
}
// Is this contact touching?
if ((ce.Contact._flags & ContactFlags.Touch) == ContactFlags.None)
{
continue;
}
_island.Add(ce.Contact);
ce.Contact._flags |= ContactFlags.Island;
Body other = ce.Other;
// Was the other body already added to this island?
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= BodyFlags.Island;
}
// Search all joints connect to this body.
for (JointEdge je = b._jointList; je != null; je = je.Next)
{
if (je.Joint._islandFlag == true)
{
continue;
}
_island.Add(je.Joint);
je.Joint._islandFlag = true;
Body other = je.Other;
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= BodyFlags.Island;
}
}
_island.Solve(ref 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 &= ~BodyFlags.Island;
}
}
}
// Synchronize fixtures, check for out of range bodies.
for (Body b = _bodyList; b != null; b = b.GetNext())
{
if ((b._flags & (BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None)
{
continue;
}
if (b.IsStatic)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
_contactManager.FindNewContacts();
}
void SolveTOI(ref TimeStep step)
{
// Reserve an island and a queue for TOI island solution.
_island.Reset( _bodyCount,
Settings.b2_maxTOIContactsPerIsland,
Settings.b2_maxTOIJointsPerIsland,
_contactManager.ContactListener);
//Simple one pass queue
//Relies on the fact that we're only making one pass
//through and each body can only be pushed/popped once.
//To push:
// queue[queueStart+queueSize++] = newElement;
//To pop:
// poppedElement = queue[queueStart++];
// --queueSize;
//#warning More Body array Allocs
int queueCapacity = _bodyCount;
Body[] queue = new Body[_bodyCount];
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~BodyFlags.Island;
b._sweep.t0 = 0.0f;
}
for (Contact c = _contactManager._contactList; c != null; c = c._next)
{
// Invalidate TOI
c._flags &= ~(ContactFlags.Toi | ContactFlags.Island);
}
for (Joint j = _jointList; j != null; j = j._next)
{
j._islandFlag = false;
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
Contact minContact = null;
float minTOI = 1.0f;
for (Contact c = _contactManager._contactList; c != null; c = c._next)
{
if ((c._flags & (ContactFlags.Slow | ContactFlags.NonSolid)) != ContactFlags.None)
{
continue;
}
// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
float toi = 1.0f;
if ((c._flags & ContactFlags.Toi) != ContactFlags.None)
{
// This contact has a valid cached TOI.
toi = c._toi;
}
else
{
// Compute the TOI for this contact.
Fixture s1 = c.GetFixtureA();
Fixture s2 = c.GetFixtureB();
Body b1 = s1.GetBody();
Body b2 = s2.GetBody();
if ((b1.IsStatic || b1.IsSleeping) && (b2.IsStatic || b2.IsSleeping))
{
continue;
}
// Put the sweeps onto the same time interval.
float t0 = b1._sweep.t0;
if (b1._sweep.t0 < b2._sweep.t0)
{
t0 = b2._sweep.t0;
b1._sweep.Advance(t0);
}
else if (b2._sweep.t0 < b1._sweep.t0)
{
t0 = b1._sweep.t0;
b2._sweep.Advance(t0);
}
Debug.Assert(t0 < 1.0f);
// Compute the time of impact.
toi = c.ComputeTOI(ref b1._sweep, ref b2._sweep);
//CalculateTimeOfImpact(c._fixtureA.GetShape(), b1._sweep, c._fixtureB.GetShape(), b2._sweep);
Debug.Assert(0.0f <= toi && toi <= 1.0f);
// If the TOI is in range ...
if (0.0f < toi && toi < 1.0f)
{
// Interpolate on the actual range.
toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f);
}
c._toi = toi;
c._flags |= ContactFlags.Toi;
}
if (Settings.b2_FLT_EPSILON < toi && toi < minTOI)
{
// This is the minimum TOI found so far.
minContact = c;
minTOI = toi;
}
}
if (minContact == null || 1.0f - 100.0f * Settings.b2_FLT_EPSILON < minTOI)
{
// No more TOI events. Done!
break;
}
// Advance the bodies to the TOI.
Fixture s1_2 = minContact.GetFixtureA();
Fixture s2_2 = minContact.GetFixtureB();
Body b1_2 = s1_2.GetBody();
Body b2_2 = s2_2.GetBody();
b1_2.Advance(minTOI);
b2_2.Advance(minTOI);
// The TOI contact likely has some new contact points.
minContact.Update(_contactManager.ContactListener);
minContact._flags &= ~ContactFlags.Toi;
if ((minContact._flags & ContactFlags.Touch) == 0)
{
// This shouldn't happen. Numerical error?
//Debug.Assert(false);
continue;
}
// Build the TOI island. We need a dynamic seed.
Body seed = b1_2;
if (seed.IsStatic)
{
seed = b2_2;
}
// Reset island and queue.
_island.Clear();
int queueStart = 0; // starting index for queue
int queueSize = 0; // elements in queue
queue[queueStart + queueSize++] = seed;
seed._flags |= BodyFlags.Island;
// Perform a breadth first search (BFS) on the contact/joint graph.
while (queueSize > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = queue[queueStart++];
--queueSize;
_island.Add(b);
// Make sure the body is awake.
b._flags &= ~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 cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next)
{
// Does the TOI island still have space for contacts?
if (_island._contacts.Count == _island._contactCapacity)
{
continue;
}
// Has this contact already been added to an island? Skip slow or non-solid contacts.
if ((cEdge.Contact._flags & (ContactFlags.Island | ContactFlags.Slow | ContactFlags.NonSolid)) != ContactFlags.None)
{
continue;
}
// Is this contact touching? For performance we are not updating this contact.
if ((cEdge.Contact._flags & ContactFlags.Touch) == 0)
{
continue;
}
_island.Add(cEdge.Contact);
cEdge.Contact._flags |= ContactFlags.Island;
// Update other body.
Body other = cEdge.Other;
// Was the other body already added to this island?
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
// March forward, this can do no harm since this is the min TOI.
if (other.IsStatic == false)
{
other.Advance(minTOI);
other.WakeUp();
}
Debug.Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other._flags |= BodyFlags.Island;
}
for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next)
{
if (_island._jointCount == _island._jointCapacity)
{
continue;
}
if (jEdge.Joint._islandFlag == true)
{
continue;
}
_island.Add(jEdge.Joint);
jEdge.Joint._islandFlag = true;
Body other = jEdge.Other;
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
if (!other.IsStatic)
{
other.Advance(minTOI);
other.WakeUp();
}
Debug.Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other._flags |= BodyFlags.Island;
}
}
TimeStep subStep;
subStep.warmStarting = false;
subStep.dt = (1.0f - minTOI) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 0.0f;
subStep.velocityIterations = step.velocityIterations;
subStep.positionIterations = step.positionIterations;
_island.SolveTOI(ref subStep);
// Post solve cleanup.
for (int i = 0; i < _island._bodyCount; ++i)
{
// Allow bodies to participate in future TOI islands.
Body b = _island._bodies[i];
b._flags &= ~BodyFlags.Island;
if ((b._flags & (BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None)
{
continue;
}
if (b.IsStatic)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
// Invalidate all contact TOIs associated with this body. Some of these
// may not be in the island because they were not touching.
for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
{
ce.Contact._flags &= ~ContactFlags.Toi;
}
}
int contactCount = _island._contacts.Count;
for (int i = 0; i < contactCount; ++i)
{
// Allow contacts to participate in future TOI islands.
Contact c = _island._contacts[i];
c._flags &= ~(ContactFlags.Toi | ContactFlags.Island);
}
for (int i = 0; i < _island._jointCount; ++i)
{
// Allow joints to participate in future TOI islands.
Joint j = _island._joints[i];
j._islandFlag = false;
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
_contactManager.FindNewContacts();
}
}
internal abstract void SolveVelocityConstraints(ref TimeStep step);
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
// Solve linear motor raint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit raint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector2 Cdot = new Vector2(Cdot1, Cdot2);
Vector2 f1 = _impulse;
Vector2 df = _K.Solve(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Y = Math.Max(_impulse.Y, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Y = Math.Min(_impulse.Y, 0.0f);
}
// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.col2.X;
float f2r = b / _K.col1.X + f1.X;
_impulse.X = f2r;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Y * _axis;
float L1 = df.X * _s1 + df.Y * _a1;
float L2 = df.X * _s2 + df.Y * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
else
{
// Limit is inactive, just solve the prismatic raint in block form.
float df = (-Cdot1) / _K.col1.X;
_impulse.X += df;
Vector2 P = df * _perp;
float L1 = df * _s1;
float L2 = df * _s2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
if (_state == LimitState.AtUpper)
{
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2);
float impulse = _pulleyMass * (-Cdot);
float oldImpulse = _impulse;
_impulse = Math.Max(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -_ratio * impulse * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpper)
{
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
float Cdot = -Vector2.Dot(_u1, v1);
float impulse = -_limitMass1 * Cdot;
float oldImpulse = _limitImpulse1;
_limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
impulse = _limitImpulse1 - oldImpulse;
Vector2 P1 = -impulse * _u1;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpper)
{
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = -Vector2.Dot(_u2, v2);
float impulse = -_limitMass2 * Cdot;
float oldImpulse = _limitImpulse2;
_limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
impulse = _limitImpulse2 - oldImpulse;
Vector2 P2 = -impulse * _u2;
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenter1);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenter2);
Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Debug.Assert(_motorMass > Settings.b2_FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic raint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.col1 = new Vector2(k11, k12);
_K.col2 = new Vector2(k12, k22);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Y = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.warmStarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse = Vector2.Zero;
_motorImpulse = 0.0f;
}
}
internal abstract void SolveVelocityConstraints(ref TimeStep step);
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
float Cdot = _J.Compute( b1._linearVelocity, b1._angularVelocity,
b2._linearVelocity, b2._angularVelocity);
float impulse = _mass * (-Cdot);
_impulse += impulse;
b1._linearVelocity += b1._invMass * impulse * _J.linear1;
b1._angularVelocity += b1._invI * impulse * _J.angular1;
b2._linearVelocity += b2._invMass * impulse * _J.linear2;
b2._angularVelocity += b2._invI * impulse * _J.angular2;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
// Solve motor raint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit raint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point raint
Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
v1 -= m1 * P;
w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z);
v2 += m2 * P;
w2 += i2 * (MathUtils.Cross(r2, P) + impulse.Z);
}
else
{
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point raint
Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
w1 -= i1 * MathUtils.Cross(r1, impulse);
v2 += m2 * impulse;
w2 += i2 * MathUtils.Cross(r2, impulse);
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
internal abstract void InitVelocityConstraints(ref TimeStep step);
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Debug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
}
// Compute the effective mass matrix.
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
_mass.col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.col1.Y = _mass.col2.X;
_mass.col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.col3.Y = r1.X * i1 + r2.X * i2;
_mass.col1.Z = _mass.col3.X;
_mass.col2.Z = _mass.col3.Y;
_mass.col3.Z = i1 + i2;
_motorMass = 1.0f / (i1 + i2);
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.a - b1._sweep.a - _referenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.b2_angularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
b1._linearVelocity -= m1 * P;
b1._angularVelocity -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z);
b2._linearVelocity += m2 * P;
b2._angularVelocity += i2 * (MathUtils.Cross(r2, P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.c + r2 - b1._sweep.c - r1;
// Handle singularity.
float length = _u.Length();
if (length > Settings.b2_linearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = new Vector2(0.0f, 0.0f);
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
Debug.Assert(invMass > Settings.b2_FLT_EPSILON);
_mass = 1.0f / invMass;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// Frequency
float omega = 2.0f * Settings.b2_pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * _mass * _dampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = 1.0f / (step.dt * (d + step.dt * k));
_bias = C * step.dt * k * _gamma;
_mass = 1.0f / (invMass + _gamma);
}
if (step.warmStarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
// Cdot = dot(u, v + cross(w, r))
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
// Solve linear motor raint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit raint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.col3.X, _K.col3.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float L1 = df.X * _s1 + df.Y + df.Z * _a1;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
else
{
// Limit is inactive, just solve the prismatic raint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float L1 = df.X * _s1 + df.Y;
float L2 = df.X * _s2 + df.Y;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
public void Solve(ref 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 = new Vector2(0.0f, 0.0f);
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 *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f);
}
_contactSolver.Reset(ref step, _contacts);
// Initialize velocity raints.
_contactSolver.InitVelocityConstraints(ref step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(ref step);
}
// Solve velocity raints.
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(ref 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.b2_maxTranslationSquared)
{
translation.Normalize();
b._linearVelocity = (Settings.b2_maxTranslation * step.inv_dt) * translation;
}
float rotation = step.dt * b._angularVelocity;
if (rotation * rotation > Settings.b2_maxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.inv_dt * Settings.b2_maxRotation;
}
else
{
b._angularVelocity = step.inv_dt * Settings.b2_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 raints.
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(_contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.b2_FLT_MAX;
#if !TARGET_float_IS_FIXED
float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & BodyFlags.AllowSleep) == 0 ||
#if TARGET_float_IS_FIXED
MathUtils.Abs(b._angularVelocity) > Settings.b2_angularSleepTolerance ||
MathUtils.Abs(b._linearVelocity.X) > Settings.b2_linearSleepTolerance ||
MathUtils.Abs(b._linearVelocity.Y) > Settings.b2_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 = Math.Min(minSleepTime, b._sleepTime);
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body g1 = _ground1;
Body g2 = _ground2;
Body b1 = _bodyA;
Body b2 = _bodyB;
float K = 0.0f;
_J.SetZero();
if (_revolute1 != null)
{
_J.angular1 = -1.0f;
K += b1._invI;
}
else
{
XForm xf1, xfg1;
b1.GetXForm(out xf1);
g1.GetXForm(out xfg1);
Vector2 ug = MathUtils.Multiply(ref xfg1.R, _prismatic1._localXAxis1);
Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
float crug = MathUtils.Cross(r, ug);
_J.linear1 = -ug;
_J.angular1 = -crug;
K += b1._invMass + b1._invI * crug * crug;
}
if (_revolute2 != null)
{
_J.angular2 = -_ratio;
K += _ratio * _ratio * b2._invI;
}
else
{
XForm xfg1, xf2;
g1.GetXForm(out xfg1);
b2.GetXForm(out xf2);
Vector2 ug = MathUtils.Multiply(ref xfg1.R, _prismatic2._localXAxis1);
Vector2 r = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
float crug = MathUtils.Cross(r, ug);
_J.linear2 = -_ratio * ug;
_J.angular2 = -_ratio * crug;
K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
}
// Compute effective mass.
Debug.Assert(K > 0.0f);
_mass = 1.0f / K;
if (step.warmStarting)
{
// Warm starting.
b1._linearVelocity += b1._invMass * _impulse * _J.linear1;
b1._angularVelocity += b1._invI * _impulse * _J.angular1;
b2._linearVelocity += b2._invMass * _impulse * _J.linear2;
b2._angularVelocity += b2._invI * _impulse * _J.angular2;
}
else
{
_impulse = 0.0f;
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = _bodyB;
float mass = b.GetMass();
// Frequency
float omega = 2.0f * Settings.b2_pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * mass * _dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Debug.Assert(d + step.dt * k > Settings.b2_FLT_EPSILON);
_gamma = 1.0f / (step.dt * (d + step.dt * k));
_beta = step.dt * k * _gamma;
// Compute the effective mass matrix.
XForm xf1;
b.GetXForm(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
float invMass = b._invMass;
float invI = b._invI;
Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));
Mat22 K;
Mat22.Add(ref K1, ref K2, out K);
K.col1.X += _gamma;
K.col2.Y += _gamma;
_mass = K.GetInverse();
_C = b._sweep.c + r - _target;
// Cheat with some damping
b._angularVelocity *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b._linearVelocity += invMass * _impulse;
b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 p2 = b2._sweep.c + r2;
Vector2 s1 = _groundAnchor1;
Vector2 s2 = _groundAnchor2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.b2_FLT_EPSILON);
Debug.Assert(_limitMass2 > Settings.b2_FLT_EPSILON);
Debug.Assert(_pulleyMass > Settings.b2_FLT_EPSILON);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (step.warmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b = _bodyB;
XForm xf1;
b.GetXForm(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());
// Cdot = v + cross(w, r)
Vector2 Cdot = b._linearVelocity + MathUtils.Cross(b._angularVelocity, r);
Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse));
Vector2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = step.dt * _maxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
b._linearVelocity += b._invMass * impulse;
b._angularVelocity += b._invI * MathUtils.Cross(r, impulse);
}
public void Solve(ref 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 = new Vector2(0.0f, 0.0f);
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 *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f);
}
_contactSolver.Reset(ref step, _contacts);
// Initialize velocity raints.
_contactSolver.InitVelocityConstraints(ref step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(ref step);
}
// Solve velocity raints.
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(ref 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.b2_maxTranslationSquared)
{
translation.Normalize();
b._linearVelocity = (Settings.b2_maxTranslation * step.inv_dt) * translation;
}
float rotation = step.dt * b._angularVelocity;
if (rotation * rotation > Settings.b2_maxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.inv_dt * Settings.b2_maxRotation;
}
else
{
b._angularVelocity = step.inv_dt * Settings.b2_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 raints.
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(_contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.b2_FLT_MAX;
#if !TARGET_float_IS_FIXED
float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & BodyFlags.AllowSleep) == 0 ||
#if TARGET_float_IS_FIXED
MathUtils.Abs(b._angularVelocity) > Settings.b2_angularSleepTolerance ||
MathUtils.Abs(b._linearVelocity.X) > Settings.b2_linearSleepTolerance ||
MathUtils.Abs(b._linearVelocity.Y) > Settings.b2_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 = Math.Min(minSleepTime, b._sleepTime);
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenter1);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenter2);
Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Debug.Assert(_motorMass > Settings.b2_FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic raint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.col1 = new Vector2(k11, k12);
_K.col2 = new Vector2(k12, k22);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Y = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.warmStarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse = Vector2.Zero;
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
// Solve linear motor raint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit raint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector2 Cdot = new Vector2(Cdot1, Cdot2);
Vector2 f1 = _impulse;
Vector2 df = _K.Solve(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Y = Math.Max(_impulse.Y, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Y = Math.Min(_impulse.Y, 0.0f);
}
// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.col2.X;
float f2r = b / _K.col1.X + f1.X;
_impulse.X = f2r;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Y * _axis;
float L1 = df.X * _s1 + df.Y * _a1;
float L2 = df.X * _s2 + df.Y * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
else
{
// Limit is inactive, just solve the prismatic raint in block form.
float df = (-Cdot1) / _K.col1.X;
_impulse.X += df;
Vector2 P = df * _perp;
float L1 = df * _s1;
float L2 = df * _s2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
internal abstract void InitVelocityConstraints(ref TimeStep step);
