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;
}
}
public void ComputeCentroidToOut(Vec2[] vs, int count, Vec2 result)
{
Debug.Assert(count >= 3);
result.Set(0.0f, 0.0f);
float area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 pRef = pool1;
pRef.SetZero();
Vec2 e1 = pool2;
Vec2 e2 = pool3;
const float inv3 = 1.0f / 3.0f;
for (int i = 0; i < count; ++i)
{
// Triangle vertices.
Vec2 p1 = pRef;
Vec2 p2 = vs[i];
Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0];
e1.Set(p2).SubLocal(p1);
e2.Set(p3).SubLocal(p1);
float D = Vec2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
e1.Set(p1).AddLocal(p2).AddLocal(p3).MulLocal(triangleArea * inv3);
result.AddLocal(e1);
}
// Centroid
Debug.Assert(area > Settings.EPSILON);
result.MulLocal(1.0f / area);
}
public void GetLinearVelocityFromWorldPointToOut(Vec2 worldPoint, Vec2 result)
{
result.Set(worldPoint).SubLocal(Sweep.C);
Vec2.CrossToOut(m_angularVelocity, result, result);
result.AddLocal(m_linearVelocity);
}