public PSMoveClientDelegate()
{
_readerThread = new Thread(new ThreadStart(PSMoveClienDelegateReadThread));
_readerThreadExit = 0;
_latest_state = new PSMoveSharpState();
_reading = false;
}
public PSMoveClientDelegate()
{
_readerThread = new Thread(new ThreadStart(PSMoveClienDelegateReadThread));
_readerThreadExit = 0;
_latest_state = new PSMoveSharpState();
_reading = false;
}
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 UpdateState(PSMoveSharpState state)
{
// not connected
if ((state.navInfo.port_status[0] & 0x1) == 0)
{
return;
}
int digitalpad1_changed = (int)state.padData[0].button[2] ^ _digitalpad1;
int digitalpad1_pressed = digitalpad1_changed & (int)state.padData[0].button[2];
int digitalpad1_released = digitalpad1_changed & (int)~state.padData[0].button[2];
int digitalpad2_changed = (int)state.padData[0].button[3] ^ _digitalpad2;
int digitalpad2_pressed = digitalpad2_changed & (int)state.padData[0].button[3];
int digitalpad2_released = digitalpad2_changed & (int)~state.padData[0].button[3];
_digitalpad1 = (int)state.padData[0].button[2];
_digitalpad2 = (int)state.padData[0].button[3];
bool cross_released = (digitalpad2_released & (1 << 6)) != 0;
bool circle_released = (digitalpad2_released & (1 << 5)) != 0;
bool dpad_down_released = (digitalpad1_released & (1 << 6)) != 0;
float pressure_L1 = (float)state.padData[0].button[16] / 255.0f;
float pressure_L2 = (float)state.padData[0].button[18] / 255.0f;
float left_right = (float)state.padData[0].button[6] / 255.0f;
float up_down = (float)state.padData[0].button[7] / 255.0f;
left_right *= 2.0f;
up_down *= 2.0f;
left_right -= 1.0f;
up_down -= 1.0f;
up_down *= -1.0f;
left_right = ZeroInRange(left_right, -0.25f, 0.25f);
up_down = ZeroInRange(up_down, -0.25f, 0.25f);
RotateX(left_right);
RotateZ(up_down);
ZoomIn(pressure_L1);
ZoomOut(pressure_L2);
if (dpad_down_released)
{
Program.flower.Reset();
}
if (cross_released)
{
Program.flower.NextCapture();
}
if (circle_released)
{
Program.flower.ToggleCaptureState();
}
_sphere_visible = state.imageStates[0].visible != 0;
}
public System.Drawing.Image GetCameraFrameAndState(ref PSMoveSharpState state)
{
System.Drawing.Image image;
camera_frame_state_rwl.AcquireReaderLock(-1);
state = camera_frame_state_collector.GetCompleteState();
image = (System.Drawing.Image)camera_frame_state_collector.GetCompleteImage().Clone();
camera_frame_state_rwl.ReleaseReaderLock();
return(image);
}
public PSMoveClientThreadedRead()
{
_readerThread = new Thread(new ThreadStart(PSMoveClientThreadedReadThreadStart));
_rwl = new ReaderWriterLock();
_readerThreadExit = 0;
_latest_state = new PSMoveSharpState();
_latest_camera_frame_state = new PSMoveSharpCameraFrameState();
_reading = false;
}
public PSMoveSharpCameraFrameStateCollector()
{
num_slices_ = 1;
current_index_ = 0;
slices_ = new PSMoveSharpCameraFrameSlice[num_slices_];
slices_[0] = new PSMoveSharpCameraFrameSlice();
last_state_ = new PSMoveSharpState();
complete_state_ = new PSMoveSharpState();
complete_image_ = new List <byte[]>();
}
public List <byte[]> GetCameraFrameAndState(ref PSMoveSharpState state)
{
List <byte[]> image;
camera_frame_state_rwl.AcquireReaderLock(-1);
state = camera_frame_state_collector.GetCompleteState();
image = camera_frame_state_collector.GetCompleteImage();
camera_frame_state_rwl.ReleaseReaderLock();
return(image);
}
public void CaptureState(int index, PSMoveSharpState state)
{
if (index == current_index_)
{
last_state_ = state;
if (IsSetComplete())
{
complete_state_ = last_state_;
Console.WriteLine("Complete set packet index {0}", complete_state_.packet_index);
complete_image_ = MakeCompleteImage();
}
}
}
private void updateTabPagePosition(PSMoveSharpState state)
{
checkBoxPosition1.Checked = (state.positionPointerStates[0].valid == 1);
checkBoxPosition2.Checked = (state.positionPointerStates[1].valid == 1);
checkBoxPosition3.Checked = (state.positionPointerStates[2].valid == 1);
checkBoxPosition4.Checked = (state.positionPointerStates[3].valid == 1);
labelPositionPos1ValX.Text = (state.positionPointerStates[0].valid == 1) ? state.positionPointerStates[0].normalized_x.ToString("N3") : "N/A";
labelPositionPos1ValY.Text = (state.positionPointerStates[0].valid == 1) ? state.positionPointerStates[0].normalized_y.ToString("N3") : "N/A";
labelPositionPos2ValX.Text = (state.positionPointerStates[1].valid == 1) ? state.positionPointerStates[1].normalized_x.ToString("N3") : "N/A";
labelPositionPos2ValY.Text = (state.positionPointerStates[1].valid == 1) ? state.positionPointerStates[1].normalized_y.ToString("N3") : "N/A";
labelPositionPos3ValX.Text = (state.positionPointerStates[2].valid == 1) ? state.positionPointerStates[2].normalized_x.ToString("N3") : "N/A";
labelPositionPos3ValY.Text = (state.positionPointerStates[2].valid == 1) ? state.positionPointerStates[2].normalized_y.ToString("N3") : "N/A";
labelPositionPos4ValX.Text = (state.positionPointerStates[3].valid == 1) ? state.positionPointerStates[3].normalized_x.ToString("N3") : "N/A";
labelPositionPos4ValY.Text = (state.positionPointerStates[3].valid == 1) ? state.positionPointerStates[3].normalized_y.ToString("N3") : "N/A";
for (int i = 0; i < PSMoveSharpState.PSMoveSharpNumMoveControllers; i++)
{
UInt16 just_pressed;
{
UInt16 changed_buttons = (UInt16)(state.gemStates[i].pad.digitalbuttons ^ last_buttons[i]);
just_pressed = (UInt16)(changed_buttons & state.gemStates[i].pad.digitalbuttons);
last_buttons[i] = state.gemStates[i].pad.digitalbuttons;
}
pointerDisplayControlPosition.valid[i] = (state.positionPointerStates[i].valid == 1);
if (state.positionPointerStates[i].valid == 1)
{
pointerDisplayControlPosition.pointer_x[i] = state.positionPointerStates[i].normalized_x;
pointerDisplayControlPosition.pointer_y[i] = state.positionPointerStates[i].normalized_y;
const int PadSelect = 1;
const int PadTriangle = 1 << 4;
if ((just_pressed & PadSelect) == PadSelect)
{
Program.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerDisable, Convert.ToUInt32(i));
Console.WriteLine("Disabling pointer");
calibration_step[i] = ClientCalibrationStep.Left;
}
}
else
{
const int PadTick = 1 << 2;
const int PadTrigger = 1 << 1;
if ((just_pressed & PadTick) == PadTick && (last_buttons[i] & PadTrigger) == PadTrigger)
{
switch (calibration_step[i])
{
case ClientCalibrationStep.Left:
Program.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerSetLeft, Convert.ToUInt32(i));
break;
case ClientCalibrationStep.Right:
Program.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerSetRight, Convert.ToUInt32(i));
break;
case ClientCalibrationStep.Bottom:
Program.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerSetBottom, Convert.ToUInt32(i));
break;
case ClientCalibrationStep.Top:
Program.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerSetTop, Convert.ToUInt32(i));
break;
}
Console.WriteLine("Calibration tick");
calibration_step[i]++;
if (calibration_step[i] == ClientCalibrationStep.Done)
{
Program.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerEnable, Convert.ToUInt32(i));
Console.WriteLine("Enabling pointer");
}
}
}
}
pointerDisplayControlPosition.Invalidate();
pointerDisplayControlPosition.Update();
}
private void updateTabPageNav(PSMoveSharpState state)
{
bool selected_nav_connected = ((state.navInfo.port_status[Program.selected_nav] & 0x1) == 0x1);
checkBoxNav1.Checked = ((state.navInfo.port_status[0] & 0x1) == 0x1);
checkBoxNav2.Checked = ((state.navInfo.port_status[1] & 0x1) == 0x1);
checkBoxNav3.Checked = ((state.navInfo.port_status[2] & 0x1) == 0x1);
checkBoxNav4.Checked = ((state.navInfo.port_status[3] & 0x1) == 0x1);
checkBoxNav5.Checked = ((state.navInfo.port_status[4] & 0x1) == 0x1);
checkBoxNav6.Checked = ((state.navInfo.port_status[5] & 0x1) == 0x1);
checkBoxNav7.Checked = ((state.navInfo.port_status[6] & 0x1) == 0x1);
checkBoxNavUpVal.Checked = selected_nav_connected && ((state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetDigital1] & PSMoveSharpConstants.ctrlUp) != 0);
checkBoxNavDownVal.Checked = selected_nav_connected && ((state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetDigital1] & PSMoveSharpConstants.ctrlDown) != 0);
checkBoxNavLeftVal.Checked = selected_nav_connected && ((state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetDigital1] & PSMoveSharpConstants.ctrlLeft) != 0);
checkBoxNavRightVal.Checked = selected_nav_connected && ((state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetDigital1] & PSMoveSharpConstants.ctrlRight) != 0);
labelNavAnalogXVal.Text = selected_nav_connected ? (state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetAnalogLeftX] - 128).ToString() : "N/A";
labelNavAnalogYVal.Text = selected_nav_connected ? (state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetAnalogLeftY] - 128).ToString() : "N/A";
checkBoxNavCrossVal.Checked = selected_nav_connected && ((state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetDigital2] & PSMoveSharpConstants.ctrlCross) != 0);
checkBoxNavCircleVal.Checked = selected_nav_connected && ((state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetDigital2] & PSMoveSharpConstants.ctrlCircle) != 0);
checkBoxNavL1Val.Checked = selected_nav_connected && ((state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetDigital2] & PSMoveSharpConstants.ctrlL1) != 0);
labelNavL2Val.Text = selected_nav_connected ? state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetPressL2].ToString() : "N/A";
checkBoxNavL3Val.Checked = selected_nav_connected && ((state.padData[Program.selected_nav].button[PSMoveSharpConstants.offsetDigital1] & PSMoveSharpConstants.ctrlL3) != 0);
}
private void updateTabPageMove(PSMoveSharpState state)
{
checkBoxMove1.Checked = (state.gemStatus[0].connected == 1);
checkBoxMove2.Checked = (state.gemStatus[1].connected == 1);
checkBoxMove3.Checked = (state.gemStatus[2].connected == 1);
checkBoxMove4.Checked = (state.gemStatus[3].connected == 1);
checkBoxMoveResetEnabled.Checked = Program.reset_enabled[Program.selected_move];
buttonResetEnabled.Text = Program.reset_enabled[Program.selected_move] ? "Disable" : "Enable";
bool selected_move_connected = (state.gemStatus[Program.selected_move].connected == 1);
PSMoveSharpGemState selected_gem = state.gemStates[Program.selected_move];
labelPosValX.Text = selected_move_connected ? Convert.ToInt32(selected_gem.pos.x).ToString() : "N/A";
labelPosValY.Text = selected_move_connected ? Convert.ToInt32(selected_gem.pos.y).ToString() : "N/A";
labelPosValZ.Text = selected_move_connected ? Convert.ToInt32(selected_gem.pos.z).ToString() : "N/A";
labelVelValX.Text = selected_move_connected ? Convert.ToInt32(selected_gem.vel.x).ToString() : "N/A";
labelVelValY.Text = selected_move_connected ? Convert.ToInt32(selected_gem.vel.y).ToString() : "N/A";
labelVelValZ.Text = selected_move_connected ? Convert.ToInt32(selected_gem.vel.z).ToString() : "N/A";
labelAccValX.Text = selected_move_connected || true ? Convert.ToInt32(selected_gem.accel.x).ToString() : "N/A";
labelAccValY.Text = selected_move_connected || true ? Convert.ToInt32(selected_gem.accel.y).ToString() : "N/A";
labelAccValZ.Text = selected_move_connected || true ? Convert.ToInt32(selected_gem.accel.z).ToString() : "N/A";
labelQuatValH.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * quatToEuler(selected_gem.quat).x).ToString() : "N/A";
labelQuatValP.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * quatToEuler(selected_gem.quat).y).ToString() : "N/A";
labelQuatValR.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * quatToEuler(selected_gem.quat).z).ToString() : "N/A";
labelAngVelValH.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * selected_gem.angvel.x).ToString() : "N/A";
labelAngVelValP.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * selected_gem.angvel.y).ToString() : "N/A";
labelAngVelValR.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * selected_gem.angvel.z).ToString() : "N/A";
labelAngAccValH.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * selected_gem.angaccel.x).ToString() : "N/A";
labelAngAccValP.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * selected_gem.angaccel.y).ToString() : "N/A";
labelAngAccValR.Text = selected_move_connected ? Convert.ToInt32((180.0 / Math.PI) * selected_gem.angaccel.z).ToString() : "N/A";
labelHandlePosValX.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_pos.x).ToString() : "N/A";
labelHandlePosValY.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_pos.y).ToString() : "N/A";
labelHandlePosValZ.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_pos.z).ToString() : "N/A";
labelHandleVelValX.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_vel.x).ToString() : "N/A";
labelHandleVelValY.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_vel.y).ToString() : "N/A";
labelHandleVelValZ.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_vel.z).ToString() : "N/A";
labelHandleAccValX.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_accel.x).ToString() : "N/A";
labelHandleAccValY.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_accel.y).ToString() : "N/A";
labelHandleAccValZ.Text = selected_move_connected ? Convert.ToInt32(selected_gem.handle_accel.z).ToString() : "N/A";
tableLayoutPanelMoveMotion.Update();
checkBoxMoveSquareVal.Checked = ((selected_gem.pad.digitalbuttons & PSMoveSharpConstants.ctrlSquare) != 0);
checkBoxMoveCrossVal.Checked = ((selected_gem.pad.digitalbuttons & PSMoveSharpConstants.ctrlCross) != 0);
checkBoxMoveCircleVal.Checked = ((selected_gem.pad.digitalbuttons & PSMoveSharpConstants.ctrlCircle) != 0);
checkBoxMoveTriangleVal.Checked = ((selected_gem.pad.digitalbuttons & PSMoveSharpConstants.ctrlTriangle) != 0);
checkBoxMoveMoveVal.Checked = ((selected_gem.pad.digitalbuttons & PSMoveSharpConstants.ctrlTick) != 0);
labelMoveTVal.Text = selected_move_connected ? selected_gem.pad.analog_trigger.ToString() : "N/A";
checkBoxMoveStartVal.Checked = ((selected_gem.pad.digitalbuttons & PSMoveSharpConstants.ctrlStart) != 0);
checkBoxMoveSelectVal.Checked = ((selected_gem.pad.digitalbuttons & PSMoveSharpConstants.ctrlSelect) != 0);
tableLayoutPanelMoveInput.Update();
if (Program.reset_enabled[Program.selected_move] && ((state.gemStates[Program.selected_move].pad.digitalbuttons & PSMoveSharpConstants.ctrlSelect) != 0))
{
Program.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestControllerReset, Convert.ToUInt32(Program.selected_move));
}
}
private void updateTabPageCamera(PSMoveSharpCameraFrameState camera_frame_state)
{
if (Program.image_paused)
{
ImagePausedToggleButton.Text = "Unpause";
} else {
ImagePausedToggleButton.Text = "Pause";
}
camera_frame_state.camera_frame_state_rwl.AcquireReaderLock(-1);
PSMoveSharpState dummy_state = new PSMoveSharpState();
imageBox.Image = camera_frame_state.GetCameraFrameAndState(ref dummy_state);
camera_frame_state.camera_frame_state_rwl.ReleaseReaderLock();
}
// Determines whether or not to add the current controller location to the list of spheres to draw
private void processSpherePos(PSMoveSharpState state)
{
PSMoveSharpGemState selected_gem = state.gemStates[Program.selected_move];
if ((selected_gem.pad.digitalbuttons & PSMoveSharpConstants.ctrlTrigger) != 0) {
Vector3 pos = getXYZ(state);
if (prevPos.X == -9999f && prevPos.Y == -9999f && prevPos.Z == -9999f ||
Vector3.Subtract(pos, prevPos).Length > minDist)
{
spheresToDraw.Add(pos);
prevPos = pos;
}
}
}
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;
}
public void ProcessStatePacket(int index, PSMoveSharpState state)
{
camera_frame_state_rwl.AcquireWriterLock(-1);
camera_frame_state_collector.CaptureState(index, state);
camera_frame_state_rwl.ReleaseWriterLock();
}
public List<byte[]> GetCameraFrameAndState(ref PSMoveSharpState state)
{
List<byte[]> image;
camera_frame_state_rwl.AcquireReaderLock(-1);
state = camera_frame_state_collector.GetCompleteState();
image = camera_frame_state_collector.GetCompleteImage();
camera_frame_state_rwl.ReleaseReaderLock();
return image;
}
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;
}
public PSMoveClientThreadedRead()
{
_readerThread = new Thread(new ThreadStart(PSMoveClientThreadedReadThreadStart));
_rwl = new ReaderWriterLock();
_readerThreadExit = 0;
_latest_state = new PSMoveSharpState();
_latest_camera_frame_state = new PSMoveSharpCameraFrameState();
_reading = false;
}
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;
}
protected override void Update(GameTime gameTime)
{
if ((GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed) ||
(Keyboard.GetState().IsKeyDown(Keys.Escape)))
this.Exit();
//required to listen to input for Gem
state = moveClient.GetLatestState();
count++;
UInt16 just_pressed;
UInt16 just_released;
{
UInt16 changed_buttons = Convert.ToUInt16(state.gemStates[0].pad.digitalbuttons ^ last_buttons);
just_pressed = Convert.ToUInt16(changed_buttons & state.gemStates[0].pad.digitalbuttons);
just_released = Convert.ToUInt16(changed_buttons & ~state.gemStates[0].pad.digitalbuttons);
last_buttons = state.gemStates[0].pad.digitalbuttons;
}
// doing this makes the curser move on the screen like a "laser pointer"
crosshair_pos.X += (int)state.gemStates[0].vel.x / 10;
crosshair_pos.Y -= (int)state.gemStates[0].vel.y / 10;
if (crosshair_pos.X > GraphicsDeviceManager.DefaultBackBufferWidth - crosshairs.Width / 2)
{
crosshair_pos.X = (GraphicsDeviceManager.DefaultBackBufferWidth - crosshairs.Width / 2);
}
else if (crosshair_pos.X < -1 * (crosshairs.Width / 2))
{
crosshair_pos.X = 0 - crosshairs.Width / 2;
}
if (crosshair_pos.Y > GraphicsDeviceManager.DefaultBackBufferHeight - crosshairs.Height / 2)
{
crosshair_pos.Y = GraphicsDeviceManager.DefaultBackBufferHeight - crosshairs.Height / 2;
}
else if (crosshair_pos.Y < -1 * (crosshairs.Height / 2))
{
crosshair_pos.Y = 0 - crosshairs.Height / 2;
}
//
//
// this coverts these cartesian coordinates to screen coordinates
X = ((GraphicsDevice.Viewport.Width / 2) + (int)(crosshair_pos.X - 0)) * 1;
Y = ((GraphicsDevice.Viewport.Height / 2) - (int)(crosshair_pos.Y - 0)) * 1;
//gets crosshair x and y based on gem position and coverted it to
//screen coordinates from cartesian
//crosshair_pos.X = GraphicsDevice.Viewport.Width / 2 + (state.gemStates[0].pos.x - center.X) * 1;
//crosshair_pos.Y = GraphicsDevice.Viewport.Height / 2 - (state.gemStates[0].pos.y - center.Y) * 1;
//gets crosshair x and y based on vel gem position(like a mouse cursor) and coverted it to
//screen coordinates from cartesian. adds to the previous curser each time to get corrct movement
//crosshair_pos.X += (GraphicsDevice.Viewport.Width / 2) + ((state.gemStates[0].vel.x / 5) - center.X) * 1;
//crosshair_pos.Y += (GraphicsDevice.Viewport.Height / 2) - ((state.gemStates[0].vel.y / 5) - center.Y) *1;
//checks to see if the trigger was pressed on the wand
// if so then it writes the coordinates to server
if ((just_pressed & PSMoveSharpConstants.ctrlTrigger) == PSMoveSharpConstants.ctrlTrigger)
{
x = (int)(crosshair_pos.X + (crosshairs.Width / 2)) - 13;
y = (int)(crosshair_pos.Y + (crosshairs.Height / 2)) - 11;
Console.WriteLine("1," + x.ToString() + "," + y.ToString());
sw.WriteLine(x.ToString() + "," + y.ToString());
sw.Flush();
}
if (listener.Pending())
{
TcpClient tc = listener.AcceptTcpClient();
Connection_Work cw = new Connection_Work(tc, ID.ToString(), this);
if (ID == 1)
{
isClient_1_connected = true;
}
if (ID == 2)
{
isClient_2_connected = true;
}
if (ID == 3)
{
isClient_3_connected = true;
}
if (ID == 4)
{
isClient_4_connected = true;
}
ID++;
cw.RaiseInputReceived += new Connection_Work.InputReceived(cw_RaiseInputReceived);
connections.Add(cw);
}
base.Update(gameTime);
}
public System.Drawing.Image GetCameraFrameAndState(ref PSMoveSharpState state)
{
System.Drawing.Image image;
camera_frame_state_rwl.AcquireReaderLock(-1);
state = camera_frame_state_collector.GetCompleteState();
image = (System.Drawing.Image)camera_frame_state_collector.GetCompleteImage().Clone();
camera_frame_state_rwl.ReleaseReaderLock();
return image;
}
private void updateTabPageSculpture(PSMoveSharpState state)
{
glControl1.Invalidate();
}
public void UpdateState(PSMoveSharpState state)
{
if (_capture_state == 1)
{
// we are not capturing when in capture state 1
return;
}
_update_lock.AcquireWriterLock(-1);
if (_samples[_capture_slot].Count >= _buffer_size)
{
// remove the first (oldest sample)
_samples[_capture_slot].RemoveFirst();
}
MoveSample sample;
sample.x = state.gemStates[0].pos.x;
sample.y = state.gemStates[0].pos.y;
sample.z = state.gemStates[0].pos.z;
sample.time = state.gemStates[0].timestamp;
_samples[_capture_slot].AddLast(sample);
_update_lock.ReleaseWriterLock();
}
public void ProcessStatePacket(int index, PSMoveSharpState state)
{
camera_frame_state_rwl.AcquireWriterLock(-1);
camera_frame_state_collector.CaptureState(index, state);
camera_frame_state_rwl.ReleaseWriterLock();
}
private void updateMouser(PSMoveSharpState state)
{
UInt16 just_pressed;
UInt16 just_released;
{
UInt16 changed_buttons = Convert.ToUInt16(state.gemStates[0].pad.digitalbuttons ^ last_buttons);
just_pressed = Convert.ToUInt16(changed_buttons & state.gemStates[0].pad.digitalbuttons);
just_released = Convert.ToUInt16(changed_buttons & ~state.gemStates[0].pad.digitalbuttons);
last_buttons = state.gemStates[0].pad.digitalbuttons;
}
checkBoxCalibrated.Checked = (state.positionPointerStates[0].valid == 1);
checkBoxEnabled.Checked = fake_mouse_events;
labelMovePosValX.Text = (state.positionPointerStates[0].valid == 1) ? state.positionPointerStates[0].normalized_x.ToString("N3") : "N/A";
labelMovePosValY.Text = (state.positionPointerStates[0].valid == 1) ? state.positionPointerStates[0].normalized_y.ToString("N3") : "N/A";
buttonEnable.Text = fake_mouse_events ? "Disable" : "Enable";
if (state.positionPointerStates[0].valid == 1)
{
if ((just_pressed & PSMoveSharpConstants.ctrlSelect) == PSMoveSharpConstants.ctrlSelect)
{
MouserProgram.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerDisable, 0);
Console.WriteLine("Disabling pointer");
calibration_step = 0;
}
if (fake_mouse_events)
{
if ((just_pressed & PSMoveSharpConstants.ctrlCross) == PSMoveSharpConstants.ctrlCross)
{
MouserRecenter(state.positionPointerStates[0].normalized_x, state.positionPointerStates[0].normalized_y);
}
else
{
MouserMove(state.positionPointerStates[0].normalized_x, state.positionPointerStates[0].normalized_y);
}
if (((just_pressed & PSMoveSharpConstants.ctrlTick) == PSMoveSharpConstants.ctrlTick))
{
SimulateMouseClickEvent(MOUSEEVENTF_LEFTDOWN);
}
else if (((just_released & PSMoveSharpConstants.ctrlTick) == PSMoveSharpConstants.ctrlTick))
{
SimulateMouseClickEvent(MOUSEEVENTF_LEFTUP);
}
if (((just_pressed & PSMoveSharpConstants.ctrlCircle) == PSMoveSharpConstants.ctrlCircle))
{
SimulateMouseClickEvent(MOUSEEVENTF_RIGHTDOWN);
}
else if (((just_released & PSMoveSharpConstants.ctrlCircle) == PSMoveSharpConstants.ctrlCircle))
{
SimulateMouseClickEvent(MOUSEEVENTF_RIGHTUP);
}
}
}
else if (((just_pressed & PSMoveSharpConstants.ctrlTick) == PSMoveSharpConstants.ctrlTick) &&
((last_buttons & PSMoveSharpConstants.ctrlTrigger) == PSMoveSharpConstants.ctrlTrigger))
{
switch (calibration_step)
{
case ClientCalibrationStep.Left:
MouserProgram.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerSetLeft, 0);
break;
case ClientCalibrationStep.Right:
MouserProgram.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerSetRight, 0);
break;
case ClientCalibrationStep.Bottom:
MouserProgram.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerSetBottom, 0);
break;
case ClientCalibrationStep.Top:
MouserProgram.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerSetTop, 0);
break;
}
Console.WriteLine("Calibration tick");
calibration_step++;
if (calibration_step == ClientCalibrationStep.Done)
{
MouserProgram.moveClient.SendRequestPacket(PSMoveClient.ClientRequest.PSMoveClientRequestPositionPointerEnable, 0);
Console.WriteLine("Enabling pointer");
}
}
}
public PSMoveSharpCameraFrameStateCollector()
{
num_slices_ = 1;
current_index_ = 0;
slices_ = new PSMoveSharpCameraFrameSlice[num_slices_];
slices_[0] = new PSMoveSharpCameraFrameSlice();
last_state_ = new PSMoveSharpState();
complete_state_ = new PSMoveSharpState();
complete_image_ = new List<byte[]>();
}
// Get the position of the controller
private Vector3 getXYZ(PSMoveSharpState state)
{
PSMoveSharpGemState selected_gem = state.gemStates[Program.selected_move];
bool selected_move_connected = (state.gemStatus[Program.selected_move].connected == 1);
int x = selected_move_connected ? Convert.ToInt32(selected_gem.pos.x) : -999999;
int y = selected_move_connected ? Convert.ToInt32(selected_gem.pos.y) : -999999;
int z = selected_move_connected ? Convert.ToInt32(selected_gem.pos.z) : -999999;
if (x == -999999 || y == -999999 || z == -999999)
{
Console.WriteLine("Invalid position state in getXYZ function");
}
//Console.WriteLine("Location at " + x + ", " + y + ", " + z);
return new Vector3(x, y, z);
}
public void CaptureState(int index, PSMoveSharpState state)
{
if (index == current_index_)
{
last_state_ = state;
if (IsSetComplete())
{
complete_state_ = last_state_;
Console.WriteLine("Complete set packet index {0}", complete_state_.packet_index);
complete_image_ = MakeCompleteImage();
}
}
}
public PSMoveSharpCameraFrameStateCollector()
{
num_slices_ = 1;
current_index_ = 0;
slices_ = new PSMoveSharpCameraFrameSlice[num_slices_];
slices_[0] = new PSMoveSharpCameraFrameSlice();
last_state_ = new PSMoveSharpState();
complete_state_ = new PSMoveSharpState();
complete_image_ = new Bitmap(640, 480);
complete_bitmap_ = new Bitmap(640, 480);
}
