private static void UpdateMarkerInList(Marker currentMarker)
 {
     listOfMarkers[currentMarker.MarkerId].TimeStamp = currentMarker.TimeStamp;
     listOfMarkers[currentMarker.MarkerId].xCoordinate = currentMarker.xCoordinate;
     listOfMarkers[currentMarker.MarkerId].yCoordinate = currentMarker.yCoordinate;
     listOfMarkers[currentMarker.MarkerId].zCoordinate = currentMarker.zCoordinate;
 }
 public static PositionData ConvertMarkerPosition(Marker marker)
 {
     PositionData position = new PositionData();
     position.MarkerID = marker.MarkerId;
     position.Time = marker.TimeStamp;
     position.XCoordinate = marker.xCoordinate;
     position.YCoordinate = marker.yCoordinate;
     position.ZCoordinate = marker.zCoordinate;
     return position;
 }
示例#3
0
        /// <summary>
        /// Constructor - creates new marker using parameter marker value
        /// </summary>
        /// <param name="newMarker"></param>
        public Marker(Marker newMarker)
        {
            markerId = newMarker.MarkerId;
            timeStamp = newMarker.TimeStamp;
            xCoordinate = newMarker.xCoordinate;
            yCoordinate = newMarker.yCoordinate;
            zCoordinate = newMarker.zCoordinate;

            prevX = newMarker.prevXCoordinate;
            prevY = newMarker.prevYCoordinate;
            prevZ = newMarker.prevZCoordinate;
            m_prevTimestamp = newMarker.PrevTimestamp;
        }
        /// <summary>
        /// Adds a given marker to the marker list
        /// </summary>
        /// <param name="currentMarker"></param>
        public static void AddMarker(Marker currentMarker)
        {
            bool markerFound = false;

            foreach (Marker marker in listOfMarkers)
            {
                if (marker.MarkerId == currentMarker.MarkerId)
                {
                    markerFound = true;
                    if (currentMarker.MarkerId != -1)
                    {
                        UpdateMarkerInList(currentMarker);
                        return;
                    }
                }
            }

            if (markerFound == false)
            {
                listOfMarkers.Add(new Marker(currentMarker));
            }
        }
