@Override public void initVelocityConstraints(TimeStep step) { Body b = m_bodyB; float mass = b.getMass(); // Frequency float omega = 2.0f * MathUtils.PI * m_frequencyHz; // Damping coefficient float d = 2.0f * mass * m_dampingRatio * omega; // Spring stiffness float k = mass * (omega * omega); // magic formulas // gamma has units of inverse mass. // beta has units of inverse time. assert (d + step.dt * k > Settings.EPSILON); m_gamma = step.dt * (d + step.dt * k); if (m_gamma != 0.0f) { m_gamma = 1.0f / m_gamma; } m_beta = step.dt * k * m_gamma; Vec2 r = pool.popVec2(); // Compute the effective mass matrix. // Vec2 r = Mul(b.getTransform().R, m_localAnchor - b.getLocalCenter()); r.set(m_localAnchor).subLocal(b.getLocalCenter()); Mat22.mulToOut(b.getTransform().R, r, r); // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y // -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y // r1.x*r1.x] float invMass = b.m_invMass; float invI = b.m_invI; Mat22 K1 = pool.popMat22(); K1.m11 = invMass; K1.m21 = 0.0f; K1.m12 = 0.0f; K1.m22 = invMass; Mat22 K2 = pool.popMat22(); K2.m11 = invI * r.y * r.y; K2.m21 = -invI * r.x * r.y; K2.m12 = -invI * r.x * r.y; K2.m22 = invI * r.x * r.x; Mat22 K = pool.popMat22(); K.set(K1).addLocal(K2); K.m11 += m_gamma; K.m22 += m_gamma; K.invertToOut(m_mass); m_C.set(b.m_sweep.c).addLocal(r).subLocal(m_target); // Cheat with some damping b.m_angularVelocity *= 0.98f; // Warm starting. m_impulse.mulLocal(step.dtRatio); // pool Vec2 temp = pool.popVec2(); temp.set(m_impulse).mulLocal(invMass); b.m_linearVelocity.addLocal(temp); b.m_angularVelocity += invI * Vec2.cross(r, m_impulse); pool.pushVec2(2); pool.pushMat22(3); }