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); } }