示例#5
0
 public void CalibrateFoot(Marker calibrationMarker)
 {
     footPositionList = new List<PositionData>();
     //TCPProcessor.WholeFrameReceivedEvent +=
     //    new TCPProcessor.WholeFrameReceivedHandler(TCPProcessor_WholeFrameReceivedEvent);
     this.calibrationMarker = calibrationMarker;
     previousMarker = calibrationMarker;
     //
     currentMarker = new Marker(previousMarker);
     //
     PositionData positionCalibration = PositionData.ConvertMarkerPosition(calibrationMarker);
     this.footPositionList.Add(positionCalibration);
     footdown_yCoordinate = calibrationMarker.yCoordinate;
     currentID = calibrationMarker.MarkerId;
     xDirection = 0;// -1 -> indicates heading in -x direction
     yDirection = 0;// +1 -> +y direction etc
     zDirection = 0;// +1 (+z) is forwards, towards screen; -1 -> moving towards back of treadmill
     xPrevDirection = 0;
     yPrevDirection = 0;
     zPrevDirection = 0;
     zPreviousCoordinate = previousMarker.zCoordinate;
     yPreviousCoordinate = previousMarker.yCoordinate;
     xPreviousCoordinate = previousMarker.xCoordinate;
 }
        private static void HandleDataFromServer(object client)
        {
            TcpClient serverConnection = (TcpClient)client;


            BinaryReader binaryReader = new BinaryReader(new BufferedStream(serverConnection.GetStream()));

            int bytesRead;
            while (true)
            {
                bytesRead = 0;
                bool readTimestamp = false;
                try
                {

                    //blocks until a client sends a message

                    char messageType = binaryReader.ReadChar();
                    bytesRead = 1;

                    switch (messageType)
                    {
                        case 'M': // Marker

                            Marker receivedMarker = new Marker();
                            receivedMarker.MarkerId = binaryReader.ReadInt32();

                            //if have been sent End of Frame indication
                            if (receivedMarker.MarkerId == -2147483648)
                            {
                                receivedMarker.TimeStamp = binaryReader.ReadInt64();
                                readTimestamp = true;
                                MarkerList.RemoveExcessMarkersFromList(receivedMarker.TimeStamp);

                                if (WholeFrameReceivedEvent != null)
                                {
                                    WholeFrameReceivedEvent();
                                }
                            }

                            if (readTimestamp == false)
                            {
                                receivedMarker.TimeStamp = binaryReader.ReadInt64();
                            }

                            receivedMarker.xCoordinate = binaryReader.ReadDouble();
                            receivedMarker.yCoordinate = binaryReader.ReadDouble();
                            receivedMarker.zCoordinate = binaryReader.ReadDouble();

                            bytesRead = 36;

                            if (receivedMarker.MarkerId != -2147483648)
                            {
                                MarkerList.AddMarker(receivedMarker);
                            }

                            break;
                        case 'F': // Filtered Marker

                            Marker receivedFilteredMarker = new Marker();
                            receivedFilteredMarker.MarkerId = binaryReader.ReadInt32();

                            //if have been sent End of Frame indication
                            if (receivedFilteredMarker.MarkerId == -2147483648)
                            {
                                receivedFilteredMarker.TimeStamp = binaryReader.ReadInt64();
                                readTimestamp = true;
                                FilteredMarkerList.RemoveExcessMarkersFromList(receivedFilteredMarker.TimeStamp);

                                if (FilteredMarkerListReceivedEvent != null)
                                {
                                    FilteredMarkerListReceivedEvent();
                                }
                            }

                            if (readTimestamp == false)
                            {
                                receivedFilteredMarker.TimeStamp = binaryReader.ReadInt64();
                            }

                            receivedFilteredMarker.xCoordinate = binaryReader.ReadDouble();
                            receivedFilteredMarker.yCoordinate = binaryReader.ReadDouble();
                            receivedFilteredMarker.zCoordinate = binaryReader.ReadDouble();

                            bytesRead = 36;

                            if (receivedFilteredMarker.MarkerId != -2147483648)
                            {
                                FilteredMarkerList.AddMarker(receivedFilteredMarker);
                            }

                            break;
                        case 'R': // Trackable

                            Trackable receivedTrackable = new Trackable();
                            receivedTrackable.ID = binaryReader.ReadInt32();

                            receivedTrackable.TimeStamp = binaryReader.ReadInt64();

                            if (receivedTrackable.ID == -2147483648)
                            {
                                if (receivedTrackable.TimeStamp == 0)
                                    trackableList.Clear();
                                //End of List
                                if (TrackableListReceivedEvent != null)
                                    TrackableListReceivedEvent(trackableList);
                                //Read in any remaining data out of the buffer
                                receivedTrackable.TrackableIndex = binaryReader.ReadInt32();
                                receivedTrackable.Name = binaryReader.ReadString();
                                receivedTrackable.xCoordinate = binaryReader.ReadInt32();
                                receivedTrackable.yCoordinate = binaryReader.ReadInt32();
                                receivedTrackable.zCoordinate = binaryReader.ReadInt32();
                                receivedTrackable.Roll = binaryReader.ReadDouble();
                                receivedTrackable.Pitch = binaryReader.ReadDouble();
                                receivedTrackable.Yaw = binaryReader.ReadDouble();

                            }
                            else
                            {
                                if (trackableList.Count != 0) // alread contains data
                                {
                                    //If from a previous time frame, clear the list
                                    if (receivedTrackable.TimeStamp != trackableList[0].TimeStamp)
                                    {
                                        trackableList.Clear();
                                    }
                                }

                                receivedTrackable.TrackableIndex = binaryReader.ReadInt32();
                                receivedTrackable.Name = binaryReader.ReadString();
                                receivedTrackable.xCoordinate = binaryReader.ReadInt32();
                                receivedTrackable.yCoordinate = binaryReader.ReadInt32();
                                receivedTrackable.zCoordinate = binaryReader.ReadInt32();
                                receivedTrackable.Roll = binaryReader.ReadDouble();
                                receivedTrackable.Pitch = binaryReader.ReadDouble();
                                receivedTrackable.Yaw = binaryReader.ReadDouble();
                                trackableList.Add(receivedTrackable);
                            }

                            break;

                        case 'J': // Joint

                            Joint receivedJoint = new Joint();
                            receivedJoint.ID = binaryReader.ReadInt32();

                            receivedJoint.TimeStamp = binaryReader.ReadInt64();

                            if (receivedJoint.ID == -2147483648)
                            {
                                if (receivedJoint.TimeStamp == 0)
                                    jointList.Clear();

                                //End of List
                                if (JointListReceivedEvent != null)
                                    JointListReceivedEvent(jointList);


                                //Read in any remaining data out of the buffer
                                receivedJoint.Exists = binaryReader.ReadBoolean();
                                receivedJoint.Name = binaryReader.ReadString();
                                receivedJoint.Trackable1 = binaryReader.ReadInt32();
                                receivedJoint.Trackable2 = binaryReader.ReadInt32();
                                receivedJoint.RollOffset = binaryReader.ReadDouble();
                                receivedJoint.PitchOffset = binaryReader.ReadDouble();
                                receivedJoint.YawOffset = binaryReader.ReadDouble();
                                receivedJoint.Roll = binaryReader.ReadDouble();
                                receivedJoint.Pitch = binaryReader.ReadDouble();
                                receivedJoint.Yaw = binaryReader.ReadDouble();
                                receivedJoint.xCoordinate = binaryReader.ReadInt32();
                                receivedJoint.yCoordinate = binaryReader.ReadInt32();
                                receivedJoint.zCoordinate = binaryReader.ReadInt32();
                            }
                            else
                            {
                                if (jointList.Count != 0) // alread contains data
                                {
                                    //If from a previous time frame, clear the list
                                    if (receivedJoint.TimeStamp != jointList[0].TimeStamp)
                                    {
                                        jointList.Clear();
                                    }
                                }

                                receivedJoint.Exists = binaryReader.ReadBoolean();
                                receivedJoint.Name = binaryReader.ReadString();
                                receivedJoint.Trackable1 = binaryReader.ReadInt32();
                                receivedJoint.Trackable2 = binaryReader.ReadInt32();
                                receivedJoint.RollOffset = binaryReader.ReadDouble();
                                receivedJoint.PitchOffset = binaryReader.ReadDouble();
                                receivedJoint.YawOffset = binaryReader.ReadDouble();
                                receivedJoint.Roll = binaryReader.ReadDouble();
                                receivedJoint.Pitch = binaryReader.ReadDouble();
                                receivedJoint.Yaw = binaryReader.ReadDouble();
                                receivedJoint.xCoordinate = binaryReader.ReadInt32();
                                receivedJoint.yCoordinate = binaryReader.ReadInt32();
                                receivedJoint.zCoordinate = binaryReader.ReadInt32();
                                jointList.Add(receivedJoint);
                            }

                            break;


                        case 'C': // Camera

                            OptiTrackCamera receivedCamera = new OptiTrackCamera();
                            receivedCamera.CameraNumber = binaryReader.ReadInt32();
                            receivedCamera.xCoordinate = binaryReader.ReadDouble();
                            receivedCamera.yCoordinate = binaryReader.ReadDouble();
                            receivedCamera.zCoordinate = binaryReader.ReadDouble();

                            double[] rotationMatrixBuffer = new double[9];

                            for (int i = 0; i < 9; i++)
                            {
                                rotationMatrixBuffer[i] = binaryReader.ReadDouble();
                            }
                            receivedCamera.RotationMatrix = rotationMatrixBuffer;

                            bytesRead = 108;

                            OptitrackCameraList.AddCamera(receivedCamera);

                            break;

                        case 'T': // Treadmill (Speed)
                            if (AvatarSpeedChangedEvent != null)
                            {
                                AvatarSpeedChangedEvent((float)binaryReader.ReadDouble());
                                bytesRead = 8;
                            }
                            else
                            {
                                binaryReader.ReadDouble();
                                bytesRead = 8;
                            }
                            break;
                        case 'B': //flag type and flag
                            {
                                System.Diagnostics.Debug.WriteLine("MessageType: " + messageType.ToString());

                                char type = binaryReader.ReadChar();

                                System.Diagnostics.Debug.WriteLine("Type: " + type.ToString());

                                bool flag = binaryReader.ReadBoolean();

                                System.Diagnostics.Debug.WriteLine("Flag: " + flag.ToString());

                                bytesRead = 9;

                                switch (type)
                                {
                                    case 'F': //Toggle Feet
                                        {
                                            if (ToggleDrawingOfFeetEvent != null)
                                            {
                                                ToggleDrawingOfFeetEvent(flag);
                                            }
                                            break;
                                        }
                                }
                                break;
                            }

                        default:
                            System.Diagnostics.Debug.WriteLine("A transmit error has occured. " + messageType.ToString() + " recieved");
                            bytesRead = 5;
                            break;
                    }


                }
                catch (System.IO.IOException)
                {
                    // a socket error has occured
                    System.Diagnostics.Debug.WriteLine("Socket error has occured in TCPServer.HandleClientComm");
                    break;
                }
                if (bytesRead == 0)
                {
                    // the client has disconnected from the server
                    System.Diagnostics.Debug.WriteLine("The client has disconnected from the server");
                    break;
                }

            }
        }
 public void Reset()
 {
     m_leftFootMarker = null;
     m_rightFootMarker = null;
     m_leftFootTrackable = null;
     m_rightFootTrackable = null;
 }
        /// <summary>
        /// Stores the calibration (inital) positions of the left and right feet.
        /// </summary>
        public void Store(Trackable leftFoot, Trackable rightFoot)
        {
            m_leftFootTrackable = new Trackable(leftFoot);
            m_rightFootTrackable = new Trackable(rightFoot);

            m_leftFootMarker = null;
            m_rightFootMarker = null;

            if (m_calibrationUpdated != null)
            {
                m_calibrationUpdated();
            }
        }
        /// <summary>
        /// Stores the calibration (inital) positions of the left and right feet.
        /// </summary>
        public void Store(Marker leftFoot, Marker rightFoot)
        {
            m_leftFootMarker = new Marker(leftFoot);
            m_rightFootMarker = new Marker(rightFoot);

            m_leftFootTrackable = null;
            m_rightFootTrackable = null;

            if (m_calibrationUpdated != null)
            {
                m_calibrationUpdated();
            }
        }
