/// Get the current length of the segment attached to bodyB. public float GetCurrentLengthB() { Vec2 p = m_bodyB.GetWorldPoint(m_localAnchorB); Vec2 s = m_groundAnchorB; Vec2 d = p - s; return(d.Length()); }
/// Initialize the bodies, anchors, and length using the world /// anchors. // 1-D constrained 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, Vec2 anchor1, Vec2 anchor2) { bodyA = b1; bodyB = b2; localAnchorA = bodyA.GetLocalPoint(anchor1); localAnchorB = bodyB.GetLocalPoint(anchor2); Vec2 d = anchor2 - anchor1; length = d.Length(); }
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. // Pulley: // length1 = norm(p1 - s1) // length2 = norm(p2 - s2) // C0 = (length1 + ratio * length2)_initial // C = C0 - (length1 + ratio * length2) // u1 = (p1 - s1) / norm(p1 - s1) // u2 = (p2 - s2) / norm(p2 - s2) // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) // J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) public void Initialize(Body bA, Body bB, Vec2 groundA, Vec2 groundB, Vec2 anchorA, Vec2 anchorB, float r) { bodyA = bA; bodyB = bB; groundAnchorA = groundA; groundAnchorB = groundB; localAnchorA = bodyA.GetLocalPoint(anchorA); localAnchorB = bodyB.GetLocalPoint(anchorB); Vec2 dA = anchorA - groundA; lengthA = dA.Length(); Vec2 dB = anchorB - groundB; lengthB = dB.Length(); ratio = r; Utilities.Assert(ratio > Single.Epsilon); }
public static float Distance(Vec2 a, Vec2 b) { Vec2 c = a - b; return(c.Length()); }
internal override bool SolvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA); Rot qB = new Rot(aB); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA); Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB); float positionError, angularError; Mat33 K; K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; K.ez.X = -rA.Y * iA - rB.Y * iB; K.ex.Y = K.ey.X; K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; K.ez.Y = rA.X * iA + rB.X * iB; K.ex.Z = K.ez.X; K.ey.Z = K.ez.Y; K.ez.Z = iA + iB; if (m_frequencyHz > 0.0f) { Vec2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vec2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * Utilities.Cross(rA, P); cB += mB * P; aB += iB * Utilities.Cross(rB, P); } else { Vec2 C1 = cB + rB - cA - rA; float C2 = aB - aA - m_referenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vec3 C = new Vec3(C1.X, C1.Y, C2); Vec3 impulse = -K.Solve33(C); Vec2 P = new Vec2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (Utilities.Cross(rA, P) + impulse.Z); cB += mB * P; aB += iB * (Utilities.Cross(rB, P) + impulse.Z); } data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(positionError <= Settings._linearSlop && angularError <= Settings._angularSlop); }
internal override bool SolvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA); Rot qB = new Rot(aB); Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA); Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB); // Get the pulley axes. Vec2 uA = cA + rA - m_groundAnchorA; Vec2 uB = cB + rB - m_groundAnchorB; float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings._linearSlop) { uA *= 1.0f / lengthA; } else { uA.SetZero(); } if (lengthB > 10.0f * Settings._linearSlop) { uB *= 1.0f / lengthB; } else { uB.SetZero(); } // Compute effective mass. float ruA = Utilities.Cross(rA, uA); float ruB = Utilities.Cross(rB, uB); float mA = m_invMassA + m_invIA * ruA * ruA; float mB = m_invMassB + m_invIB * ruB * ruB; float mass = mA + m_ratio * m_ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = m_constant - lengthA - m_ratio * lengthB; float linearError = Math.Abs(C); float impulse = -mass * C; Vec2 PA = -impulse * uA; Vec2 PB = -m_ratio * impulse * uB; cA += m_invMassA * PA; aA += m_invIA * Utilities.Cross(rA, PA); cB += m_invMassB * PB; aB += m_invIB * Utilities.Cross(rB, PB); data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(linearError < Settings._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); } } } }
internal override bool SolvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA); Rot qB = new Rot(aB); float angularError = 0.0f; float positionError = 0.0f; bool fixedRotation = (m_invIA + m_invIB == 0.0f); // Solve angular limit constraint. if (m_enableLimit && m_limitState != LimitState.e_inactiveLimit && fixedRotation == false) { float angle = aB - aA - m_referenceAngle; float limitImpulse = 0.0f; if (m_limitState == LimitState.e_equalLimits) { // Prevent large angular corrections float C = Utilities.Clamp(angle - m_lowerAngle, -Settings._maxAngularCorrection, Settings._maxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = Math.Abs(C); } else if (m_limitState == LimitState.e_atLowerLimit) { float C = angle - m_lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = Utilities.Clamp(C + Settings._angularSlop, -Settings._maxAngularCorrection, 0.0f); limitImpulse = -m_motorMass * C; } else if (m_limitState == LimitState.e_atUpperLimit) { float C = angle - m_upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = Utilities.Clamp(C - Settings._angularSlop, 0.0f, Settings._maxAngularCorrection); limitImpulse = -m_motorMass * C; } aA -= m_invIA * limitImpulse; aB += m_invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA); Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB); Vec2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Mat22 K; K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y; K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y; K.ey.X = K.ex.Y; K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X; Vec2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * Utilities.Cross(rA, impulse); cB += mB * impulse; aB += iB * Utilities.Cross(rB, impulse); } data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(positionError <= Settings._linearSlop && angularError <= Settings._angularSlop); }