public Contact(Shape s1, Shape s2) { this._flags = (Contact.CollisionFlags) 0; if (s1.IsSensor || s2.IsSensor) { this._flags |= Contact.CollisionFlags.NonSolid; } this._shape1 = s1; this._shape2 = s2; this._manifoldCount = 0; this._prev = null; this._next = null; this._node1 = new ContactEdge(); this._node1.Contact = null; this._node1.Prev = null; this._node1.Next = null; this._node1.Other = null; this._node2 = new ContactEdge(); this._node2.Contact = null; this._node2.Prev = null; this._node2.Next = null; this._node2.Other = null; }
public Contact(Shape s1, Shape s2) { this._flags = (Contact.CollisionFlags)0; if (s1.IsSensor || s2.IsSensor) { this._flags |= Contact.CollisionFlags.NonSolid; } this._shape1 = s1; this._shape2 = s2; this._manifoldCount = 0; this._prev = null; this._next = null; this._node1 = new ContactEdge(); this._node1.Contact = null; this._node1.Prev = null; this._node1.Next = null; this._node1.Other = null; this._node2 = new ContactEdge(); this._node2.Contact = null; this._node2.Prev = null; this._node2.Next = null; this._node2.Other = null; }
public void SetFilterData(Filter filter) { Filter = filter; if (Body == null) { return; } // Flag associated contacts for filtering. ContactEdge edge = Body.GetContactList(); while (edge != null) { Contact contact = edge.Contact; Fixture fixtureA = contact.GetFixtureA(); Fixture fixtureB = contact.GetFixtureB(); if (fixtureA == this || fixtureB == this) { contact.FlagForFiltering(); } } }
public void AddPair(object proxyUserDataA, object proxyUserDataB) { Fixture fixtureA = (Fixture)proxyUserDataA; Fixture fixtureB = (Fixture)proxyUserDataB; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); // Are the fixtures on the same body? if (bodyA == bodyB) { return; } // Are both bodies static? if (bodyA.IsStatic() && bodyB.IsStatic()) { return; } // Does a contact already exist? ContactEdge edge = bodyB.GetContactList(); while (edge != null) { if (edge.Other == bodyA) { Fixture fA = edge.Contact.GetFixtureA(); Fixture fB = edge.Contact.GetFixtureB(); if (fA == fixtureA && fB == fixtureB) { // A contact already exists. return; } if (fA == fixtureB && fB == fixtureA) { // A contact already exists. return; } } edge = edge.Next; } // Does a joint override collision? if (bodyB.IsConnected(bodyA)) { return; } // Check user filtering. if (_contactFilter.ShouldCollide(fixtureA, fixtureB) == false) { return; } // Call the factory. Contact c = Contact.Create(fixtureA, fixtureB); // Contact creation may swap fixtures. fixtureA = c.GetFixtureA(); fixtureB = c.GetFixtureB(); bodyA = fixtureA.GetBody(); bodyB = fixtureB.GetBody(); // Insert into the world. c.Prev = null; c.Next = _contactList; if (_contactList != null) { _contactList.Prev = c; } _contactList = c; // Connect to island graph. // Connect to body A c.NodeA.Contact = c; c.NodeA.Other = bodyB; c.NodeA.Prev = null; c.NodeA.Next = bodyA._contactList; if (bodyA._contactList != null) { bodyA._contactList.Prev = c.NodeA; } bodyA._contactList = c.NodeA; // Connect to body B c.NodeB.Contact = c; c.NodeB.Other = bodyA; c.NodeB.Prev = null; c.NodeB.Next = bodyB._contactList; if (bodyB._contactList != null) { bodyB._contactList.Prev = c.NodeB; } bodyB._contactList = c.NodeB; ++_contactCount; }
public Contact(Fixture fixtureA, Fixture fixtureB) { Flags = 0; if (fixtureA.IsSensor || fixtureB.IsSensor) { Flags |= ContactFlag.SensorFlag; } Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet()) { Flags |= ContactFlag.ContinuousFlag; } else { Flags &= ~ContactFlag.ContinuousFlag; } _fixtureA = fixtureA; _fixtureB = fixtureB; Manifold = new Manifold(); Manifold.PointCount = 0; Prev = null; Next = null; NodeA = new ContactEdge(); NodeA.Contact = null; NodeA.Prev = null; NodeA.Next = null; NodeA.Other = null; NodeB = new ContactEdge(); NodeB.Contact = null; NodeB.Prev = null; NodeB.Next = null; NodeB.Other = null; }
public Contact(Shape s1, Shape s2) { _flags = 0; if (s1.IsSensor || s2.IsSensor) { _flags |= CollisionFlags.NonSolid; } _shape1 = s1; _shape2 = s2; _manifoldCount = 0; _prev = null; _next = null; _node1 = new ContactEdge(); _node1.Contact = null; _node1.Prev = null; _node1.Next = null; _node1.Other = null; _node2 = new ContactEdge(); _node2.Contact = null; _node2.Prev = null; _node2.Next = null; _node2.Other = null; }
public Contact(Fixture fA, Fixture fB) { _flags = 0; if (fA.IsSensor || fB.IsSensor) { _flags |= CollisionFlags.NonSolid; } _fixtureA = fA; _fixtureB = fB; _manifold.PointCount = 0; _prev = null; _next = null; _nodeA = new ContactEdge(); _nodeB = new ContactEdge(); }
internal bool _useGravity; // STEVE Added internal Body(BodyDef bd, World world) { Box2DXDebug.Assert(!world._lock); this._flags = (Body.BodyFlags) 0; if (bd.IsBullet) { this._flags |= Body.BodyFlags.Bullet; } if (bd.FixedRotation) { this._flags |= Body.BodyFlags.FixedRotation; } if (bd.AllowSleep) { this._flags |= Body.BodyFlags.AllowSleep; } if (bd.IsSleeping) { this._flags |= Body.BodyFlags.Sleep; } this._world = world; this._xf.Position = bd.Position; this._xf.R.Set(bd.Angle); this._sweep.LocalCenter = bd.MassData.Center; this._sweep.T0 = 1f; this._sweep.A0 = (this._sweep.A = bd.Angle); this._sweep.C0 = (this._sweep.C = Box2DX.Common.Math.Mul(this._xf, this._sweep.LocalCenter)); this._jointList = null; this._contactList = null; this._prev = null; this._next = null; this._linearDamping = bd.LinearDamping; this._angularDamping = bd.AngularDamping; this._force.Set(0f, 0f); this._torque = 0f; this._linearVelocity.SetZero(); this._angularVelocity = 0f; this._sleepTime = 0f; this._invMass = 0f; this._I = 0f; this._invI = 0f; this._mass = bd.MassData.Mass; if (this._mass > 0f) { this._invMass = 1f / this._mass; } if ((this._flags & Body.BodyFlags.FixedRotation) == (Body.BodyFlags) 0) { this._I = bd.MassData.I; } if (this._I > 0f) { this._invI = 1f / this._I; } if (this._invMass == 0f && this._invI == 0f) { this._type = Body.BodyType.Static; } else { this._type = Body.BodyType.Dynamic; } this._userData = bd.UserData; this._shapeList = null; this._shapeCount = 0; this._gravity = _world.Gravity; // STEVE Added this._useGravity = false; // STEVE Added }
/// Destroy a fixture. This removes the fixture from the broad-phase and /// therefore destroys any contacts associated with this fixture. All fixtures /// attached to a body are implicitly destroyed when the body is destroyed. /// @param fixture the fixture to be removed. /// @warning This function is locked during callbacks. public void DestroyFixture(ref Fixture fixture) { Box2DXDebug.Assert(_world.IsLocked() == false); if (_world.IsLocked() == true) { return; } Box2DXDebug.Assert(fixture.Body == this); // Remove the fixture from this body's singly linked list. Box2DXDebug.Assert(_fixtureCount > 0); Fixture node = _fixtureList; bool found = false; while (node != null) { if (node == fixture) { _fixtureList = fixture._next; found = true; break; } node = node._next; } // You tried to remove a shape that is not attached to this body. Box2DXDebug.Assert(found); // Destroy any contacts associated with the fixture. ContactEdge edge = _contactList; while (edge != null) { Contact c = edge.Contact; edge = edge.Next; Fixture fixtureA = c.GetFixtureA(); Fixture fixtureB = c.GetFixtureB(); if (fixture == fixtureA || fixture == fixtureB) { // This destroys the contact and removes it from // this body's contact list. _world._contactManager.Destroy(c); } } bool needMassUpdate = fixture._massData.Mass > 0.0f || fixture._massData.I > 0.0f; BroadPhase broadPhase = _world._contactManager._broadPhase; fixture.Destroy(broadPhase); fixture.Body = null; fixture._next = null; --_fixtureCount; // Adjust mass properties if needed. if (needMassUpdate) { ResetMass(); } }
/// <summary> /// Destroy a joint. This may cause the connected bodies to begin colliding. /// @warning This function is locked during callbacks. /// </summary> /// <param name="j"></param> public void DestroyJoint(Joint j) { Box2DXDebug.Assert(IsLocked() == false); if (IsLocked()) { return; } bool collideConnected = j._collideConnected; // Remove from the doubly linked list. if (j._prev != null) { j._prev._next = j._next; } if (j._next != null) { j._next._prev = j._prev; } if (j == _jointList) { _jointList = j._next; } // Disconnect from island graph. Body bodyA = j._bodyA; Body bodyB = j._bodyB; // Wake up connected bodies. bodyA.WakeUp(); bodyB.WakeUp(); // Remove from body 1. if (j._edgeA.Prev != null) { j._edgeA.Prev.Next = j._edgeA.Next; } if (j._edgeA.Next != null) { j._edgeA.Next.Prev = j._edgeA.Prev; } if (j._edgeA == bodyA._jointList) { bodyA._jointList = j._edgeA.Next; } j._edgeA.Prev = null; j._edgeA.Next = null; // Remove from body 2 if (j._edgeB.Prev != null) { j._edgeB.Prev.Next = j._edgeB.Next; } if (j._edgeB.Next != null) { j._edgeB.Next.Prev = j._edgeB.Prev; } if (j._edgeB == bodyB._jointList) { bodyB._jointList = j._edgeB.Next; } j._edgeB.Prev = null; j._edgeB.Next = null; Joint.Destroy(j); Box2DXDebug.Assert(_jointCount > 0); --_jointCount; // If the joint prevents collisions, then flag any contacts for filtering. if (collideConnected == false) { ContactEdge edge = bodyB.GetContactList(); while (edge != null) { if (edge.Other == bodyA) { // Flag the contact for filtering at the next time step (where either // body is awake). edge.Contact.FlagForFiltering(); } edge = edge.Next; } } }
/// <summary> /// Create a joint to constrain bodies together. No reference to the definition /// is retained. This may cause the connected bodies to cease colliding. /// @warning This function is locked during callbacks. /// </summary> /// <param name="def"></param> /// <returns></returns> public Joint CreateJoint(JointDef def) { Box2DXDebug.Assert(IsLocked() == false); if (IsLocked()) { return(null); } Joint j = Joint.Create(def); // Connect to the world list. j._prev = null; j._next = _jointList; if (_jointList != null) { _jointList._prev = j; } _jointList = j; ++_jointCount; // Connect to the bodies' doubly linked lists. j._edgeA.Joint = j; j._edgeA.Other = j._bodyB; j._edgeA.Prev = null; j._edgeA.Next = j._bodyA._jointList; if (j._bodyA._jointList != null) { j._bodyA._jointList.Prev = j._edgeA; } j._bodyA._jointList = j._edgeA; j._edgeB.Joint = j; j._edgeB.Other = j._bodyA; j._edgeB.Prev = null; j._edgeB.Next = j._bodyB._jointList; if (j._bodyB._jointList != null) { j._bodyB._jointList.Prev = j._edgeB; } j._bodyB._jointList = j._edgeB; Body bodyA = def.Body1; Body bodyB = def.Body2; bool staticA = bodyA.IsStatic(); bool staticB = bodyB.IsStatic(); // If the joint prevents collisions, then flag any contacts for filtering. if (def.CollideConnected == false && (staticA == false || staticB == false)) { // Ensure we iterate over contacts on a dynamic body (usually have less contacts // than a static body). Ideally we will have a contact count on both bodies. if (staticB) { Math.Swap(ref bodyA, ref bodyB); } ContactEdge edge = bodyB.GetContactList(); while (edge != null) { if (edge.Other == bodyA) { // Flag the contact for filtering at the next time step (where either // body is awake). edge.Contact.FlagForFiltering(); } edge = edge.Next; } } // Note: creating a joint doesn't wake the bodies. return(j); }
public void DestroyBody(Body b) { Box2DXDebug.Assert(_bodyCount > 0); Box2DXDebug.Assert(IsLocked() == false); 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); f0 = null; } 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; b = null; }
internal Body(BodyDef bd, World world) { Box2DXDebug.Assert(world._lock == false); _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 = Common.Math.Mul(_xf, _sweep.LocalCenter); _jointList = null; _contactList = null; _prev = null; _next = null; _linearDamping = bd.LinearDamping; _angularDamping = bd.AngularDamping; _force.Set(0.0f, 0.0f); _torque = 0.0f; _linearVelocity.SetZero(); _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; } if ((_flags & BodyFlags.FixedRotation) == 0) { _I = bd.MassData.I; } if (_I > 0.0f) { _invI = 1.0f / _I; } if (_invMass == 0.0f && _invI == 0.0f) { _type = BodyType.Static; } else { _type = BodyType.Dynamic; } _userData = bd.UserData; _shapeList = null; _shapeCount = 0; }
internal Body(BodyDef bd, World world) { Box2DXDebug.Assert(world._lock == false); _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 = Common.Math.Mul(_xf, _sweep.LocalCenter); _jointList = null; _contactList = null; _prev = null; _next = null; _linearDamping = bd.LinearDamping; _angularDamping = bd.AngularDamping; _force.Set(0.0f, 0.0f); _torque = 0.0f; _linearVelocity.SetZero(); _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; } if ((_flags & BodyFlags.FixedRotation) == 0) { _I = bd.MassData.I; } if (_I > 0.0f) { _invI = 1.0f / _I; } if (_invMass == 0.0f && _invI == 0.0f) { _type = BodyType.Static; } else { _type = BodyType.Dynamic; } _userData = bd.UserData; _shapeList = null; _shapeCount = 0; }
// Find islands, integrate and solve constraints, solve position constraints private void Solve(TimeStep step) { // Size the island for the worst case. Island island = new Island(_bodyCount, _contactManager._contactCount, _jointCount, _contactManager._contactListener); // Clear all the island flags. for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~Body.BodyFlags.Island; } for (Contact c = _contactManager._contactList; c != null; c = c.Next) { c.Flags &= ~ContactFlag.IslandFlag; } for (Joint j = _jointList; j != null; j = j._next) { j._islandFlag = false; } // Build and simulate all awake islands. int stackSize = _bodyCount; Body[] stack = new Body[stackSize]; for (Body seed = _bodyList; seed != null; seed = seed._next) { if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep)) != 0) { continue; } if (seed.IsStatic()) { continue; } // Reset island and stack. island.Clear(); int stackCount = 0; stack[stackCount++] = seed; seed._flags |= Body.BodyFlags.Island; // 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]; island.Add(ref b); // Make sure the body is awake. b._flags &= ~Body.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? if ((ce.Contact.Flags & ContactFlag.IslandFlag) != 0) { continue; } // Is this contact touching? if (ce.Contact.IsSolid() == false || ce.Contact.IsTouching() == false) { continue; } island.Add(ref ce.Contact); ce.Contact.Flags |= ContactFlag.IslandFlag; Body other = ce.Other; // Was the other body already added to this island? if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } Box2DXDebug.Assert(stackCount < stackSize); stack[stackCount++] = other; other._flags |= Body.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 & Body.BodyFlags.Island) != 0) { continue; } Box2DXDebug.Assert(stackCount < stackSize); stack[stackCount++] = other; other._flags |= Body.BodyFlags.Island; } } island.Solve(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 &= ~Body.BodyFlags.Island; } } } stack = null; // Synchronize shapes, check for out of range bodies. for (Body b = _bodyList; b != null; b = b.GetNext()) { if ((b._flags & Body.BodyFlags.Sleep) != 0) { continue; } if (b.IsStatic()) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. _contactManager.FindNewContacts(); }
private Transform _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.SetZero(); _sweep.T0 = 1.0f; _sweep.A0 = _sweep.A = bd.Angle; _sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter); _jointList = null; _contactList = null; _prev = null; _next = null; _linearVelocity = bd.LinearVelocity; _angularVelocity = bd.AngularVelocity; _linearDamping = bd.LinearDamping; _angularDamping = bd.AngularDamping; _force.Set(0.0f, 0.0f); _torque = 0.0f; _sleepTime = 0.0f; _mass = 0; _invMass = 0.0f; _I = 0.0f; _invI = 0.0f; _type = BodyType.Static; _userData = bd.UserData; _fixtureList = null; _fixtureCount = 0; }
// Find TOI contacts and solve them. private void SolveTOI(TimeStep step) { // Reserve an island and a queue for TOI island solution. Island island = new Island(_bodyCount, Settings.MaxTOIContactsPerIsland, Settings.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; int queueCapacity = _bodyCount; Body[] queue = new Body[queueCapacity]; for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~Body.BodyFlags.Island; b._sweep.T0 = 0.0f; } for (Contact c = _contactManager._contactList; c != null; c = c.Next) { // Invalidate TOI c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag); } 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) { // Can this contact generate a solid TOI contact? if (c.IsSolid() == false || c.IsContinuous() == false) { continue; } // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact. float toi = 1.0f; if ((c.Flags & ContactFlag.ToiFlag) != 0) { // 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); } Box2DXDebug.Assert(t0 < 1.0f); // Compute the time of impact. toi = c.ComputeTOI(b1._sweep, b2._sweep); Box2DXDebug.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 |= ContactFlag.ToiFlag; } if (Settings.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.FLT_EPSILON < minTOI) { // No more TOI events. Done! break; } // Advance the bodies to the TOI. Fixture f1 = minContact.GetFixtureA(); Fixture f2 = minContact.GetFixtureB(); Body b3 = f1.GetBody(); Body b4 = f2.GetBody(); Sweep backup1 = b3._sweep; Sweep backup2 = b4._sweep; b3.Advance(minTOI); b4.Advance(minTOI); // The TOI contact likely has some new contact points. minContact.Update(_contactManager._contactListener); minContact.Flags &= ~ContactFlag.ToiFlag; // Is the contact solid? if (minContact.IsSolid() == false) { // Restore the sweeps. b3._sweep = backup1; b4._sweep = backup2; b3.SynchronizeTransform(); b4.SynchronizeTransform(); continue; } // Did numerical issues prevent a contact point from being generated? if (minContact.IsTouching() == false) { // Give up on this TOI. continue; } // Build the TOI island. We need a dynamic seed. Body seed = b3; if (seed.IsStatic()) { seed = b4; } // 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 |= Body.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(ref b); // Make sure the body is awake. b._flags &= ~Body.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.ContactCount == island.ContactCapacity) { break; } // Has this contact already been added to an island? Skip slow or non-solid contacts. if ((cEdge.Contact.Flags & ContactFlag.IslandFlag) != 0) { continue; } // Is this contact touching? For performance we are not updating this contact. if (cEdge.Contact.IsSolid() == false || cEdge.Contact.IsTouching() == false) { continue; } island.Add(ref cEdge.Contact); cEdge.Contact.Flags |= ContactFlag.IslandFlag; // Update other body. Body other = cEdge.Other; // Was the other body already added to this island? if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } // March forward, this can do no harm since this is the min TOI. if (other.IsStatic() == false) { other.Advance(minTOI); other.WakeUp(); } Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); queue[queueStart + queueSize] = other; ++queueSize; other._flags |= Body.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 & Body.BodyFlags.Island) != 0) { continue; } if (!other.IsStatic()) { other.Advance(minTOI); other.WakeUp(); } Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); queue[queueStart + queueSize] = other; ++queueSize; other._flags |= Body.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 &= ~Body.BodyFlags.Island; if ((b._flags & Body.BodyFlags.Sleep) != 0) { continue; } if (b.IsStatic()) { continue; } 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 &= ~ContactFlag.ToiFlag; } } for (int i = 0; i < island.ContactCount; ++i) { // Allow contacts to participate in future TOI islands. Contact c = island.Contacts[i]; c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag); } 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(); } queue = null; }
/// This resets the mass properties to the sum of the mass properties of the fixtures. /// This normally does not need to be called unless you called SetMassData to override /// the mass and you later want to reset the mass. public void ResetMass() { // Compute mass data from shapes. Each shape has its own density. _mass = 0.0f; _invMass = 0.0f; _I = 0.0f; _invI = 0.0f; Vec2 center = Vec2.Zero; for (Fixture f = _fixtureList; f != null; f = f._next) { MassData massData = f.GetMassData(); _mass += massData.Mass; center += massData.Mass * massData.Center; _I += massData.I; } // Compute center of mass. if (_mass > 0.0f) { _invMass = 1.0f / _mass; center *= _invMass; } if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) { // Center the inertia about the center of mass. _I -= _mass * Vec2.Dot(center, center); Box2DXDebug.Assert(_I > 0.0f); _invI = 1.0f / _I; } else { _I = 0.0f; _invI = 0.0f; } // Move center of mass. Vec2 oldCenter = _sweep.C; _sweep.LocalCenter = center; _sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter); // Update center of mass velocity. _linearVelocity += Vec2.Cross(_angularVelocity, _sweep.C - oldCenter); // Determine the new body type. BodyType oldType = _type; if (_invMass == 0.0f && _invI == 0.0f) { _type = BodyType.Static; } else { _type = BodyType.Dynamic; } // If the body type changed, we need to flag contacts for filtering. if (oldType != _type) { for (ContactEdge ce = _contactList; ce != null; ce = ce.Next) { ce.Contact.FlagForFiltering(); } } }
internal Body(BodyDef bd, World world) { Box2DXDebug.Assert(!world._lock); this._flags = (Body.BodyFlags)0; if (bd.IsBullet) { this._flags |= Body.BodyFlags.Bullet; } if (bd.FixedRotation) { this._flags |= Body.BodyFlags.FixedRotation; } if (bd.AllowSleep) { this._flags |= Body.BodyFlags.AllowSleep; } if (bd.IsSleeping) { this._flags |= Body.BodyFlags.Sleep; } this._world = world; this._xf.Position = bd.Position; this._xf.R.Set(bd.Angle); this._sweep.LocalCenter = bd.MassData.Center; this._sweep.T0 = 1f; this._sweep.A0 = (this._sweep.A = bd.Angle); this._sweep.C0 = (this._sweep.C = Box2DX.Common.Math.Mul(this._xf, this._sweep.LocalCenter)); this._jointList = null; this._contactList = null; this._prev = null; this._next = null; this._linearDamping = bd.LinearDamping; this._angularDamping = bd.AngularDamping; this._force.Set(0f, 0f); this._torque = 0f; this._linearVelocity.SetZero(); this._angularVelocity = 0f; this._sleepTime = 0f; this._invMass = 0f; this._I = 0f; this._invI = 0f; this._mass = bd.MassData.Mass; if (this._mass > 0f) { this._invMass = 1f / this._mass; } if ((this._flags & Body.BodyFlags.FixedRotation) == (Body.BodyFlags)0) { this._I = bd.MassData.I; } if (this._I > 0f) { this._invI = 1f / this._I; } if (this._invMass == 0f && this._invI == 0f) { this._type = Body.BodyType.Static; } else { this._type = Body.BodyType.Dynamic; } this._userData = bd.UserData; this._shapeList = null; this._shapeCount = 0; this._gravity = _world.Gravity; // STEVE Added this._useGravity = false; // STEVE Added }