示例#10
0
 private void TCPProcessor_TrackableListReceivedEvent(List<Trackable> newTrackableList)
 {
     trackableList = new List<Trackable>(newTrackableList);
     Marker m1 = (Marker)(trackableList.Find(LeftFoot));
     Marker m2 = (Marker)(trackableList.Find(RightFoot));
     if (currentID == 1) currentMarker = m1;
     if (currentID == 2) currentMarker = m2;
     PositionData footData = PositionData.ConvertMarkerPosition(currentMarker);
     this.footPositionList.Add(footData);
     MarkerTrack();
     //FrontFoot(m1, m2);
 }
示例#11
0
        /// <summary>
        /// Transmits a marker to the current tcpClient.
        /// </summary>
        /// <param name="marker"></param>
        public void Transmit(Marker marker)
        {
            //lock this code so that if multiple Transmits are called (marker + speed for example), only one thread can access the connection stream at a time
            lock (m_lock)
            {
                //create a list to store any dead connections in (for removal from the main list at the end of the function)
                List<TcpClient> connectionsToRemoveFromList = new List<TcpClient>(clientConnectionList.Count);

                foreach (TcpClient singleClientConnection in clientConnectionList)
                {
                    try
                    {
                        if (singleClientConnection.Connected == false)
                        {
                            throw new System.IO.IOException(singleClientConnection.Client.RemoteEndPoint.ToString() + " is no longer connected to the server");
                        }
                        else
                        {
                            //send the data
                            BinaryWriter binaryWriter = new BinaryWriter(new BufferedStream(singleClientConnection.GetStream()));

                            binaryWriter.Write('M');

                            binaryWriter.Write(marker.MarkerId);
                            binaryWriter.Write(marker.TimeStamp);

                            binaryWriter.Write(marker.xCoordinate);
                            binaryWriter.Write(marker.yCoordinate);
                            binaryWriter.Write(marker.zCoordinate);
                            binaryWriter.Flush();
                        }
                    }

                    catch (System.IO.IOException ex)
                    {
                        System.Diagnostics.Debug.WriteLine("The client has disconnected. Exception message: " + ex.Message);
                        connectionsToRemoveFromList.Add(singleClientConnection);
                    }

                    catch (InvalidOperationException ex)
                    {
                        System.Diagnostics.Debug.WriteLine("The client has disconnected. Exception message: " + ex.Message);
                        connectionsToRemoveFromList.Add(singleClientConnection);
                    }

                }

                foreach (TcpClient connectionToRemove in connectionsToRemoveFromList)
                {
                    clientConnectionList.Remove(connectionToRemove);
                }
            }
        }
