public PSMoveSharpState ReadStateBlocking()
{
IPEndPoint remoteEP = new IPEndPoint(IPAddress.Any, 0);
Byte[] buffer = _udpClient.Receive(ref remoteEP);
PSMoveSharpState state = new PSMoveSharpState();
state.packet_index = NetworkReaderHelper.ReadUint32(ref buffer, 12);
state.serverConfig.FillFromNetworkBuffer(ref buffer);
for (int i = 0; i < PSMoveSharpState.PSMoveSharpNumMoveControllers; i++)
{
state.gemStatus[i].FillFromNetworkBuffer(ref buffer, i);
state.gemStates[i].FillFromNetworkBuffer(ref buffer, i);
state.imageStates[i].FillFromNetworkBuffer(ref buffer, i);
state.pointerStates[i].FillFromNetworkBuffer(ref buffer, i);
state.sphereStates[i].FillFromNetworkBuffer(ref buffer, i);
state.positionPointerStates[i].FillFromNetworkBuffer(ref buffer, i);
}
state.cameraState.FillFromNetworkBuffer(ref buffer);
state.navInfo.FillFromNetworkBuffer(ref buffer);
for (int i = 0; i < PSMoveSharpNavInfo.PSMoveSharpNumNavControllers; i++)
{
state.padData[i].FillFromNetworkBuffer(ref buffer, i);
}
return(state);
}
public void FillFromNetworkBuffer(ref Byte[] buffer, int i)
{
int offset = 2100 + i * 12;
valid = NetworkReaderHelper.ReadUint32(ref buffer, offset);
normalized_x = NetworkReaderHelper.ReadFloat(ref buffer, offset + 4);
normalized_y = NetworkReaderHelper.ReadFloat(ref buffer, offset + 8);
}
public void FillFromNetworkBuffer(ref Byte[] buffer, int i)
{
int offset = 40 + i * 16;
connected = NetworkReaderHelper.ReadUint32(ref buffer, offset);
status_code = NetworkReaderHelper.ReadUint32(ref buffer, offset + 4);
status_flags = NetworkReaderHelper.ReadUint64(ref buffer, offset + 8);
}
public void FillFromNetworkBuffer(ref Byte[] buffer, int i)
{
int offset = 2000 + i * 20;
tracking = NetworkReaderHelper.ReadUint32(ref buffer, offset);
tracking_hue = NetworkReaderHelper.ReadUint32(ref buffer, offset + 4);
r = NetworkReaderHelper.ReadFloat(ref buffer, offset + 8);
g = NetworkReaderHelper.ReadFloat(ref buffer, offset + 12);
b = NetworkReaderHelper.ReadFloat(ref buffer, offset + 16);
}
public void FillFromNetworkBuffer(ref Byte[] buffer)
{
int offset = 1048;
for (int i = 0; i < PSMoveSharpNumNavControllers; i++)
{
port_status[i] = NetworkReaderHelper.ReadUint32(ref buffer, offset);
offset += 4;
}
}
protected void ReceiveCallback(IAsyncResult ar)
{
UdpClient u = (UdpClient)((UdpState)(ar.AsyncState)).u;
IPEndPoint e = (IPEndPoint)((UdpState)(ar.AsyncState)).e;
Byte[] buffer = null;
try
{
buffer = u.EndReceive(ar, ref e);
}
catch (System.ObjectDisposedException)
{
_reading = false;
return;
}
catch (System.Net.Sockets.SocketException)
{
_reading = false;
return;
}
int packet_length = NetworkReaderHelper.ReadInt32(ref buffer, 12);
int magic = NetworkReaderHelper.ReadInt32(ref buffer, 0);
int code = NetworkReaderHelper.ReadInt32(ref buffer, 8);
uint packet_index = NetworkReaderHelper.ReadUint32(ref buffer, 16);
if (code == 1)
{
_latest_state.packet_index = NetworkReaderHelper.ReadUint32(ref buffer, 16);
_latest_state.serverConfig.FillFromNetworkBuffer(ref buffer);
for (int i = 0; i < PSMoveSharpState.PSMoveSharpNumMoveControllers; i++)
{
_latest_state.gemStatus[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.gemStates[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.imageStates[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.pointerStates[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.sphereStates[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.positionPointerStates[i].FillFromNetworkBuffer(ref buffer, i);
}
_latest_state.navInfo.FillFromNetworkBuffer(ref buffer);
for (int i = 0; i < PSMoveSharpNavInfo.PSMoveSharpNumNavControllers; i++)
{
_latest_state.padData[i].FillFromNetworkBuffer(ref buffer, i);
}
if (_state_update_delegate != null)
{
_state_update_delegate(_latest_state);
}
// fire delegate here
}
_reading = false;
}
public void FillFromNetworkBuffer(ref Byte[] buffer, int i)
{
int offset = 104 + i * 176;
pos = NetworkReaderHelper.ReadFloat4(ref buffer, offset);
vel = NetworkReaderHelper.ReadFloat4(ref buffer, offset + 16);
accel = NetworkReaderHelper.ReadFloat4(ref buffer, offset + 32);
quat = NetworkReaderHelper.ReadFloat4(ref buffer, offset + 48);
angvel = NetworkReaderHelper.ReadFloat4(ref buffer, offset + 64);
angaccel = NetworkReaderHelper.ReadFloat4(ref buffer, offset + 80);
handle_pos = NetworkReaderHelper.ReadFloat4(ref buffer, offset + 96);
handle_vel = NetworkReaderHelper.ReadFloat4(ref buffer, offset + 112);
handle_accel = NetworkReaderHelper.ReadFloat4(ref buffer, offset + 128);
pad.FillFromNetworkBuffer(ref buffer, i);
timestamp = NetworkReaderHelper.ReadInt64(ref buffer, offset + 152);
temperature = NetworkReaderHelper.ReadFloat(ref buffer, offset + 160);
camera_pitch_angle = NetworkReaderHelper.ReadFloat(ref buffer, offset + 164);
tracking_flags = NetworkReaderHelper.ReadUint32(ref buffer, offset + 168);
}
protected void ReceiveCallback(IAsyncResult ar)
{
UdpClient u = (UdpClient)((UdpState)(ar.AsyncState)).u;
IPEndPoint e = (IPEndPoint)((UdpState)(ar.AsyncState)).e;
Byte[] buffer = null;
try
{
buffer = u.EndReceive(ar, ref e);
}
catch (System.ObjectDisposedException)
{
_reading = false;
return;
}
catch (System.Net.Sockets.SocketException)
{
_reading = false;
return;
}
_rwl.AcquireWriterLock(-1);
int packet_length = NetworkReaderHelper.ReadInt32(ref buffer, 12);
int magic = NetworkReaderHelper.ReadInt32(ref buffer, 0);
int code = NetworkReaderHelper.ReadInt32(ref buffer, 8);
uint packet_index = NetworkReaderHelper.ReadUint32(ref buffer, 16);
//UnityEngine.Debug.Log(code);
if (code == 1)
{
_latest_state.packet_index = NetworkReaderHelper.ReadUint32(ref buffer, 16);
_latest_state.serverConfig.FillFromNetworkBuffer(ref buffer);
for (int i = 0; i < PSMoveSharpState.PSMoveSharpNumMoveControllers; i++)
{
_latest_state.gemStatus[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.gemStates[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.imageStates[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.pointerStates[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.sphereStates[i].FillFromNetworkBuffer(ref buffer, i);
_latest_state.positionPointerStates[i].FillFromNetworkBuffer(ref buffer, i);
}
_latest_state.navInfo.FillFromNetworkBuffer(ref buffer);
for (int i = 0; i < PSMoveSharpNavInfo.PSMoveSharpNumNavControllers; i++)
{
_latest_state.padData[i].FillFromNetworkBuffer(ref buffer, i);
}
}
else if (code == 2)
{
byte[] slice = new byte[packet_length - 3];
int slice_num = NetworkReaderHelper.ReadByte(ref buffer, 20);
int num_slices = NetworkReaderHelper.ReadByte(ref buffer, 21);
int format = NetworkReaderHelper.ReadByte(ref buffer, 22);
int row_height = 480 / num_slices;
int row_start = row_height * slice_num;
Array.Copy(buffer, 23, slice, 0, packet_length - 3);
/*
* System.IO.MemoryStream jpeg_stream = new System.IO.MemoryStream(jpeg_data);
* System.Drawing.Image slice = null;
* try
* {
* slice = Image.FromStream(jpeg_stream);
* testByte = jpeg_data;
* }
* catch
* {
* // server sent a bad frame
* }
*/
if (slice != null)
{
_latest_camera_frame_state.ProcessPacket((int)packet_index, slice, row_start, row_height);
//_latest_camera_frame_state.BlitSliceOntoFullImage(slice, row_start, row_height);
}
}
else if (code == 3)
{
PSMoveSharpState camera_state = new PSMoveSharpState();
camera_state.packet_index = NetworkReaderHelper.ReadUint32(ref buffer, 16);
camera_state.serverConfig.FillFromNetworkBuffer(ref buffer);
for (int i = 0; i < PSMoveSharpState.PSMoveSharpNumMoveControllers; i++)
{
camera_state.gemStatus[i].FillFromNetworkBuffer(ref buffer, i);
camera_state.gemStates[i].FillFromNetworkBuffer(ref buffer, i);
camera_state.imageStates[i].FillFromNetworkBuffer(ref buffer, i);
camera_state.pointerStates[i].FillFromNetworkBuffer(ref buffer, i);
camera_state.sphereStates[i].FillFromNetworkBuffer(ref buffer, i);
camera_state.positionPointerStates[i].FillFromNetworkBuffer(ref buffer, i);
}
camera_state.navInfo.FillFromNetworkBuffer(ref buffer);
for (int i = 0; i < PSMoveSharpNavInfo.PSMoveSharpNumNavControllers; i++)
{
camera_state.padData[i].FillFromNetworkBuffer(ref buffer, i);
}
_latest_camera_frame_state.ProcessStatePacket((int)packet_index, camera_state);
}
_rwl.ReleaseWriterLock();
_reading = false;
}
