private void ResetRotation()
{
var rotData = new USBRotationData();
rotData.w = 1;
rotData.x = 0;
rotData.y = 0;
rotData.z = 0;
var packet = USBPacket.Create((byte)(ROTATION_DATA | HMD_SOURCE), (ushort)(DateTime.Now.Ticks / 1000), rotData);
var d = StructToBytes(packet);
SetPacketCrc(ref d);
_sharedMem.WriteIncomingPacket(d);
}
public void Init(out USBRotationData rotation, out USBPositionData position)
{
rotation = new USBRotationData();
//Convert yaw, pitch, roll to quaternion
var t0 = Math.Cos(Yaw * DegToRad * 0.5);
var t1 = Math.Sin(Yaw * DegToRad * 0.5);
var t2 = Math.Cos(Roll * DegToRad * 0.5);
var t3 = Math.Sin(Roll * DegToRad * 0.5);
var t4 = Math.Cos(Pitch * DegToRad * 0.5);
var t5 = Math.Sin(Pitch * DegToRad * 0.5);
//Set head tracking rotation
rotation.w = (float)(t0 * t2 * t4 + t1 * t3 * t5);
rotation.x = (float)(t0 * t3 * t4 - t1 * t2 * t5);
rotation.y = (float)(t0 * t2 * t5 + t1 * t3 * t4);
rotation.z = (float)(t1 * t2 * t4 - t0 * t3 * t5);
position = new USBPositionData();
position.Position = new float[3];
position.Position[0] = (float)X;
position.Position[1] = (float)Y;
position.Position[2] = (float)Z;
}