示例#12
0
        private void MotionCapture_MarkerListAvaliableEvent(List<Marker> markerList, long timeStamp)
        {
            tempMarkerList = new List<Marker>(markerList);
            tempMarkerListTimeStamp = timeStamp;

            foreach (Marker marker in tempMarkerList)
            {
                Transmit(marker);
            }
            Marker endOfFrameMarker = new Marker();
            endOfFrameMarker.TimeStamp = markerList.Count;
            endOfFrameMarker.MarkerId = -2147483648;
            Transmit(endOfFrameMarker);


        }
示例#13
0
        void FilteredMarkerProcessor_FilteredMarkerListAvaliableEvent()
        {
            List<Marker> tempFilteredMarkerList = new List<Marker>(FilteredMarkerList.listOfMarkers);

            int[] treadmillMakrerIDNumbers;
            if (mcTreadmill.DetectTreadmillSpeed)
            {
                treadmillMakrerIDNumbers = mcTreadmill.FindSpeed(tempFilteredMarkerList); // Find the speed and get the 1st and (if applicable) 2nd treadmill marker id
               
                if (treadmillMakrerIDNumbers != null) // If a marker number was returned then strip them from the list
                {
                    int newMarkerNumber = 0;
                    List<Marker> newTempFilteredMarkerList = new List<Marker>();
                    foreach (Marker tempMarker in tempFilteredMarkerList)
                    {
                        //If the marker ID doesn't match the first or second tredmill marker then add it to the new filtered makrer lsit
                        //or the 3rd or 4th - now extended to allow for upto 4 visible treadmill markers
                        if ((tempMarker.MarkerId != treadmillMakrerIDNumbers[0]) && (tempMarker.MarkerId != treadmillMakrerIDNumbers[1]))
                            //&& (tempMarker.MarkerId != treadmillMakrerIDNumbers[2]) && (tempMarker.MarkerId != treadmillMakrerIDNumbers[3]))
                        {
                            Marker newTempMarker = new Marker(tempMarker);
                            newTempMarker.MarkerId = newMarkerNumber;
                            newMarkerNumber++;
                            newTempFilteredMarkerList.Add(newTempMarker);
                            System.Diagnostics.Debug.WriteLine("Filtered marker x/y/z:" + newTempMarker.xCoordinate + " " + newTempMarker.yCoordinate + " " + newTempMarker.zCoordinate);
                        }
                    }
                    //System.Diagnostics.Debug.WriteLine("\n");
                        
                    // Save the newly generated list
                    tempFilteredMarkerList = new List<Marker>(newTempFilteredMarkerList);
                }

            }



            foreach (Marker marker in tempFilteredMarkerList)
            {
                TransmitFiltered(marker);
            }
            Marker endOfFrameMarker = new Marker();
            endOfFrameMarker.TimeStamp = tempFilteredMarkerList.Count;
            endOfFrameMarker.MarkerId = -2147483648;
            TransmitFiltered(endOfFrameMarker);

        }
        /// <summary>
        /// Updates the list of markers with lastest marker coordinates
        /// </summary>
        public static void UpdateCoordinates(bool updateMarker, bool updateTrackable)
        {
            if (APIRunning)
            {
                // Get the timestamp to apply to all markers and trackables
                long timeStamp = System.DateTime.Now.Ticks;
                try
                {
                    if (TrackingTools.UpdateAll() == 0) // If it updates correctly
                    {
                        if (updateMarker)
                        {
                            int markerCount = TrackingTools.FrameMarkerCount();

                            Marker marker = new Marker();
                            for (int i = 0; i < markerCount; i++)
                            {
                                //Get the marker details from TrackingTools
                                marker.MarkerId = i;
                                marker.TimeStamp = timeStamp;
                                marker.xCoordinate = -(int)(TrackingTools.FrameMarkerX(i) * 1000);
                                marker.yCoordinate = (int)(TrackingTools.FrameMarkerY(i) * 1000);
                                marker.zCoordinate = (int)(TrackingTools.FrameMarkerZ(i) * 1000);

                                MarkerList.AddMarker(marker);
                                marker = new Marker();
                            }
                            MarkerList.RemoveExcessMarkersFromList(markerCount);

                            OnMarkerListAvaliable(MarkerList.listOfMarkers, timeStamp);

                        }

                        if (updateTrackable)
                        {
                            int trackableCount = TrackingTools.TrackableCount();
                            trackableList.Clear();
                            Trackable newTrackable = null;

                            // Go through the loaded trackables and if they are in the current frame
                            // add them to the trackableList
                            for (int i = 0; i < trackableCount; i++)
                            {
                                if (TrackingTools.IsTrackableTracked(i))
                                {
                                    newTrackable = TrackingTools.GetTrackableLocation(i);
                                    newTrackable.TimeStamp = timeStamp;
                                    trackableList.Add(newTrackable);
                                }
                            }
                            OnTrackableListAvaliable(trackableList, timeStamp);
                        }
                    }
                }
                catch (AccessViolationException ex)
                {
                    System.Diagnostics.Debug.WriteLine("Error With UpdateALL() " + ex + "   Time Stamp : " + timeStamp);

                }
                catch (System.Runtime.InteropServices.SEHException ex)
                {
                    System.Diagnostics.Debug.WriteLine("Error With UpdateALL() " + ex + "   Time Stamp : " + timeStamp);

                }
            }
        }
 /// <summary>
 /// Constructor. Creates OpenGlMarker from an optitrack Marker.
 /// </summary>
 /// <param name="marker"></param>
 public OpenGLMarker(Marker marker)
     : base((float)(marker.xCoordinate / SCALING_FACTOR) - m_radius, (float)(marker.yCoordinate / SCALING_FACTOR), -(float)(marker.zCoordinate / SCALING_FACTOR) - m_radius, (float)(marker.xCoordinate / SCALING_FACTOR) + m_radius, (float)(marker.yCoordinate / SCALING_FACTOR), -(float)(marker.zCoordinate / SCALING_FACTOR) + m_radius)
 {
     m_marker = marker;
 }
 /// <summary>
 /// Gets the marker that makes up the trackable
 /// </summary>
 /// <param name="TrackableIndex">The trackable index</param>
 /// <param name="MarkerIndex">The marker index</param>
 /// <returns>The marker</returns>
 public static Marker TrackableMarker(int TrackableIndex, int MarkerIndex)
 {
     Marker marker = new Marker();
     float x, y, z;
     TrackableMarker(TrackableIndex, MarkerIndex, out x, out y, out z);
     marker.TimeStamp = System.DateTime.Now.Ticks;
     marker.xCoordinate = (int)(x * 1000);
     marker.yCoordinate = (int)(y * 1000);
     marker.zCoordinate = (int)(z * 1000);
     return marker;
 }
        /// <summary>
        /// Filters out all of the markers that are part of a trackable and leaves just single markers
        /// </summary>
        public static void UpdatedFilteredMarkers(List<Marker> markerList, List<Trackable> trackableList)
        {
            int markerCount = 0;
            filteredMarkerList = new List<Marker>();
            Marker trackableMarker = null;
            if (trackableList.Count != 0) // If there are trackable
            {
                foreach (Marker marker in markerList) // Go through every marker
                {
                    bool singleMarker = true;
                    foreach (Trackable trackable in trackableList) // Go through every trackable
                    {
                        foreach (Marker tempTrackableMarker in trackable.TrackableMarkers) // Go through every marker making up the trackable
                        {
                            trackableMarker = tempTrackableMarker;

                            if ((marker.yCoordinate >= trackableMarker.yCoordinate + trackable.yCoordinate - markerTollerence))
                            {
                                if ((marker.yCoordinate <= trackableMarker.yCoordinate + trackable.yCoordinate + markerTollerence))
                                {
                                    if ((marker.xCoordinate >= trackableMarker.xCoordinate + trackable.xCoordinate - markerTollerence))
                                    {
                                        if ((marker.xCoordinate <= trackableMarker.xCoordinate + trackable.xCoordinate + markerTollerence))
                                        {
                                            if ((marker.zCoordinate >= trackableMarker.zCoordinate + trackable.zCoordinate - markerTollerence))
                                            {
                                                if ((marker.zCoordinate <= trackableMarker.zCoordinate + trackable.zCoordinate + markerTollerence))
                                                    singleMarker = false;
                                                // This marker is part of a trackable and isn't a single marker
                                            }
                                        }
                                    }
                                }
                            }
                            if (!singleMarker)
                                break; // If its been found the break out

                        }
                        if (!singleMarker)
                            break; // If its been found the break out

                    }
                    if (singleMarker) // If its a single marker make a copy and add it to the new list
                    {
                        markerCount++;
                        Marker filteredMarker = new Marker(marker);
                        filteredMarker.MarkerId= markerCount -1;
                        filteredMarkerList.Add(filteredMarker);
                    }
                }
                FilteredMarkerList.listOfMarkers = new List<Marker>(filteredMarkerList);
            }
            else // If there are no trackables then just copy across the list
            {
               FilteredMarkerList.listOfMarkers = new List<Marker>(markerList);
            }
            OnFilteredMarkerListAvaliable();
        }
