示例#1
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="newTrackable">New Trackable</param>
 public Trackable(Trackable newTrackable)
 {
     x                = newTrackable.xCoordinate;
     y                = newTrackable.yCoordinate;
     x                = newTrackable.zCoordinate;
     pitch            = newTrackable.Pitch;
     yaw              = newTrackable.Yaw;
     roll             = newTrackable.Roll;
     qw               = newTrackable.qw;
     qx               = newTrackable.qx;
     qy               = newTrackable.qy;
     qz               = newTrackable.qz;
     id               = newTrackable.ID;
     timeStamp        = newTrackable.TimeStamp;
     name             = newTrackable.Name;
     trackableIndex   = newTrackable.TrackableIndex;
     trackableMarkers = null;
 }
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="newTrackable">New Trackable</param>
 public Trackable(Trackable newTrackable)
 {
     x = newTrackable.xCoordinate;
     y = newTrackable.yCoordinate;
     x = newTrackable.zCoordinate;
     pitch = newTrackable.Pitch;
     yaw = newTrackable.Yaw;
     roll = newTrackable.Roll;
     qw = newTrackable.qw;
     qx = newTrackable.qx;
     qy = newTrackable.qy;
     qz = newTrackable.qz;
     id = newTrackable.ID;
     timeStamp = newTrackable.TimeStamp;
     name = newTrackable.Name;
     trackableIndex = newTrackable.TrackableIndex;
     trackableMarkers = null;
 }
        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();
            }
        }
        /// <summary>
        /// Transmits a trackable to the current tcpClient.
        /// </summary>
        /// <param name="trackable"></param>
        public void Transmit(Trackable trackable)
        {
            //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('R'); // T is used for Treadmill

                            binaryWriter.Write(trackable.ID);
                            binaryWriter.Write(trackable.TimeStamp);
                            binaryWriter.Write(trackable.TrackableIndex);
                            binaryWriter.Write(trackable.Name);
                            binaryWriter.Write(trackable.xCoordinate);
                            binaryWriter.Write(trackable.yCoordinate);
                            binaryWriter.Write(trackable.zCoordinate);
                            binaryWriter.Write(trackable.Roll);
                            binaryWriter.Write(trackable.Pitch);
                            binaryWriter.Write(trackable.Yaw);
                            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);
                }
            }
        }
        private void MotionCapture_TrackableListAvaliableEvent(List<Trackable> trackableList, long timeStamp)
        {
            tempTrackableList = new List<Trackable>(trackableList);
            tempTrackableListTimeStamp = timeStamp;

            foreach (Trackable trackable in tempTrackableList)
            {
                trackable.TimeStamp = timeStamp;
                if (trackable.Name == null) // Prevents binarywriter in Transmit trying to write a null name
                    trackable.Name = "Unknown";
                Transmit(trackable);
            }
            Trackable endOfFrameTrackable = new Trackable();
            endOfFrameTrackable.TimeStamp = trackableList.Count;
            endOfFrameTrackable.ID = -2147483648;
            endOfFrameTrackable.Name = "End";
            Transmit(endOfFrameTrackable);



            //Filter Markers - if they are from the same camera update FilteredMarkers
            if (tempTrackableListTimeStamp == tempMarkerListTimeStamp && processFilteredMarkers) 
                FilteredMarkerProcessor.UpdatedFilteredMarkers(tempMarkerList, tempTrackableList);

            //Update Joints
            JointProcessor.UpdateJoints(tempTrackableList);
            
        }
        /// <summary>
        /// Returns the Trackable for the index specified
        /// </summary>
        /// <param name="TrackableIndex">The index of the trackable</param>
        /// <returns>The result</returns>
        public static Trackable GetTrackableLocation(int TrackableIndex)
        {
            Trackable trackable = new Trackable();

            float x, y, z;
            float qx, qy, qz, qw;
            float yaw, pitch, roll;

            GetTrackableLocation(TrackableIndex, out x, out y, out z, out qx, out qy, out qz, out qw, out yaw, out pitch, out roll);

            trackable.xCoordinate = (int)(x * 1000); // Positive faceing the screen going right
            trackable.yCoordinate = (int)(y * 1000); // Positive Up
            trackable.zCoordinate = (int)(z * 1000); // Positive Towards the screen


            trackable.Yaw = -Math.Round(yaw, 1); // invert yaw
            trackable.Pitch = Math.Round(roll, 1); // Swap around pitch and roll
            trackable.Roll = Math.Round(pitch, 1);
            trackable.TimeStamp = System.DateTime.Now.Ticks;
            trackable.ID = TrackableID(TrackableIndex);
            trackable.Name = TT_TrackableName(TrackableIndex);
            trackable.TrackableIndex = TrackableIndex;
            trackable.QW = qw;
            trackable.QX = qx;
            trackable.QY = qy;
            trackable.QZ = qz;

            Marker trackableMarker = null;
            trackable.TrackableMarkers = new List<Marker>();

            // Build the rotation matrix
            StroMoHab_Matrix.RotationMatrix rotationMatix = new StroMoHab_Matrix.RotationMatrix(Math.PI * trackable.Pitch / 180, Math.PI * trackable.Yaw / 180, Math.PI * trackable.Roll / 180);

            //loop through and get all of the markers that are part of the trackable, apply the rotation matrix, and add them
            for (int j = 0; j < TrackingTools.TrackableMarkerCount(TrackableIndex); j++)
            {
                trackableMarker = TrackingTools.TrackableMarker(TrackableIndex, j); // get the marker
                // rotate the marker
                StroMoHab_Matrix.PointMatrix newPointMatrix = StroMoHab_Matrix.Operations.Rotate(new StroMoHab_Matrix.PointMatrix(trackableMarker.xCoordinate, trackableMarker.yCoordinate, trackableMarker.zCoordinate), rotationMatix);

                //get the new marker position
                trackableMarker.xCoordinate = Math.Round(newPointMatrix.XCoordinate, 1);
                trackableMarker.yCoordinate = Math.Round(newPointMatrix.YCoordinate, 1);
                trackableMarker.zCoordinate = Math.Round(newPointMatrix.ZCoordinate, 1);
                trackable.TrackableMarkers.Add(trackableMarker);
            }
            return trackable;
        }
