// 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(); 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; } } }
public static Quaternion GetAxesOrderRotation(ECoordinateAxes eNewX, ECoordinateAxes eNewY, ECoordinateAxes eNewZ) { Quaternion oRotation = Quaternion.identity; switch (eNewY) { case ECoordinateAxes.AxisXpositive: { oRotation *= QuaternionHelper.RotationZ(90 * Mathf.PI / 180); switch (eNewX) { case ECoordinateAxes.AxisYpositive: { oRotation *= QuaternionHelper.RotationX(180 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisZpositive: { oRotation *= QuaternionHelper.RotationX(90 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisZnegative: { oRotation *= QuaternionHelper.RotationX(-90 * Mathf.PI / 180); break; } } break; } case ECoordinateAxes.AxisXnegative: { oRotation *= QuaternionHelper.RotationZ(-90 * Mathf.PI / 180); switch (eNewX) { case ECoordinateAxes.AxisYnegative: { oRotation *= QuaternionHelper.RotationX(180 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisZpositive: { oRotation *= QuaternionHelper.RotationX(-90 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisZnegative: { oRotation *= QuaternionHelper.RotationX(90 * Mathf.PI / 180); break; } } break; } case ECoordinateAxes.AxisYpositive: { // Retain Y axis. switch (eNewX) { case ECoordinateAxes.AxisXnegative: { oRotation *= QuaternionHelper.RotationY(180 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisZpositive: { oRotation *= QuaternionHelper.RotationY(90 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisZnegative: { oRotation *= QuaternionHelper.RotationY(-90 * Mathf.PI / 180); break; } } break; } case ECoordinateAxes.AxisYnegative: { oRotation *= QuaternionHelper.RotationX(180 * Mathf.PI / 180); switch (eNewX) { case ECoordinateAxes.AxisXnegative: { oRotation *= QuaternionHelper.RotationY(180 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisZpositive: { oRotation *= QuaternionHelper.RotationY(90 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisZnegative: { oRotation *= QuaternionHelper.RotationY(-90 * Mathf.PI / 180); break; } } break; } case ECoordinateAxes.AxisZpositive: { oRotation *= QuaternionHelper.RotationX(-90 * Mathf.PI / 180); switch (eNewX) { case ECoordinateAxes.AxisXnegative: { oRotation *= QuaternionHelper.RotationZ(180 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisYpositive: { oRotation *= QuaternionHelper.RotationZ(-90 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisYnegative: { oRotation *= QuaternionHelper.RotationZ(90 * Mathf.PI / 180); break; } } break; } case ECoordinateAxes.AxisZnegative: { oRotation *= QuaternionHelper.RotationX(90 * Mathf.PI / 180); switch (eNewX) { case ECoordinateAxes.AxisXnegative: { oRotation *= QuaternionHelper.RotationZ(180 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisYpositive: { oRotation *= QuaternionHelper.RotationZ(180 * Mathf.PI / 180); break; } case ECoordinateAxes.AxisYnegative: { oRotation *= QuaternionHelper.RotationZ(180 * Mathf.PI / 180); break; } } break; } } //oRotation.normalize(); return(oRotation); }
// 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++; } } } }