internal Fixture()
{
_userData = null;
_body = null;
_next = null;
_proxyId = BroadPhase.NullProxy;
_shape = null;
}
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
public void Initialize(Body b1, Body b2,
Vector2 ga1, Vector2 ga2,
Vector2 anchor1, Vector2 anchor2,
float r)
{
body1 = b1;
body2 = b2;
groundAnchor1 = ga1;
groundAnchor2 = ga2;
localAnchor1 = body1.GetLocalPoint(anchor1);
localAnchor2 = body2.GetLocalPoint(anchor2);
Vector2 d1 = anchor1 - ga1;
length1 = d1.Length();
Vector2 d2 = anchor2 - ga2;
length2 = d2.Length();
ratio = r;
Debug.Assert(ratio > Settings.b2_FLT_EPSILON);
float C = length1 + ratio * length2;
maxLength1 = C - ratio * b2_minPulleyLength;
maxLength2 = (C - b2_minPulleyLength) / ratio;
}
internal GearJoint(GearJointDef def)
: base(def)
{
JointType type1 = def.joint1.JointType;
JointType type2 = def.joint2.JointType;
Debug.Assert(type1 == JointType.Revolute || type1 == JointType.Prismatic);
Debug.Assert(type2 == JointType.Revolute || type2 == JointType.Prismatic);
Debug.Assert(def.joint1.GetBody1().IsStatic);
Debug.Assert(def.joint2.GetBody1().IsStatic);
_revolute1 = null;
_prismatic1 = null;
_revolute2 = null;
_prismatic2 = null;
float coordinate1, coordinate2;
_ground1 = def.joint1.GetBody1();
_bodyA = def.joint1.GetBody2();
if (type1 == JointType.Revolute)
{
_revolute1 = (RevoluteJoint)def.joint1;
_groundAnchor1 = _revolute1._localAnchor1;
_localAnchor1 = _revolute1._localAnchor2;
coordinate1 = _revolute1.GetJointAngle();
}
else
{
_prismatic1 = (PrismaticJoint)def.joint1;
_groundAnchor1 = _prismatic1._localAnchor1;
_localAnchor1 = _prismatic1._localAnchor2;
coordinate1 = _prismatic1.GetJointTranslation();
}
_ground2 = def.joint2.GetBody1();
_bodyB = def.joint2.GetBody2();
if (type2 == JointType.Revolute)
{
_revolute2 = (RevoluteJoint)def.joint2;
_groundAnchor2 = _revolute2._localAnchor1;
_localAnchor2 = _revolute2._localAnchor2;
coordinate2 = _revolute2.GetJointAngle();
}
else
{
_prismatic2 = (PrismaticJoint)def.joint2;
_groundAnchor2 = _prismatic2._localAnchor1;
_localAnchor2 = _prismatic2._localAnchor2;
coordinate2 = _prismatic2.GetJointTranslation();
}
_ratio = def.ratio;
_ant = coordinate1 + _ratio * coordinate2;
_impulse = 0.0f;
}
// We need separation create/destroy functions from the ructor/destructor because
// the destructor cannot access the allocator or broad-phase (no destructor arguments allowed by C++).
internal void Create(BroadPhase broadPhase, Body body, ref XForm xf, FixtureDef def)
{
_userData = def.userData;
_friction = def.friction;
_restitution = def.restitution;
_density = def.density;
_body = body;
_next = null;
_filter = def.filter;
_isSensor = def.isSensor;
_shape = def.shape.Clone();
// Create proxy in the broad-phase.
AABB aabb;
_shape.ComputeAABB(out aabb, ref xf);
_proxyId = broadPhase.CreateProxy(ref aabb, this);
}
// This is used to prevent connected bodies from colliding.
// It may lie, depending on the collideConnected flag.
internal bool IsConnected(Body other)
{
for (JointEdge jn = _jointList; jn != null; jn = jn.Next)
{
if (jn.Other == other)
{
return jn.Joint._collideConnected == false;
}
}
return false;
}
internal XForm _xf; // the body origin transform
#endregion Fields
#region Constructors
internal Body(BodyDef bd, World world)
{
_flags = 0;
if (bd.isBullet)
{
_flags |= BodyFlags.Bullet;
}
if (bd.fixedRotation)
{
_flags |= BodyFlags.FixedRotation;
}
if (bd.allowSleep)
{
_flags |= BodyFlags.AllowSleep;
}
if (bd.isSleeping)
{
_flags |= BodyFlags.Sleep;
}
_world = world;
_xf.Position = bd.position;
_xf.R.Set(bd.angle);
_sweep.localCenter = bd.massData.center;
_sweep.t0 = 1.0f;
_sweep.a0 = _sweep.a = bd.angle;
_sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);
_jointList = null;
_contactList = null;
_prev = null;
_next = null;
_linearVelocity = bd.linearVelocity;
_angularVelocity = bd.angularVelocity;
_linearDamping = bd.linearDamping;
_angularDamping = bd.angularDamping;
_force = new Vector2(0.0f, 0.0f);
_torque = 0.0f;
_linearVelocity = Vector2.Zero;
_angularVelocity = 0.0f;
_sleepTime = 0.0f;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
_mass = bd.massData.mass;
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
}
_I = bd.massData.i;
if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
_invI = 1.0f / _I;
}
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
_userData = bd.userData;
_fixtureList = null;
_fixtureCount = 0;
}
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();
}
protected Joint(JointDef def)
{
_type = def.type;
_bodyA = def.body1;
_bodyB = def.body2;
_collideConnected = def.collideConnected;
_userData = def.userData;
_edgeA = new JointEdge();
_edgeB = new JointEdge();
}
/// Create a rigid body given a definition. No reference to the definition
/// is retained.
/// @warning This function is locked during callbacks.
public Body CreateBody(BodyDef def)
{
Debug.Assert(!IsLocked);
if (IsLocked)
{
return null;
}
var b = new Body(def, this);
// Add to world doubly linked list.
b._prev = null;
b._next = _bodyList;
if (_bodyList != null)
{
_bodyList._prev = b;
}
_bodyList = b;
++_bodyCount;
return b;
}
/// Destroy a rigid body given a definition. No reference to the definition
/// is retained. This function is locked during callbacks.
/// @warning This automatically deletes all associated shapes and joints.
/// @warning This function is locked during callbacks.
public void DestroyBody(Body b)
{
Debug.Assert(_bodyCount > 0);
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
// Delete the attached joints.
JointEdge je = b._jointList;
while (je != null)
{
JointEdge je0 = je;
je = je.Next;
if (DestructionListener != null)
{
DestructionListener.SayGoodbye(je0.Joint);
}
DestroyJoint(je0.Joint);
}
b._jointList = null;
// Delete the attached contacts.
ContactEdge ce = b._contactList;
while (ce != null)
{
ContactEdge ce0 = ce;
ce = ce.Next;
_contactManager.Destroy(ce0.Contact);
}
b._contactList = null;
// Delete the attached fixtures. This destroys broad-phase proxies.
Fixture f = b._fixtureList;
while (f != null)
{
Fixture f0 = f;
f = f._next;
if (DestructionListener != null)
{
DestructionListener.SayGoodbye(f0);
}
f0.Destroy(_contactManager._broadPhase);
}
b._fixtureList = null;
b._fixtureCount = 0;
// Remove world body list.
if (b._prev != null)
{
b._prev._next = b._next;
}
if (b._next != null)
{
b._next._prev = b._prev;
}
if (b == _bodyList)
{
_bodyList = b._next;
}
--_bodyCount;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 c1 = b1._sweep.c;
float a1 = b1._sweep.a;
Vector2 c2 = b2._sweep.c;
float a2 = b2._sweep.a;
// Solve linear limit raint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, _localAnchor1 - _localCenter1);
Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenter2);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.b2_linearSlop, 0.0f, Settings.b2_maxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector2 impulse;
float C1;
C1 = Vector2.Dot(_perp, d);
linearError = Math.Max(linearError, Math.Abs(C1));
angularError = 0.0f;
if (active)
{
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);
Vector2 C = new Vector2(-C1, -C2);
impulse = _K.Solve(C); //note i inverted above
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1 = (-C1) / k11;
impulse.X = impulse1;
impulse.Y = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Y * _axis;
float L1 = impulse.X * _s1 + impulse.Y * _a1;
float L2 = impulse.X * _s2 + impulse.Y * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.c = c1;
b1._sweep.a = a1;
b2._sweep.c = c2;
b2._sweep.a = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
}
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 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;
}
}
public void Add(Body body)
{
Debug.Assert(_bodyCount < _bodyCapacity);
body._islandIndex = _bodyCount;
_bodies[_bodyCount++] = body;
}
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();
}
}
/// Initialize the bodies, anchors, axis, and reference angle using the world
/// anchor and world axis.
public void Initialize(Body b1, Body b2, Vector2 anchor, Vector2 axis)
{
body1 = b1;
body2 = b2;
localAnchor1 = body1.GetLocalPoint(anchor);
localAnchor2 = body2.GetLocalPoint(anchor);
localAxis1 = body1.GetLocalVector(axis);
}
/// Initialize the bodies, anchors, and length using the world
/// anchors.
// 1-D rained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
public void Initialize(Body b1, Body b2,
Vector2 anchor1, Vector2 anchor2)
{
body1 = b1;
body2 = b2;
localAnchor1 = body1.GetLocalPoint(anchor1);
localAnchor2 = body2.GetLocalPoint(anchor2);
Vector2 d = anchor2 - anchor1;
length = d.Length();
}
