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(); }