@Override
	public void solveVelocityConstraints(final TimeStep step) {
		final Body b1 = m_bodyA;
		final Body b2 = m_bodyB;
		
		final Vec2 v1 = b1.m_linearVelocity;
		float w1 = b1.m_angularVelocity;
		final Vec2 v2 = b2.m_linearVelocity;
		float w2 = b2.m_angularVelocity;
		
		float m1 = b1.m_invMass, m2 = b2.m_invMass;
		float i1 = b1.m_invI, i2 = b2.m_invI;
		
		// Solve motor constraint.
		if (m_enableMotor && m_limitState != LimitState.EQUAL) {
			float Cdot = w2 - w1 - m_motorSpeed;
			float impulse = m_motorMass * (-Cdot);
			float oldImpulse = m_motorImpulse;
			float maxImpulse = step.dt * m_maxMotorTorque;
			m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
			impulse = m_motorImpulse - oldImpulse;
			
			w1 -= i1 * impulse;
			w2 += i2 * impulse;
		}
		final Vec2 temp = pool.popVec2();
		final Vec2 r1 = pool.popVec2();
		final Vec2 r2 = pool.popVec2();
		
		// Solve limit constraint.
		if (m_enableLimit && m_limitState != LimitState.INACTIVE) {
			
			r1.set(m_localAnchor1).subLocal(b1.getLocalCenter());
			r2.set(m_localAnchor2).subLocal(b2.getLocalCenter());
			Mat22.mulToOut(b1.getTransform().R, r1, r1);
			Mat22.mulToOut(b2.getTransform().R, r2, r2);
			// Vec2 r1 = b2Mul(b1.getTransform().R, m_localAnchor1 - b1.getLocalCenter());
			// Vec2 r2 = b2Mul(b2.getTransform().R, m_localAnchor2 - b2.getLocalCenter());
			
			final Vec2 Cdot1 = pool.popVec2();
			final Vec3 Cdot = pool.popVec3();
			
			// Solve point-to-point constraint
			Vec2.crossToOut(w1, r1, temp);
			Vec2.crossToOut(w2, r2, Cdot1);
			Cdot1.addLocal(v2).subLocal(v1).subLocal(temp);
			float Cdot2 = w2 - w1;
			Cdot.set(Cdot1.x, Cdot1.y, Cdot2);
			// Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
			// float Cdot2 = w2 - w1;
			// b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
			
			Vec3 impulse = pool.popVec3();
			m_mass.solve33ToOut(Cdot.negateLocal(), impulse);
			// Cdot.negateLocal(); just leave negated, we don't use later
			
			if (m_limitState == LimitState.EQUAL) {
				m_impulse.addLocal(impulse);
			}
			else if (m_limitState == LimitState.AT_LOWER) {
				float newImpulse = m_impulse.z + impulse.z;
				if (newImpulse < 0.0f) {
					m_mass.solve22ToOut(Cdot1.negateLocal(), temp);
					//Cdot1.negateLocal(); just leave negated, we don't use it again
					impulse.x = temp.x;
					impulse.y = temp.y;
					impulse.z = -m_impulse.z;
					m_impulse.x += temp.x;
					m_impulse.y += temp.y;
					m_impulse.z = 0.0f;
				}
			}
			else if (m_limitState == LimitState.AT_UPPER) {
				float newImpulse = m_impulse.z + impulse.z;
				if (newImpulse > 0.0f) {
					m_mass.solve22ToOut(Cdot1.negateLocal(), temp);
					//Cdot1.negateLocal(); just leave negated, we don't use it again
					impulse.x = temp.x;
					impulse.y = temp.y;
					impulse.z = -m_impulse.z;
					m_impulse.x += temp.x;
					m_impulse.y += temp.y;
					m_impulse.z = 0.0f;
				}
			}
			final Vec2 P = pool.popVec2();
			
			P.set(impulse.x, impulse.y);
			
			temp.set(P).mulLocal(m1);
			v1.subLocal(temp);
			w1 -= i1 * (Vec2.cross(r1, P) + impulse.z);
			
			temp.set(P).mulLocal(m2);
			v2.addLocal(temp);
			w2 += i2 * (Vec2.cross(r2, P) + impulse.z);
			
			pool.pushVec2(2);
			pool.pushVec3(2);
		}
		else {
			r1.set(m_localAnchor1).subLocal(b1.getLocalCenter());
			r2.set(m_localAnchor2).subLocal(b2.getLocalCenter());
			Mat22.mulToOut(b1.getTransform().R, r1, r1);
			Mat22.mulToOut(b2.getTransform().R, r2, r2);
			// Vec2 r1 = b2Mul(b1.getTransform().R, m_localAnchor1 - b1.getLocalCenter());
			// Vec2 r2 = b2Mul(b2.getTransform().R, m_localAnchor2 - b2.getLocalCenter());
			
			// Solve point-to-point constraint
			Vec2 Cdot = pool.popVec2();
			Vec2 impulse = pool.popVec2();
			
			Vec2.crossToOut(w1, r1, temp);
			Vec2.crossToOut(w2, r2, Cdot);
			Cdot.addLocal(v2).subLocal(v1).subLocal(temp);
			m_mass.solve22ToOut(Cdot.negateLocal(), impulse); // just leave negated
			
			m_impulse.x += impulse.x;
			m_impulse.y += impulse.y;
			
			temp.set(impulse).mulLocal(m1);
			v1.subLocal(temp);
			w1 -= i1 * Vec2.cross(r1, impulse);
			
			temp.set(impulse).mulLocal(m2);
			v2.addLocal(temp);
			w2 += i2 * Vec2.cross(r2, impulse);
			
			pool.pushVec2(2);
		}
		
		b1.m_angularVelocity = w1;
		b2.m_angularVelocity = w2;
		
		pool.pushVec2(3);
	}