internal override bool SolvePositionConstraints(float baumgarte) { if (_frequencyHz > 0.0f) { // There is no position correction for soft distance constraints. return(true); } Body b1 = _bodyA; Body b2 = _bodyB; Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2); Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter()); Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1; float length = d.magnitude; if (length < _length) { return(true); } if (length == 0.0f) { return(true); } d /= length; float C = length - _length; C = MathUtils.Clamp(C, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection); float impulse = -_mass * C; _u = d; Vector2 P = impulse * _u; b1._sweep.c -= b1._invMass * P; b1._sweep.a -= b1._invI * MathUtils.Cross(r1, P); b2._sweep.c += b2._invMass * P; b2._sweep.a += b2._invI * MathUtils.Cross(r2, P); b1.SynchronizeTransform(); b2.SynchronizeTransform(); return(Math.Abs(C) < Settings.b2_linearSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { float linearError = 0.0f; Body b1 = _bodyA; Body b2 = _bodyB; float coordinate1, coordinate2; if (_revolute1 != null) { coordinate1 = _revolute1.GetJointAngle(); } else { coordinate1 = _prismatic1.GetJointTranslation(); } if (_revolute2 != null) { coordinate2 = _revolute2.GetJointAngle(); } else { coordinate2 = _prismatic2.GetJointTranslation(); } float C = _ant - (coordinate1 + _ratio * coordinate2); float impulse = _mass * (-C); b1._sweep.c += b1._invMass * impulse * _J.linearA; b1._sweep.a += b1._invI * impulse * _J.angularA; b2._sweep.c += b2._invMass * impulse * _J.linearB; b2._sweep.a += b2._invI * impulse * _J.angularB; b1.SynchronizeTransform(); b2.SynchronizeTransform(); // TODO_ERIN not implemented return(linearError < Settings.b2_linearSlop); }
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 constraint. 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 - _localCenterA); Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenterB); 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); Vector3 impulse; Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle); linearError = Math.Max(linearError, Math.Abs(C1.x)); angularError = Math.Abs(C1.y); if (active) { float m1 = _invMassA, m2 = _invMassB; float i1 = _invIA, i2 = _invIB; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 + i2 * _s2; float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; float k22 = i1 + i2; float k23 = i1 * _a1 + i2 * _a2; float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; _K.col1 = new Vector3(k11, k12, k13); _K.col2 = new Vector3(k12, k22, k23); _K.col3 = new Vector3(k13, k23, k33); Vector3 C = new Vector3(-C1.x, -C1.y, -C2); impulse = _K.Solve33(C); // negated above } else { float m1 = _invMassA, m2 = _invMassB; float i1 = _invIA, i2 = _invIB; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 + i2 * _s2; float k22 = i1 + i2; _K.col1 = new Vector3(k11, k12, 0.0f); _K.col2 = new Vector3(k12, k22, 0.0f); Vector2 impulse1 = _K.Solve22(-C1); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } Vector2 P = impulse.x * _perp + impulse.z * _axis; float L1 = impulse.x * _s1 + impulse.y + impulse.z * _a1; float L2 = impulse.x * _s2 + impulse.y + impulse.z * _a2; c1 -= _invMassA * P; a1 -= _invIA * L1; c2 += _invMassB * P; a2 += _invIB * 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); }
public bool SolvePositionConstraints(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = _constraints[i]; Body bodyA = c.bodyA; Body bodyB = c.bodyB; float invMassA = bodyA._mass * bodyA._invMass; float invIA = bodyA._mass * bodyA._invI; float invMassB = bodyB._mass * bodyB._invMass; float invIB = bodyB._mass * bodyB._invI; // Solve normal constraints for (int j = 0; j < c.pointCount; ++j) { PositionSolverManifold psm = new PositionSolverManifold(ref c, j); Vector2 normal = psm._normal; Vector2 point = psm._point; float separation = psm._separation; Vector2 rA = point - bodyA._sweep.c; Vector2 rB = point - bodyB._sweep.c; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(rA, normal); float rnB = MathUtils.Cross(rB, normal); float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; #if MATH_OVERLOADS Vector2 P = impulse * normal; bodyA._sweep.c -= invMassA * P; bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P); bodyB._sweep.c += invMassB * P; bodyB._sweep.a += invIB * MathUtils.Cross(rB, P); #else Vector2 P = new Vector2(impulse * normal.x, impulse * normal.y); bodyA._sweep.c.x -= invMassA * P.x; bodyA._sweep.c.y -= invMassA * P.y; bodyA._sweep.a -= invIA * (rA.x * P.y - rA.y * P.x); bodyB._sweep.c.x += invMassB * P.x; bodyB._sweep.c.y += invMassB * P.y; bodyB._sweep.a += invIB * (rB.x * P.y - rB.y * P.x); #endif bodyA.SynchronizeTransform(); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't // push the separation above -Settings.b2_linearSlop. return(minSeparation >= -1.5f * Settings.b2_linearSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { // TODO_ERIN block solve with limit. COME ON ERIN Body b1 = _bodyA; Body b2 = _bodyB; float angularError = 0.0f; float positionError = 0.0f; // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { float angle = b2._sweep.a - b1._sweep.a - _referenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.b2_maxAngularCorrection, Settings.b2_maxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.b2_angularSlop, -Settings.b2_maxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.b2_angularSlop, 0.0f, Settings.b2_maxAngularCorrection); limitImpulse = -_motorMass * C; } b1._sweep.a -= b1._invI * limitImpulse; b2._sweep.a += b2._invI * limitImpulse; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } // Solve point-to-point constraint. { Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2); Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter()); Vector2 C = b2._sweep.c + r2 - b1._sweep.c - r1; positionError = C.magnitude; float invMass1 = b1._invMass, invMass2 = b2._invMass; float invI1 = b1._invI, invI2 = b2._invI; // Handle large detachment. const float k_allowedStretch = 10.0f * Settings.b2_linearSlop; if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch) { // Use a particle solution (no rotation). Vector2 u = C; u.Normalize(); float k = invMass1 + invMass2; //Debug.Assert(k > Settings.b2_epsilon); float m = 1.0f / k; Vector2 impulse2 = m * (-C); const float k_beta = 0.5f; b1._sweep.c -= k_beta * invMass1 * impulse2; b2._sweep.c += k_beta * invMass2 * impulse2; C = b2._sweep.c + r2 - b1._sweep.c - r1; } Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y), new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x)); Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y), new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x)); Mat22 Ka; Mat22 K; Mat22.Add(ref K1, ref K2, out Ka); Mat22.Add(ref Ka, ref K3, out K); Vector2 impulse = K.Solve(-C); b1._sweep.c -= b1._invMass * impulse; b1._sweep.a -= b1._invI * MathUtils.Cross(r1, impulse); b2._sweep.c += b2._invMass * impulse; b2._sweep.a += b2._invI * MathUtils.Cross(r2, impulse); b1.SynchronizeTransform(); b2.SynchronizeTransform(); } return(positionError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _bodyA; Body b2 = _bodyB; Vector2 s1 = _groundAnchor1; Vector2 s2 = _groundAnchor2; float linearError = 0.0f; if (_state == LimitState.AtUpper) { Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2); Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter()); Vector2 p1 = b1._sweep.c + r1; Vector2 p2 = b2._sweep.c + r2; // Get the pulley axes. _u1 = p1 - s1; _u2 = p2 - s2; float length1 = _u1.magnitude; float length2 = _u2.magnitude; if (length1 > Settings.b2_linearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.zero; } if (length2 > Settings.b2_linearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.zero; } float C = _ant - length1 - _ratio * length2; linearError = Math.Max(linearError, -C); C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f); float impulse = -_pulleyMass * C; Vector2 P1 = -impulse * _u1; Vector2 P2 = -_ratio * impulse * _u2; b1._sweep.c += b1._invMass * P1; b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1); b2._sweep.c += b2._invMass * P2; b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2); b1.SynchronizeTransform(); b2.SynchronizeTransform(); } if (_limitState1 == LimitState.AtUpper) { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter()); Vector2 p1 = b1._sweep.c + r1; _u1 = p1 - s1; float length1 = _u1.magnitude; if (length1 > Settings.b2_linearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.zero; } float C = _maxLength1 - length1; linearError = Math.Max(linearError, -C); C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f); float impulse = -_limitMass1 * C; Vector2 P1 = -impulse * _u1; b1._sweep.c += b1._invMass * P1; b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1); b1.SynchronizeTransform(); } if (_limitState2 == LimitState.AtUpper) { Transform xf2; b2.GetTransform(out xf2); Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter()); Vector2 p2 = b2._sweep.c + r2; _u2 = p2 - s2; float length2 = _u2.magnitude; if (length2 > Settings.b2_linearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.zero; } float C = _maxLength2 - length2; linearError = Math.Max(linearError, -C); C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f); float impulse = -_limitMass2 * C; Vector2 P2 = -impulse * _u2; b2._sweep.c += b2._invMass * P2; b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2); b2.SynchronizeTransform(); } return(linearError < Settings.b2_linearSlop); }
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); } } } }
public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep) { // Integrate velocities and apply damping. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() != BodyType.Dynamic) { continue; } // Integrate velocities. b._linearVelocity += step.dt * (gravity + b._invMass * b._force); b._angularVelocity += step.dt * b._invI * b._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 b._linearVelocity *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f); b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f); } // Partition contacts so that contacts with static bodies are solved last. int i1 = -1; for (int i2 = 0; i2 < _contactCount; ++i2) { Fixture fixtureA = _contacts[i2].GetFixtureA(); Fixture fixtureB = _contacts[i2].GetFixtureB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static; if (nonStatic) { ++i1; //b2Swap(_contacts[i1], _contacts[i2]); Contact temp = _contacts[i1]; _contacts[i1] = _contacts[i2]; _contacts[i2] = temp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, _contactCount, step.dtRatio); _contactSolver.WarmStart(); for (int i = 0; i < _jointCount; ++i) { _joints[i].InitVelocityConstraints(ref step); } // Solve velocity constraints. for (int i = 0; i < step.velocityIterations; ++i) { for (int j = 0; j < _jointCount; ++j) { _joints[j].SolveVelocityConstraints(ref step); } _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b._linearVelocity; if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared) { float ratio = Settings.b2_maxTranslation / translation.magnitude; b._linearVelocity *= ratio; } float rotation = step.dt * b._angularVelocity; if (rotation * rotation > Settings.b2_maxRotationSquared) { float ratio = Settings.b2_maxRotation / Math.Abs(rotation); b._angularVelocity *= ratio; } // Store positions for continuous collision. b._sweep.c0 = b._sweep.c; b._sweep.a0 = b._sweep.a; // Integrate b._sweep.c += step.dt * b._linearVelocity; b._sweep.a += step.dt * b._angularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < step.positionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte); bool jointsOkay = true; for (int j = 0; j < _jointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } Report(_contactSolver._constraints); if (allowSleep) { float minSleepTime = Settings.b2_maxFloat; const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance; const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance; for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() == BodyType.Static) { continue; } if ((b._flags & BodyFlags.AutoSleep) == 0) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } if ((b._flags & BodyFlags.AutoSleep) == 0 || b._angularVelocity * b._angularVelocity > angTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.b2_timeToSleep) { for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; b.SetAwake(false); } } } }
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; } } }
// Perform one solver iteration. Returns true if converged. public bool Solve(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < _count; ++i) { TOIConstraint c = _constraints[i]; Body bodyA = c.bodyA; Body bodyB = c.bodyB; float massA = bodyA._mass; float massB = bodyB._mass; // Only the TOI body should move. if (bodyA == _toiBody) { massB = 0.0f; } else { massA = 0.0f; } float invMassA = massA * bodyA._invMass; float invIA = massA * bodyA._invI; float invMassB = massB * bodyB._invMass; float invIB = massB * bodyB._invI; // Solve normal constraints for (int j = 0; j < c.pointCount; ++j) { TOISolverManifold psm = new TOISolverManifold(ref c, j); Vector2 normal = psm.normal; Vector2 point = psm.point; float separation = psm.separation; Vector2 rA = point - bodyA._sweep.c; Vector2 rB = point - bodyB._sweep.c; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(rA, normal); float rnB = MathUtils.Cross(rB, normal); float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; bodyA._sweep.c -= invMassA * P; bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P); bodyA.SynchronizeTransform(); bodyB._sweep.c += invMassB * P; bodyB._sweep.a += invIB * MathUtils.Cross(rB, P); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * Settings.b2_linearSlop); }