internal override void InitVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; // You cannot create a prismatic joint between bodies that // both have fixed rotation. Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f); _localCenter1 = b1.GetLocalCenter(); _localCenter2 = b2.GetLocalCenter(); Transform xf1 = b1.GetTransform(); Transform xf2 = b2.GetTransform(); // Compute the effective masses. Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1); Vector2 r2 = xf2.TransformDirection(_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 = xf1.TransformDirection(_localXAxis1); _a1 = (d + r1).Cross(_axis); _a2 = r2.Cross(_axis); _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2; Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON); _motorMass = 1.0f / _motorMass; } // Prismatic constraint. { _perp = xf1.TransformDirection(_localYAxis1); _s1 = (d + r1).Cross(_perp); _s2 = r2.Cross(_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 + 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); } // Compute motor and limit terms. if (_enableLimit) { float jointTranslation = Vector2.Dot(_axis, d); if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { _limitState = LimitState.EqualLimits; } else if (jointTranslation <= _lowerTranslation) { if (_limitState != LimitState.AtLowerLimit) { _limitState = LimitState.AtLowerLimit; _impulse.z = 0.0f; } } else if (jointTranslation >= _upperTranslation) { if (_limitState != LimitState.AtUpperLimit) { _limitState = LimitState.AtUpperLimit; _impulse.z = 0.0f; } } else { _limitState = LimitState.InactiveLimit; _impulse.z = 0.0f; } } else { _limitState = LimitState.InactiveLimit; } 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.z) * _axis; float L1 = _impulse.x * _s1 + _impulse.y + (_motorImpulse + _impulse.z) * _a1; float L2 = _impulse.x * _s2 + _impulse.y + (_motorImpulse + _impulse.z) * _a2; b1._linearVelocity -= _invMass1 * P; b1._angularVelocity -= _invI1 * L1; b2._linearVelocity += _invMass2 * P; b2._angularVelocity += _invI2 * L2; } else { _impulse = Vector3.zero; _motorImpulse = 0.0f; } }