private async Task ClientLoop()
{
//_client.UpdateRate = 100;
while (!_requestStopClient)
{
await Task.Delay(10);
_client.Update();
}
_requestStopClient = false;
_client.Dispose();
_client = null;
}
public void RunClient(string serverName, string serverAdress, int sensor)
{
serverAdress = string.IsNullOrEmpty(serverAdress) ? "localhost" : serverAdress;
//var connection = Vrpn.Connection.CreateServerConnection (port.Value);
_client = new Vrpn.TrackerRemote(String.Format("{0}@{1}", serverName, serverAdress));
_client.MuteWarnings = false;
_client.PositionChanged += (o1, e1) =>
{
if (e1.Sensor != sensor)
{
return;
}
Vrpn.Vector3 axis;
double angle;
e1.Orientation.ToAxisAngle(out axis, out angle);
axis.Normalize();
double x = axis.X;
double y = axis.Y;
double z = axis.Z;
double heading;
double attitude;
double bank;
double sin = Math.Sin(angle);
double cos = Math.Cos(angle);
double t = 1 - cos;
if ((x * y * t + z * sin) > 0.998)
{
heading = 2 * Math.Atan2(x * Math.Sin(angle / 2), Math.Cos(angle / 2));
attitude = Math.PI / 2;
bank = 0;
}
else if ((x * y * t + z * sin) < -0.998)
{
heading = -2 * Math.Atan2(x * Math.Sin(angle / 2), Math.Cos(angle / 2));
attitude = -Math.PI / 2;
bank = 0;
}
else
{
heading = Math.Atan2(y * sin - x * z * (1 - cos), 1 - (y * y + z * z) * (1 - cos));
attitude = Math.Asin(x * y * (1 - cos) + z * sin);
bank = Math.Atan2(x * sin - y * z * (1 - cos), 1 - (x * x + z * z) * (1 - cos));
}
_currentPosition = e1.Position.ToMyVector();
_currentRotation = new Vector3(heading, attitude, bank);
if (_linesBuffer == null)
{
return;
}
AddLine("sensor[{0}] pos[{1}, {2}, {3}] orientation[{4}, {5}, {6}] time[{7}]\r\n",
e1.Sensor.ToString().PadLeft(2),
e1.Position.X.ToString("F2").PadLeft(10),
e1.Position.Y.ToString("F2").PadLeft(10),
e1.Position.Z.ToString("F2").PadLeft(10),
heading.ToString("F2").PadLeft(10),
attitude.ToString("F2").PadLeft(10),
bank.ToString("F2").PadLeft(10),
e1.Time.ToString("hh:mm:ss"));
};
FireConnectionChanged();
_clientLoopTask = Task.Run(() => ClientLoop());
}
