void Process(RTState state, RTPacket packet)
        {
            state.frameNumber = packet.Frame;
            packet.Get6DOFData(cachedSixDof);
            for (int i = 0; i < cachedSixDof.Count; i++)
            {
                var rot = QuaternionHelper.FromMatrix(cachedSixDof[i].Matrix);
                state.bodies[i].Rotation = rot.QtmRhsToUnityLhs(state.coordinateSystemChange);
                state.bodies[i].Position = cachedSixDof[i].Position.QtmRhsToUnityLhs(state.coordinateSystemChange);
            }

            packet.Get3DMarkerResidualData(cachedLabeledMarkers);
            for (int i = 0; i < cachedLabeledMarkers.Count; i++)
            {
                Q3D marker = cachedLabeledMarkers[i];
                state.markers[i].Position = marker.Position.QtmRhsToUnityLhs(state.coordinateSystemChange);
                state.markers[i].Residual = cachedLabeledMarkers[i].Residual;
            }

            packet.Get3DMarkerNoLabelsResidualData(cachedUnabeledMarkers);
            state.unlabeledMarkers.Clear();
            for (int i = 0; i < cachedUnabeledMarkers.Count; i++)
            {
                Q3D             marker          = cachedUnabeledMarkers[i];
                UnlabeledMarker unlabeledMarker = new UnlabeledMarker()
                {
                    Position = marker.Position.QtmRhsToUnityLhs(state.coordinateSystemChange),
                    Residual = marker.Residual,
                    Id       = marker.Id,
                };
                state.unlabeledMarkers.Add(unlabeledMarker);
            }

            packet.GetGazeVectorData(cachedGazeVectors);
            for (int i = 0; i < cachedGazeVectors.Count; i++)
            {
                QTMRealTimeSDK.Data.GazeVector gazeVector = cachedGazeVectors[i];
                if (gazeVector.GazeVectorData != null && gazeVector.GazeVectorData.Length > 0)
                {
                    state.gazeVectors[i].Position  = gazeVector.GazeVectorData[0].Position.QtmRhsToUnityLhs(state.coordinateSystemChange);
                    state.gazeVectors[i].Direction = gazeVector.GazeVectorData[0].Gaze.QtmRhsToUnityLhsNormalizedDirection(state.coordinateSystemChange);
                }
            }

            packet.GetAnalogData(cachedAnalog);
            if (cachedAnalog != null)
            {
                int channelIndex = 0;
                foreach (var analogDevice in cachedAnalog)
                {
                    for (int i = 0; i < analogDevice.Channels.Length; i++)
                    {
                        var analogChannel = analogDevice.Channels[i];
                        state.analogChannels[channelIndex].Values = analogChannel.Samples;
                        channelIndex++;
                    }
                }
            }

            packet.GetSkeletonData(cachedSkeletons);
            for (int skeletonIndex = 0; skeletonIndex < cachedSkeletons.Count; skeletonIndex++)
            {
                foreach (var segmentData in cachedSkeletons[skeletonIndex].Segments)
                {
                    Segment targetSegment;
                    if (!state.skeletons[skeletonIndex].Segments.TryGetValue(segmentData.ID, out targetSegment))
                    {
                        continue;
                    }

                    if (targetSegment.ParentId == 0)
                    {
                        targetSegment.Position = segmentData.Position.QtmRhsToUnityLhs(state.coordinateSystemChange);
                        targetSegment.Rotation = segmentData.Rotation.QtmRhsToUnityLhs(state.coordinateSystemChange);
                    }
                    else
                    {
                        targetSegment.Position = segmentData.Position.QtmRhsToUnityLhs();
                        targetSegment.Rotation = segmentData.Rotation.QtmRhsToUnityLhs();
                    }
                }
            }
        }
