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])));
}
public void UpdateTracker()
{
if (_camera.Handle != IntPtr.Zero)
{
try
{
swGrab.Restart();
PsMoveApi.psmove_tracker_update_image(_camera.Handle);
swGrab.Stop();
foreach (MotionControllerModel mc in _camera.Controllers)
{
if (mc.Id == 0)
{
swController1.Restart();
}
if (mc.Id == 1)
{
swController2.Restart();
}
if (mc.Tracking[_camera])
{
PsMoveApi.psmove_tracker_update(_camera.Handle, mc.Handle);
ProcessData(mc);
}
if (mc.Id == 0)
{
swController1.Stop();
}
if (mc.Id == 1)
{
swController2.Stop();
}
}
}
catch (AccessViolationException e)
{
Console.WriteLine(e.StackTrace);
Stop();
}
}
}