示例#18
0
        /*private PositionData ConvertMarkerPosition(Marker calibrationMarker)
        {
            PositionData position = new PositionData();
            position.Time = calibrationMarker.TimeStamp;
            position.XCoordinate = calibrationMarker.xCoordinate;
            position.YCoordinate = calibrationMarker.yCoordinate;
            position.ZCoordinate = calibrationMarker.zCoordinate;
            return position;
        }
        */
        #endregion

        //public void FootCleanUp()
        //{
        //    TCPProcessor.WholeFrameReceivedEvent -=
        //            new TCPProcessor.WholeFrameReceivedHandler(TCPProcessor_WholeFrameReceivedEvent);
        //}

        #region Foot tracking code (depracated)
    
        private void FootTrack()
        {
            //For each new frame need to determine which Marker 'describes' this foot instance
            //An extension of Marker_Bin - it has to allow for feet crossing so can not rely on 
            //which marker is most left/right but has to use historical information to determine
            //the track of this foot instance. Initially previous position only but Kalman filter may be necessary.
            //This function can also determine foot events eg. peaks, foot-down etc.
            double nearestPosition = 9999;
            double relativePosition = 0;

            //if (MarkerList.listOfMarkers.Count != 2)
            
            foreach (Marker singleMarker in MarkerList.listOfMarkers)
            {
                int xDiff = ((int)(singleMarker.xCoordinate + (0.5 * Math.Sign(singleMarker.xCoordinate)))) - ((int)(previousMarker.xCoordinate + (0.5 * Math.Sign(previousMarker.xCoordinate))));
                int yDiff = ((int)(singleMarker.yCoordinate + (0.5 * Math.Sign(singleMarker.yCoordinate)))) - ((int)(previousMarker.yCoordinate + (0.5 * Math.Sign(previousMarker.yCoordinate))));
                int zDiff = ((int)(singleMarker.zCoordinate + (0.5 * Math.Sign(singleMarker.zCoordinate)))) - ((int)(previousMarker.zCoordinate + (0.5 * Math.Sign(previousMarker.zCoordinate))));
                relativePosition = Math.Sqrt(Math.Pow(xDiff, 2) + Math.Pow(zDiff, 2) + Math.Pow(yDiff, 2));

                //Console.WriteLine(this.footName + singleMarker.MarkerId.ToString() + ">>" + " x" + ((int)singleMarker.xCoordinate).ToString() +
                    //" y" + ((int)singleMarker.yCoordinate).ToString() + " z" + ((int)singleMarker.zCoordinate).ToString() + " ><" + relativePosition.ToString());
                //Console.WriteLine("Previous: x" + previousMarker.xCoordinate.ToString() + " y" + previousMarker.yCoordinate.ToString() + " z" + previousMarker.zCoordinate.ToString());

                if (relativePosition < nearestPosition)
                {
                    nearestPosition = relativePosition;
                    markerShift = (int)nearestPosition;
                    currentMarker = new Marker (singleMarker);
                    //if (Math.Abs(xDiff) > 1) xChange = xDiff;//check current-previous>1 to illiminate small errors
                    //if (Math.Abs(yDiff) > 1) yChange = yDiff;//
                    //if (Math.Abs(zDiff) > 1) zChange = zDiff;//
                    xChange = xDiff;
                    yChange = yDiff;
                    zChange = zDiff;
                    //Console.WriteLine("zDiff = " + zDiff.ToString());
                }
            }
            //Console.WriteLine("==" + footName + currentMarker.MarkerId.ToString() + ">nearest<" + nearestPosition.ToString());

            if (nearestPosition < 50)
            {
                //Console.WriteLine(this.footName + this.currentMarker.MarkerId.ToString());


                xDirection = Math.Sign(xChange);
                yDirection = Math.Sign(yChange);
                zDirection = Math.Sign(zChange);

                if ((xDirection != xPrevDirection) && (xFootPeakDetected != null) && (xDirection != 0))
                {
                    xFootPeakDetected(this);
                }
                if ((yDirection != yPrevDirection) && (yFootPeakDetected != null) && (yDirection != 0))
                {
                    yFootPeakDetected(this);
                }
                if ((zDirection != zPrevDirection) && (zFootPeakDetected != null) && (zDirection != 0))
                {
                    zFootPeakDetected(this);
                }

                if ((zDirection == 1))
                {
                    footDownEventIsFired = false;//prevent further footdown events for the current step
                }

                //Console.WriteLine("DownEventData: " + "E" + footDownEventIsFired.ToString() + " zD" + zDirection.ToString() +
                    //" y" + currentMarker.yCoordinate.ToString() + " xChange" + (Math.Abs(xChange)).ToString() + " yChange" + (Math.Abs(yChange)).ToString());
                if ((footDownEventIsFired == false) && (zDirection == -1) && (currentMarker.yCoordinate < (footdown_yCoordinate + 15)))// && (Math.Abs(xChange) < 3) && (Math.Abs(yChange) < 3))
                {
                    footDownEventIsFired = true;
                    FootDownDetected(this);
                }

                previousMarker = new Marker(currentMarker);
                xPrevDirection = xDirection;
                yPrevDirection = yDirection;
                zPrevDirection = zDirection;
                //Console.WriteLine("Passing previous: x" + previousMarker.xCoordinate + " y" + previousMarker.yCoordinate + " z" + previousMarker.zCoordinate + "\n");
            }
            //else
            //{
            //currentMarker = null;
            //}
        }
