private void SolveTOI(TimeStep step){
Island island = new Island(m_contactManager.m_contactListener);
if (m_stepComplete)
{
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
foreach (Contact c in m_contactManager.m_contactList)
{
// Invalidate TOI
c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
Fixture fA = null;
Fixture fB = null;
Body bA = null;
Body bB = null;
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
foreach (Contact c in m_contactManager.m_contactList)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > Settings._maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.m_flags.HasFlag(ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
fA = c.FixtureA;
fB = c.FixtureB;
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
bA = fA.GetBody();
bB = fB.GetBody();
BodyType typeA = bA.m_type;
BodyType typeB = bB.m_type;
Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody);
bool activeA = bA.IsAwake() && typeA != BodyType._staticBody;
bool activeB = bB.IsAwake() && typeB != BodyType._staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody;
bool collideB = bB.IsBullet() || typeB != BodyType._dynamicBody;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.m_sweep.alpha0;
if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0)
{
alpha0 = bB.m_sweep.alpha0;
bA.m_sweep.Advance(alpha0);
}
else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0)
{
alpha0 = bA.m_sweep.alpha0;
bB.m_sweep.Advance(alpha0);
}
Utilities.Assert(alpha0 < 1.0f);
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.proxyA.Set(fA.GetShape(), indexA);
input.proxyB.Set(fB.GetShape(), indexB);
input.sweepA = bA.m_sweep;
input.sweepB = bB.m_sweep;
input.tMax = 1.0f;
TOIOutput output;
Utilities.TimeOfImpact(out output, input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == TOIOutput.State.e_touching)
{
alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.m_flags |= ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
fA = minContact.FixtureA;
fB = minContact.FixtureB;
bA = fA.GetBody();
bB = fB.GetBody();
Sweep backup1 = bA.m_sweep;
Sweep backup2 = bB.m_sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.m_contactListener);
minContact.m_flags &= ~ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.m_sweep = backup1;
bB.m_sweep = backup2;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
continue;
}
bA.SetAwake(true);
bB.SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA.m_flags |= Body.BodyFlags.e_islandFlag;
bB.m_flags |= Body.BodyFlags.e_islandFlag;
minContact.m_flags |= ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
Body[] bodies = {bA, bB};
for (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.m_type == BodyType._dynamicBody)
{
foreach (ContactEdge ce in body.m_contactList)
{
throw new NotImplementedException();
//if (island.m_bodies.Count() == island.m_bodyCapacity)
//{
// break;
//}
//if (island.m_bodies.Count() == island.m_contactCapacity)
//{
// break;
//}
//Contact* contact = ce.contact;
//// Has this contact already been added to the island?
//if (contact.m_flags & ContactFlags.e_islandFlag)
//{
// continue;
//}
//// Only add static, kinematic, or bullet bodies.
//Body* other = ce.other;
//if (other.m_type == _dynamicBody &&
// body.IsBullet() == false && other.IsBullet() == false)
//{
// continue;
//}
//// Skip sensors.
//bool sensorA = contact.m_fixtureA.m_isSensor;
//bool sensorB = contact.m_fixtureB.m_isSensor;
//if (sensorA || sensorB)
//{
// continue;
//}
//// Tentatively advance the body to the TOI.
//Sweep backup = other.m_sweep;
//if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0)
//{
// other.Advance(minAlpha);
//}
//// Update the contact points
//contact.Update(m_contactManager.m_contactListener);
//// Was the contact disabled by the user?
//if (contact.IsEnabled() == false)
//{
// other.m_sweep = backup;
// other.SynchronizeTransform();
// continue;
//}
//// Are there contact points?
//if (contact.IsTouching() == false)
//{
// other.m_sweep = backup;
// other.SynchronizeTransform();
// continue;
//}
//// Add the contact to the island
//contact.m_flags |= ContactFlags.e_islandFlag;
//island.Add(contact);
//// Has the other body already been added to the island?
//if (other.m_flags & Body.BodyFlags.e_islandFlag)
//{
// continue;
//}
//// Add the other body to the island.
//other.m_flags |= Body.BodyFlags.e_islandFlag;
//if (other.m_type != _staticBody)
//{
// other.SetAwake(true);
//}
//island.Add(other);
}
}
}
TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodies.Count(); ++i)
{
throw new NotImplementedException();
//Body* body = island.m_bodies[i];
//body.m_flags &= ~Body.BodyFlags.e_islandFlag;
//if (body.m_type != _dynamicBody)
//{
// continue;
//}
//body.SynchronizeFixtures();
//// Invalidate all contact TOIs on this displaced body.
//for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next)
//{
// ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
//}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
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.
int stackSize = _bodyCount;
if (stackSize > stack.Length)
{
stack = new Body[Math.Max(stack.Length * 2, stackSize)];
}
for (Body seed = _bodyList; seed != null; seed = seed._next)
{
if ((seed._flags & (BodyFlags.Island)) != BodyFlags.None)
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.GetType() == BodyType.Static)
{
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];
//Debug.Assert(b.IsActive() == true);
_island.Add(b);
// Make sure the body is awake.
b.SetAwake(true);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.GetType() == BodyType.Static)
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
// Has this contact already been added to an island?
if ((contact._flags & ContactFlags.Island) != ContactFlags.None)
{
continue;
}
// Is this contact solid and touching?
if (!ce.Contact.IsEnabled() || !ce.Contact.IsTouching())
{
continue;
}
// Skip sensors.
bool sensorA = contact._fixtureA._isSensor;
bool sensorB = contact._fixtureB._isSensor;
if (sensorA || sensorB)
{
continue;
}
_island.Add(contact);
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;
}
Body other = je.Other;
// Don't simulate joints connected to inactive bodies.
if (other.IsActive() == false)
{
continue;
}
_island.Add(je.Joint);
je.Joint._islandFlag = true;
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.GetType() == BodyType.Static)
{
b._flags &= ~BodyFlags.Island;
}
}
}
// Synchronize fixtures, check for out of range bodies.
for (Body b = _bodyList; b != null; b = b.GetNext())
{
// If a body was not in an island then it did not move.
if ((b._flags & BodyFlags.Island) != BodyFlags.Island)
{
continue;
}
if (b.GetType() == BodyType.Static)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
_contactManager.FindNewContacts();
}
private void Solve(TimeStep step){
m_profile.solveInit = 0.0f;
m_profile.solveVelocity = 0.0f;
m_profile.solvePosition = 0.0f;
// Size the island for the worst case.
Island island = new Island(m_contactManager.m_contactListener);
// Clear all the island flags.
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
foreach (Contact c in m_contactManager.m_contactList)
{
c.m_flags &= ~ContactFlags.e_islandFlag;
}
foreach (Joint j in m_jointList)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
List<Body> stack = new List<Body>(m_bodyList.Count());
foreach (Body seed in m_bodyList)
{
if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack.Add(seed); stackCount++;
seed.m_flags |= Body.BodyFlags.e_islandFlag;
// 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];
Utilities.Assert(b.IsActive() == true);
island.Add(b);
// Make sure the body is awake.
b.SetAwake(true);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Search all contacts connected to this body.
foreach (ContactEdge ce in b.m_contactList)
{
Contact contact = ce.contact;
// Has this contact already been added to an island?
if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag))
{
continue;
}
// Is this contact solid and touching?
if (contact.IsEnabled() == false ||
contact.IsTouching() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
island.Add(contact);
contact.m_flags |= ContactFlags.e_islandFlag;
Body other = ce.other;
// Was the other body already added to this island?
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
Utilities.Assert(stackCount < m_bodyList.Count());
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
// Search all joints connect to this body.
foreach (JointEdge je in b.m_jointList){
if (je.joint.m_islandFlag == true) {
continue;
}
Body other = je.other;
// Don't simulate joints connected to inactive bodies.
if (other.IsActive() == false) {
continue;
}
island.Add(je.joint);
je.joint.m_islandFlag = true;
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) {
continue;
}
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
}
Profile profile = new Profile();
island.Solve(profile, step, m_gravity, m_allowSleep);
m_profile.solveInit += profile.solveInit;
m_profile.solveVelocity += profile.solveVelocity;
m_profile.solvePosition += profile.solvePosition;
// Post solve cleanup.
for (int i = 0; i < island.m_bodies.Count(); ++i) {
// Allow static bodies to participate in other islands.
Body b = island.m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody) {
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
}
}
{
Timer timer = new Timer();
// Synchronize fixtures, check for out of range bodies.
foreach (Body b in m_bodyList)
{
// If a body was not in an island then it did not move.
if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0)
{
continue;
}
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
m_profile.broadphase = timer.GetMilliseconds();
}
}
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 constraint.
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 -= _invMassA * P;
w1 -= _invIA * L1;
v2 += _invMassB * P;
w2 += _invIB * L2;
}
float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint 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;
if (_K.col1.x != 0.0f)
{
f2r = b / _K.col1.x + f1.x;
}
else
{
f2r = 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 -= _invMassA * P;
w1 -= _invIA * L1;
v2 += _invMassB * P;
w2 += _invIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
float df;
if (_K.col1.x != 0.0f)
{
df = -Cdot1 / _K.col1.x;
}
else
{
df = 0.0f;
}
_impulse.x += df;
Vector2 P = df * _perp;
float L1 = df * _s1;
float L2 = df * _s2;
v1 -= _invMassA * P;
w1 -= _invIA * L1;
v2 += _invMassB * P;
w2 += _invIB * L2;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(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.magnitude;
if (length < _length)
{
return;
}
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_epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
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 = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
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;
}
}
private void Solve(TimeStep step)
{
m_profile.solveInit = 0.0f;
m_profile.solveVelocity = 0.0f;
m_profile.solvePosition = 0.0f;
// Size the island for the worst case.
Island island = new Island(m_contactManager.m_contactListener);
// Clear all the island flags.
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
foreach (Contact c in m_contactManager.m_contactList)
{
c.m_flags &= ~ContactFlags.e_islandFlag;
}
foreach (Joint j in m_jointList)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
List <Body> stack = new List <Body>(m_bodyList.Count());
foreach (Body seed in m_bodyList)
{
if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack.Add(seed); stackCount++;
seed.m_flags |= Body.BodyFlags.e_islandFlag;
// 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];
Utilities.Assert(b.IsActive() == true);
island.Add(b);
// Make sure the body is awake.
b.SetAwake(true);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Search all contacts connected to this body.
foreach (ContactEdge ce in b.m_contactList)
{
Contact contact = ce.contact;
// Has this contact already been added to an island?
if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag))
{
continue;
}
// Is this contact solid and touching?
if (contact.IsEnabled() == false ||
contact.IsTouching() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
island.Add(contact);
contact.m_flags |= ContactFlags.e_islandFlag;
Body other = ce.other;
// Was the other body already added to this island?
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
Utilities.Assert(stackCount < m_bodyList.Count());
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
// Search all joints connect to this body.
foreach (JointEdge je in b.m_jointList)
{
if (je.joint.m_islandFlag == true)
{
continue;
}
Body other = je.other;
// Don't simulate joints connected to inactive bodies.
if (other.IsActive() == false)
{
continue;
}
island.Add(je.joint);
je.joint.m_islandFlag = true;
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
}
Profile profile = new Profile();
island.Solve(profile, step, m_gravity, m_allowSleep);
m_profile.solveInit += profile.solveInit;
m_profile.solveVelocity += profile.solveVelocity;
m_profile.solvePosition += profile.solvePosition;
// Post solve cleanup.
for (int i = 0; i < island.m_bodies.Count(); ++i)
{
// Allow static bodies to participate in other islands.
Body b = island.m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
}
}
{
Timer timer = new Timer();
// Synchronize fixtures, check for out of range bodies.
foreach (Body b in m_bodyList)
{
// If a body was not in an island then it did not move.
if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0)
{
continue;
}
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
m_profile.broadphase = timer.GetMilliseconds();
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
_localCenterA = b1.GetLocalCenter();
_localCenterB = b2.GetLocalCenter();
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenterA);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenterB);
Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;
_invMassA = b1._invMass;
_invIA = b1._invI;
_invMassB = b2._invMass;
_invIB = 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 = _invMassA + _invMassB + _invIA * _a1 * _a1 + _invIB * _a2 * _a2;
if (_motorMass > Settings.b2_epsilon)
{
_motorMass = 1.0f / _motorMass;
}
else
{
_motorMass = 0.0f;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localyAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = _invMassA, m2 = _invMassB;
float i1 = _invIA, i2 = _invIB;
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 -= _invMassA * P;
b1._angularVelocity -= _invIA * L1;
b2._linearVelocity += _invMassB * P;
b2._angularVelocity += _invIB * L2;
}
else
{
_impulse = Vector2.zero;
_motorImpulse = 0.0f;
}
}
private void SolveTOI(TimeStep step)
{
Island island = new Island(m_contactManager.m_contactListener);
if (m_stepComplete)
{
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
foreach (Contact c in m_contactManager.m_contactList)
{
// Invalidate TOI
c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
Fixture fA = null;
Fixture fB = null;
Body bA = null;
Body bB = null;
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
foreach (Contact c in m_contactManager.m_contactList)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > Settings._maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.m_flags.HasFlag(ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
fA = c.FixtureA;
fB = c.FixtureB;
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
bA = fA.GetBody();
bB = fB.GetBody();
BodyType typeA = bA.m_type;
BodyType typeB = bB.m_type;
Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody);
bool activeA = bA.IsAwake() && typeA != BodyType._staticBody;
bool activeB = bB.IsAwake() && typeB != BodyType._staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody;
bool collideB = bB.IsBullet() || typeB != BodyType._dynamicBody;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.m_sweep.alpha0;
if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0)
{
alpha0 = bB.m_sweep.alpha0;
bA.m_sweep.Advance(alpha0);
}
else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0)
{
alpha0 = bA.m_sweep.alpha0;
bB.m_sweep.Advance(alpha0);
}
Utilities.Assert(alpha0 < 1.0f);
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.proxyA.Set(fA.GetShape(), indexA);
input.proxyB.Set(fB.GetShape(), indexB);
input.sweepA = bA.m_sweep;
input.sweepB = bB.m_sweep;
input.tMax = 1.0f;
TOIOutput output;
Utilities.TimeOfImpact(out output, input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == TOIOutput.State.e_touching)
{
alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.m_flags |= ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
fA = minContact.FixtureA;
fB = minContact.FixtureB;
bA = fA.GetBody();
bB = fB.GetBody();
Sweep backup1 = bA.m_sweep;
Sweep backup2 = bB.m_sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.m_contactListener);
minContact.m_flags &= ~ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.m_sweep = backup1;
bB.m_sweep = backup2;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
continue;
}
bA.SetAwake(true);
bB.SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA.m_flags |= Body.BodyFlags.e_islandFlag;
bB.m_flags |= Body.BodyFlags.e_islandFlag;
minContact.m_flags |= ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
Body[] bodies = { bA, bB };
for (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.m_type == BodyType._dynamicBody)
{
foreach (ContactEdge ce in body.m_contactList)
{
throw new NotImplementedException();
//if (island.m_bodies.Count() == island.m_bodyCapacity)
//{
// break;
//}
//if (island.m_bodies.Count() == island.m_contactCapacity)
//{
// break;
//}
//Contact* contact = ce.contact;
//// Has this contact already been added to the island?
//if (contact.m_flags & ContactFlags.e_islandFlag)
//{
// continue;
//}
//// Only add static, kinematic, or bullet bodies.
//Body* other = ce.other;
//if (other.m_type == _dynamicBody &&
// body.IsBullet() == false && other.IsBullet() == false)
//{
// continue;
//}
//// Skip sensors.
//bool sensorA = contact.m_fixtureA.m_isSensor;
//bool sensorB = contact.m_fixtureB.m_isSensor;
//if (sensorA || sensorB)
//{
// continue;
//}
//// Tentatively advance the body to the TOI.
//Sweep backup = other.m_sweep;
//if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0)
//{
// other.Advance(minAlpha);
//}
//// Update the contact points
//contact.Update(m_contactManager.m_contactListener);
//// Was the contact disabled by the user?
//if (contact.IsEnabled() == false)
//{
// other.m_sweep = backup;
// other.SynchronizeTransform();
// continue;
//}
//// Are there contact points?
//if (contact.IsTouching() == false)
//{
// other.m_sweep = backup;
// other.SynchronizeTransform();
// continue;
//}
//// Add the contact to the island
//contact.m_flags |= ContactFlags.e_islandFlag;
//island.Add(contact);
//// Has the other body already been added to the island?
//if (other.m_flags & Body.BodyFlags.e_islandFlag)
//{
// continue;
//}
//// Add the other body to the island.
//other.m_flags |= Body.BodyFlags.e_islandFlag;
//if (other.m_type != _staticBody)
//{
// other.SetAwake(true);
//}
//island.Add(other);
}
}
}
TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodies.Count(); ++i)
{
throw new NotImplementedException();
//Body* body = island.m_bodies[i];
//body.m_flags &= ~Body.BodyFlags.e_islandFlag;
//if (body.m_type != _dynamicBody)
//{
// continue;
//}
//body.SynchronizeFixtures();
//// Invalidate all contact TOIs on this displaced body.
//for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next)
//{
// ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
//}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
public void SolveTOI(TimeStep subStep, int toiIndexA, int toiIndexB) {
throw new NotImplementedException();
// Utilities.Assert(toiIndexA < m_bodies.Count());
// Utilities.Assert(toiIndexB < m_bodies.Count());
// // Initialize the body state.
// for (int i = 0; i < m_bodies.Count(); ++i)
// {
// Body* b = m_bodies[i];
// m_positions[i].c = b.m_sweep.c;
// m_positions[i].a = b.m_sweep.a;
// m_velocities[i].v = b.m_linearVelocity;
// m_velocities[i].w = b.m_angularVelocity;
// }
// ContactSolverDef contactSolverDef;
// contactSolverDef.contacts = m_contacts;
// contactSolverDef.count = m_contactCount;
// contactSolverDef.allocator = m_allocator;
// contactSolverDef.step = subStep;
// contactSolverDef.positions = m_positions;
// contactSolverDef.velocities = m_velocities;
// ContactSolver contactSolver(&contactSolverDef);
// // Solve position constraints.
// for (int i = 0; i < subStep.positionIterations; ++i)
// {
// bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
// if (contactsOkay)
// {
// break;
// }
// }
//#if ZERO
// // Is the new position really safe?
// for (int i = 0; i < m_contactCount; ++i)
// {
// Contact* c = m_contacts[i];
// Fixture fA = c.FixtureA;
// Fixture fB = c.FixtureB;
// Body* bA = fA.GetBody();
// Body* bB = fB.GetBody();
// int indexA = c.GetChildIndexA();
// int indexB = c.GetChildIndexB();
// DistanceInput input;
// input.proxyA.Set(fA.GetShape(), indexA);
// input.proxyB.Set(fB.GetShape(), indexB);
// input.transformA = bA.GetTransform();
// input.transformB = bB.GetTransform();
// input.useRadii = false;
// DistanceOutput output;
// SimplexCache cache;
// cache.count = 0;
// Utilities.Distance(out output, cache, input);
// if (output.distance == 0 || cache.count == 3)
// {
// cache.count += 0;
// }
// }
//#endif
// // Leap of faith to new safe state.
// m_bodies[toiIndexA].m_sweep.c0 = m_positions[toiIndexA].c;
// m_bodies[toiIndexA].m_sweep.a0 = m_positions[toiIndexA].a;
// m_bodies[toiIndexB].m_sweep.c0 = m_positions[toiIndexB].c;
// m_bodies[toiIndexB].m_sweep.a0 = m_positions[toiIndexB].a;
// // No warm starting is needed for TOI events because warm
// // starting impulses were applied in the discrete solver.
// contactSolver.InitializeVelocityConstraints();
// // Solve velocity constraints.
// for (int i = 0; i < subStep.velocityIterations; ++i)
// {
// contactSolver.SolveVelocityConstraints();
// }
// // Don't store the TOI contact forces for warm starting
// // because they can be quite large.
// float h = subStep.dt;
// // Integrate positions
// for (int i = 0; i < m_bodies.Count(); ++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
// Vec2 translation = h * v;
// if (Utilities.Dot(translation, translation) > _maxTranslationSquared)
// {
// float ratio = _maxTranslation / translation.Length();
// v *= ratio;
// }
// float rotation = h * w;
// if (rotation * rotation > _maxRotationSquared)
// {
// float ratio = _maxRotation / Math.Abs(rotation);
// w *= ratio;
// }
// // Integrate
// c += h * v;
// a += h * w;
// m_positions[i].c = c;
// m_positions[i].a = a;
// m_velocities[i].v = v;
// m_velocities[i].w = w;
// // Sync bodies
// Body* body = m_bodies[i];
// body.m_sweep.c = c;
// body.m_sweep.a = a;
// body.m_linearVelocity = v;
// body.m_angularVelocity = w;
// body.SynchronizeTransform();
// }
// Report(contactSolver.m_velocityConstraints);
}
public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep) {
Timer timer = new Timer();
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodies.Count(); 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 = b.m_sweep.c;
b.m_sweep.a0 = b.m_sweep.a;
if (b.m_type == BodyType._dynamicBody)
{
// Integrate velocities.
v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
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 *= Utilities.Clamp(1.0f - h * b.m_linearDamping, 0.0f, 1.0f);
w *= Utilities.Clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
}
Position pos = new Position();
pos.c = c;
pos.a = a;
m_positions.Add(pos);
Velocity vel = new Velocity();
vel.v = v;
vel.w = w;
m_velocities.Add(vel);
}
timer.Reset();
// Solver data
SolverData solverData;
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
ContactSolver contactSolver = new ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_joints.Count(); ++i)
{
m_joints[i].InitVelocityConstraints(solverData);
}
profile.solveInit = timer.GetMilliseconds();
// Solve velocity constraints
timer.Reset();
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < m_joints.Count(); ++j)
{
m_joints[j].SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile.solveVelocity = timer.GetMilliseconds();
// Integrate positions
for (int i = 0; i < m_bodies.Count(); ++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
Vec2 translation = h * v;
if (Utilities.Dot(translation, translation) > Settings._maxTranslationSquared)
{
float ratio = Settings._maxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings._maxRotationSquared)
{
float ratio = Settings._maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
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_joints.Count; ++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_bodies.Count(); ++i)
{
Body body = m_bodies[i];
body.m_sweep.c = m_positions[i].c;
body.m_sweep.a = m_positions[i].a;
body.m_linearVelocity = m_velocities[i].v;
body.m_angularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
profile.solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = Single.MaxValue;
const float linTolSqr = Settings._linearSleepTolerance * Settings._linearSleepTolerance;
const float angTolSqr = Settings._angularSleepTolerance * Settings._angularSleepTolerance;
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0 ||
b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
Utilities.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
{
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.m_sleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.m_sleepTime);
}
}
if (minSleepTime >= Settings._timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = _bodyA;
Body bB = _bodyB;
Vector2 vA = bA._linearVelocity;
float wA = bA._angularVelocity;
Vector2 vB = bB._linearVelocity;
float wB = bB._angularVelocity;
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = step.dt * _maxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = step.dt * _maxForce;
if (_linearImpulse.sqrMagnitude > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(rB, impulse);
}
bA._linearVelocity = vA;
bA._angularVelocity = wA;
bB._linearVelocity = vB;
bB._angularVelocity = wB;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = _bodyA;
Body bB = _bodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Mat22 K1 = new Mat22();
K1.col1.x = mA + mB; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = mA + mB;
Mat22 K2 = new Mat22();
K2.col1.x = iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y;
K2.col1.y = -iA * rA.x * rA.y; K2.col2.y = iA * rA.x * rA.x;
Mat22 K3 = new Mat22();
K3.col1.x = iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y;
K3.col1.y = -iB * rB.x * rB.y; K3.col2.y = iB * rB.x * rB.x;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
Mat22 K;
Mat22.Add(ref K12, ref K3, out K);
_linearMass = K.GetInverse();
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);
bA._linearVelocity -= mA * P;
bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
bB._linearVelocity += mB * P;
bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.zero;
_angularImpulse = 0.0f;
}
}