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(); } } } }
// 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++; } } } }