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++;
}
}
}
}
