public static List <Body> GetTrackedBodies(this Body[] bodies, short maxDistanceMM)
{
//First filter to the bodies which we want to engage
List <Body> result = new List <Body>();
for (int i = 0; i < bodies.Length; i++)
{
Body currentBody = bodies[i];
if (currentBody.IsTracked)
{
Joint headJoint = currentBody.Joints[JointType.Head];
if (headJoint.IsTracked() && headJoint.Position.DistanceToCamera() <= maxDistanceMM)
{
result.Add(currentBody);
}
}
}
//Now do insertion sort to really quickly sort by closest
for (int i = 1; i < result.Count; i++)
{
Body temp = result[i];
int j = i;
while (j > 0 && result[j - 1].DistanceToCamera() > temp.DistanceToCamera())
{
result[j] = result[j - 1];
j--;
}
result[j] = temp;
}
return(result);
}
public static double DistanceToCamera(this Body value)
{
Joint headJoint = value.Joints[JointType.Head];
if (headJoint.IsTracked())
{
return(headJoint.Position.DistanceToCamera());
}
else
{
return(short.MaxValue);
}
}
