@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;
}
}
