/**
* @see org.jbox2d.dynamics.joints.Joint#solvePositionConstraints(float)
*/
@Override
public boolean solvePositionConstraints(float baumgarte) {
Body b1 = m_bodyA;
Body b2 = m_bodyB;
final Vec2 c1 = b1.m_sweep.c;
float a1 = b1.m_sweep.a;
final Vec2 c2 = b2.m_sweep.c;
float a2 = b2.m_sweep.a;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
boolean active = false;
float C2 = 0.0f;
Mat22 R1 = pool.popMat22();
Mat22 R2 = pool.popMat22();
R1.set(a1);
R2.set(a2);
final Vec2 r1 = pool.popVec2();
final Vec2 r2 = pool.popVec2();
final Vec2 temp = pool.popVec2();
final Vec2 d = pool.popVec2();
r1.set(m_localAnchor1).subLocal(m_localCenterA);
r2.set(m_localAnchor2).subLocal(m_localCenterB);
Mat22.mulToOut(R1, r1, r1);
Mat22.mulToOut(R2, r2, r2);
d.set(c2).addLocal(r2).subLocal(c1).subLocal(r1);
if (m_enableLimit) {
Mat22.mulToOut(R1, m_localXAxis1, m_axis);
temp.set(d).addLocal(r1);
m_a1 = Vec2.cross(temp, m_axis);
m_a2 = Vec2.cross(r2, m_axis);
float translation = Vec2.dot(m_axis, d);
if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop) {
// Prevent large angular corrections
C2 = MathUtils.clamp(translation, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
linearError = MathUtils.abs(translation);
active = true;
}
else if (translation <= m_lowerTranslation) {
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop,
-Settings.maxLinearCorrection, 0.0f);
linearError = m_lowerTranslation - translation;
active = true;
}
else if (translation >= m_upperTranslation) {
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0f,
Settings.maxLinearCorrection);
linearError = translation - m_upperTranslation;
active = true;
}
}
Mat22.mulToOut(R1, m_localYAxis1, m_perp);
temp.set(d).addLocal(r1);
m_s1 = Vec2.cross(temp, m_perp);
m_s2 = Vec2.cross(r2, m_perp);
final Vec2 impulse = pool.popVec2();
float C1;
C1 = Vec2.dot(m_perp, d);
linearError = MathUtils.max(linearError, MathUtils.abs(C1));
angularError = 0.0f;
if (active) {
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.m11 = k11;
m_K.m12 = k12;
m_K.m21 = k12;
m_K.m22 = k22;
final Vec2 C = pool.popVec2();
C.x = C1;
C.y = C2;
m_K.solveToOut(C.negateLocal(), impulse);
pool.pushVec2(1);
}
else {
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 impulse1;
if (k11 != 0.0f) {
impulse1 = -C1 / k11;
}
else {
impulse1 = 0.0f;
}
impulse.x = impulse1;
impulse.y = 0.0f;
}
final Vec2 P = pool.popVec2();
temp.set(m_axis).mulLocal(impulse.y);
P.set(m_perp).mulLocal(impulse.x).add(temp);
float L1 = impulse.x * m_s1 + impulse.y * m_a1;
float L2 = impulse.x * m_s2 + impulse.y * m_a2;
temp.set(P).mulLocal(m_invMassA);
c1.subLocal(temp);
a1 -= m_invIA * L1;
temp.set(P).mulLocal(m_invMassB);
c2.addLocal(temp);
a2 += m_invIB * L2;
// TODO_ERIN remove need for this.
b1.m_sweep.a = a1;
b2.m_sweep.a = a2;
b1.synchronizeTransform();
b2.synchronizeTransform();
pool.pushVec2(6);
pool.pushMat22(2);
return linearError <= Settings.linearSlop && angularError <= Settings.angularSlop;