static bool Get6DOFSettings(RTState state, RTProtocol mProtocol) { // Get settings and information for streamed bodies bool getstatus = mProtocol.Get6dSettings(); if (getstatus) { state.bodies.Clear(); Settings6D settings = mProtocol.Settings6DOF; foreach (Settings6DOF body in settings.Bodies) { SixDOFBody newbody = new SixDOFBody(); newbody.Name = body.Name; newbody.Color.r = (body.ColorRGB) & 0xFF; newbody.Color.g = (body.ColorRGB >> 8) & 0xFF; newbody.Color.b = (body.ColorRGB >> 16) & 0xFF; newbody.Color /= 255; newbody.Color.a = 1F; newbody.Position = Vector3.zero; newbody.Rotation = Quaternion.identity; state.bodies.Add(newbody); } return(true); } return(false); }
static bool UpdateSettings(RTState rtState, RTProtocol rtProtocol, List <ComponentType> componentSelection) { if (GetGeneralSettings(rtState, rtProtocol) == false) { return(false); } rtState.componentsInStream = componentSelection.Select(x => { switch (x) { case ComponentType.Component3dResidual: return(Get3DSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone); case ComponentType.Component3dNoLabelsResidual: return(Get3DSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone); case ComponentType.Component6d: return(Get6DOFSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone); case ComponentType.ComponentGazeVector: return(GetGazeVectorSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone); case ComponentType.ComponentAnalog: return(GetAnalogSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone); case ComponentType.ComponentSkeleton: return(GetSkeletonSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone); default: return(ComponentType.ComponentNone); } ; }) .Where(x => x != ComponentType.ComponentNone) .ToList(); return(true); }
static bool GetGeneralSettings(RTState state, RTProtocol mProtocol) { bool getStatus = mProtocol.GetGeneralSettings(); if (!getStatus) { return(false); } state.frequency = mProtocol.GeneralSettings.CaptureFrequency; return(true); }
static bool StartStreaming(RTState state, RTProtocol rtProtocol, StreamRate streamRate, short udpPort) { if (rtProtocol.StreamFrames(streamRate, -1, state.componentsInStream, udpPort) == false) { state.isStreaming = false; } else { state.isStreaming = true; } return(state.isStreaming); }
static bool Get3DSettings(RTState state, RTProtocol mProtocol) { bool getstatus = mProtocol.Get3dSettings(); if (getstatus) { state.upAxis = mProtocol.Settings3D.AxisUpwards; state.coordinateSystemChange = Rotation.GetCoordinateSystemRotation(state.upAxis); // Save marker settings state.markers.Clear(); foreach (Settings3DLabel marker in mProtocol.Settings3D.Labels) { LabeledMarker newMarker = new LabeledMarker(); newMarker.Name = marker.Name; newMarker.Position = Vector3.zero; newMarker.Residual = 0; newMarker.Color.r = (marker.ColorRGB) & 0xFF; newMarker.Color.g = (marker.ColorRGB >> 8) & 0xFF; newMarker.Color.b = (marker.ColorRGB >> 16) & 0xFF; newMarker.Color /= 255; newMarker.Color.a = 1F; state.markers.Add(newMarker); } // Save bone settings if (mProtocol.Settings3D.Bones != null) { state.bones.Clear(); foreach (var settingsBone in mProtocol.Settings3D.Bones) { Bone bone = new Bone(); bone.From = settingsBone.From; bone.FromMarker = state.GetMarker(settingsBone.From); bone.To = settingsBone.To; bone.ToMarker = state.GetMarker(settingsBone.To); bone.Color.r = (settingsBone.Color) & 0xFF; bone.Color.g = (settingsBone.Color >> 8) & 0xFF; bone.Color.b = (settingsBone.Color >> 16) & 0xFF; bone.Color /= 255; bone.Color.a = 1F; state.bones.Add(bone); } } return(true); } return(false); }
static bool GetSkeletonSettings(RTState state, RTProtocol mProtocol) { bool getStatus = mProtocol.GetSkeletonSettings(); if (!getStatus) { return(false); } state.skeletons.Clear(); var skeletonSettings = mProtocol.SkeletonSettings; foreach (var settingSkeleton in skeletonSettings.Skeletons) { Skeleton skeleton = new Skeleton(); skeleton.Name = settingSkeleton.Name; foreach (var settingSegment in settingSkeleton.Segments) { var segment = new Segment(); segment.Name = settingSegment.Name; segment.Id = settingSegment.Id; segment.ParentId = settingSegment.ParentId; if (settingSegment.ParentId == 0) { segment.TPosition = settingSegment.Position.QtmRhsToUnityLhs(state.coordinateSystemChange); segment.TRotation = settingSegment.Rotation.QtmRhsToUnityLhs(state.coordinateSystemChange); } else { segment.TPosition = settingSegment.Position.QtmRhsToUnityLhs(); segment.TRotation = settingSegment.Rotation.QtmRhsToUnityLhs(); } skeleton.Segments.Add(segment.Id, segment); } state.skeletons.Add(skeleton); } return(true); }
static bool GetGazeVectorSettings(RTState state, RTProtocol mProtocol) { bool getStatus = mProtocol.GetGazeVectorSettings(); if (getStatus) { state.gazeVectors.Clear(); SettingsGazeVectors settings = mProtocol.GazeVectorSettings; foreach (var gazeVector in settings.GazeVectors) { var newGazeVector = new GazeVector(); newGazeVector.Name = gazeVector.Name; newGazeVector.Position = Vector3.zero; newGazeVector.Direction = Vector3.zero; state.gazeVectors.Add(newGazeVector); } return(true); } return(false); }
static bool GetAnalogSettings(RTState state, RTProtocol mProtocol) { bool getStatus = mProtocol.GetAnalogSettings(); if (getStatus) { state.analogChannels.Clear(); var settings = mProtocol.AnalogSettings; foreach (var device in settings.Devices) { foreach (var channel in device.ChannelInformation) { var analogChannel = new AnalogChannel(); analogChannel.Name = channel.Name; analogChannel.Values = new float[0]; state.analogChannels.Add(analogChannel); } } return(true); } return(false); }
public RTStreamThread(string IpAddress, short udpPort, StreamRate streamRate, bool stream6d, bool stream3d, bool stream3dNoLabels, bool streamGaze, bool streamAnalog, bool streamSkeleton) { this.writerThreadState = new RTState(); this.ReaderThreadState = new RTState(); this.IpAddress = IpAddress; this.streamRate = streamRate; this.udpPort = udpPort; if (stream3d) { componentSelection.Add(ComponentType.Component3dResidual); } if (stream3dNoLabels) { componentSelection.Add(ComponentType.Component3dNoLabelsResidual); } if (stream6d) { componentSelection.Add(ComponentType.Component6d); } if (streamGaze) { componentSelection.Add(ComponentType.ComponentGazeVector); } if (streamAnalog) { componentSelection.Add(ComponentType.ComponentAnalog); } if (streamSkeleton) { componentSelection.Add(ComponentType.ComponentSkeleton); } killThread = false; writerThread = new Thread(WriterThreadFunction); writerThread.Name = "RTStreamThread::WriterThreadFunction"; writerThread.Start(); }
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(); } } } }