Example #2
0
        // processor of realtime data
        // Function is called every time protocol receives a datapacket from server
        public void Process(RTPacket packet)
        {
            mPacket = packet;

            List <Q6DOF> bodyData   = packet.Get6DOFData();
            List <Q3D>   markerData = packet.Get3DMarkerData();
            List <QTMRealTimeSDK.Data.GazeVector> gazeVectorData = packet.GetGazeVectors();

            if (bodyData != null)
            {
                for (int i = 0; i < bodyData.Count; i++)
                {
                    Vector3 position = new Vector3(bodyData[i].Position.X, bodyData[i].Position.Y, bodyData[i].Position.Z);

                    //Set rotation and position to work with unity
                    position /= 1000;

                    mBodies[i].Position    = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
                    mBodies[i].Position.z *= -1;

                    mBodies[i].Rotation    = mCoordinateSystemChange * QuaternionHelper.FromMatrix(bodyData[i].Matrix);
                    mBodies[i].Rotation.z *= -1;
                    mBodies[i].Rotation.w *= -1;

                    mBodies[i].Rotation *= QuaternionHelper.RotationZ(Mathf.PI * .5f);
                    mBodies[i].Rotation *= QuaternionHelper.RotationX(-Mathf.PI * .5f);
                }
            }

            //Get marker data that is labeled and update values
            if (markerData != null)
            {
                for (int i = 0; i < markerData.Count; i++)
                {
                    Q3D     marker   = markerData[i];
                    Vector3 position = new Vector3(marker.Position.X, marker.Position.Y, marker.Position.Z);

                    position /= 1000;

                    mMarkers[i].Position    = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
                    mMarkers[i].Position.z *= -1;
                }
            }


            if (gazeVectorData != null)
            {
                for (int i = 0; i < gazeVectorData.Count; i++)
                {
                    QTMRealTimeSDK.Data.GazeVector gazeVector = gazeVectorData[i];

                    Vector3 position = new Vector3(gazeVector.Position.X, gazeVector.Position.Y, gazeVector.Position.Z);
                    position /= 1000;
                    mGazeVectors[i].Position    = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
                    mGazeVectors[i].Position.z *= -1;

                    Vector3 direction = new Vector3(gazeVector.Gaze.X, gazeVector.Gaze.Y, gazeVector.Gaze.Z);
                    mGazeVectors[i].Direction    = QuaternionHelper.Rotate(mCoordinateSystemChange, direction);
                    mGazeVectors[i].Direction.z *= -1;
                }
            }
        }
        /// <summary>
        /// Set the data of packet.
        /// </summary>
        /// <param name="data">byte data recieved from server</param>
        internal void SetData(byte[] data)
        {
            /*  === Data packet setup ===
             *  Packet size - 4 bytes
             *  packet type - 4 bytes
             *  timestamp - 8 bytes
             *  Component count - 4 bytes
             *  [for each component]
             *    Component size - 4 bytes
             *    Component type - 4 bytes
             *    Component Data - [Component size] bytes
             */

            lock (packetLock)
            {

                ClearData();
                mData = data;
                SetPacketHeader();

                if (mPacketType == PacketType.PacketData)
                {
                    SetTimeStamp();
                    SetFrameNumber();
                    SetComponentCount();

                    int position = RTProtocol.Constants.PACKET_HEADER_SIZE + RTProtocol.Constants.DATA_PACKET_HEADER_SIZE;

                    for (int component = 1; component <= mComponentCount; component++)
                    {
                        ComponentType componentType = GetComponentType(position);
                        position += RTProtocol.Constants.COMPONENT_HEADER;
                        if (componentType == ComponentType.Component3d)
                        {
                            /* Marker count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per marker]
                             *   X - 4 bytes
                             *   Y - 4 bytes
                             *   Z - 4 bytes
                            */
                            uint markerCount = BitConvert.GetUInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < markerCount; i++)
                            {
                                Q3D marker = new Q3D();
                                marker.Id = 0;
                                marker.Residual = -1;
                                marker.Position = BitConvert.GetPoint(mData, ref position);

                                m3DMarkerData.Add(marker);
                            }
                        }
                        else if (componentType == ComponentType.Component3dNoLabels)
                        {
                            /* Marker count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per marker]
                             *   X - 4 bytes
                             *   Y - 4 bytes
                             *   Z - 4 bytes
                             *   ID - 4 bytes
                             */

                            int markerCount = BitConvert.GetInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < markerCount; i++)
                            {
                                Q3D marker = new Q3D();
                                marker.Residual = -1;
                                marker.Position = BitConvert.GetPoint(mData, ref position);
                                marker.Id = BitConvert.GetUInt32(mData, ref position);
                                m3DMarkerNoLabelData.Add(marker);
                            }

                        }
                        else if (componentType == ComponentType.ComponentAnalog)
                        {
                            /* Analog Device count - 4 bytes
                             * [Repeated per device]
                             *   Device id - 4 bytes
                             *   Channel count - 4 bytes
                             *   Sample count - 4 bytes
                             *   Sample number - 4 bytes
                             *   Analog data - 4 * channelcount * sampleCount
                             */

                            uint deviceCount = BitConvert.GetUInt32(mData, ref position);
                            for (int i = 0; i < deviceCount; i++)
                            {
                                Analog analogDeviceData = new Analog();
                                analogDeviceData.DeviceID = BitConvert.GetUInt32(mData, ref position);
                                analogDeviceData.ChannelCount = BitConvert.GetUInt32(mData, ref position);
                                analogDeviceData.Channels = new AnalogChannelData[analogDeviceData.ChannelCount];

                                uint sampleCount = BitConvert.GetUInt32(mData, ref position);
                                if (sampleCount * analogDeviceData.ChannelCount * 4 > mData.Length)
                                {
                                }
                                else if (sampleCount > 0)
                                {
                                    uint sampleNumber = BitConvert.GetUInt32(mData, ref position);
                                    for (uint j = 0; j < analogDeviceData.ChannelCount; j++)
                                    {
                                        AnalogChannelData sample = new AnalogChannelData();
                                        sample.Samples = new float[sampleCount];
                                        for (uint k = 0; k < sampleCount; k++)
                                        {
                                            sample.SampleNumber = sampleNumber + k;
                                            sample.Samples[k] = BitConvert.GetFloat(mData, ref position);
                                        }

                                        analogDeviceData.Channels[j] = sample;
                                    }
                                }
                                mAnalogDevices.Add(analogDeviceData);
                            }
                        }
                        else if (componentType == ComponentType.ComponentForce)
                        {
                            /* Force plate count - 4 bytes
                             * [Repeated per plate]
                             *   Force plate ID - 4 bytes
                             *   Force count - 4 bytes
                             *   forceNumber - 4 bytes
                             *   Force data - 36 * force count bytes
                             */

                            int forcePlateCount = BitConvert.GetInt32(mData, ref position);
                            for (int i = 0; i < forcePlateCount; i++)
                            {
                                ForcePlate plate = new ForcePlate();
                                plate.PlateId = BitConvert.GetInt32(mData, ref position);
                                plate.ForceCount = BitConvert.GetInt32(mData, ref position);
                                plate.ForceNumber = BitConvert.GetInt32(mData, ref position);
                                plate.ForceSamples = new ForceSample[plate.ForceCount];

                                for (int j = 0; j < plate.ForceCount; j++)
                                {
                                    ForceSample sample;
                                    sample.Force = BitConvert.GetPoint(mData, ref position);
                                    sample.Moment = BitConvert.GetPoint(mData, ref position);
                                    sample.ApplicationPoint = BitConvert.GetPoint(mData, ref position);
                                    plate.ForceSamples[j] = sample;
                                }

                                mForcePlates.Add(plate);
                            }

                        }
                        else if (componentType == ComponentType.Component6d)
                        {
                            /* Body count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per body]
                             *   X - 4 bytes
                             *   Y - 4 bytes
                             *   Z - 4 bytes
                             *   rotation matrix - 9*4 bytes
                             */

                            int bodyCount = BitConvert.GetInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < bodyCount; i++)
                            {
                                Q6DOF body = new Q6DOF();
                                body.Position = BitConvert.GetPoint(mData, ref position);
                                body.Matrix = new float[9];

                                for (int j = 0; j < 9; j++)
                                {
                                    body.Matrix[j] = BitConvert.GetFloat(mData, ref position);
                                }

                                body.Residual = -1;
                                m6DOFData.Add(body);
                            }
                        }
                        else if (componentType == ComponentType.Component6dEuler)
                        {

                            /* Body count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per body]
                             *   X - 4 bytes
                             *   Y - 4 bytes
                             *   Z - 4 bytes
                             *   Euler Angles - 3*4 bytes
                             */

                            int bodyCount = BitConvert.GetInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < bodyCount; i++)
                            {
                                Q6DOFEuler body = new Q6DOFEuler();
                                body.Position = BitConvert.GetPoint(mData, ref position);
                                body.Rotation = BitConvert.GetPoint(mData, ref position);
                                body.Residual = -1;
                                m6DOFEulerData.Add(body);
                            }

                        }
                        else if (componentType == ComponentType.Component2d || componentType == ComponentType.Component2dLinearized)
                        {
                            /* Camera Count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per Camera]
                             *   Marker Count - 4 bytes
                             *   Status Flags - 1 byte
                             *   [Repeated per Marker]
                             *     X - 4 Bytes
                             *     Y - 4 Bytes
                             *     Diameter X - 4 bytes
                             *     Diameter Y - 4 bytes
                             */

                            uint cameraCount = BitConvert.GetUInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < cameraCount; i++)
                            {
                                Camera camera = new Camera();
                                camera.MarkerCount = BitConvert.GetUInt32(mData, ref position);
                                camera.StatusFlags = mData[position++];
                                camera.MarkerData2D = new Q2D[camera.MarkerCount];
                                for (int j = 0; j < camera.MarkerCount; j++)
                                {
                                    Q2D marker = new Q2D();
                                    marker.X = BitConvert.GetUInt32(mData, ref position);
                                    marker.Y = BitConvert.GetUInt32(mData, ref position);
                                    marker.DiameterX = BitConvert.GetUShort(mData, ref position);
                                    marker.DiameterY = BitConvert.GetUShort(mData, ref position);
                                    camera.MarkerData2D[j] = marker;
                                }
                                if (componentType == ComponentType.Component2d)
                                    m2DMarkerData.Add(camera);
                                else if (componentType == ComponentType.Component2dLinearized)
                                    m2DLinearMarkerData.Add(camera);
                            }

                        }
                        else if (componentType == ComponentType.Component3dResidual)
                        {
                            /* Marker count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per marker]
                             *   X - 4 bytes
                             *   Y - 4 bytes
                             *   Z - 4 bytes
                             *   Residual - 4 bytes
                            */
                            int markerCount = BitConvert.GetInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < markerCount; i++)
                            {
                                Q3D marker = new Q3D();
                                marker.Id = 0;
                                marker.Position = BitConvert.GetPoint(mData, ref position);
                                marker.Residual = BitConvert.GetFloat(mData, ref position);

                                m3DMarkerResidualData.Add(marker);
                            }
                        }
                        else if (componentType == ComponentType.Component3dNoLabelsResidual)
                        {
                            /* Marker count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per marker]
                             *   X - 4 bytes
                             *   Y - 4 bytes
                             *   Z - 4 bytes
                             *   Residual - 4 bytes
                             *   ID - 4 bytes
                            */
                            int markerCount = BitConvert.GetInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < markerCount; i++)
                            {
                                Q3D marker = new Q3D();
                                marker.Position = BitConvert.GetPoint(mData, ref position);
                                marker.Residual = BitConvert.GetFloat(mData, ref  position);
                                marker.Id = BitConvert.GetUInt32(mData, ref  position);

                                m3DMarkerNoLabelResidualData.Add(marker);
                            }

                        }
                        else if (componentType == ComponentType.Component6dResidual)
                        {
                            /* Body count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per marker]
                             *   X - 4 bytes
                             *   Y - 4 bytes
                             *   Z - 4 bytes
                             *   rotation matrix - 9*4 bytes
                             *   residual - 9*4 bytes
                             */

                            int bodyCount = BitConvert.GetInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < bodyCount; i++)
                            {
                                Q6DOF body = new Q6DOF();
                                body.Position = BitConvert.GetPoint(mData, ref position);
                                body.Matrix = new float[9];
                                for (int j = 0; j < 9; j++)
                                    body.Matrix[j] = BitConvert.GetFloat(mData, ref position);
                                body.Residual = BitConvert.GetFloat(mData, ref position); ;
                                m6DOFResidualData.Add(body);
                            }

                        }
                        else if (componentType == ComponentType.Component6dEulerResidual)
                        {

                            /* Body count - 4 bytes
                             * 2D Drop rate - 2 bytes
                             * 2D Out of sync rate - 2 bytes
                             * [Repeated per marker]
                             *   X - 4 bytes
                             *   Y - 4 bytes
                             *   Z - 4 bytes
                             *   Euler Angles - 3*4 bytes
                             *   residual - 9*4 bytes
                             */

                            int bodyCount = BitConvert.GetInt32(mData, ref position);
                            m2DDropRate = BitConvert.GetUShort(mData, ref position);
                            m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);

                            for (int i = 0; i < bodyCount; i++)
                            {
                                Q6DOFEuler body = new Q6DOFEuler();
                                body.Position = BitConvert.GetPoint(mData, ref position);
                                body.Rotation = BitConvert.GetPoint(mData, ref position);
                                body.Residual = BitConvert.GetFloat(mData, ref position);
                                m6DOFEulerResidualData.Add(body);
                            }

                        }
                        else if (componentType == ComponentType.ComponentAnalogSingle)
                        {
                            /* Analog Device count - 4 bytes
                             * [Repeated per device]
                             *   Device id - 4 bytes
                             *   Channel count - 4 bytes
                             *   Analog data - 4 * channelcount
                             */

                            int deviceCount = BitConvert.GetInt32(mData, ref position);
                            for (int i = 0; i < deviceCount; i++)
                            {
                                Analog device = new Analog();
                                device.DeviceID = BitConvert.GetUInt32(mData, ref position);
                                device.ChannelCount = BitConvert.GetUInt32(mData, ref position);
                                device.Channels = new AnalogChannelData[device.ChannelCount];
                                for (int j = 0; j < device.ChannelCount; j++)
                                {
                                    AnalogChannelData sample = new AnalogChannelData();
                                    sample.Samples = new float[1];
                                    sample.Samples[0] = BitConvert.GetFloat(mData, ref position);

                                    device.Channels[j] = sample;
                                }

                                mAnalogSingleSample.Add(device);
                            }
                        }
                        else if (componentType == ComponentType.ComponentImage)
                        {
                            /* Camera count - 4 bytes
                             * [Repeated per marker]
                             *   Camera ID - 4 bytes
                             *   Image Format - 4 bytes
                             *   Width - 4 bytes
                             *   Height- 4 bytes
                             *   Left crop - 4 bytes
                             *   Top crop - 4 bytes
                             *   Right crop - 4 bytes
                             *   Bottom crop - 4 bytes
                             *   Image size- 4 bytes
                             *   Image data - [Image size bytes]
                             */

                            int cameraCount = BitConvert.GetInt32(mData, ref position);
                            for (int i = 0; i < cameraCount; i++)
                            {
                                CameraImage image = new CameraImage();
                                image.CameraID = BitConvert.GetUInt32(mData, ref position);
                                image.ImageFormat = (ImageFormat)BitConvert.GetUInt32(mData, ref position);
                                image.Width = BitConvert.GetUInt32(mData, ref position);
                                image.Height = BitConvert.GetUInt32(mData, ref position);
                                image.LeftCrop = BitConvert.GetFloat(mData, ref position);
                                image.TopCrop = BitConvert.GetFloat(mData, ref position);
                                image.RightCrop = BitConvert.GetFloat(mData, ref position);
                                image.BottomCrop = BitConvert.GetFloat(mData, ref position);
                                image.ImageSize = BitConvert.GetInt32(mData, ref position);
                                image.ImageData = new byte[image.ImageSize];
                                Array.Copy(mData, position, image.ImageData, 0, image.ImageSize);
                                position += image.ImageSize;

                                mImageData.Add(image);
                            }
                        }
                        else if (componentType == ComponentType.ComponentForceSingle)
                        {
                            /* Force plate count - 4 bytes
                             * [Repeated per plate]
                             *   Force plate ID - 4 bytes
                             *   Force data - 36 bytes
                             */

                            int forcePlateCount = BitConvert.GetInt32(mData, ref position);
                            for (int i = 0; i < forcePlateCount; i++)
                            {
                                ForcePlate plate = new ForcePlate();
                                plate.PlateId = BitConvert.GetInt32(mData, ref position);
                                plate.ForceCount = 1;
                                plate.ForceNumber = -1;
                                plate.ForceSamples = new ForceSample[plate.ForceCount];
                                plate.ForceSamples[0].Force = BitConvert.GetPoint(mData, ref position);
                                plate.ForceSamples[0].Moment = BitConvert.GetPoint(mData, ref position);
                                plate.ForceSamples[0].ApplicationPoint = BitConvert.GetPoint(mData, ref position);

                                mForceSinglePlate.Add(plate);
                            }
                        }
                        else if (componentType == ComponentType.ComponentGazeVector)
                        {
                            /* Gaze vector count - 4 bytes
                             * Gaze vector sample count - 4 bytes
                             * Gaze vector sample number - 4 bytes (omitted if sample count is 0)
                             * [Repeated per gaze vector (omitted if sample count is 0)]
                             *   Gaze vector data - 24 bytes
                             */

                            int gazeVectorCount = BitConvert.GetInt32(mData, ref position);
                            for (int i = 0; i < gazeVectorCount; i++)
                            {
                                GazeVector gazeVector = new GazeVector();
                                uint sampleCount = BitConvert.GetUInt32(mData, ref position);
                                if (sampleCount > 0)
                                {
                                    uint sampleNumber = BitConvert.GetUInt32(mData, ref position);
                                    gazeVector.SampleNumber = sampleNumber;
                                    for (var sample = 0; sample < sampleCount; sample++)
                                    {
                                        gazeVector.Gaze = BitConvert.GetPoint(mData, ref position);
                                        gazeVector.Position = BitConvert.GetPoint(mData, ref position);
                                    }
                                }
                                mGazeVector.Add(gazeVector);
                            }
                        }
                    }
                }
            }
        }
