private static void WorkerContextTeardownTracking(WorkerContext context)
{
// Disable tracking on all active controllers
for (int psmove_id = 0; psmove_id < context.PSMoves.Length; psmove_id++)
{
if (context.PSMoves[psmove_id] != IntPtr.Zero &&
context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled)
{
Debug.WriteLine("Disabling tracking on PSMove controller {0}", psmove_id);
context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = false;
}
}
// Delete the tracking fusion state
if (context.PSMoveFusion != IntPtr.Zero)
{
Debug.WriteLine("PSMove fusion disposed");
PSMoveAPI.psmove_fusion_free(context.PSMoveFusion);
context.PSMoveFusion = IntPtr.Zero;
}
// Delete the tracker state
if (context.PSMoveTracker != IntPtr.Zero)
{
Debug.WriteLine("PSMove tracker disposed");
PSMoveAPI.psmove_tracker_free(context.PSMoveTracker);
context.PSMoveTracker = IntPtr.Zero;
}
}
public void ThreadSetup()
{
// Maintains the following psmove state on the stack
// * psmove tracking state
// * psmove fusion state
// * psmove controller state
// Tracking state is only initialized when we have a non-zero number of tracking contexts
Context = new WorkerContext(WorkerControllerDataArray, WorkerSettings);
if (PSMoveAPI.psmove_init(PSMoveAPI.PSMove_Version.PSMOVE_CURRENT_VERSION) == PSMove_Bool.PSMove_False)
{
throw new Exception("PS Move API init failed (wrong version?)");
}
}
private static void WorkerContextTeardown(WorkerContext context)
{
// Delete the controllers
for (int psmove_id = 0; psmove_id < context.PSMoves.Length; psmove_id++)
{
if (context.PSMoves[psmove_id] != IntPtr.Zero)
{
Debug.WriteLine(string.Format("Disconnecting PSMove controller {0}", psmove_id));
context.WorkerControllerDataArray[psmove_id].IsConnected = false;
context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = false;
PSMoveAPI.psmove_disconnect(context.PSMoves[psmove_id]);
context.PSMoves[psmove_id] = IntPtr.Zero;
}
}
// Delete the tracker
WorkerContextTeardownTracking(context);
context.Reset();
}
private static bool WorkerContextUpdateControllerConnections(WorkerContext context)
{
bool controllerCountChanged = false;
if (context.moveCountCheckTimer.ElapsedMilliseconds >= WorkerContext.CONTROLLER_COUNT_POLL_INTERVAL)
{
// Update the number
int newcount = PSMoveAPI.psmove_count_connected();
if (context.PSMoveCount != newcount)
{
Debug.WriteLine("PSMove Controllers count changed: {0} -> {1}.", context.PSMoveCount, newcount);
context.PSMoveCount = newcount;
controllerCountChanged = true;
}
// Refresh the connection and tracking state of every controller entry
for (int psmove_id = 0; psmove_id < context.PSMoves.Length; psmove_id++)
{
if (psmove_id < context.PSMoveCount)
{
if (context.PSMoves[psmove_id] == IntPtr.Zero)
{
// The controller should be connected
context.PSMoves[psmove_id] = PSMoveAPI.psmove_connect_by_id(psmove_id);
if (context.PSMoves[psmove_id] != IntPtr.Zero)
{
PSMoveAPI.psmove_enable_orientation(context.PSMoves[psmove_id], PSMove_Bool.PSMove_True);
System.Diagnostics.Debug.Assert(PSMoveAPI.psmove_has_orientation(context.PSMoves[psmove_id]) ==
PSMove_Bool.PSMove_True);
context.WorkerControllerDataArray[psmove_id].IsConnected = true;
}
else
{
context.WorkerControllerDataArray[psmove_id].IsConnected = false;
Debug.WriteLine("Failed to connect to PSMove controller {0}", psmove_id);
}
}
if (context.PSMoves[psmove_id] != IntPtr.Zero &&
context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled == false &&
context.WorkerSettings.bTrackerEnabled &&
WorkerContextIsTrackingSetup(context))
{
// The controller is connected, but not tracking yet
// Enable tracking for this controller with next available color.
if (PSMoveAPI.psmove_tracker_enable(
context.PSMoveTracker,
context.PSMoves[psmove_id]) == PSMoveTracker_Status.Tracker_CALIBRATED)
{
context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = true;
}
else
{
Debug.WriteLine("Failed to enable tracking for PSMove controller {0}", psmove_id);
}
}
}
else
{
// The controller should no longer be tracked
if (context.PSMoves[psmove_id] != IntPtr.Zero)
{
PSMoveAPI.psmove_disconnect(context.PSMoves[psmove_id]);
context.PSMoves[psmove_id] = IntPtr.Zero;
context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = false;
context.WorkerControllerDataArray[psmove_id].IsConnected = false;
}
}
}
// Remember the last time we polled the move count
context.moveCountCheckTimer.Reset();
context.moveCountCheckTimer.Start();
}
return(controllerCountChanged);
}
private static bool WorkerContextIsTrackingSetup(WorkerContext context)
{
return(context.PSMoveTracker != IntPtr.Zero && context.PSMoveFusion != IntPtr.Zero);
}
private static bool WorkerContextSetupTracking(
PSMoveWorkerSettings WorkerSettings,
WorkerContext context)
{
bool success = true;
// Clear out the tracking state
// Reset the shared worker data
context.Reset();
Debug.WriteLine("Setting up PSMove Tracking Context");
// Initialize and configure the psmove_tracker.
{
PSMoveAPI.PSMoveTrackerSettings settings = new PSMoveAPI.PSMoveTrackerSettings();
PSMoveAPI.psmove_tracker_settings_set_default(ref settings);
settings.color_mapping_max_age = 0; // Don't used cached color mapping file
if (WorkerSettings.bUseManualExposure)
{
settings.exposure_mode = PSMoveTracker_Exposure.Exposure_MANUAL;
settings.camera_exposure =
(int)(Math.Max(Math.Min(WorkerSettings.ManualExposureValue, 1.0f), 0.0f) * 65535.0f);
}
else
{
settings.exposure_mode = PSMoveTracker_Exposure.Exposure_LOW;
}
settings.use_fitEllipse = 1;
settings.camera_mirror = PSMove_Bool.PSMove_True;
settings.color_list_start_ind = (int)WorkerSettings.InitialTrackingColor;
context.PSMoveTracker = PSMoveAPI.psmove_tracker_new_with_settings(ref settings);
}
if (context.PSMoveTracker != IntPtr.Zero)
{
Debug.WriteLine("PSMove tracker initialized.");
PSMoveAPI.PSMoveTrackerSmoothingSettings smoothing_settings = new PSMoveAPI.PSMoveTrackerSmoothingSettings();
PSMoveAPI.psmove_tracker_get_smoothing_settings(context.PSMoveTracker, ref smoothing_settings);
smoothing_settings.filter_do_2d_r = 0;
smoothing_settings.filter_do_2d_xy = 0;
smoothing_settings.filter_3d_type = WorkerSettings.Filter3DType;
PSMoveAPI.psmove_tracker_set_smoothing_settings(context.PSMoveTracker, ref smoothing_settings);
PSMoveAPI.psmove_tracker_get_size(context.PSMoveTracker, ref context.TrackerWidth, ref context.TrackerHeight);
Debug.WriteLine("Camera Dimensions: {0} x {1}", context.TrackerWidth, context.TrackerHeight);
}
else
{
PSMoveTracker_ErrorCode errorCode = PSMoveAPI.psmove_tracker_get_last_error();
Debug.WriteLine("PSMove tracker failed to initialize: {0}", errorCode.ToString());
success = false;
}
// Initialize fusion API if the tracker started
if (success)
{
context.PSMoveFusion = PSMoveAPI.psmove_fusion_new(context.PSMoveTracker, 1.0f, 1000.0f);
if (context.PSMoveFusion != IntPtr.Zero)
{
Debug.WriteLine("PSMove fusion initialized.");
}
else
{
Debug.WriteLine("PSMove failed to initialize.");
success = false;
}
}
if (!success)
{
WorkerContextTeardownTracking(context);
}
return(success);
}
public void ThreadTeardown()
{
WorkerContextTeardown(Context);
Context = null;
}