示例#10
0
 public void CalibrateFoot(Trackable calibrationTrackable)
 {
     //Stromohab_MCE_Connection.TCPProcessor.TrackableListReceivedEvent +=
     //    new TCPProcessor.TrackableListReceivedHandler(TCPProcessor_TrackableListReceivedEvent);
     if (footName == "left") currentID = 1;
     if (footName == "right") currentID = 2;
     this.calibrationTrackable = calibrationTrackable;
     previousTrackable = calibrationTrackable;
     PositionData positionCalibration = PositionData.ConvertMarkerPosition((Marker)calibrationTrackable);
     this.footPositionList.Add(positionCalibration);
     footdown_yCoordinate = calibrationTrackable.yCoordinate;
     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 = previousTrackable.zCoordinate;
     yPreviousCoordinate = previousTrackable.yCoordinate;
     xPreviousCoordinate = previousTrackable.xCoordinate;
 }
示例#11
0
 private static bool RightFoot(Trackable t)
 {
     if (t.ID == 2)
         return true;
     else
         return false;
 }
示例#12
0
 private static bool LeftFoot(Trackable t)
 {
     if (t.ID == 1)
         return true;
     else
         return false;
 }
示例#13
0
        /// <summary>
        /// Finds an updated CoR for the two Trackables given.
        /// </summary>
        /// <param name="T1">Trackable1</param>
        /// <param name="T2">Trackable2</param>
        /// <returns>The CoR Matrix, containg the x, y, and z coordinates</returns>
        public Matrix FindCoR(Trackable T1, Trackable T2)
        {
            //Increase the frame number
            frameNumber++;

            // Declare rotation matricies
            Matrix RT1, RT2;
            
            // Build rotation matrix from quaternion rotation data
            RT1 = StroMoHab_Matrix.Operations.BuildRotorFromQuaternionData(T1);
            RT2 = StroMoHab_Matrix.Operations.BuildRotorFromQuaternionData(T2);

            // Transpose the two rotation matricies
            Matrix RT1_Trans = StroMoHab_Matrix.Operations.Transpose(RT1);
            Matrix RT2_Trans = StroMoHab_Matrix.Operations.Transpose(RT2);

            // Add the next set of data to the summation
            RT1_TransRT2 = RT1_TransRT2 + RT1_Trans * RT2;
            RT2_TransRT1 = RT2_TransRT1 + RT2_Trans * RT1;
            
            // Put the centre of the Trackables and the distances between them in matrix form
            Matrix T1p = Matrix.Create(new double[, ] { { T1.xCoordinate }, { T1.yCoordinate }, { T1.zCoordinate } });
            Matrix T2p = Matrix.Create(new double[, ] { { T2.xCoordinate }, { T2.yCoordinate }, { T2.zCoordinate } });
            Matrix bT1T2 = T1p - T2p;

            // Add the next set of data to the summation
            RT1_TransbT1T2 = RT1_TransbT1T2 + RT1_Trans * bT1T2;
            RT2_TransbT1T2 = RT2_TransbT1T2 + RT2_Trans * bT1T2;

            // Build matrix b
            Matrix b = new Matrix(6,1);
            for (int i = 0; i < 3; i++)
            {
                b[i, 0] = RT1_TransbT1T2[i, 0];
                b[i + 3, 0] = -RT2_TransbT1T2[i, 0];
            }

            // Build then get the inverse of matrix M
            Matrix M_Inverse = BuildM_Inverse();
            if (M_Inverse == null) // Inversion of M failed because its determinant was 0 so just return the previous CoR
                return CoR;
            
            // Solve Ma = b to find a
            Matrix a = M_Inverse * b;

            // Extract the two parts of a
            Matrix aT1 = new Matrix(3,1);
            Matrix aT2 = new Matrix(3,1);
            for (int i = 0; i < 3; i++)
            {
                    aT1[i, 0] = a[i, 0];
                    aT2[i, 0] = a[i + 3, 0];
            }

            // Calculate the two centres of rotation
            Matrix CoR1 = (RT1 * aT1) + T1p;
            Matrix CoR2 = (RT2 * aT2) + T2p;

            // The final CoR is the average of the two CoR
            for (int i = 0; i < 3; i++)
            {
                CoR[i, 0] = (CoR1[i, 0] + CoR2[i, 0]) / 2;
            }          

            return CoR;
        }
 public OpenGlTrackable(Trackable trackable)
     : base((float)(trackable.xCoordinate/SCALING_FACTOR)-m_radius,(float)(trackable.yCoordinate/SCALING_FACTOR),-(float)(trackable.zCoordinate /SCALING_FACTOR)-m_radius,(float)(trackable.xCoordinate/SCALING_FACTOR)+m_radius,(float)(trackable.yCoordinate/SCALING_FACTOR),-(float)(trackable.zCoordinate/SCALING_FACTOR)+m_radius)
 {
     m_trackable = trackable;
 }