pool.pushVec2(3);
}
@Override
public boolean solvePositionConstraints(float baumgarte) {
final Body b1 = m_bodyA;
final Body b2 = m_bodyB;
// TODO_ERIN block solve with limit.
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != LimitState.INACTIVE) {
float angle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState == LimitState.EQUAL) {
// Prevent large angular corrections
float C = MathUtils.clamp(angle - m_lowerAngle, -Settings.maxAngularCorrection,
Settings.maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = MathUtils.abs(C);
}
else if (m_limitState == LimitState.AT_LOWER) {
float C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.clamp(C + Settings.angularSlop, -Settings.maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == LimitState.AT_UPPER) {
float C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.clamp(C - Settings.angularSlop, 0.0f, Settings.maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
b1.m_sweep.a -= b1.m_invI * limitImpulse;
b2.m_sweep.a += b2.m_invI * limitImpulse;
b1.synchronizeTransform();
b2.synchronizeTransform();
}
// Solve point-to-point constraint.
{
Vec2 impulse = pool.popVec2();
Vec2 r1 = pool.popVec2();
Vec2 r2 = pool.popVec2();
Vec2 C = 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);
C.set(b2.m_sweep.c).addLocal(r2).subLocal(b1.m_sweep.c).subLocal(r1);
positionError = C.length();
float invMass1 = b1.m_invMass, invMass2 = b2.m_invMass;
float invI1 = b1.m_invI, invI2 = b2.m_invI;
// Handle large detachment.
final float k_allowedStretch = 10.0f * Settings.linearSlop;
if (C.lengthSquared() > k_allowedStretch * k_allowedStretch) {
Vec2 u = pool.popVec2();
// Use a particle solution (no rotation).
// u.set(C);
// u.normalize(); ?? we don't even use this
float m = invMass1 + invMass2;
if (m > 0.0f) {
m = 1.0f / m;
}
impulse.set(C).negateLocal().mulLocal(m);
final float k_beta = 0.5f;
// using u as temp variable
u.set(impulse).mulLocal(k_beta * invMass1);
b1.m_sweep.c.subLocal(u);
u.set(impulse).mulLocal(k_beta * invMass2);
b2.m_sweep.c.addLocal(u);
C.set(b2.m_sweep.c).addLocal(r2).subLocal(b1.m_sweep.c).subLocal(r1);
pool.pushVec2(1);
}
Mat22 K1 = pool.popMat22();
K1.m11 = invMass1 + invMass2;
K1.m21 = 0.0f;
K1.m12 = 0.0f;
K1.m22 = invMass1 + invMass2;
Mat22 K2 = pool.popMat22();
K2.m11 = invI1 * r1.y * r1.y;
K2.m21 = -invI1 * r1.x * r1.y;
K2.m12 = -invI1 * r1.x * r1.y;
K2.m22 = invI1 * r1.x * r1.x;
Mat22 K3 = pool.popMat22();
K3.m11 = invI2 * r2.y * r2.y;
K3.m21 = -invI2 * r2.x * r2.y;
K3.m12 = -invI2 * r2.x * r2.y;
K3.m22 = invI2 * r2.x * r2.x;
K1.addLocal(K2).addLocal(K3);
K1.solveToOut(C.negateLocal(), impulse); // just leave c negated
// using C as temp variable
C.set(impulse).mulLocal(b1.m_invMass);
b1.m_sweep.c.subLocal(C);
b1.m_sweep.a -= b1.m_invI * Vec2.cross(r1, impulse);
C.set(impulse).mulLocal(b2.m_invMass);
b2.m_sweep.c.addLocal(C);
b2.m_sweep.a += b2.m_invI * Vec2.cross(r2, impulse);
b1.synchronizeTransform();
b2.synchronizeTransform();
pool.pushMat22(3);
pool.pushVec2(4);
}