private static CollisionShape ExtractCapsule(SubMesh subMesh)
{
// Find the two triangles that are farthest apart. The
// distance between them is the distance betwen
// bottomCenter and topCenter, plus twice the capRadius.
// The centers of these two triangles define a line which
// contains the capsule segment. Finally, find the
// triangle whose center is furthest from the centers of
// those two triangles, and determine the distance from
// the center of that triangle to the line. That distance
// is the capRadius. Move from the original two triangles
// toward each other capRadius distance and that
// determines the bottomCenter and topCenter
int[] farthest = new int[2] {
-1, -1
};
float farthestDistanceSquared = 0.0f;
MeshTriangle[] triangles = ExtractSubmeshTriangles(subMesh);
int count = triangles.Length;
for (int i = 0; i < count; i++)
{
MeshTriangle t = triangles[i];
Vector3 c = t.Center();
for (int j = 0; j < count; j++)
{
float d = (c - triangles[j].Center()).LengthSquared;
if (farthestDistanceSquared < d)
{
farthest[0] = i;
farthest[1] = j;
farthestDistanceSquared = d;
}
}
}
Vector3 bottom = triangles[farthest[0]].Center();
Vector3 top = triangles[farthest[1]].Center();
float bottomTopDistance = (float)Math.Sqrt(farthestDistanceSquared);
float capRadiusSquared = 0f;
for (int i = 0; i < count; i++)
{
MeshTriangle t = triangles[i];
Vector3 c = t.Center();
float d = Primitives.SqDistPointSegment(bottom, top, c);
if (capRadiusSquared < d)
{
capRadiusSquared = d;
}
}
float capRadius = (float)Math.Sqrt(capRadiusSquared);
Vector3 unitBottomTop = (top - bottom) / bottomTopDistance;
return(new CollisionCapsule(bottom + (unitBottomTop * capRadius),
top - (unitBottomTop * capRadius),
capRadius));
}