private void WriteFrame(FrameOfMocapData data, TelemetryData telemetry)
        {
            String str = "";
            bool   recordMarkerData = true;

            //bool recordForcerData = false;
            //bool recordRBData = false;

            str += data.fTimestamp.ToString("F3") + "\t";
            str += telemetry.TransmitLatency.ToString("F3") + "\t";
            str += telemetry.TotalLatency.ToString("F3") + "\t";
            str += telemetry.DroppedFrames.ToString() + "\t";

            // 'all' markerset data
            if (recordMarkerData)
            {
                for (int i = 0; i < m_FrameOfData.nMarkerSets; i++)
                {
                    NatNetML.MarkerSetData ms = m_FrameOfData.MarkerSets[i];
                    if (ms.MarkerSetName == "all")
                    {
                        for (int j = 0; j < ms.nMarkers; j++)
                        {
                            str += ms.Markers[j].x.ToString("F3") + "\t";
                            str += ms.Markers[j].y.ToString("F3") + "\t";
                            str += ms.Markers[j].z.ToString("F3") + "\t";
                        }
                    }
                }
            }

            mWriter.WriteLine(str);
        }
        /// <summary>
        /// Update the spreadsheet.
        /// Note: This refresh is quite slow and provided here only as a complete example.
        /// In a production setting this would be optimized.
        /// </summary>
        private void UpdateDataGrid()
        {
            // update MarkerSet data
            for (int i = 0; i < m_FrameOfData.nMarkerSets; i++)
            {
                NatNetML.MarkerSetData ms = m_FrameOfData.MarkerSets[i];
                for (int j = 0; j < ms.nMarkers; j++)
                {
                    string strUniqueName = ms.MarkerSetName + j.ToString();
                    int    key           = strUniqueName.GetHashCode();
                    if (htMarkers.Contains(key))
                    {
                        int rowIndex = (int)htMarkers[key];
                        if (rowIndex >= 0)
                        {
                            dataGridView1.Rows[rowIndex].Cells[1].Value = ms.Markers[j].x;
                            dataGridView1.Rows[rowIndex].Cells[2].Value = ms.Markers[j].y;
                            dataGridView1.Rows[rowIndex].Cells[3].Value = ms.Markers[j].z;
                        }
                    }
                }
            }

            // update RigidBody data
            for (int i = 0; i < m_FrameOfData.nRigidBodies; i++)
            {
                NatNetML.RigidBodyData rb = m_FrameOfData.RigidBodies[i];
                int key = rb.ID.GetHashCode();

                // note : must add rb definitions here one time instead of on get data descriptions because we don't know the marker list yet.
                if (!htRigidBodies.ContainsKey(key))
                {
                    // Add RigidBody def to the grid
                    if (rb.Markers[0].ID != -1)
                    {
                        RigidBody rbDef = FindRB(rb.ID);
                        if (rbDef != null)
                        {
                            int rowIndex = dataGridView1.Rows.Add("RigidBody: " + rbDef.Name);
                            key = rb.ID.GetHashCode();
                            htRigidBodies.Add(key, rowIndex);
                            // Add Markers associated with this rigid body to the grid
                            for (int j = 0; j < rb.nMarkers; j++)
                            {
                                String strUniqueName     = rbDef.Name + "-" + rb.Markers[j].ID.ToString();
                                int    keyMarker         = strUniqueName.GetHashCode();
                                int    newRowIndexMarker = dataGridView1.Rows.Add(strUniqueName);
                                htMarkers.Add(keyMarker, newRowIndexMarker);
                            }
                        }
                    }
                }
                else
                {
                    // update RigidBody data
                    int rowIndex = (int)htRigidBodies[key];
                    if (rowIndex >= 0)
                    {
                        bool tracked = rb.Tracked;
                        if (!tracked)
                        {
                            OutputMessage("RigidBody not tracked in this frame.");
                        }

                        dataGridView1.Rows[rowIndex].Cells[1].Value = rb.x * m_ServerToMillimeters;
                        dataGridView1.Rows[rowIndex].Cells[2].Value = rb.y * m_ServerToMillimeters;
                        dataGridView1.Rows[rowIndex].Cells[3].Value = rb.z * m_ServerToMillimeters;

                        // Convert quaternion to eulers.  Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
                        float[] quat = new float[4] {
                            rb.qx, rb.qy, rb.qz, rb.qw
                        };
                        float[] eulers = new float[3];
                        eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr);
                        double x = RadiansToDegrees(eulers[0]);     // convert to degrees
                        double y = RadiansToDegrees(eulers[1]);
                        double z = RadiansToDegrees(eulers[2]);

                        /*
                         * if (m_UpAxis == 2)
                         * {
                         *  double yOriginal = y;
                         *  y = -z;
                         *  z = yOriginal;
                         * }
                         */


                        dataGridView1.Rows[rowIndex].Cells[4].Value = x;
                        dataGridView1.Rows[rowIndex].Cells[5].Value = y;
                        dataGridView1.Rows[rowIndex].Cells[6].Value = z;

                        // update Marker data associated with this rigid body
                        for (int j = 0; j < rb.nMarkers; j++)
                        {
                            if (rb.Markers[j].ID != -1)
                            {
                                RigidBody rbDef = FindRB(rb.ID);
                                if (rbDef != null)
                                {
                                    String strUniqueName = rbDef.Name + "-" + rb.Markers[j].ID.ToString();
                                    int    keyMarker     = strUniqueName.GetHashCode();
                                    if (htMarkers.ContainsKey(keyMarker))
                                    {
                                        int             rowIndexMarker = (int)htMarkers[keyMarker];
                                        NatNetML.Marker m = rb.Markers[j];
                                        dataGridView1.Rows[rowIndexMarker].Cells[1].Value = m.x;
                                        dataGridView1.Rows[rowIndexMarker].Cells[2].Value = m.y;
                                        dataGridView1.Rows[rowIndexMarker].Cells[3].Value = m.z;
                                    }
                                }
                            }
                        }
                    }
                }
            }

            // update Skeleton data
            for (int i = 0; i < m_FrameOfData.nSkeletons; i++)
            {
                NatNetML.SkeletonData sk = m_FrameOfData.Skeletons[i];
                for (int j = 0; j < sk.nRigidBodies; j++)
                {
                    // note : skeleton rigid body ids are of the form:
                    // parent skeleton ID   : high word (upper 16 bits of int)
                    // rigid body id        : low word  (lower 16 bits of int)
                    NatNetML.RigidBodyData rb = sk.RigidBodies[j];
                    int skeletonID            = HighWord(rb.ID);
                    int rigidBodyID           = LowWord(rb.ID);
                    int uniqueID = skeletonID * 1000 + rigidBodyID;
                    int key      = uniqueID.GetHashCode();
                    if (htRigidBodies.ContainsKey(key))
                    {
                        int rowIndex = (int)htRigidBodies[key];
                        if (rowIndex >= 0)
                        {
                            dataGridView1.Rows[rowIndex].Cells[1].Value = rb.x;
                            dataGridView1.Rows[rowIndex].Cells[2].Value = rb.y;
                            dataGridView1.Rows[rowIndex].Cells[3].Value = rb.z;

                            // Convert quaternion to eulers.  Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
                            float[] quat = new float[4] {
                                rb.qx, rb.qy, rb.qz, rb.qw
                            };
                            float[] eulers = new float[3];
                            eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr);
                            double x = RadiansToDegrees(eulers[0]);     // convert to degrees
                            double y = RadiansToDegrees(eulers[1]);
                            double z = RadiansToDegrees(eulers[2]);

                            dataGridView1.Rows[rowIndex].Cells[4].Value = x;
                            dataGridView1.Rows[rowIndex].Cells[5].Value = y;
                            dataGridView1.Rows[rowIndex].Cells[6].Value = z;

                            // Marker data associated with this rigid body
                            for (int k = 0; k < rb.nMarkers; k++)
                            {
                            }
                        }
                    }
                }
            }   // end skeleton update


            // update labeled markers data
            // remove previous dynamic marker list
            // for testing only - this simple approach to grid updating too slow for large marker count use
            if (false)
            {
                int nRows      = htMarkers.Count + htRigidBodies.Count;
                int nTotalRows = dataGridView1.Rows.Count;
                for (int i = nRows; i < nTotalRows; i++)
                {
                    dataGridView1.Rows.RemoveAt(nRows);
                }
                for (int i = 0; i < m_FrameOfData.nMarkers; i++)
                {
                    NatNetML.Marker m = m_FrameOfData.LabeledMarkers[i];

                    int modelID, markerID;
                    m_NatNet.DecodeID(m.ID, out modelID, out markerID);
                    int rowIndex = dataGridView1.Rows.Add("Labeled Marker (ModelID: " + modelID + "  MarkerID: " + markerID + ")");
                    dataGridView1.Rows[rowIndex].Cells[1].Value = m.x;
                    dataGridView1.Rows[rowIndex].Cells[2].Value = m.y;
                    dataGridView1.Rows[rowIndex].Cells[3].Value = m.z;
                }
            }
        }
