private List <KinectSkeleton> SortSkeletons(List <KinectSkeleton> unsortedSkeletons, SkeletonSortMethod sortMethod)
{
if (sortMethod == SkeletonSortMethod.NoSort)
{
return(unsortedSkeletons);
}
else
{
//Seperate the tracked and untracked skeletons
List <KinectSkeleton> trackedSkeletons = new List <KinectSkeleton>();
List <KinectSkeleton> untrackedSkeletons = new List <KinectSkeleton>();
for (int i = 0; i < unsortedSkeletons.Count; i++)
{
if (unsortedSkeletons[i].skeleton.TrackingState == SkeletonTrackingState.NotTracked)
{
untrackedSkeletons.Add(unsortedSkeletons[i]);
}
else
{
trackedSkeletons.Add(unsortedSkeletons[i]);
}
}
if (sortMethod == SkeletonSortMethod.OriginXClosest || sortMethod == SkeletonSortMethod.OriginXFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && tempSkeleton.skeleton.Position.X < trackedSkeletons[insertIndex - 1].skeleton.Position.X)
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.OriginXFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.OriginYClosest || sortMethod == SkeletonSortMethod.OriginYFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && tempSkeleton.skeleton.Position.Y < trackedSkeletons[insertIndex - 1].skeleton.Position.Y)
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.OriginYFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.OriginZClosest || sortMethod == SkeletonSortMethod.OriginZFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && tempSkeleton.skeleton.Position.Z < trackedSkeletons[insertIndex - 1].skeleton.Position.Z)
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.OriginZFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.OriginEuclidClosest || sortMethod == SkeletonSortMethod.OriginEuclidFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
SkeletonPoint origin = new SkeletonPoint()
{
X = 0, Y = 0, Z = 0
};
double tempDistance = InterPointDistance(origin, trackedSkeletons[i].skeleton.Position);
while (insertIndex > 0 && tempDistance < InterPointDistance(origin, trackedSkeletons[insertIndex - 1].skeleton.Position))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.OriginEuclidFarthest)
{
trackedSkeletons.Reverse();
}
}
//TODO: Add feedback sort methods here
//Add the untracked skeletons to the tracked ones before sending everything back
trackedSkeletons.AddRange(untrackedSkeletons);
return(trackedSkeletons);
}
}
private List <KinectSkeleton> SortSkeletons(List <KinectSkeleton> unsortedSkeletons, SkeletonSortMethod sortMethod)
{
if (sortMethod == SkeletonSortMethod.NoSort)
{
return(unsortedSkeletons);
}
else
{
//Seperate the tracked and untracked skeletons
List <KinectSkeleton> trackedSkeletons = new List <KinectSkeleton>();
List <KinectSkeleton> untrackedSkeletons = new List <KinectSkeleton>();
for (int i = 0; i < unsortedSkeletons.Count; i++)
{
if (unsortedSkeletons[i].skeleton.TrackingState == SkeletonTrackingState.NotTracked)
{
untrackedSkeletons.Add(unsortedSkeletons[i]);
}
else
{
trackedSkeletons.Add(unsortedSkeletons[i]);
}
}
if (sortMethod == SkeletonSortMethod.OriginXClosest || sortMethod == SkeletonSortMethod.OriginXFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.X) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.X))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.OriginXFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.OriginYClosest || sortMethod == SkeletonSortMethod.OriginYFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.Y) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.Y))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.OriginYFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.OriginZClosest || sortMethod == SkeletonSortMethod.OriginZFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.Z) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.Z))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.OriginZFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.OriginEuclidClosest || sortMethod == SkeletonSortMethod.OriginEuclidFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
SkeletonPoint origin = new SkeletonPoint()
{
X = 0, Y = 0, Z = 0
};
double tempDistance = InterPointDistance(origin, trackedSkeletons[i].skeleton.Position);
while (insertIndex > 0 && tempDistance < InterPointDistance(origin, trackedSkeletons[insertIndex - 1].skeleton.Position))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.OriginEuclidFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (feedbackPosition != null) //Sort based on the feedback position, if it isn't null
{
if (sortMethod == SkeletonSortMethod.FeedbackXClosest || sortMethod == SkeletonSortMethod.FeedbackXFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.X - feedbackPosition.Value.X) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.X - feedbackPosition.Value.X))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.FeedbackXFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.FeedbackYClosest || sortMethod == SkeletonSortMethod.FeedbackYFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.Y - feedbackPosition.Value.Y) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.Y - feedbackPosition.Value.Y))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.FeedbackYFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.FeedbackZClosest || sortMethod == SkeletonSortMethod.FeedbackZFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.Z - feedbackPosition.Value.Z) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.Z - feedbackPosition.Value.Z))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.FeedbackZFarthest)
{
trackedSkeletons.Reverse();
}
}
else if (sortMethod == SkeletonSortMethod.FeedbackEuclidClosest || sortMethod == SkeletonSortMethod.FeedbackEuclidFarthest)
{
//We only care about the tracked skeletons, so only sort those
for (int i = 1; i < trackedSkeletons.Count; i++)
{
int insertIndex = i;
KinectSkeleton tempSkeleton = trackedSkeletons[i];
SkeletonPoint feedPosition = new SkeletonPoint()
{
X = (float)feedbackPosition.Value.X, Y = (float)feedbackPosition.Value.Y, Z = (float)feedbackPosition.Value.Z
};
double tempDistance = InterPointDistance(feedPosition, trackedSkeletons[i].skeleton.Position);
while (insertIndex > 0 && tempDistance < InterPointDistance(feedPosition, trackedSkeletons[insertIndex - 1].skeleton.Position))
{
trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1];
insertIndex--;
}
trackedSkeletons[insertIndex] = tempSkeleton;
}
if (sortMethod == SkeletonSortMethod.FeedbackEuclidFarthest)
{
trackedSkeletons.Reverse();
}
}
else
{
return(unsortedSkeletons);
}
}
else
{
return(unsortedSkeletons);
}
//Add the untracked skeletons to the tracked ones before sending everything back
trackedSkeletons.AddRange(untrackedSkeletons);
return(trackedSkeletons);
}
}