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