if (m_revolute1 != null) {
m_J.angularA = -1.0f;
K += b1.m_invI;
}
else {
final Vec2 ug = pool.popVec2();
final Vec2 r = pool.popVec2();
Mat22.mulToOut(g1.getTransform().R, m_prismatic1.m_localXAxis1, ug);
r.set(m_localAnchor1).subLocal(b1.getLocalCenter());
Mat22.mulToOut(b1.getTransform().R, r, r);
float crug = Vec2.cross(r, ug);
m_J.linearA.set(ug).negateLocal();
m_J.angularA = -crug;
K += b1.m_invMass + b1.m_invI * crug * crug;
pool.pushVec2(2);
}
if (m_revolute2 != null) {
m_J.angularB = -m_ratio;
K += m_ratio * m_ratio * b2.m_invI;
}
else {
final Vec2 ug = pool.popVec2();
final Vec2 r = pool.popVec2();
Mat22.mulToOut(g2.getTransform().R, m_prismatic2.m_localXAxis1, ug);
r.set(m_localAnchor2).subLocal(b2.getLocalCenter());
Mat22.mulToOut(b2.getTransform().R, r, r);
float crug = Vec2.cross(r, ug);
m_J.linearB.set(ug).mulLocal(-m_ratio);
m_J.angularB = -m_ratio * crug;
K += m_ratio * m_ratio * (b2.m_invMass + b2.m_invI * crug * crug);
pool.pushVec2(2);
}
// Compute effective mass.
m_mass = K > 0.0f ? 1.0f / K : 0.0f;
if (step.warmStarting) {
final Vec2 temp = pool.popVec2();
// Warm starting.
temp.set(m_J.linearA).mulLocal(b1.m_invMass).mulLocal(m_impulse);
b1.m_linearVelocity.addLocal(temp);
b1.m_angularVelocity += b1.m_invI * m_impulse * m_J.angularA;
temp.set(m_J.linearB).mulLocal(b2.m_invMass).mulLocal(m_impulse);
b2.m_linearVelocity.addLocal(temp);
b2.m_angularVelocity += b2.m_invI * m_impulse * m_J.angularB;
pool.pushVec2(1);
}