final Vec2 axis = pool.popVec2();
final Vec2 perp = pool.popVec2();
final Vec2 temp = pool.popVec2();
final Vec2 C1 = pool.popVec2();
final Vec3 impulse = pool.popVec3();
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
qA.set(aA);
qB.set(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
Rot.mulToOutUnsafe(qA, m_localXAxisA, axis);
float a1 = Vec2.cross(temp.set(d).addLocal(rA), axis);
float a2 = Vec2.cross(rB, axis);
Rot.mulToOutUnsafe(qA, m_localYAxisA, perp);
float s1 = Vec2.cross(temp.set(d).addLocal(rA), perp);
float s2 = Vec2.cross(rB, perp);
C1.x = Vec2.dot(perp, d);
C1.y = aB - aA - m_referenceAngle;
float linearError = MathUtils.abs(C1.x);
float angularError = MathUtils.abs(C1.y);
boolean active = false;
float C2 = 0.0f;
if (m_enableLimit) {
float translation = Vec2.dot(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.max(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 = MathUtils.max(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 = MathUtils.max(linearError, translation - m_upperTranslation);
active = true;
}
}
if (active) {
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f) {
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
final Mat33 K = pool.popMat33();
K.ex.set(k11, k12, k13);
K.ey.set(k12, k22, k23);
K.ez.set(k13, k23, k33);
final Vec3 C = pool.popVec3();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
K.solve33ToOut(C.negateLocal(), impulse);
pool.pushVec3(1);
pool.pushMat33(1);
} else {
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;