public void DisableTracking(MotionControllerModel mc)
{
if (mc.Design)
{
return;
}
PsMoveApi.psmove_tracker_disable(_camera.Handle, mc.Handle);
mc.Tracking[_camera] = false;
mc.TrackerStatus[_camera] = PSMoveTrackerStatus.NotCalibrated;
ConsoleService.Write(string.Format("[Tracker, {0}] Tracking of Motion Controller ({1}) disabled.",
_camera.GUID, mc.Serial));
}
public bool StartTracker(PSMoveTrackerExposure exposure)
{
if (_camera.Handle == IntPtr.Zero)
{
_camera.Handle = PsMoveApi.psmove_tracker_new_with_camera(_camera.TrackerId);
ConsoleService.Write(string.Format("[Tracker, {0}] Started.", _camera.GUID));
PsMoveApi.psmove_tracker_set_exposure(_camera.Handle, exposure);
// full led intensity
PsMoveApi.psmove_tracker_set_dimming(_camera.Handle, 1f);
_camera.Fusion = PsMoveApi.new_PSMoveFusion(_camera.Handle, 1.0f, 1000f);
}
return(_camera.Handle != IntPtr.Zero);
}
public void Destroy()
{
if (_camera.Handle != IntPtr.Zero)
{
DisableTracking();
CancelUpdateTask();
ConsoleService.Write(string.Format("[Tracker, {0}] Tracker destroyed.", _camera.GUID));
}
if (_camera.Fusion != IntPtr.Zero)
{
PsMoveApi.psmove_fusion_free(_camera.Fusion);
_camera.Fusion = IntPtr.Zero;
ConsoleService.Write(string.Format("[Tracker, {0}] Fusion destroyed.", _camera.GUID));
}
}
public void EnableTracking(MotionControllerModel mc)
{
if (mc.Design)
{
return;
}
if (_camera.Handle == IntPtr.Zero)
{
StartTracker(PSMoveTrackerExposure.Low);
}
ConsoleService.Write(string.Format("[Tracker, {0}] Calibrating Motion Controller ({1}).", _camera.GUID, mc.Serial));
byte r = (byte)((mc.Color.r * 255) + 0.5f);
byte g = (byte)((mc.Color.g * 255) + 0.5f);
byte b = (byte)((mc.Color.b * 255) + 0.5f);
mc.TrackerStatus[_camera] = PsMoveApi.psmove_tracker_enable_with_color(_camera.Handle, mc.Handle, r, g, b);
if (mc.TrackerStatus[_camera] == PSMoveTrackerStatus.Tracking ||
mc.TrackerStatus[_camera] == PSMoveTrackerStatus.Calibrated)
{
PsMoveApi.psmove_tracker_update_image(_camera.Handle);
PsMoveApi.psmove_tracker_update(_camera.Handle, mc.Handle);
mc.TrackerStatus[_camera] = PsMoveApi.psmove_tracker_get_status(_camera.Handle, mc.Handle);
PsMoveApi.psmove_enable_orientation(mc.Handle, PSMoveBool.True);
PsMoveApi.psmove_reset_orientation(mc.Handle);
}
//Matrix4x4 proj = new Matrix4x4();
//for (int row = 0; row < 4; row++)
//{
// for (int col = 0; col < 4; col++)
// {
// proj[row, col] = PsMoveApi.PSMoveMatrix4x4_at(PsMoveApi.psmove_fusion_get_projection_matrix(_camera.Fusion), row * 4 + col);
// }
//}
//mc.ProjectionMatrix[_camera] = proj;
ConsoleService.Write(string.Format("[Tracker, {0}] Tracker Status of {1} = {2}",
_camera.GUID, mc.Name, Enum.GetName(typeof(PSMoveTrackerStatus), mc.TrackerStatus[_camera])));
}
protected override bool HandleMessage(object message, SimpleServerChildTcpSocket socket)
{
ChildSocketState socketState = ChildSocketState.Disconnecting;
Server.ChildSockets.TryGetValue(socket, out socketState);
KeyValuePair <SimpleServerChildTcpSocket, ChildSocketState> childSocket = new KeyValuePair <SimpleServerChildTcpSocket, ChildSocketState>(socket, socketState);
PositionRequest positionRequest = message as PositionRequest;
if (positionRequest != null)
{
string consoleString;
Float3 position = Float3.Zero;
if (positionRequest.Type != PositionType.Bundled)
{
consoleString = String.Format("{0} requests {1} position of controller {2} from camera {3}.",
socket.RemoteEndPoint,
Enum.GetName(typeof(PositionType), positionRequest.Type),
positionRequest.ControllerIndex,
positionRequest.CameraIndex);
}
else
{
consoleString = String.Format("{0} requests {1} position of controller {2}.",
socket.RemoteEndPoint,
Enum.GetName(typeof(PositionType), positionRequest.Type),
positionRequest.ControllerIndex);
}
switch (positionRequest.Type)
{
case PositionType.Bundled:
position = GetBundledPosition(positionRequest.ControllerIndex);
break;
case PositionType.Camera:
break;
case PositionType.Fusion:
position = GetFusionPosition(positionRequest.CameraIndex, positionRequest.ControllerIndex);
break;
case PositionType.Raw:
break;
case PositionType.World:
position = GetWorldPosition(positionRequest.CameraIndex, positionRequest.ControllerIndex);
break;
}
ConsoleService.Write(consoleString);
SendPositionMessage(childSocket, new PositionMessage()
{
Position = position,
Type = positionRequest.Type,
StartTick = positionRequest.StartTick,
CameraIndex = positionRequest.CameraIndex,
ControllerIndex = positionRequest.CameraIndex
});
return(true);
}
return(base.HandleMessage(message, socket));
}
