static void parseDataDescriptor(List <NatNetML.DataDescriptor> description) { // [NatNet] Request a description of the Active Model List from the server. // This sample will list only names of the data sets, but you can access int numDataSet = description.Count; Console.WriteLine("Total {0} data sets in the capture:", numDataSet); for (int i = 0; i < numDataSet; ++i) { int dataSetType = description[i].type; // Parse Data Descriptions for each data sets and save them in the delcared lists and hashtables for later uses. switch (dataSetType) { case ((int)NatNetML.DataDescriptorType.eMarkerSetData): NatNetML.MarkerSet mkset = (NatNetML.MarkerSet)description[i]; Console.WriteLine("\tMarkerSet ({0})", mkset.Name); break; case ((int)NatNetML.DataDescriptorType.eRigidbodyData): NatNetML.RigidBody rb = (NatNetML.RigidBody)description[i]; Console.WriteLine("\tRigidBody ({0})", rb.Name); // Saving Rigid Body Descriptions mRigidBodies.Add(rb); break; default: // When a Data Set does not match any of the descriptions provided by the SDK. Console.WriteLine("\tError: Invalid Data Set"); break; } } }
private NatNetPoseData ProcessPoseData(NatNetML.FrameOfMocapData data) { NatNetPoseData poseData = new NatNetPoseData(); /* Parsing Simple Marker Data */ for (int i = 0; i < data.nMarkers; i++) { Marker marker = data.LabeledMarkers[i]; int mID = marker.ID; // Console.WriteLine("\tMarker ({0}): \t\tpos ({0:N3}, {1:N3}, {2:N3})", mID, marker.x, marker.y, marker.z); poseData.mPos = new Vector3(marker.x, marker.y, marker.z); } /* Parsing Rigid Body Frame Data */ for (int i = 0; i < mRigidBodies.Count; i++) { int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions for (int j = 0; j < data.nRigidBodies; j++) { if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data. { NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions if (rbData.Tracked == true) { poseData.rbPos = new Vector3(rbData.x, rbData.y, rbData.z); poseData.rbRot = new Quaternion(rbData.qx, rbData.qy, rbData.qz, rbData.qw); } else { NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions Console.WriteLine("\t{0} is not tracked in current frame", rb.Name); } } } } return(poseData); }
private void RequestDataDescription() { //Clear previous rigid bodies records mRBNames.Clear(); // [NatNet] request data descriptions from server app. List <NatNetML.DataDescriptor> descs = new List <NatNetML.DataDescriptor>(); bool bSuccess = mNatNet.GetDataDescriptions(out descs); if (bSuccess) { foreach (NatNetML.DataDescriptor d in descs) { // RigidBodies if (d.type == (int)NatNetML.DataDescriptorType.eRigidbodyData) { NatNetML.RigidBody rb = (NatNetML.RigidBody)d; //FLogger.Log(LogType.Debug, "RigidBody: " + rb.Name); mRBNames.Add(rb.Name); } } } }
/// <summary> /// [NatNet] Request a description of the Active Model List from the server (e.g. Motive) and build up a new spreadsheet /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void buttonGetDataDescriptions_Click(object sender, EventArgs e) { mRigidBodies.Clear(); dataGridView1.Rows.Clear(); htMarkers.Clear(); htRigidBodies.Clear(); needMarkerListUpdate = true; OutputMessage("Retrieving Data Descriptions...."); List <NatNetML.DataDescriptor> descs = new List <NatNetML.DataDescriptor>(); bool bSuccess = m_NatNet.GetDataDescriptions(out descs); if (bSuccess) { OutputMessage(String.Format("Retrieved {0} Data Descriptions....", descs.Count)); int iObject = 0; foreach (NatNetML.DataDescriptor d in descs) { iObject++; // MarkerSets if (d.type == (int)NatNetML.DataDescriptorType.eMarkerSetData) { NatNetML.MarkerSet ms = (NatNetML.MarkerSet)d; OutputMessage("Data Def " + iObject.ToString() + " [MarkerSet]"); OutputMessage(" Name : " + ms.Name); OutputMessage(String.Format(" Markers ({0}) ", ms.nMarkers)); dataGridView1.Rows.Add("MarkerSet: " + ms.Name); for (int i = 0; i < ms.nMarkers; i++) { OutputMessage((" " + ms.MarkerNames[i])); int rowIndex = dataGridView1.Rows.Add(" " + ms.MarkerNames[i]); // MarkerNameIndexToRow map String strUniqueName = ms.Name + i.ToString(); int key = strUniqueName.GetHashCode(); htMarkers.Add(key, rowIndex); } } // RigidBodies else if (d.type == (int)NatNetML.DataDescriptorType.eRigidbodyData) { NatNetML.RigidBody rb = (NatNetML.RigidBody)d; OutputMessage("Data Def " + iObject.ToString() + " [RigidBody]"); OutputMessage(" Name : " + rb.Name); OutputMessage(" ID : " + rb.ID); OutputMessage(" ParentID : " + rb.parentID); OutputMessage(" OffsetX : " + rb.offsetx); OutputMessage(" OffsetY : " + rb.offsety); OutputMessage(" OffsetZ : " + rb.offsetz); mRigidBodies.Add(rb); int rowIndex = dataGridView1.Rows.Add("RigidBody: " + rb.Name); // RigidBodyIDToRow map int key = rb.ID.GetHashCode(); htRigidBodies.Add(key, rowIndex); } // Skeletons else if (d.type == (int)NatNetML.DataDescriptorType.eSkeletonData) { NatNetML.Skeleton sk = (NatNetML.Skeleton)d; OutputMessage("Data Def " + iObject.ToString() + " [Skeleton]"); OutputMessage(" Name : " + sk.Name); OutputMessage(" ID : " + sk.ID); dataGridView1.Rows.Add("Skeleton: " + sk.Name); for (int i = 0; i < sk.nRigidBodies; i++) { RigidBody rb = sk.RigidBodies[i]; OutputMessage(" RB Name : " + rb.Name); OutputMessage(" RB ID : " + rb.ID); OutputMessage(" ParentID : " + rb.parentID); OutputMessage(" OffsetX : " + rb.offsetx); OutputMessage(" OffsetY : " + rb.offsety); OutputMessage(" OffsetZ : " + rb.offsetz); int rowIndex = dataGridView1.Rows.Add("Bone: " + rb.Name); // RigidBodyIDToRow map int uniqueID = sk.ID * 1000 + rb.ID; int key = uniqueID.GetHashCode(); if (htRigidBodies.ContainsKey(key)) { MessageBox.Show("Duplicate RigidBody ID"); } else { htRigidBodies.Add(key, rowIndex); } } } else { OutputMessage("Unknown DataType"); } } } else { OutputMessage("Unable to retrieve DataDescriptions"); } }
static void processFrameData(NatNetML.FrameOfMocapData data) { /* Parsing Rigid Body Frame Data */ for (int i = 0; i < mRigidBodies.Count; i++) { int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions for (int j = 0; j < data.nRigidBodies; j++) { if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data. { NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions if (rbData.Tracked == true) { Console.WriteLine("\tRigidBody ({0}):", rb.Name); Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z); // Rigid Body Euler Orientation float[] quat = new float[4] { rbData.qx, rbData.qy, rbData.qz, rbData.qw }; float[] eulers = new float[3]; eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation. double xrot = RadiansToDegrees(eulers[0]); double yrot = RadiansToDegrees(eulers[1]); double zrot = RadiansToDegrees(eulers[2]); Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot); } else { Console.WriteLine("\t{0} is not tracked in current frame", rb.Name); } } } } /* Parsing Skeleton Frame Data */ for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions { int sklID = mSkeletons[i].ID; for (int j = 0; j < data.nSkeletons; j++) { if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data. { NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data Console.WriteLine("\tSkeleton ({0}):", skl.Name); Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies); /* Now, for each of the skeleton segments */ for (int k = 0; k < sklData.nRigidBodies; k++) { NatNetML.RigidBodyData boneData = sklData.RigidBodies[k]; /* Decoding skeleton bone ID */ int skeletonID = HighWord(boneData.ID); int rigidBodyID = LowWord(boneData.ID); int uniqueID = skeletonID * 1000 + rigidBodyID; int key = uniqueID.GetHashCode(); NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions //Outputting only the hip segment data for the purpose of this sample. if (k == 0) { Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z); } } } } } /* Parsing Force Plate Frame Data */ for (int i = 0; i < mForcePlates.Count; i++) { int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions for (int j = 0; j < data.nForcePlates; j++) { if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data. { NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data Console.WriteLine("\tForce Plate ({0}):", fp.Serial); // Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame. for (int k = 0; k < fpData.nChannels; k++) { Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]); } } } } Console.WriteLine("\n"); }
static void parseDataDescriptor(List <NatNetML.DataDescriptor> description) { // [NatNet] Request a description of the Active Model List from the server. // This sample will list only names of the data sets, but you can access int numDataSet = description.Count; Console.WriteLine("Total {0} data sets in the capture:", numDataSet); for (int i = 0; i < numDataSet; ++i) { int dataSetType = description[i].type; // Parse Data Descriptions for each data sets and save them in the delcared lists and hashtables for later uses. switch (dataSetType) { case ((int)NatNetML.DataDescriptorType.eMarkerSetData): NatNetML.MarkerSet mkset = (NatNetML.MarkerSet)description[i]; Console.WriteLine("\tMarkerSet ({0})", mkset.Name); break; case ((int)NatNetML.DataDescriptorType.eRigidbodyData): NatNetML.RigidBody rb = (NatNetML.RigidBody)description[i]; Console.WriteLine("\tRigidBody ({0})", rb.Name); // Saving Rigid Body Descriptions mRigidBodies.Add(rb); break; case ((int)NatNetML.DataDescriptorType.eSkeletonData): NatNetML.Skeleton skl = (NatNetML.Skeleton)description[i]; Console.WriteLine("\tSkeleton ({0}), Bones:", skl.Name); //Saving Skeleton Descriptions mSkeletons.Add(skl); // Saving Individual Bone Descriptions for (int j = 0; j < skl.nRigidBodies; j++) { Console.WriteLine("\t\t{0}. {1}", j + 1, skl.RigidBodies[j].Name); int uniqueID = skl.ID * 1000 + skl.RigidBodies[j].ID; int key = uniqueID.GetHashCode(); mHtSkelRBs.Add(key, skl.RigidBodies[j]); //Saving the bone segments onto the hashtable } break; case ((int)NatNetML.DataDescriptorType.eForcePlateData): NatNetML.ForcePlate fp = (NatNetML.ForcePlate)description[i]; Console.WriteLine("\tForcePlate ({0})", fp.Serial); // Saving Force Plate Channel Names mForcePlates.Add(fp); for (int j = 0; j < fp.ChannelCount; j++) { Console.WriteLine("\t\tChannel {0}: {1}", j + 1, fp.ChannelNames[j]); } break; default: // When a Data Set does not match any of the descriptions provided by the SDK. Console.WriteLine("\tError: Invalid Data Set"); break; } } }
static int processFrameData(NatNetML.FrameOfMocapData data, int index) { int index2 = 1; string startupPath = Environment.CurrentDirectory; //Console.WriteLine(startupPath); string pathCmd = string.Format("cd {0}\\..\\..", startupPath); //Console.WriteLine(pathCmd); // Create the MATLAB instance MLApp.MLApp matlab = new MLApp.MLApp(); // Change to the directory where the function is located matlab.Execute(pathCmd); Console.WriteLine("labeled markers: " + data.nMarkers); /* Parsing Rigid Body Frame Data */ for (int i = 0; i < mRigidBodies.Count; i++) { int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions for (int j = 0; j < data.nRigidBodies; j++) { if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data. { NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions if (rbData.Tracked == true && HLdata.Count > 0 && index > 0) { NatNetML.Marker marker = data.LabeledMarkers[i]; int mID = marker.ID; //Console.WriteLine("\tMarker ({0}):", mID); //Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", marker.x, marker.y, marker.z); //Console.WriteLine("\tRigidBody ({0}):", rb.Name); //Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z); // Rigid Body Euler Orientation double[] rbquat = new double[4] { rbData.qx, rbData.qy, rbData.qz, rbData.qw }; double[] rbpos = new double[3] { rbData.x, rbData.y, rbData.z }; double[] mkpos = new double[3] { marker.x, marker.y, marker.z }; int idxHl = HLdata.Count - 1; double[] kbP = new double[3] { (double)HLdata[idxHl][0], (double)HLdata[idxHl][1], (double)HLdata[idxHl][2] }; double[] kbQ = new double[4] { (double)HLdata[idxHl][3], (double)HLdata[idxHl][4], (double)HLdata[idxHl][5], (double)HLdata[idxHl][6] }; double[] hlPs = new double[3] { (double)HLdata[idxHl][7], (double)HLdata[idxHl][8], (double)HLdata[idxHl][9] }; double[] hlQs = new double[4] { (double)HLdata[idxHl][10], (double)HLdata[idxHl][11], (double)HLdata[idxHl][12], (double)HLdata[idxHl][13] }; bool mlEval = true; if (mlEval) { object newmarkerpos = null; // object result = null; //float[] A = { 1, 2 }; //float[] B = { 2, 2 }; // matlab.Feval("add", 1, out result, A, B); //object[] res2 = result as object[]; //Console.WriteLine(Convert.ToSingle(res2[0])); //Console.WriteLine(res2[1]); matlab.Feval("trans", 3, out newmarkerpos, mkpos, rbpos, rbquat, kbP, kbQ, hlPs, hlQs); object[] res = newmarkerpos as object[]; double xpos = Convert.ToDouble(res[0]); double ypos = Convert.ToDouble(res[1]); double zpos = Convert.ToDouble(res[2]); double[] newmarkerpos1 = new double[3] { xpos, ypos, zpos }; object result1 = null; //Console.WriteLine(index); matlab.Feval("recognition", 2, out result1, newmarkerpos1, index); object[] res2 = result1 as object[]; Console.WriteLine(res2[0]); Console.WriteLine(res2[1]); index2 = Convert.ToInt32(res2[0]); } } else { if (rbData.Tracked) { Console.WriteLine("\t HLData {0}, index {1}", HLdata.Count, index); } else { Console.WriteLine("\t{0} is not tracked in current frame", rb.Name); } } } } } //Console.WriteLine(index2); return(index2); }
//static uint GPS_LEAPSECONDS_MILLIS = 18000; static void processFrameData(NatNetML.FrameOfMocapData data) { bool data_gogo = true; /* Parsing Rigid Body Frame Data */ for (int i = 0; i < mRigidBodies.Count; i++) { int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions for (int j = 0; j < data.nRigidBodies; j++) { if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data. { NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions if (rbData.Tracked == true) { #if DEBUG_MSG Console.WriteLine("\tRigidBody ({0}):", rb.Name); Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z); // Rigid Body Euler Orientation float[] quat = new float[4] { rbData.qx, rbData.qy, rbData.qz, rbData.qw }; float[] eulers = new float[3]; eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation. double xrot = RadiansToDegrees(eulers[0]); double yrot = RadiansToDegrees(eulers[1]); double zrot = RadiansToDegrees(eulers[2]); Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot); #endif if (drones.ContainsKey(rb.Name)) { DroneData drone = drones[rb.Name]; drone.lost_count = 0; long cur_ms = stopwatch.ElapsedMilliseconds; if (drone.send_count > 0) { drone.send_count--; } else if (data_gogo) { drone.send_count = 10; data_gogo = false; MAVLink.mavlink_att_pos_mocap_t att_pos = new MAVLink.mavlink_att_pos_mocap_t(); att_pos.time_usec = (ulong)(cur_ms * 1000); att_pos.x = rbData.x; //north att_pos.y = rbData.z; //east att_pos.z = -rbData.y; //down att_pos.q = new float[4] { rbData.qw, rbData.qx, rbData.qz, -rbData.qy }; byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.ATT_POS_MOCAP, att_pos); if (drone.lastTime >= 0) { MAVLink.mavlink_vision_speed_estimate_t vis_speed = new MAVLink.mavlink_vision_speed_estimate_t(); float total_s = (cur_ms - drone.lastTime) * 0.001f; vis_speed.x = (rbData.x - drone.lastPosN) / total_s; vis_speed.y = (rbData.z - drone.lastPosE) / total_s; vis_speed.z = (-rbData.y - drone.lastPosD) / total_s; vis_speed.usec = (ulong)(cur_ms * 1000); byte[] pkt2 = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.VISION_SPEED_ESTIMATE, vis_speed); int total_len = pkt.Length + pkt2.Length; byte[] uwb_data = new byte[10 + total_len + 1]; uwb_data[0] = 0x54; uwb_data[1] = 0xf1; uwb_data[2] = 0xff; uwb_data[3] = 0xff; uwb_data[4] = 0xff; uwb_data[5] = 0xff; uwb_data[6] = 2; uwb_data[7] = drone.uwb_tag_id; uwb_data[8] = (byte)(total_len & 0xff); uwb_data[9] = (byte)(total_len >> 16); Array.Copy(pkt, 0, uwb_data, 10, pkt.Length); Array.Copy(pkt2, 0, uwb_data, 10 + pkt.Length, pkt2.Length); byte chk_sum = 0; foreach (byte uwb_data_byte in uwb_data) { chk_sum += uwb_data_byte; } uwb_data[uwb_data.Length - 1] = chk_sum; mavSock.SendTo(uwb_data, drone.ep); } } drone.lastTime = cur_ms; drone.lastPosN = rbData.x; drone.lastPosE = rbData.z; drone.lastPosD = -rbData.y; } } else { Console.WriteLine("\t{0} is not tracked in current frame", rb.Name); /*if (drones.ContainsKey(rb.Name)) * { * DroneData drone = drones[rb.Name]; * drone.lost_count++; * if (drone.lost_count > 3) * { * drone.lost_count = 0; * MAVLink.mavlink_gps_input_t gps_input = new MAVLink.mavlink_gps_input_t(); * gps_input.gps_id = 0; * gps_input.fix_type = (byte)MAVLink.GPS_FIX_TYPE.NO_FIX; * byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.GPS_INPUT, gps_input); * mavSock.SendTo(pkt, drone.ep); * } * }*/ } } } } /* Parsing Skeleton Frame Data */ for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions { int sklID = mSkeletons[i].ID; for (int j = 0; j < data.nSkeletons; j++) { if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data. { NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data Console.WriteLine("\tSkeleton ({0}):", skl.Name); Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies); /* Now, for each of the skeleton segments */ for (int k = 0; k < sklData.nRigidBodies; k++) { NatNetML.RigidBodyData boneData = sklData.RigidBodies[k]; /* Decoding skeleton bone ID */ int skeletonID = HighWord(boneData.ID); int rigidBodyID = LowWord(boneData.ID); int uniqueID = skeletonID * 1000 + rigidBodyID; int key = uniqueID.GetHashCode(); NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions //Outputting only the hip segment data for the purpose of this sample. if (k == 0) { Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z); } } } } } /* Parsing Force Plate Frame Data */ for (int i = 0; i < mForcePlates.Count; i++) { int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions for (int j = 0; j < data.nForcePlates; j++) { if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data. { NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data Console.WriteLine("\tForce Plate ({0}):", fp.Serial); // Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame. for (int k = 0; k < fpData.nChannels; k++) { Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]); } } } } #if DEBUG_MSG Console.WriteLine("\n"); #endif }
private NatNetPoseData ProcessPoseData(NatNetML.FrameOfMocapData data) { NatNetPoseData poseData = new NatNetPoseData(); /* Parsing Simple Marker Data */ for (int i = 0; i < data.nMarkers; i++) { Marker marker = data.LabeledMarkers[i]; int mID = marker.ID; // Console.WriteLine("\tMarker ({0}): \t\tpos ({0:N3}, {1:N3}, {2:N3})", mID, marker.x, marker.y, marker.z); poseData.mPos = new Vector3(marker.x, marker.y, marker.z); } /* Parsing Rigid Body Frame Data */ for (int i = 0; i < mRigidBodies.Count; i++) { int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions for (int j = 0; j < data.nRigidBodies; j++) { if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data. { NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions if (rbData.Tracked == true) { poseData.rbPos = new Vector3(rbData.x, rbData.y, rbData.z); poseData.rbRot = new Quaternion(rbData.qx, rbData.qy, rbData.qz, rbData.qw); //Console.WriteLine("\tRigidBody ({0}):", rb.Name); //Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z); // Rigid Body Euler Orientation float[] quat = new float[4] { rbData.qx, rbData.qy, rbData.qz, rbData.qw }; float[] eulers = new float[3]; eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation. double xrot = RadiansToDegrees(eulers[0]); double yrot = RadiansToDegrees(eulers[1]); double zrot = RadiansToDegrees(eulers[2]); //Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot); string display = string.Format("\tRigidBody ({0}):", rb.Name); display += string.Format("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z); display += string.Format("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot); //Console.WriteLine(display); } else { Console.WriteLine("\t{0} is not tracked in current frame", rb.Name); } } } } return(poseData); }