private static void WorkerContextTeardown(WorkerContext context)
{
UnityEngine.Debug.Log("Tearing down PSMove Tracking Context");
// Delete the controllers
for (int psmove_id = 0; psmove_id < context.PSMoves.Length; psmove_id++)
{
if (context.PSMoves[psmove_id] != IntPtr.Zero)
{
UnityEngine.Debug.Log(string.Format("Disconnecting PSMove controller {0}", psmove_id));
context.WorkerControllerDataArray[psmove_id].IsConnected = false;
context.WorkerControllerDataArray[psmove_id].IsEnabled = false;
PSMoveAPI.psmove_disconnect(context.PSMoves[psmove_id]);
context.PSMoves[psmove_id] = IntPtr.Zero;
}
}
// Delete the tracking fusion state
if (context.PSMoveFusion != IntPtr.Zero)
{
UnityEngine.Debug.Log("PSMove fusion disposed");
PSMoveAPI.psmove_fusion_free(context.PSMoveFusion);
context.PSMoveFusion = IntPtr.Zero;
}
// Delete the tracker state
if (context.PSMoveTracker != IntPtr.Zero)
{
UnityEngine.Debug.Log("PSMove tracker disposed");
PSMoveAPI.psmove_tracker_free(context.PSMoveTracker);
context.PSMoveTracker = IntPtr.Zero;
}
context.Reset();
}
private void Update()
{
if (initDone)
{
PSMoveAPI.psmove_update_leds(handle);
}
}
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;
}
}
private static void ControllerUpdatePositions(
PSMoveWorkerSettings WorkerSettings,
IntPtr psmove_tracker, // PSMoveTracker*
IntPtr psmove_fusion, // PSMoveFusion*
IntPtr psmove, // PSMove*
PSMoveRawControllerData_Base controllerData)
{
// Find the sphere position in the camera
PSMoveAPI.psmove_tracker_update(psmove_tracker, psmove);
PSMoveTracker_Status curr_status =
PSMoveAPI.psmove_tracker_get_status(psmove_tracker, psmove);
// Can we actually see the controller this frame?
controllerData.IsSeenByTracker = curr_status == PSMoveTracker_Status.Tracker_TRACKING;
// Update the position of the controller
if (controllerData.IsSeenByTracker)
{
float xcm = 0.0f, ycm = 0.0f, zcm = 0.0f;
PSMoveAPI.psmove_fusion_get_transformed_location(psmove_fusion, psmove, ref xcm, ref ycm, ref zcm);
// [Store the controller position]
// Remember the position the ps move controller in either its native space
// or in a transformed space if a transform file existed.
controllerData.PSMovePosition =
new Vector3(
xcm + WorkerSettings.PSMoveOffset.X,
ycm + WorkerSettings.PSMoveOffset.Y,
zcm + WorkerSettings.PSMoveOffset.Z);
}
}
private static void ControllerUpdateButtonState(
IntPtr psmove, // PSMove*
PSMoveRawControllerData_Base controllerData)
{
// Get the controller button state
controllerData.Buttons = PSMoveAPI.psmove_get_buttons(psmove); // Bitwise; tells if each button is down.
// Get the controller trigger value (uint8; 0-255)
controllerData.TriggerValue = (byte)PSMoveAPI.psmove_get_trigger(psmove);
}
private static void ControllerUpdateOrientations(
IntPtr psmove, // PSMove*
PSMoveRawControllerData_Base controllerData)
{
float oriw = 1.0f, orix = 0.0f, oriy = 0.0f, oriz = 0.0f;
// Get the controller orientation (uses IMU).
PSMoveAPI.psmove_get_orientation(psmove, ref oriw, ref orix, ref oriy, ref oriz);
//NOTE: This orientation is in the PSMoveApi coordinate system
controllerData.PSMoveOrientation = new Quaternion(orix, oriy, oriz, oriw);
}
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 ControllerUpdateSensors(
IntPtr psmove, // PSMove*
PSMoveRawControllerData_Base controllerData)
{
float acc_x = 0.0f, acc_y = 0.0f, acc_z = 0.0f;
float gyro_x = 0.0f, gyro_y = 0.0f, gyro_z = 0.0f;
float mag_x = 0.0f, mag_y = 0.0f, mag_z = 0.0f;
PSMoveAPI.psmove_get_accelerometer_frame(psmove, PSMove_Frame.Frame_SecondHalf, ref acc_x, ref acc_y, ref acc_z);
PSMoveAPI.psmove_get_gyroscope_frame(psmove, PSMove_Frame.Frame_SecondHalf, ref gyro_x, ref gyro_y, ref gyro_z);
PSMoveAPI.psmove_get_magnetometer_vector(psmove, ref mag_x, ref mag_y, ref mag_z);
controllerData.Accelerometer = new Vector3(acc_x, acc_y, acc_z);
controllerData.Gyroscope = new Vector3(gyro_x, gyro_y, gyro_z);
controllerData.Magnetometer = new Vector3(mag_x, mag_y, mag_z);
}
private static void ControllerUpdatePositions(
PSMoveWorkerSettings WorkerSettings,
IntPtr[] psmove_trackers, // PSMoveTracker*
IntPtr[] psmove_fusions, // PSMoveFusion*
int tracker_count,
IntPtr position_filter,
IntPtr psmove, // PSMove*
PSMoveRawControllerData_Base controllerData)
{
// Update the tracked position of the psmove for each tracker
for (int tracker_index = 0; tracker_index < tracker_count; ++tracker_index)
{
PSMoveAPI.psmove_tracker_update(psmove_trackers[tracker_index], psmove);
}
// Compute the triangulated camera position
PSMoveAPI.PSMove_3AxisVector measured_position = new PSMoveAPI.PSMove_3AxisVector();
controllerData.IsSeenByTracker =
PSMoveAPI.psmove_fusion_get_multicam_tracking_space_location(
psmove_fusions, tracker_count, psmove,
ref measured_position.x, ref measured_position.y, ref measured_position.z) == PSMove_Bool.PSMove_True;
// Update the position of the controller
if (controllerData.IsSeenByTracker)
{
// Update the filtered position
PSMoveAPI.psmove_position_filter_update(
ref measured_position,
controllerData.IsSeenByTracker ? PSMove_Bool.PSMove_True : PSMove_Bool.PSMove_False,
position_filter);
// Get the filtered position
PSMoveAPI.PSMove_3AxisVector filtered_position =
PSMoveAPI.psmove_position_filter_get_position(position_filter);
// [Store the controller position]
// Remember the position the ps move controller in either its native space
// or in a transformed space if a transform file existed.
controllerData.PSMovePosition =
new Vector3(
filtered_position.x + WorkerSettings.PSMoveOffset.x,
filtered_position.y + WorkerSettings.PSMoveOffset.y,
filtered_position.z + WorkerSettings.PSMoveOffset.z);
}
}
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 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)
{
UnityEngine.Debug.Log(string.Format("Disabling tracking on PSMove controller {0}", psmove_id));
context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = false;
}
}
for (int tracker_index = 0; tracker_index < WorkerContext.MAX_TRACKER_COUNT; ++tracker_index)
{
// Delete the tracking fusion state
if (context.PSMoveFusions[tracker_index] != IntPtr.Zero)
{
UnityEngine.Debug.Log("PSMove fusion disposed");
PSMoveAPI.psmove_fusion_free(context.PSMoveFusions[tracker_index]);
context.PSMoveFusions[tracker_index] = IntPtr.Zero;
}
// Delete the tracker state
if (context.PSMoveTrackers[tracker_index] != IntPtr.Zero)
{
UnityEngine.Debug.Log("PSMove tracker disposed");
PSMoveAPI.psmove_tracker_free(context.PSMoveTrackers[tracker_index]);
context.PSMoveTrackers[tracker_index] = IntPtr.Zero;
}
}
context.TrackerCount = 0;
// Delete the position filter
if (context.PSMovePositionFilter != IntPtr.Zero)
{
PSMoveAPI.psmove_position_filter_free(context.PSMovePositionFilter);
context.PSMovePositionFilter = IntPtr.Zero;
}
}
private void Start()
{
PSMove_Bool init = PSMoveAPI.psmove_init(PSMoveAPI.PSMove_Version.PSMOVE_CURRENT_VERSION);
if (init == PSMove_Bool.PSMove_True)
{
handle = PSMoveAPI.psmove_connect();
if (handle == System.IntPtr.Zero || PSMoveAPI.psmove_update_leds(handle) == 0)
{
Debug.LogError("Could not connect to default PSMove controller");
}
else
{
Debug.Log("Connection established to default PSMove controller");
initDone = true;
SetLED(Color.magenta);
}
}
else
{
Debug.LogError("Could not init PSMove API");
}
}
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)
{
UnityEngine.Debug.Log(string.Format("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;
UnityEngine.Debug.LogError(string.Format("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))
{
int happyTrackerCount = 0;
// Attempt to enable any trackers that haven't successfully calibrated the controller yet
for (int tracker_index = 0; tracker_index < context.TrackerCount; ++tracker_index)
{
PSMoveTracker_Status tracker_status =
PSMoveAPI.psmove_tracker_get_status(
context.PSMoveTrackers[tracker_index],
context.PSMoves[psmove_id]);
if (tracker_status == PSMoveTracker_Status.Tracker_CALIBRATED ||
tracker_status == PSMoveTracker_Status.Tracker_TRACKING)
{
++happyTrackerCount;
}
else
{
// The controller is connected, but not tracking yet
// Enable tracking for this controller with next available color.
if (PSMoveAPI.psmove_tracker_enable(
context.PSMoveTrackers[tracker_index],
context.PSMoves[psmove_id]) == PSMoveTracker_Status.Tracker_CALIBRATED)
{
++happyTrackerCount;
}
else
{
UnityEngine.Debug.LogError(string.Format("Failed to enable tracking for PSMove controller {0} on tracker {1}", psmove_id, tracker_index));
}
}
}
if (happyTrackerCount >= context.TrackerCount)
{
context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = true;
}
}
}
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 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 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 ThreadUpdate()
{
using (new PSMoveHitchWatchdog("PSMoveWorker_ThreadUpdate", 34 * PSMoveHitchWatchdog.MICROSECONDS_PER_MILLISECOND)) {
// Setup or teardown tracking based on the updated tracking state
if (WorkerSettings.bTrackerEnabled && !WorkerContextIsTrackingSetup(Context))
{
WorkerContextSetupTracking(WorkerSettings, Context);
}
else if (!WorkerSettings.bTrackerEnabled && WorkerContextIsTrackingSetup(Context))
{
WorkerContextTeardownTracking(Context);
}
// Setup or tear down controller connections based on the number of active controllers
WorkerContextUpdateControllerConnections(Context);
// Renew the image on camera, if tracking is enabled
if (WorkerContextIsTrackingSetup(Context))
{
using (
new PSMoveHitchWatchdog("PSMoveWorker_UpdateImage", 33 * PSMoveHitchWatchdog.MICROSECONDS_PER_MILLISECOND)) {
PSMoveAPI.psmove_tracker_update_image(Context.PSMoveTracker); // Sometimes libusb crashes here.
}
}
// Update the raw positions on the local controller data
if (WorkerContextIsTrackingSetup(Context))
{
for (int psmove_id = 0; psmove_id < Context.PSMoveCount; psmove_id++)
{
PSMoveRawControllerData_TLS localControllerData = WorkerControllerDataArray[psmove_id];
if (WorkerSettings.bTrackerEnabled)
{
ControllerUpdatePositions(
WorkerSettings,
Context.PSMoveTracker,
Context.PSMoveFusion,
Context.PSMoves[psmove_id],
localControllerData);
}
else
{
localControllerData.IsSeenByTracker = false;
}
}
}
// Do bluetooth IO: Orientation, Buttons, Rumble
for (int psmove_id = 0; psmove_id < Context.PSMoveCount; psmove_id++)
{
//TODO: Is it necessary to keep polling until no frames are left?
while (PSMoveAPI.psmove_poll(Context.PSMoves[psmove_id]) > 0)
{
PSMoveRawControllerData_TLS localControllerData = WorkerControllerDataArray[psmove_id];
// Update the controller status (via bluetooth)
PSMoveAPI.psmove_poll(Context.PSMoves[psmove_id]); // Necessary to poll yet again?
// Store the controller orientation
ControllerUpdateOrientations(Context.PSMoves[psmove_id], localControllerData);
// Store the button state
ControllerUpdateButtonState(Context.PSMoves[psmove_id], localControllerData);
// Now read in requested changes from Component. e.g., RumbleRequest, CycleColourRequest
localControllerData.WorkerRead();
// Set the controller rumble (uint8; 0-255)
PSMoveAPI.psmove_set_rumble(Context.PSMoves[psmove_id], localControllerData.RumbleRequest);
// Push the updated rumble state to the controller
PSMoveAPI.psmove_update_leds(Context.PSMoves[psmove_id]);
if (localControllerData.CycleColourRequest)
{
if (WorkerSettings.bTrackerEnabled)
{
Debug.WriteLine("PSMoveWorker:: CYCLE COLOUR");
PSMoveAPI.psmove_tracker_cycle_color(Context.PSMoveTracker, Context.PSMoves[psmove_id]);
}
else
{
Debug.WriteLine("PSMoveWorker:: CYCLE COLOUR ignored! Tracking is disabled!");
}
localControllerData.CycleColourRequest = false;
}
// Publish Position, Orientation, and Button state to the concurrent data
// This also publishes updated CycleColourRequest.
localControllerData.WorkerPost();
}
}
}
}
private static bool WorkerContextSetupTracking(
PSMoveWorkerSettings WorkerSettings,
WorkerContext context)
{
bool success = true;
// Clear out the tracking state
// Reset the shared worker data
context.Reset();
UnityEngine.Debug.Log("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.filter_do_2d_r = 0;
settings.filter_do_2d_xy = 0;
settings.camera_mirror = PSMove_Bool.PSMove_True;
settings.color_list_start_ind = (int)WorkerSettings.InitialTrackingColor;
context.TrackerCount = 0;
for (int tracker_index = 0; tracker_index < WorkerContext.MAX_TRACKER_COUNT; ++tracker_index)
{
context.PSMoveTrackers[tracker_index] =
PSMoveAPI.psmove_tracker_new_with_camera_and_settings(tracker_index, ref settings);
if (context.PSMoveTrackers[tracker_index] != IntPtr.Zero)
{
UnityEngine.Debug.Log(string.Format("PSMove tracker({0}) initialized.", tracker_index));
++context.TrackerCount;
PSMoveAPI.psmove_tracker_get_size(
context.PSMoveTrackers[tracker_index],
ref context.TrackerWidth, ref context.TrackerHeight);
UnityEngine.Debug.Log(string.Format("Camera Dimensions: {0} x {1}", context.TrackerWidth, context.TrackerHeight));
}
else
{
PSMoveTracker_ErrorCode errorCode = PSMoveAPI.psmove_tracker_get_last_error();
UnityEngine.Debug.Log(string.Format("PSMove tracker({0}) not available: {1}",
tracker_index, errorCode.ToString()));
break;
}
}
if (context.TrackerCount <= 0)
{
UnityEngine.Debug.LogError(string.Format("Failed to open any trackers"));
success = false;
}
}
// Initialize fusion API if the tracker started
if (success)
{
for (int tracker_index = 0; tracker_index < context.TrackerCount; ++tracker_index)
{
context.PSMoveFusions[tracker_index] =
PSMoveAPI.psmove_fusion_new(context.PSMoveTrackers[tracker_index], 1.0f, 1000.0f);
if (context.PSMoveFusions[tracker_index] != IntPtr.Zero)
{
UnityEngine.Debug.Log(string.Format("PSMove fusion({0}) initialized.", tracker_index));
}
else
{
UnityEngine.Debug.LogError(string.Format("PSMove fusion({0}) failed to initialize.", tracker_index));
success = false;
break;
}
}
}
// Initialize a position filter to smooth out the tracking data
if (success)
{
context.PSMovePositionFilter = PSMoveAPI.psmove_position_filter_new();
if (context.PSMovePositionFilter != IntPtr.Zero)
{
UnityEngine.Debug.Log("PSMove position filter initialized.");
PSMoveAPI.PSMove_3AxisVector initial_position = new PSMoveAPI.PSMove_3AxisVector()
{
x = 0.0f,
y = 0.0f,
z = 0.0f,
};
PSMoveAPI.PSMovePositionFilterSettings filter_settings = new PSMoveAPI.PSMovePositionFilterSettings();
PSMoveAPI.psmove_position_filter_get_default_settings(ref filter_settings);
filter_settings.filter_type = WorkerSettings.FilterType;
PSMoveAPI.psmove_position_filter_init(ref filter_settings, ref initial_position, context.PSMovePositionFilter);
}
else
{
UnityEngine.Debug.LogError(string.Format("Failed to allocate PSMove Position Filter"));
success = false;
}
}
if (!success)
{
WorkerContextTeardownTracking(context);
}
return(success);
}
private void OnApplicationQuit()
{
PSMoveAPI.psmove_disconnect(handle);
}
public void SetLED(Color color)
{
PSMoveAPI.psmove_set_leds(handle, (char)(color.r * 255), (char)(color.g * 255), (char)(color.b * 255));
}
public void ThreadUpdate()
{
using (new PSMoveHitchWatchdog("FPSMoveWorker_ThreadUpdate", 34 * PSMoveHitchWatchdog.MICROSECONDS_PER_MILLISECOND))
{
int AcquiredContextCounter_TLS;
lock (this)
{
AcquiredContextCounter_TLS = this.AcquiredContextCounter;
}
// If there are component contexts active, make sure the tracking context is setup
if (!WorkerSettings.DisableTracking)
{
if (AcquiredContextCounter_TLS > 0 && !WorkerContextIsTrackingSetup(Context))
{
WorkerContextSetupTracking(WorkerSettings, Context);
}
// If there are no component contexts active, make sure the tracking context is torn-down
else if (AcquiredContextCounter_TLS <= 0 && WorkerContextIsTrackingSetup(Context))
{
WorkerContextTeardown(Context);
}
}
// Update controller state while tracking is active (or if we don't care about tracking)
if (WorkerSettings.DisableTracking || WorkerContextIsTrackingSetup(Context))
{
// Setup or tear down controller connections based on the number of active controllers
WorkerContextUpdateControllerConnections(Context);
// Renew the image on camera
using (new PSMoveHitchWatchdog("FPSMoveWorker_UpdateImage", 33 * PSMoveHitchWatchdog.MICROSECONDS_PER_MILLISECOND))
{
PSMoveAPI.psmove_tracker_update_image(Context.PSMoveTracker); // Sometimes libusb crashes here.
}
// Update the raw positions on the local controller data
for (int psmove_id = 0; psmove_id < Context.PSMoveCount; psmove_id++)
{
PSMoveRawControllerData_TLS localControllerData = WorkerControllerDataArray[psmove_id];
if (!Context.WorkerSettings.DisableTracking)
{
ControllerUpdatePositions(
WorkerSettings,
Context.PSMoveTracker,
Context.PSMoveFusion,
Context.PSMoves[psmove_id],
localControllerData);
}
else
{
localControllerData.IsTracking = false;
}
}
// Do bluetooth IO: Orientation, Buttons, Rumble
for (int psmove_id = 0; psmove_id < Context.PSMoveCount; psmove_id++)
{
//TODO: Is it necessary to keep polling until no frames are left?
while (PSMoveAPI.psmove_poll(Context.PSMoves[psmove_id]) > 0)
{
PSMoveRawControllerData_TLS localControllerData = WorkerControllerDataArray[psmove_id];
// Update the controller status (via bluetooth)
PSMoveAPI.psmove_poll(Context.PSMoves[psmove_id]); // Necessary to poll yet again?
// Store the controller orientation
ControllerUpdateOrientations(Context.PSMoves[psmove_id], localControllerData);
// Store the button state
ControllerUpdateButtonState(Context.PSMoves[psmove_id], localControllerData);
// Now read in requested changes from Component. e.g., RumbleRequest, CycleColourRequest
localControllerData.WorkerRead();
// Set the controller rumble (uint8; 0-255)
PSMoveAPI.psmove_set_rumble(Context.PSMoves[psmove_id], localControllerData.RumbleRequest);
// Push the updated rumble state to the controller
PSMoveAPI.psmove_update_leds(Context.PSMoves[psmove_id]);
if (localControllerData.CycleColourRequest)
{
if (!Context.WorkerSettings.DisableTracking)
{
UnityEngine.Debug.Log("PSMoveWorker:: CYCLE COLOUR");
PSMoveAPI.psmove_tracker_cycle_color(Context.PSMoveTracker, Context.PSMoves[psmove_id]);
}
else
{
UnityEngine.Debug.LogWarning("PSMoveWorker:: CYCLE COLOUR ignored! Tracking is disabled!");
}
localControllerData.CycleColourRequest = false;
}
// Publish the worker data to the component. e.g., Position, Orientation, Buttons
// This also publishes updated CycleColourRequest.
localControllerData.WorkerPost();
}
}
}
}
}
public void ThreadUpdate()
{
using (new PSMoveHitchWatchdog("PSMoveWorker_ThreadUpdate", 34 * PSMoveHitchWatchdog.MICROSECONDS_PER_MILLISECOND))
{
// Setup or teardown tracking based on the updated tracking state
if (WorkerSettings.bTrackerEnabled && !WorkerContextIsTrackingSetup(Context))
{
WorkerContextSetupTracking(WorkerSettings, Context);
}
else if (!WorkerSettings.bTrackerEnabled && WorkerContextIsTrackingSetup(Context))
{
WorkerContextTeardownTracking(Context);
}
// Setup or tear down controller connections based on the number of active controllers
WorkerContextUpdateControllerConnections(Context);
// Renew the image on camera, if tracking is enabled
if (WorkerContextIsTrackingSetup(Context))
{
using (new PSMoveHitchWatchdog("PSMoveWorker_UpdateImage", 33 * PSMoveHitchWatchdog.MICROSECONDS_PER_MILLISECOND))
{
for (int tracker_index = 0; tracker_index < Context.TrackerCount; ++tracker_index)
{
PSMoveAPI.psmove_tracker_update_image(Context.PSMoveTrackers[tracker_index]); // Sometimes libusb crashes here.
}
}
}
// Update the raw positions on the local controller data
if (WorkerContextIsTrackingSetup(Context))
{
for (int psmove_id = 0; psmove_id < Context.PSMoveCount; psmove_id++)
{
PSMoveRawControllerData_TLS localControllerData = WorkerControllerDataArray[psmove_id];
if (WorkerSettings.bTrackerEnabled)
{
ControllerUpdatePositions(
WorkerSettings,
Context.PSMoveTrackers,
Context.PSMoveFusions,
Context.TrackerCount,
Context.PSMovePositionFilter,
Context.PSMoves[psmove_id],
localControllerData);
}
else
{
localControllerData.IsSeenByTracker = false;
}
}
}
// Do bluetooth IO: Orientation, Buttons, Rumble
for (int psmove_id = 0; psmove_id < Context.PSMoveCount; psmove_id++)
{
//TODO: Is it necessary to keep polling until no frames are left?
while (PSMoveAPI.psmove_poll(Context.PSMoves[psmove_id]) > 0)
{
PSMoveRawControllerData_TLS localControllerData = WorkerControllerDataArray[psmove_id];
// Update the controller status (via bluetooth)
PSMoveAPI.psmove_poll(Context.PSMoves[psmove_id]); // Necessary to poll yet again?
// Store the controller sensor data
ControllerUpdateSensors(Context.PSMoves[psmove_id], localControllerData);
// Store the controller orientation
ControllerUpdateOrientations(Context.PSMoves[psmove_id], localControllerData);
// Store the button state
ControllerUpdateButtonState(Context.PSMoves[psmove_id], localControllerData);
// Now read in requested changes from Component. e.g., RumbleRequest, CycleColourRequest
localControllerData.WorkerRead();
// Set the controller rumble (uint8; 0-255)
PSMoveAPI.psmove_set_rumble(Context.PSMoves[psmove_id], localControllerData.RumbleRequest);
// Push the updated rumble state to the controller
PSMoveAPI.psmove_update_leds(Context.PSMoves[psmove_id]);
if (localControllerData.CycleColourRequest)
{
if (WorkerSettings.bTrackerEnabled)
{
UnityEngine.Debug.Log("PSMoveWorker:: CYCLE COLOUR");
// Attempt to cycle the color for each tracker and re-acquire tracking
int happyTrackerCount = 0;
for (int tracker_index = 0; tracker_index < Context.TrackerCount; ++tracker_index)
{
PSMoveAPI.psmove_tracker_cycle_color(
Context.PSMoveTrackers[tracker_index], Context.PSMoves[psmove_id]);
PSMoveTracker_Status tracker_status =
PSMoveAPI.psmove_tracker_get_status(
Context.PSMoveTrackers[tracker_index],
Context.PSMoves[psmove_id]);
if (tracker_status == PSMoveTracker_Status.Tracker_CALIBRATED ||
tracker_status == PSMoveTracker_Status.Tracker_TRACKING)
{
++happyTrackerCount;
}
}
// If not all trackers re-acquired,
// mark the controller as no having tracking enabled,
// and let WorkerContextUpdateControllerConnections() try
// and reacquire next update.
if (happyTrackerCount < Context.TrackerCount)
{
Context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = false;
}
}
else
{
UnityEngine.Debug.LogWarning("PSMoveWorker:: CYCLE COLOUR ignored! Tracking is disabled!");
}
localControllerData.CycleColourRequest = false;
}
// Publish Position, Orientation, and Button state to the concurrent data
// This also publishes updated CycleColourRequest.
localControllerData.WorkerPost();
}
}
}
}