Beispiel #3
0
        private void UpdateDataGrid()
        {
            // update MarkerSet data
            for (int i = 0; i < m_FrameOfData.nMarkerSets; i++)
            {
                NatNetML.MarkerSetData ms = m_FrameOfData.MarkerSets[i];
                for (int j = 0; j < ms.nMarkers; j++)
                {
                    string strUniqueName = ms.MarkerSetName + j.ToString();
                    int    key           = strUniqueName.GetHashCode();
                    if (htMarkers.Contains(key))
                    {
                        int rowIndex = (int)htMarkers[key];
                        if (rowIndex >= 0)
                        {
                            dataGridView1.Rows[rowIndex].Cells[1].Value = ms.Markers[j].x;
                            dataGridView1.Rows[rowIndex].Cells[2].Value = ms.Markers[j].y;
                            dataGridView1.Rows[rowIndex].Cells[3].Value = ms.Markers[j].z;
                        }
                    }
                }
            }

            // update RigidBody data
            for (int i = 0; i < m_FrameOfData.nRigidBodies; i++)
            {
                NatNetML.RigidBodyData rb = m_FrameOfData.RigidBodies[i];

                // testing - tools inverts qz for packetizing/sending, so invert it back
                float qx, qy, qw, qz;
                qx = rb.qx;
                qy = rb.qy;// *-1.0f;
                qz = -rb.qz;
                qw = rb.qw;

                // quats coming from Tools are already normalized
                //QuaternionNormalise(ref qx, ref qy, ref qw, ref qz);

                //qy = qy * 0.5f;


                int key = rb.ID.GetHashCode();
                if (htRigidBodies.ContainsKey(key))
                {
                    int rowIndex = (int)htRigidBodies[key];
                    if (rowIndex >= 0)
                    {
                        dataGridView1.Rows[rowIndex].Cells[1].Value = rb.x * m_ServerToMillimeters;
                        dataGridView1.Rows[rowIndex].Cells[2].Value = rb.y * m_ServerToMillimeters;
                        dataGridView1.Rows[rowIndex].Cells[3].Value = rb.z * m_ServerToMillimeters;
                        double y, z, x;
                        QuaternionToEuler(qx, qy, qz, qw, out y, out z, out x);
                        //y *= -1.0f;
                        z *= -1.0f;
                        y  = RadiansToDegrees(y);
                        z  = RadiansToDegrees(z);
                        x  = RadiansToDegrees(x);
                        dataGridView1.Rows[rowIndex].Cells[4].Value = y;
                        dataGridView1.Rows[rowIndex].Cells[5].Value = z;
                        dataGridView1.Rows[rowIndex].Cells[6].Value = x;

                        // Marker data associated with this rigid body
                        for (int j = 0; j < rb.nMarkers; j++)
                        {
                        }
                    }
                }
            }

            // update Skeleton data
            for (int i = 0; i < m_FrameOfData.nSkeletons; i++)
            {
                NatNetML.SkeletonData sk = m_FrameOfData.Skeletons[i];
                for (int j = 0; j < sk.nRigidBodies; j++)
                {
                    NatNetML.RigidBodyData rb = sk.RigidBodies[j];
                    int key = rb.ID.GetHashCode();
                    if (htRigidBodies.ContainsKey(key))
                    {
                        int rowIndex = (int)htRigidBodies[key];
                        if (rowIndex >= 0)
                        {
                            dataGridView1.Rows[rowIndex].Cells[1].Value = rb.x;
                            dataGridView1.Rows[rowIndex].Cells[2].Value = rb.y;
                            dataGridView1.Rows[rowIndex].Cells[3].Value = rb.z;
                            double y, z, x;
                            QuaternionToEuler(rb.qx, rb.qy, rb.qz, rb.qw, out y, out z, out x);
                            y = RadiansToDegrees(y);
                            z = RadiansToDegrees(z);
                            x = RadiansToDegrees(x);
                            dataGridView1.Rows[rowIndex].Cells[4].Value = y;
                            dataGridView1.Rows[rowIndex].Cells[5].Value = z;
                            dataGridView1.Rows[rowIndex].Cells[6].Value = x;

                            // Marker data associated with this rigid body
                            for (int k = 0; k < rb.nMarkers; k++)
                            {
                            }
                        }
                    }
                }
            }   // end skeleton update
        }
        public double[] get_last_frame(string track_type, int object_num)
        {
            /*
             * when asking for data set, default to the first marker set
             */
            //grab a frame
            FrameOfMocapData data = mNatNet.GetLastFrameOfData();


            //
            switch (track_type)
            {
            default:
                Console.WriteLine("Unrecognized track object type {0}", track_type);
                return(null);

            case "markers":     //defailt to return first marker of the firsr marker set
                NatNetML.MarkerSetData ms = data.MarkerSets[0];
                if (object_num < ms.nMarkers)
                {
                    return(new double[3] {
                        ms.Markers[object_num].x * m_ServerToMillimeters,
                        ms.Markers[object_num].x * m_ServerToMillimeters,
                        ms.Markers[object_num].z * m_ServerToMillimeters
                    });
                }
                else
                {
                    Console.WriteLine("the specified object number( {0} ) is greater than the total markers ({1})", object_num, ms.nMarkers);
                    return(null);
                }



            case "rb":

                if (debug)
                {
                    Console.WriteLine("asked for rigid body number {0}", object_num);
                    Console.WriteLine("number of rigid body: {0}", data.nRigidBodies);
                }

                if (object_num < data.nRigidBodies)
                {
                    NatNetML.RigidBodyData rb = data.RigidBodies[object_num];
                    //get the quotenions
                    float[] quat = new float[4] {
                        rb.qx, rb.qy, rb.qz, rb.qw
                    };
                    float[] eulers = new float[3];
                    eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr);
                    double x = RadiansToDegrees(eulers[0]);         // convert to degrees
                    double y = RadiansToDegrees(eulers[1]);
                    double z = RadiansToDegrees(eulers[2]);
                    if (debug)
                    {
                        Console.WriteLine("received x y z positions");
                        Console.WriteLine("{0}, {1}, {2}", x, y, z);
                        stopwatch.Stop();
                        Console.WriteLine("Reading the quene takes {0} ms", stopwatch.ElapsedMilliseconds);
                    }


                    return(new double[7] {
                        rb.x *m_ServerToMillimeters,                         //mm for obvious reaons
                        rb.y *m_ServerToMillimeters,
                        rb.z *m_ServerToMillimeters,
                        x, y, z, data.iFrame
                    });                                                //angles in degrees
                }
                else
                {
                    return(null);
                }
            }
        }
