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 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);
}
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();
}
}
}
}