Example #4
0
        // processor of realtime data
        // Function is called every time protocol receives a datapacket from server
        public void Process(RTPacket packet)
        {
            mPacket = packet;

            var bodyData            = packet.Get6DOFData();
            var labeledMarkerData   = packet.Get3DMarkerResidualData();
            var unlabeledMarkerData = packet.Get3DMarkerNoLabelsResidualData();
            var gazeVectorData      = packet.GetGazeVectorData();
            var analogData          = packet.GetAnalogData();

            if (bodyData != null)
            {
                for (int i = 0; i < bodyData.Count; i++)
                {
                    Vector3 position = new Vector3(bodyData[i].Position.X, bodyData[i].Position.Y, bodyData[i].Position.Z);

                    //Set rotation and position to work with unity
                    position /= 1000;

                    mBodies[i].Position    = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
                    mBodies[i].Position.z *= -1;

                    mBodies[i].Rotation    = mCoordinateSystemChange * QuaternionHelper.FromMatrix(bodyData[i].Matrix);
                    mBodies[i].Rotation.z *= -1;
                    mBodies[i].Rotation.w *= -1;

                    mBodies[i].Rotation *= QuaternionHelper.RotationZ(Mathf.PI * .5f);
                    mBodies[i].Rotation *= QuaternionHelper.RotationX(-Mathf.PI * .5f);
                }
            }

            // Get marker data that is labeled and update values
            if (labeledMarkerData != null)
            {
                for (int i = 0; i < labeledMarkerData.Count; i++)
                {
                    Q3D     marker   = labeledMarkerData[i];
                    Vector3 position = new Vector3(marker.Position.X, marker.Position.Y, marker.Position.Z);

                    position /= 1000;

                    mMarkers[i].Position    = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
                    mMarkers[i].Position.z *= -1;
                    mMarkers[i].Residual    = labeledMarkerData[i].Residual;
                }
            }

            // Get unlabeled marker data
            if (unlabeledMarkerData != null)
            {
                mUnlabeledMarkers.Clear();
                for (int i = 0; i < unlabeledMarkerData.Count; i++)
                {
                    UnlabeledMarker unlabeledMarker = new UnlabeledMarker();
                    Q3D             marker          = unlabeledMarkerData[i];
                    Vector3         position        = new Vector3(marker.Position.X, marker.Position.Y, marker.Position.Z);

                    position /= 1000;

                    unlabeledMarker.Position    = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
                    unlabeledMarker.Position.z *= -1;
                    unlabeledMarker.Residual    = unlabeledMarkerData[i].Residual;
                    unlabeledMarker.Id          = unlabeledMarkerData[i].Id;
                    mUnlabeledMarkers.Add(unlabeledMarker);
                }
            }

            if (gazeVectorData != null)
            {
                for (int i = 0; i < gazeVectorData.Count; i++)
                {
                    QTMRealTimeSDK.Data.GazeVector gazeVector = gazeVectorData[i];

                    Vector3 position = new Vector3(gazeVector.Position.X, gazeVector.Position.Y, gazeVector.Position.Z);
                    position /= 1000;
                    mGazeVectors[i].Position    = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
                    mGazeVectors[i].Position.z *= -1;

                    Vector3 direction = new Vector3(gazeVector.Gaze.X, gazeVector.Gaze.Y, gazeVector.Gaze.Z);
                    mGazeVectors[i].Direction    = QuaternionHelper.Rotate(mCoordinateSystemChange, direction);
                    mGazeVectors[i].Direction.z *= -1;
                }
            }

            if (analogData != null)
            {
                int channelIndex = 0;
                foreach (var analogDevice in analogData)
                {
                    for (int i = 0; i < analogDevice.Channels.Length; i++)
                    {
                        var analogChannel = analogDevice.Channels[i];
                        mAnalogChannels[channelIndex].Values = analogChannel.Samples;
                        channelIndex++;
                    }
                }
            }
        }