internal override void InitVelocityConstraints(ref TimeStep step) { Body b = _bodyB; float mass = b.GetMass(); // 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 has units of inverse mass. // beta has units of inverse time. //Debug.Assert(d + step.dt * k > Settings.b2_epsilon); _gamma = step.dt * (d + step.dt * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = step.dt * k * _gamma; // Compute the effective mass matrix. Transform xf1; b.GetTransform(out xf1); Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter()); // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] float invMass = b._invMass; float invI = b._invI; Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass)); Mat22 K2 = new Mat22(new Vector2(invI * r.y * r.y, -invI * r.x * r.y), new Vector2(-invI * r.x * r.y, invI * r.x * r.x)); Mat22 K; Mat22.Add(ref K1, ref K2, out K); K.col1.x += _gamma; K.col2.y += _gamma; _mass = K.GetInverse(); _C = b._sweep.c + r - _target; // Cheat with some damping b._angularVelocity *= 0.98f; // Warm starting. _impulse *= step.dtRatio; b._linearVelocity += invMass * _impulse; b._angularVelocity += invI * MathUtils.Cross(r, _impulse); }
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 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; } }