示例#19
0
 private void FrontFoot(Marker m1, Marker m2)
 {
     if (m1.zCoordinate >= m2.zCoordinate)
     {
         if (this.currentMarker.zCoordinate == m1.zCoordinate)
         {
             frontfoot = this;
         }
     }
     else
     {
         if (this.currentMarker.zCoordinate == m2.zCoordinate)
         {
             frontfoot = this;
         }
     }
 }
        /// <summary>
        /// Saves the new marker data
        /// </summary>
        /// <param name="markerList"></param>
        /// <param name="timeStamp"></param>
        static void MotionCaptureController_MarkerListAvaliable(List<Marker> markerList, long timeStamp)
        {
            //Deep copy the list
            _currentMarkerList = new List<Marker>();
            foreach (Marker m in markerList)
            {
                Marker newM = new Marker(m);
                _currentMarkerList.Add(newM);
            }
            _currentMarkerTimeStamp = timeStamp;

            if (_currentMarkerTimeStamp == _currentTrackableTimeStamp)
                SaveFrame();
        }
        void TCPProcessor_FilteredMarkerListReceivedEvent(List<StroMoHab_Objects.Objects.Marker> filteredMarkerList)
        {
            TCPProcessor.FilteredMarkerListReceivedEvent -= new TCPProcessor.FilteredMarkerListReceivedHandler(TCPProcessor_FilteredMarkerListReceivedEvent);

            _filteredMarkerList = new List<Marker>();

            foreach (Marker currentMarker in filteredMarkerList)
            {
                Marker copyOfMarker = new Marker(currentMarker);
                _filteredMarkerList.Add(copyOfMarker);
            }


            //if (_waitForVisualiser.Enabled)
            //{
            //    return;
            //}
            if (!_updatingGraph)
            {

                //_filteredMarkerList = new List<Marker>(filteredMarkerList);

                if (_filteredMarkerList.Count > 0)
                {
                    if (_filteredMarkerList.Count > 1)
                    {
                        //Make sure index 0 is always the leftmost so can keep GUI consistent.
                        if (_filteredMarkerList[0].xCoordinate < _filteredMarkerList[1].xCoordinate) //this feels backwards to me, but after 2 80+ hour weeks me brain can't figure out the logic. TODO: Investigate and resolve as necessary.
                        {
                            Marker tempMarker = new Marker(_filteredMarkerList[1]);
                            _filteredMarkerList[1] = new Marker(_filteredMarkerList[0]);
                            _filteredMarkerList[0] = new Marker(tempMarker);
                        }
                    }



                    #region ZedGraphControl1

                    if (zedGraphControl1.GraphPane != null)
                    {
                        if (zedGraphControl1.MasterPane.PaneList.Count > 0)
                        {
                            if (zedGraphControl1.MasterPane.PaneList[0] != null)
                            {
                                PopulatePositionGraph(zedGraphControl1.MasterPane.PaneList[0], 0);
                            }
                        }
                        if (zedGraphControl1.MasterPane.PaneList.Count > 1)
                        {
                            if (zedGraphControl1.MasterPane.PaneList[1] != null)
                            {
                                PopulatePositionGraph(zedGraphControl1.MasterPane.PaneList[1], 1);
                            }
                        }
                        if (zedGraphControl1.MasterPane.PaneList.Count > 2)
                        {
                            if (zedGraphControl1.MasterPane.PaneList[2] != null)
                            {
                                PopulateVelocityGraph(zedGraphControl1.MasterPane.PaneList[2], 2);
                            }
                        }
                        if (zedGraphControl1.MasterPane.PaneList.Count > 3)
                        {
                            if (zedGraphControl1.MasterPane.PaneList[3] != null)
                            {
                                PopulateSymmetryGraph(zedGraphControl1.MasterPane.PaneList[3]);
                            }
                        }

                        if (zedGraphControl1.MasterPane.PaneList.Count > 4)
                        {
                            if (zedGraphControl1.MasterPane.PaneList[4] != null)
                            {
                                PopulateVelocityGraph(zedGraphControl1.MasterPane.PaneList[4], 4);
                            }
                        }
                    }


                    #endregion ZedGraphControl1


                }
            }
            TCPProcessor.FilteredMarkerListReceivedEvent += new TCPProcessor.FilteredMarkerListReceivedHandler(TCPProcessor_FilteredMarkerListReceivedEvent);
        }