/**
* @see org.jbox2d.dynamics.joints.Joint#solvePositionConstraints(float)
*/
@Override
public boolean solvePositionConstraints(float baumgarte) {
Body b1 = m_bodyA;
Body b2 = m_bodyB;
final Vec2 s1 = pool.popVec2();
final Vec2 s2 = pool.popVec2();
s1.set(m_groundAnchor1);
s2.set(m_groundAnchor2);
float linearError = 0.0f;
if (m_state == LimitState.AT_UPPER) {
final Vec2 r1 = pool.popVec2();
final Vec2 r2 = pool.popVec2();
final Vec2 p1 = pool.popVec2();
final Vec2 p2 = pool.popVec2();
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);
p1.set(b1.m_sweep.c).addLocal(r1);
p2.set(b2.m_sweep.c).addLocal(r2);
// Get the pulley axes.
m_u1.set(p1).subLocal(s1);
m_u2.set(p2).subLocal(s2);
float length1 = m_u1.length();
float length2 = m_u2.length();
if (length1 > Settings.linearSlop) {
m_u1.mulLocal(1.0f / length1);
}
else {
m_u1.setZero();
}
if (length2 > Settings.linearSlop) {
m_u2.mulLocal(1.0f / length2);
}
else {
m_u2.setZero();
}
float C = m_constant - length1 - m_ratio * length2;
linearError = MathUtils.max(linearError, -C);
C = MathUtils.clamp(C + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0f);
float impulse = -m_pulleyMass * C;
final Vec2 P1 = pool.popVec2();
final Vec2 P2 = pool.popVec2();
final Vec2 temp = pool.popVec2();
P1.set(m_u1).mulLocal(-impulse);
P2.set(m_u2).mulLocal(-m_ratio * impulse);
temp.set(P1).mulLocal(b1.m_invMass);
b1.m_sweep.c.addLocal(temp);
b1.m_sweep.a += b1.m_invI * Vec2.cross(r1, P1);
temp.set(P2).mulLocal(b2.m_invMass);
b2.m_sweep.c.addLocal(temp);
b2.m_sweep.a += b2.m_invI * Vec2.cross(r2, P2);
b1.synchronizeTransform();
b2.synchronizeTransform();
pool.pushVec2(7);
}
if (m_limitState1 == LimitState.AT_UPPER) {
final Vec2 r1 = pool.popVec2();
final Vec2 p1 = pool.popVec2();
r1.set(m_localAnchor1).subLocal(b1.getLocalCenter());
Mat22.mulToOut(b1.getTransform().R, r1, r1);
p1.set(b1.m_sweep.c).addLocal(r1);
m_u1.set(p1).subLocal(s1);
float length1 = m_u1.length();
if (length1 > Settings.linearSlop) {
m_u1.mulLocal(1.0f / length1);
}
else {
m_u1.setZero();
}
float C = m_maxLength1 - length1;
linearError = MathUtils.max(linearError, -C);
C = MathUtils.clamp(C + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0f);
float impulse = -m_limitMass1 * C;
final Vec2 P1 = pool.popVec2();
final Vec2 temp = pool.popVec2();
P1.set(m_u1).mulLocal(-impulse);
temp.set(P1).mulLocal(b1.m_invMass);
b1.m_sweep.c.addLocal(temp);
b1.m_sweep.a += b1.m_invI * Vec2.cross(r1, P1);
b1.synchronizeTransform();
pool.pushVec2(4);
}
if (m_limitState2 == LimitState.AT_UPPER) {
final Vec2 r2 = pool.popVec2();
final Vec2 p2 = pool.popVec2();
r2.set(m_localAnchor2).subLocal(b2.getLocalCenter());
Mat22.mulToOut(b2.getTransform().R, r2, r2);
p2.set(b2.m_sweep.c).addLocal(r2);
// Get the pulley axes.
m_u2.set(p2).subLocal(s2);
float length2 = m_u2.length();
if (length2 > Settings.linearSlop) {
m_u2.mulLocal(1.0f / length2);
}
else {
m_u2.setZero();
}
float C = m_maxLength2 - length2;
linearError = MathUtils.max(linearError, -C);
C = MathUtils.clamp(C + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0f);
float impulse = -m_limitMass2 * C;
final Vec2 P2 = pool.popVec2();
final Vec2 temp = pool.popVec2();
P2.set(m_u2).mulLocal(-impulse);
temp.set(P2).mulLocal(b2.m_invMass);
b2.m_sweep.c.addLocal(temp);
b2.m_sweep.a += b2.m_invI * Vec2.cross(r2, P2);
b2.synchronizeTransform();
pool.pushVec2(4);
}
pool.pushVec2(2);