public void GetWitnessPoints(Vec2 pA, Vec2 pB)
{
switch (Count)
{
case 0:
Debug.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:
Debug.Assert(false);
break;
}
}