@Override public void solveVelocityConstraints(final SolverData data) { Vec2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; final Vec2 vpA = pool.popVec2(); final Vec2 vpB = pool.popVec2(); final Vec2 PA = pool.popVec2(); final Vec2 PB = pool.popVec2(); Vec2.crossToOutUnsafe(wA, m_rA, vpA); vpA.addLocal(vA); Vec2.crossToOutUnsafe(wB, m_rB, vpB); vpB.addLocal(vB); float Cdot = -Vec2.dot(m_uA, vpA) - m_ratio * Vec2.dot(m_uB, vpB); float impulse = -m_mass * Cdot; m_impulse += impulse; PA.set(m_uA).mulLocal(-impulse); PB.set(m_uB).mulLocal(-m_ratio * impulse); vA.x += m_invMassA * PA.x; vA.y += m_invMassA * PA.y; wA += m_invIA * Vec2.cross(m_rA, PA); vB.x += m_invMassB * PB.x; vB.y += m_invMassB * PB.y; wB += m_invIB * Vec2.cross(m_rB, PB); // data.velocities[m_indexA].v.set(vA); data.velocities[m_indexA].w = wA; // data.velocities[m_indexB].v.set(vB); data.velocities[m_indexB].w = wB; pool.pushVec2(4); }
public void getWitnessPoints(Vec2 pA, Vec2 pB) { switch (m_count) { case 0: assert (false); break; case 1: pA.set(m_v1.wA); pB.set(m_v1.wB); break; case 2: case2.set(m_v1.wA).mulLocal(m_v1.a); pA.set(m_v2.wA).mulLocal(m_v2.a).addLocal(case2); // m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; // *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; case2.set(m_v1.wB).mulLocal(m_v1.a); pB.set(m_v2.wB).mulLocal(m_v2.a).addLocal(case2); break; case 3: pA.set(m_v1.wA).mulLocal(m_v1.a); case3.set(m_v2.wA).mulLocal(m_v2.a); case33.set(m_v3.wA).mulLocal(m_v3.a); pA.addLocal(case3).addLocal(case33); pB.set(pA); // *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; // *pB = *pA; break; default: assert (false); break; } }