@Override
	public void initVelocityConstraints(TimeStep step) {
		Body b1 = m_bodyA;
		Body b2 = m_bodyB;
		
		m_localCenterA.set(b1.getLocalCenter());
		m_localCenterB.set(b2.getLocalCenter());
		
		Transform xf1 = b1.getTransform();
		Transform xf2 = b2.getTransform();
		
		// Compute the effective masses.
		final Vec2 r1 = pool.popVec2();
		final Vec2 r2 = pool.popVec2();
		final Vec2 temp = pool.popVec2();
		
		r1.set(m_localAnchor1).subLocal(m_localCenterA);
		r2.set(m_localAnchor2).subLocal(m_localCenterB);
		Mat22.mulToOut(xf1.R, r1, r1);
		Mat22.mulToOut(xf2.R, r2, r2);
		
		final Vec2 d = pool.popVec2();
		d.set(b2.m_sweep.c).addLocal(r2).subLocal(b1.m_sweep.c).subLocal(r1);
		
		m_invMassA = b1.m_invMass;
		m_invIA = b1.m_invI;
		m_invMassB = b2.m_invMass;
		m_invIB = b2.m_invI;
		
		// Compute motor Jacobian and effective mass.
		{
			Mat22.mulToOut(xf1.R, m_localXAxis1, m_axis);
			temp.set(d).addLocal(r1);
			m_a1 = Vec2.cross(temp, m_axis);
			m_a2 = Vec2.cross(r2, m_axis);
			
			m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
			if (m_motorMass > Settings.EPSILON) {
				m_motorMass = 1.0f / m_motorMass;
			}
			else {
				m_motorMass = 0.0f;
			}
		}
		
		// Prismatic constraint.
		{
			Mat22.mulToOut(xf1.R, m_localYAxis1, m_perp);
			
			temp.set(d).addLocal(r1);
			m_s1 = Vec2.cross(temp, m_perp);
			m_s2 = Vec2.cross(r2, m_perp);
			
			float m1 = m_invMassA, m2 = m_invMassB;
			float i1 = m_invIA, i2 = m_invIB;
			
			float k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
			float k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
			float k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
			
			m_K.col1.set(k11, k12);
			m_K.col2.set(k12, k22);
		}
		
		// Compute motor and limit terms.
		if (m_enableLimit) {
			float jointTranslation = Vec2.dot(m_axis, d);
			if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop) {
				m_limitState = LimitState.EQUAL;
			}
			else if (jointTranslation <= m_lowerTranslation) {
				if (m_limitState != LimitState.AT_LOWER) {
					m_limitState = LimitState.AT_LOWER;
					m_impulse.y = 0.0f;
				}
			}
			else if (jointTranslation >= m_upperTranslation) {
				if (m_limitState != LimitState.AT_UPPER) {
					m_limitState = LimitState.AT_UPPER;
					m_impulse.y = 0.0f;
				}
			}
			else {
				m_limitState = LimitState.INACTIVE;
				m_impulse.y = 0.0f;
			}
		}
		else {
			m_limitState = LimitState.INACTIVE;
		}
		
		if (m_enableMotor == false) {
			m_motorImpulse = 0.0f;
		}
		
		if (step.warmStarting) {
			// Account for variable time step.
			m_impulse.mulLocal(step.dtRatio);
			m_motorImpulse *= step.dtRatio;
			
			final Vec2 P = pool.popVec2();
			temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.y);
			P.set(m_perp).mulLocal(m_impulse.x).addLocal(temp);
			
			float L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1;
			float L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2;
			
			temp.set(P).mulLocal(m_invMassA);
			b1.m_linearVelocity.subLocal(temp);
			b1.m_angularVelocity -= m_invIA * L1;
			
			temp.set(P).mulLocal(m_invMassB);
			b2.m_linearVelocity.addLocal(temp);
			b2.m_angularVelocity += m_invIB * L2;
			pool.pushVec2(1);
		}
		else {
			m_impulse.setZero();
			m_motorImpulse = 0.0f;
		}
		pool.pushVec2(4);
	}