/// <summary>
/// Obtain smoothed position (x, y, z) and orientation (p, r, y) data
/// </summary>
private void ExtractData()
{
const char REMOVE_F = 'f';
// Get next OSC Packet for this GameObject (Pozyx Tag)
_PozyxData = OSC.GetOSCPacket(TagID);
// Position Data
_XCoord = _AVG_X.SmoothPositionData(Convert.ToInt32(_PozyxData[2]));
// z and y axis reversed for unity reference frame
_YCoord = _AVG_Y.SmoothPositionData(Convert.ToInt32(_PozyxData[4]));
_ZCoord = _AVG_Z.SmoothPositionData(Convert.ToInt32(_PozyxData[3]));
// Orientation Data
_Pitch = _AVG_Pitch.SmoothOrientationData(float.Parse(_PozyxData[5].TrimEnd(REMOVE_F)));
_Roll = _AVG_Roll.SmoothOrientationData(float.Parse(_PozyxData[6].TrimEnd(REMOVE_F)));
_Yaw = _AVG_Yaw.SmoothOrientationData(float.Parse(_PozyxData[7].TrimEnd(REMOVE_F))) - 90; // Rotate 90 deg to compensate for Unity Reference Frame / Pozyx difference
Debug.Log(string.Format("TagID: [{6}] Position: [x:{0} y:{1} z:{2}], Rotation: [p:{3:f2} r:{4:f2} y:{5:f2}]", _XCoord, _YCoord, _ZCoord, _Pitch, _Roll, _Yaw, TagID));
//Debug.Log(string.Format("Position: [x:{0} y:{1} z:{2}]", _XCoord, _YCoord, _ZCoord));
// Convert Euler (Rotation) Angles to Quaternions
_QT = Quaternion.Euler(_Roll, _Yaw, -_Pitch);
}
