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