private Vector3 GetJointPositionOnRgb(Xtr3D.Net.ExtremeMotion.Data.Joint joint)
{
float jointX = joint.skeletonPoint.ImgCoordNormHorizontal;
float jointY = joint.skeletonPoint.ImgCoordNormVertical;
float displayX = SkeletonDrawer.upperRightX + jointX * SkeletonDrawer.rgbWidth;
float displayY = SkeletonDrawer.upperRightY - jointY * SkeletonDrawer.rgbHeight;
return new Vector3(displayX, displayY);
}
public void UpdateJointsPosition(Xtr3D.Net.ExtremeMotion.Data.Joint firstJoint, Xtr3D.Net.ExtremeMotion.Data.Joint secondJoint)
{
if(firstJoint.jointTrackingState == JointTrackingState.NotTracked || secondJoint.jointTrackingState == JointTrackingState.NotTracked)
{
isTracked = false;
return;
}
else
{
Vector3 firstJointPosition = GetJointPositionOnRgb(firstJoint);
Vector3 secondJointPosition = GetJointPositionOnRgb(secondJoint);
boneLine.points2.Clear();
boneLine.points2.Add(firstJointPosition);
boneLine.points2.Add(secondJointPosition);
isTracked = true;
}
}
void MyDataFrameReady(object sender, Xtr3D.Net.ExtremeMotion.Data.DataFrameReadyEventArgs e)
{
// Opening the received frame
using (var dataFrame = e.OpenFrame() as DataFrame)
{
if (dataFrame != null) // Making sure it's really DataFrame
{
StringBuilder text = new StringBuilder();
Skeleton skel = dataFrame.Skeletons[0];
text.AppendFormat("Skeleton frame: {0}, state: {1}, proximity: {2}",
dataFrame.FrameKey.FrameNumberKey, skel.TrackingState.ToString(), skel.Proximity.SkeletonProximity);
//Console.WriteLine(text);//written to console for automatic tests
if (dataFrame.Skeletons[0] != null)
{
var joints = dataFrame.Skeletons[0].Joints; // Possibly several Skeletons, we'll use the first
TrackingState state = dataFrame.Skeletons[0].TrackingState;
SkeletonTrackingState.Text = state.ToString();
SetCalibrationIconVisibility(state);
// We only want to display a tracked Skeleton
if (joints.Head.jointTrackingState == JointTrackingState.Tracked)
{
m_skeletonDrawer.DrawSkeleton(joints);
}
else
{
m_skeletonDrawer.WipeSkeleton();
maestro.DisableAll();
}
if(state == TrackingState.Tracked)
{
double L0 = Math.Atan2(
joints.ElbowLeft.skeletonPoint.Y - joints.ShoulderLeft.skeletonPoint.Y,
-1 * joints.ElbowLeft.skeletonPoint.X - -1 * joints.ShoulderLeft.skeletonPoint.X) * 180.0 / Math.PI;
double L1 = Math.Atan2(
joints.HandLeft.skeletonPoint.Y - joints.ElbowLeft.skeletonPoint.Y,
-1 * joints.HandLeft.skeletonPoint.X - -1 * joints.ElbowLeft.skeletonPoint.X) * 180.0 / Math.PI;
double R0 = Math.Atan2(
joints.ElbowRight.skeletonPoint.Y - joints.ShoulderRight.skeletonPoint.Y,
joints.ElbowRight.skeletonPoint.X - joints.ShoulderRight.skeletonPoint.X) * 180.0 / Math.PI;
double R1 = Math.Atan2(
joints.HandRight.skeletonPoint.Y - joints.ElbowRight.skeletonPoint.Y,
joints.HandRight.skeletonPoint.X - joints.ElbowRight.skeletonPoint.X) * 180.0 / Math.PI;
StringBuilder text2 = new StringBuilder();
text2.AppendFormat("{4} L0:{0},R0:{1},L1:{2},R1:{3} \t",
Math.Round(L0), Math.Round(R0), Math.Round(L1) - Math.Round(L0), Math.Round(R1) - Math.Round(R0), DateTime.UtcNow.ToString("yy-MM-ddThh:mm:ss"));
ushort TL0 = maestro.DegreeToTarget(1, (int) Math.Round(L0));
ushort TR0 = maestro.DegreeToTarget(2, (int) Math.Round(R0));
ushort TL1 = maestro.DegreeToTarget(4, (int) (Math.Round(L1) - Math.Round(L0)));
ushort TR1 = maestro.DegreeToTarget(5, (int) (Math.Round(R1) - Math.Round(R0)));
text2.AppendFormat("TL0:{0},TR0:{1},TL1:{2},TR1:{3}",
TL0, TR0, TL1, TR1);
file.WriteLine(text2);//written to console for automatic tests
maestro.TrySetTarget(1, TL0);
maestro.TrySetTarget(2, TR0);
maestro.TrySetTarget(4, TL1);
maestro.TrySetTarget(5, TR1);
}
UpdateFrameEdges(dataFrame.Skeletons[0].ClippedEdges); // Reading the Skeleton Edge warnings
}
}
}
}