Beispiel #5
0
        private void UpdateDataGrid()
        {
            broadcast();

            for (int i = 0; i < m_FrameOfData.nMarkerSets; i++)
            {
                NatNetML.MarkerSetData ms = m_FrameOfData.MarkerSets[i];
                for (int j = 0; j < ms.nMarkers; j++)
                {
                    string strUniqueName = ms.MarkerSetName + j.ToString();
                    int    key           = strUniqueName.GetHashCode();
                    if (htMarkers.Contains(key))
                    {
                        int rowIndex = (int)htMarkers[key];
                        if (rowIndex >= 0)
                        {
                            dataGridView1.Rows[rowIndex].Cells[1].Value = ms.Markers[j].x;
                            dataGridView1.Rows[rowIndex].Cells[2].Value = ms.Markers[j].y;
                            dataGridView1.Rows[rowIndex].Cells[3].Value = ms.Markers[j].z;
                        }
                    }
                }
            }

            // update RigidBody data
            for (int i = 0; i < m_FrameOfData.nRigidBodies; i++)
            {
                NatNetML.RigidBodyData rb = m_FrameOfData.RigidBodies[i];
                int key = rb.ID.GetHashCode();

                float[] quat2 = new float[4] {
                    rb.qx, rb.qy, rb.qz, rb.qw
                };
                float[] euler2 = new float[3];
                euler2 = m_NatNet.QuatToEuler(quat2, (int)NATEulerOrder.NAT_XYZr);

                /*double x = RadiansToDegrees(eulers[0]);
                *  double y = RadiansToDegrees(eulers[1]);
                *  double z = RadiansToDegrees(eulers[2]);*/

                DateTime dt = DateTime.Now;
                int      t  = ((dt.Hour * 60 + dt.Minute) * 60 + dt.Second) * 1000 + dt.Millisecond;

                // note : must add rb definitions here one time instead of on get data descriptions because we don't know the marker list yet.
                if (!htRigidBodies.ContainsKey(key))
                {
                    // Add RigidBody def to the grid
                    if (rb.Markers[0].ID != -1)
                    {
                        RigidBody rbDef = FindRB(rb.ID);
                        if (rbDef != null)
                        {
                            int rowIndex = dataGridView1.Rows.Add("RigidBody: " + rbDef.Name);
                            key = rb.ID.GetHashCode();
                            htRigidBodies.Add(key, rowIndex);
                            // Add Markers associated with this rigid body to the grid
                            for (int j = 0; j < rb.nMarkers; j++)
                            {
                                String strUniqueName     = rbDef.Name + "-" + rb.Markers[j].ID.ToString();
                                int    keyMarker         = strUniqueName.GetHashCode();
                                int    newRowIndexMarker = dataGridView1.Rows.Add(strUniqueName);
                                htMarkers.Add(keyMarker, newRowIndexMarker);
                            }
                        }
                    }
                }
                else
                {
                    // update RigidBody data
                    int rowIndex = (int)htRigidBodies[key];
                    if (rowIndex >= 0)
                    {
                        dataGridView1.Rows[rowIndex].Cells[1].Value = rb.x * m_ServerToMillimeters;
                        dataGridView1.Rows[rowIndex].Cells[2].Value = rb.y * m_ServerToMillimeters;
                        dataGridView1.Rows[rowIndex].Cells[3].Value = rb.z * m_ServerToMillimeters;

                        // Convert quaternion to eulers.  Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
                        float[] quat = new float[4] {
                            rb.qx, rb.qy, rb.qz, rb.qw
                        };
                        float[] eulers = new float[3];
                        eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr);
                        double x = RadiansToDegrees(eulers[0]);     // convert to degrees
                        double y = RadiansToDegrees(eulers[1]);
                        double z = RadiansToDegrees(eulers[2]);

                        dataGridView1.Rows[rowIndex].Cells[4].Value = x;
                        dataGridView1.Rows[rowIndex].Cells[5].Value = y;
                        dataGridView1.Rows[rowIndex].Cells[6].Value = z;

                        // update Marker data associated with this rigid body
                        for (int j = 0; j < rb.nMarkers; j++)
                        {
                            if (rb.Markers[j].ID != -1)
                            {
                                RigidBody rbDef = FindRB(rb.ID);
                                if (rbDef != null)
                                {
                                    String strUniqueName = rbDef.Name + "-" + rb.Markers[j].ID.ToString();
                                    int    keyMarker     = strUniqueName.GetHashCode();
                                    if (htMarkers.ContainsKey(keyMarker))
                                    {
                                        int             rowIndexMarker = (int)htMarkers[keyMarker];
                                        NatNetML.Marker m = rb.Markers[j];
                                        dataGridView1.Rows[rowIndexMarker].Cells[1].Value = m.x;
                                        dataGridView1.Rows[rowIndexMarker].Cells[2].Value = m.y;
                                        dataGridView1.Rows[rowIndexMarker].Cells[3].Value = m.z;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }