| private RenderSkeletonOnColor ( KinectSkeleton skeleton, Color renderColor, int kinectID ) : void | ||
| skeleton | KinectSkeleton | |
| renderColor | Color | |
| kinectID | int | |
| return | void | |
private void RenderSkeletonOnColor(KinectSkeleton skeleton, Color renderColor, int kinectID)
{
if (colorSource != null)
{
//Calculate the offset
Point offset = new Point(0.0, 0.0);
if (ColorImageCanvas.ActualWidth != ColorImage.ActualWidth)
{
offset.X = (ColorImageCanvas.ActualWidth - ColorImage.ActualWidth) / 2;
}
if (ColorImageCanvas.ActualHeight != ColorImage.ActualHeight)
{
offset.Y = (ColorImageCanvas.ActualHeight - ColorImage.ActualHeight) / 2;
}
//Render all the bones (this can't be looped because the enum isn't ordered in order of bone connections)
//If there is a neck, we need to draw it different
if (skeleton.skeleton[JointType.Neck].TrackingState != TrackingState.NotTracked)
{
DrawBoneOnColor(skeleton.skeleton[JointType.Head], skeleton.skeleton[JointType.Neck], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.Neck], skeleton.skeleton[JointType.SpineShoulder], renderColor, 2.0, offset, kinectID);
}
else if (skeleton.skeleton[JointType.Head].TrackingState != TrackingState.NotTracked && skeleton.skeleton[JointType.ShoulderCenter].TrackingState != TrackingState.NotTracked)
{
DrawBoneOnColor(skeleton.skeleton[JointType.Head], skeleton.skeleton[JointType.ShoulderCenter], renderColor, 2.0, offset, kinectID);
}
DrawBoneOnColor(skeleton.skeleton[JointType.ShoulderCenter], skeleton.skeleton[JointType.ShoulderLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.ShoulderLeft], skeleton.skeleton[JointType.ElbowLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.ElbowLeft], skeleton.skeleton[JointType.WristLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.WristLeft], skeleton.skeleton[JointType.HandLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.ShoulderCenter], skeleton.skeleton[JointType.ShoulderRight], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.ShoulderRight], skeleton.skeleton[JointType.ElbowRight], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.ElbowRight], skeleton.skeleton[JointType.WristRight], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.WristRight], skeleton.skeleton[JointType.HandRight], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.ShoulderCenter], skeleton.skeleton[JointType.Spine], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.Spine], skeleton.skeleton[JointType.HipCenter], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.HipCenter], skeleton.skeleton[JointType.HipLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.HipLeft], skeleton.skeleton[JointType.KneeLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.KneeLeft], skeleton.skeleton[JointType.AnkleLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.AnkleLeft], skeleton.skeleton[JointType.FootLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.HipCenter], skeleton.skeleton[JointType.HipRight], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.HipRight], skeleton.skeleton[JointType.KneeRight], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.KneeRight], skeleton.skeleton[JointType.AnkleRight], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.AnkleRight], skeleton.skeleton[JointType.FootRight], renderColor, 2.0, offset, kinectID);
//The thumb and hand tip only get draw if the skeleton is a Kinect v2, but that's okay, the logic in the method will figure it out
DrawBoneOnColor(skeleton.skeleton[JointType.HandLeft], skeleton.skeleton[JointType.ThumbLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.HandLeft], skeleton.skeleton[JointType.HandTipLeft], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.HandRight], skeleton.skeleton[JointType.ThumbRight], renderColor, 2.0, offset, kinectID);
DrawBoneOnColor(skeleton.skeleton[JointType.HandRight], skeleton.skeleton[JointType.HandTipRight], renderColor, 2.0, offset, kinectID);
for (int i = 0; i < skeleton.skeleton.Count; i++)
{
DrawJointPointOnColor(skeleton.skeleton[i], renderColor, 2.0, offset, kinectID);
}
DrawHandStateOnColor(skeleton.skeleton[JointType.HandLeft], skeleton.leftHandClosed, 5.0, offset, kinectID);
DrawHandStateOnColor(skeleton.skeleton[JointType.HandRight], skeleton.rightHandClosed, 5.0, offset, kinectID);
}
drawingColorSkeleton = false;
}
//Merges all the skeletons together and transmits the new master skeletons VIA vrpn
private void mergeAndTransmitSkeletons()
{
List <KinectSkeleton> masterSkeletons = new List <KinectSkeleton>();
List <int[]> pointsAverages = new List <int[]>();
List <List <bool> > rightHandStates = new List <List <bool> >();
List <List <bool> > leftHandStates = new List <List <bool> >();
#region Merge the skeleton data
for (int i = 0; i < serverMasterOptions.kinectOptions.Count; i++)
{
if (serverMasterOptions.kinectOptions[i].trackSkeletons)
{
List <KinectSkeleton> localCopy = new List <KinectSkeleton>(kinects[i].skeletonData.actualSkeletons);
for (int localIndex = localCopy.Count - 1; localIndex >= 0; localIndex--)
{
if (localCopy[localIndex].skeleton == null || localCopy[localIndex].skeleton.TrackingState == SkeletonTrackingState.NotTracked)
{
localCopy.RemoveAt(localIndex);
}
else
{
bool skeletonFound = false;
for (int masterIndex = 0; masterIndex < masterSkeletons.Count; masterIndex++)
{
double dist = InterPointDistance(localCopy[localIndex].skeleton.Position, masterSkeletons[masterIndex].skeleton.Position);
if (dist < 0.2)
{
skeletonFound = true;
masterSkeletons[masterIndex].skeleton.Position = IncAverage(masterSkeletons[masterIndex].skeleton.Position, localCopy[localIndex].skeleton.Position, pointsAverages[masterIndex][20]);
pointsAverages[masterIndex][20]++;
//Average the joints
for (int jointIndex = 0; jointIndex < masterSkeletons[masterIndex].skeleton.Joints.Count; jointIndex++)
{
Joint tempJoint = new Joint();
//If the new skeleton joint has tracking data, and the old one doesn't, use the new one and update the tracking state
if (masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex].TrackingState == JointTrackingState.NotTracked && localCopy[localIndex].skeleton.Joints[(JointType)jointIndex].TrackingState != JointTrackingState.NotTracked)
{
tempJoint = localCopy[localIndex].skeleton.Joints[(JointType)jointIndex];
masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex] = tempJoint;
pointsAverages[masterIndex][jointIndex] = 0;
}
//If they are both inferred, just average them
else if (masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex].TrackingState == JointTrackingState.Inferred && localCopy[localIndex].skeleton.Joints[(JointType)jointIndex].TrackingState == JointTrackingState.Inferred)
{
tempJoint = masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex];
tempJoint.Position = IncAverage(masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex].Position, localCopy[localIndex].skeleton.Joints[(JointType)jointIndex].Position, pointsAverages[masterIndex][jointIndex]);
pointsAverages[masterIndex][jointIndex]++;
masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex] = tempJoint;
}
//If they are both tracked, just average them
else if (masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex].TrackingState == JointTrackingState.Tracked && localCopy[localIndex].skeleton.Joints[(JointType)jointIndex].TrackingState == JointTrackingState.Tracked)
{
tempJoint = masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex];
tempJoint.Position = IncAverage(masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex].Position, localCopy[localIndex].skeleton.Joints[(JointType)jointIndex].Position, pointsAverages[masterIndex][jointIndex]);
pointsAverages[masterIndex][jointIndex]++;
masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex] = tempJoint;
}
//If they new one is tracked, and the old one is inferred, use the new one and update the tracking state - COULD DO A WEIGHTED AVERAGE HERE!!
else if (masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex].TrackingState == JointTrackingState.Inferred && localCopy[localIndex].skeleton.Joints[(JointType)jointIndex].TrackingState == JointTrackingState.Tracked)
{
tempJoint = localCopy[localIndex].skeleton.Joints[(JointType)jointIndex];
pointsAverages[masterIndex][jointIndex] = 0;
masterSkeletons[masterIndex].skeleton.Joints[(JointType)jointIndex] = tempJoint;
}
//Otherwise, we just ignore it
//Merge the hands
if ((JointType)jointIndex == JointType.HandRight)
{
if (localCopy[localIndex].skeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked)
{
rightHandStates[masterIndex].Add(localCopy[localIndex].rightHandClosed);
}
}
else if ((JointType)jointIndex == JointType.HandLeft)
{
if (localCopy[localIndex].skeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked)
{
leftHandStates[masterIndex].Add(localCopy[localIndex].leftHandClosed);
}
}
}
localCopy.RemoveAt(localIndex);
break;
}
}
if (!skeletonFound)
{
masterSkeletons.Add(localCopy[localIndex]);
pointsAverages.Add(new int[] { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }); //Use 21 N's for the averaging, even though there at 20 joints (the 21st is for the position)
if (localCopy[localIndex].skeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked)
{
rightHandStates.Add(new List <bool>(new bool[] { localCopy[localIndex].rightHandClosed }));
}
else
{
rightHandStates.Add(new List <bool>());
}
if (localCopy[localIndex].skeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked)
{
leftHandStates.Add(new List <bool>(new bool[] { localCopy[localIndex].leftHandClosed }));
}
else
{
leftHandStates.Add(new List <bool>());
}
localCopy.RemoveAt(localIndex);
}
}
}
}
kinects[i].skeletonData.needsUpdate = false;
}
#endregion
//Sort all the skeletons before transmitting them
masterSkeletons = SortSkeletons(masterSkeletons, serverMasterOptions.skeletonOptions.skeletonSortMode);
//Transmit all skeletons here
if (isRunning)
{
for (int skeletonIndex = 0; skeletonIndex < masterSkeletons.Count; skeletonIndex++)
{
SendSkeletonVRPN(masterSkeletons[skeletonIndex].skeleton, skeletonIndex);
//Transmit the hand grips, if we have good data on them
if (leftHandStates.Count > 0)
{
int grippedLeft = leftHandStates[skeletonIndex].FindAll(x => x == true).Count;
bool leftState = grippedLeft >= ((leftHandStates.Count + 1) / 2); //If at least half of the available data says it is gripped, it is gripped
for (int j = 0; j < buttonServers.Count; j++)
{
if (serverMasterOptions.buttonServers[j].serverName == serverMasterOptions.skeletonOptions.individualSkeletons[skeletonIndex].leftGripServerName)
{
lock (buttonServers[j])
{
buttonServers[j].Buttons[serverMasterOptions.skeletonOptions.individualSkeletons[skeletonIndex].leftGripButtonNumber] = leftState;
}
break;
}
}
}
if (rightHandStates.Count > 0)
{
int grippedRight = rightHandStates[skeletonIndex].FindAll(x => x == true).Count;
bool rightState = grippedRight >= ((rightHandStates.Count + 1) / 2); //If at least half of the available data says it is gripped, it is gripped
for (int j = 0; j < buttonServers.Count; j++)
{
if (serverMasterOptions.buttonServers[j].serverName == serverMasterOptions.skeletonOptions.individualSkeletons[skeletonIndex].rightGripServerName)
{
lock (buttonServers[j])
{
buttonServers[j].Buttons[serverMasterOptions.skeletonOptions.individualSkeletons[skeletonIndex].rightGripButtonNumber] = rightState;
}
break;
}
}
}
}
}
//Update stuff on the GUI
if (GUI)
{
if (parent.Dispatcher.Thread.ManagedThreadId == Thread.CurrentThread.ManagedThreadId)
{
//Update the number of skeletons being tracked on the GUI
parent.TrackedSkeletonsTextBlock.Text = masterSkeletons.Count.ToString();
parent.ColorImageCanvas.Children.Clear();
for (int skeletonIndex = 0; skeletonIndex < masterSkeletons.Count; skeletonIndex++)
{
parent.RenderSkeletonOnColor(masterSkeletons[skeletonIndex].skeleton, serverMasterOptions.skeletonOptions.individualSkeletons[skeletonIndex].renderColor);
}
}
else
{
parent.Dispatcher.BeginInvoke((Action)(() =>
{
//Update the number of skeletons being tracked on the GUI
parent.TrackedSkeletonsTextBlock.Text = masterSkeletons.Count.ToString();
parent.ColorImageCanvas.Children.Clear();
for (int skeletonIndex = 0; skeletonIndex < masterSkeletons.Count; skeletonIndex++)
{
parent.RenderSkeletonOnColor(masterSkeletons[skeletonIndex].skeleton, serverMasterOptions.skeletonOptions.individualSkeletons[skeletonIndex].renderColor);
}
}), null
);
}
}
//Call the GUI skeleton renderer
}
