/**
* @see org.jbox2d.dynamics.joints.Joint#initVelocityConstraints(org.jbox2d.dynamics.TimeStep)
*/
@Override
public void initVelocityConstraints(TimeStep step) {
Body g1 = m_ground1;
Body g2 = m_ground2;
Body b1 = m_bodyA;
Body b2 = m_bodyB;
float K = 0.0f;
m_J.setZero();
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);