public virtual void getWitnessPoints(Vec2 pA, Vec2 pB)
{
switch (m_count)
{
case 0:
Debug.Assert(false);
break;
case 1:
pA.set_Renamed(m_v1.wA);
pB.set_Renamed(m_v1.wB);
break;
case 2:
case2.set_Renamed(m_v1.wA).mulLocal(m_v1.a);
pA.set_Renamed(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_Renamed(m_v1.wB).mulLocal(m_v1.a);
pB.set_Renamed(m_v2.wB).mulLocal(m_v2.a).addLocal(case2);
break;
case 3:
pA.set_Renamed(m_v1.wA).mulLocal(m_v1.a);
case3.set_Renamed(m_v2.wA).mulLocal(m_v2.a);
case33.set_Renamed(m_v3.wA).mulLocal(m_v3.a);
pA.addLocal(case3).addLocal(case33);
pB.set_Renamed(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 getLinearVelocityFromWorldPointToOut(Vec2 worldPoint, Vec2 out_Renamed)
{
out_Renamed.set_Renamed(worldPoint).subLocal(m_sweep.c);
Vec2.crossToOut(m_angularVelocity, out_Renamed, out_Renamed);
out_Renamed.addLocal(m_linearVelocity);
}
public void computeCentroidToOut(Vec2[] vs, int count, Vec2 out_Renamed)
{
Debug.Assert(count >= 3);
out_Renamed.set_Renamed(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;
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_Renamed(p2).subLocal(p1);
e2.set_Renamed(p3).subLocal(p1);
float D = Vec2.cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
e1.set_Renamed(p1).addLocal(p2).addLocal(p3).mulLocal(triangleArea * inv3);
out_Renamed.addLocal(e1);
}
// Centroid
Debug.Assert(area > Settings.EPSILON);
out_Renamed.mulLocal(1.0f / area);
}