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