void Tracker::update() {
for (int i = 0; i < m_count; i++) {
while (psmove_poll(m_moves[i]))
;
float x, y, z;
psmove_fusion_get_position(m_fusion, m_moves[i], &x, &y, &z);
int buttons = psmove_get_buttons(m_moves[i]);
if (buttons & Btn_MOVE) {
psmove_reset_orientation(m_moves[i]);
}
else if (buttons & Btn_PS) {
exit(0);
}
else if (buttons & Btn_SELECT) {
m_rotation += 2.;
}
else if (buttons & Btn_CROSS) {
m_trace.push_back(Point3D(x, y, z));
}
if (buttons & Btn_START) {
if (m_has_last_offset) {
m_offset = Point3D(m_offset.x + x - m_last_offset.x,
m_offset.y + y - m_last_offset.y,
m_offset.z + z - m_last_offset.z);
}
else {
m_has_last_offset = true;
}
m_last_offset = Point3D(x, y, z);
}
else {
m_has_last_offset = false;
}
unsigned int pressed, released;
psmove_get_button_events(m_moves[i], &pressed, &released);
if (pressed & Btn_SQUARE) {
m_items[i] -= 1;
if (m_items[i] < 0) m_items[i] = ITEM_MAX - 1;
}
else if (pressed & Btn_TRIANGLE) {
m_items[i] += 1;
if (m_items[i] == ITEM_MAX) m_items[i] = 0;
}
else if (pressed & Btn_CIRCLE) {
m_trace.clear();
m_rotation = 0.;
m_offset = Point3D(0., 0., 0.);
}
}
psmove_tracker_update_image(m_tracker);
psmove_tracker_update(m_tracker, NULL);
}
uint32 FPSMoveWorker::Run()
{
if (!psmove_init(PSMOVE_CURRENT_VERSION))
{
UE_LOG(LogPSMove, Error, TEXT("PS Move API init failed (wrong version?)"));
return -1;
}
// I want the psmoves and psmove_tracker to be local variables in the thread.
// Initialize an empty array of psmove controllers
PSMove* psmoves[FPSMoveWorker::k_max_controllers];
memset(psmoves, 0, sizeof(psmoves));
// Initialize and configure the psmove_tracker
PSMoveTracker *psmove_tracker = psmove_tracker_new(); // Unfortunately the API does not have a way to change the resolution and framerate.
PSMoveFusion *psmove_fusion = psmove_fusion_new(psmove_tracker, 1., 1000.);
int tracker_width = 640;
int tracker_height = 480;
if (psmove_tracker)
{
UE_LOG(LogPSMove, Log, TEXT("PSMove tracker initialized."));
//Set exposure. TODO: Expose this to component.
psmove_tracker_set_exposure(psmove_tracker, Exposure_LOW); //Exposure_LOW, Exposure_MEDIUM, Exposure_HIGH
psmove_tracker_set_smoothing(psmove_tracker, 0, 1);
psmove_tracker_set_mirror(psmove_tracker, PSMove_True);
psmove_tracker_get_size(psmove_tracker, &tracker_width, &tracker_height);
UE_LOG(LogPSMove, Log, TEXT("Camera Dimensions: %d x %d"), tracker_width, tracker_height);
}
else {
UE_LOG(LogPSMove, Log, TEXT("PSMove tracker failed to initialize."));
}
//Initial wait before starting.
FPlatformProcess::Sleep(0.03);
float xcm, ycm, zcm, oriw, orix, oriy, oriz;
while (StopTaskCounter.GetValue() == 0)
{
// Get positional data from tracker
if (psmove_tracker)
{
// Setup or tear down controller connections based on the number of active controllers
UpdateControllerConnections(psmove_tracker, psmoves);
// Renew the image on camera
if (PSMoveCount > 0)
{
psmove_tracker_update_image(psmove_tracker); // Sometimes libusb crashes here.
psmove_tracker_update_cbb(psmove_tracker, NULL); // Passing null (instead of m_moves[i]) updates all controllers.
}
}
else {
FPlatformProcess::Sleep(0.001);
}
for (int i = 0; i < PSMoveCount; i++)
{
FPSMoveRawControllerData_TLS &localControllerData = WorkerControllerDataArray[i];
//--------------
// Read the published data from the component
//--------------
localControllerData.WorkerRead();
// Get positional data from tracker
if (psmove_tracker)
{
localControllerData.IsTracked = psmove_tracker_get_status(psmove_tracker, psmoves[i]) == Tracker_TRACKING;
psmove_fusion_get_transformed_location(psmove_fusion, psmoves[i], &xcm, &ycm, &zcm);
if (localControllerData.IsTracked &&
xcm && ycm && zcm &&
!isnan(xcm) && !isnan(ycm) && !isnan(zcm) &&
xcm == xcm && ycm == ycm && zcm == zcm)
{
localControllerData.PosX = xcm;
localControllerData.PosY = ycm;
localControllerData.PosZ = zcm;
}
else {
localControllerData.IsTracked = false;
}
//UE_LOG(LogPSMove, Log, TEXT("X: %f, Y: %f, Z: %f"), xcm, ycm, zcm);
if (localControllerData.ResetPoseRequest && localControllerData.IsTracked)
{
psmove_tracker_reset_location(psmove_tracker, psmoves[i]);
}
// If we are to change the tracked colour.
if (localControllerData.UpdateLedRequest)
{
// Stop tracking the controller with the existing color
psmove_tracker_disable(psmove_tracker, psmoves[i]);
localControllerData.IsTracked = false;
localControllerData.IsCalibrated = false;
psmove_tracker_set_dimming(psmove_tracker, 0.0); // Set dimming to 0 to trigger blinking calibration.
psmove_set_leds(psmoves[i], 0, 0, 0); // Turn off the LED to make sure it isn't trackable until new colour set.
psmove_update_leds(psmoves[i]);
FColor newFColor = localControllerData.LedColourRequest.Quantize();
if (psmove_tracker_enable_with_color(psmove_tracker, psmoves[i], newFColor.R, newFColor.G, newFColor.B) == Tracker_CALIBRATED)
{
this->WorkerControllerDataArray[i].IsCalibrated = true;
}
else
{
UE_LOG(LogPSMove, Error, TEXT("Failed to change tracking color for PSMove controller %d"), i);
}
localControllerData.LedColourWasUpdated = true;
}
else
{
localControllerData.LedColourWasUpdated = false;
}
}
else {
FPlatformProcess::Sleep(0.001);
}
// Do bluetooth IO: Orientation, Buttons, Rumble
//TODO: Is it necessary to keep polling until no frames are left?
while (psmove_poll(psmoves[i]) > 0)
{
// Update the controller status (via bluetooth)
psmove_poll(psmoves[i]);
// Get the controller orientation (uses IMU).
psmove_get_orientation(psmoves[i],
&oriw, &orix, &oriy, &oriz);
localControllerData.OriW = oriw;
localControllerData.OriX = orix;
localControllerData.OriY = oriy;
localControllerData.OriZ = oriz;
//UE_LOG(LogPSMove, Log, TEXT("Ori w,x,y,z: %f, %f, %f, %f"), oriw, orix, oriy, oriz);
// Get the controller button state
localControllerData.Buttons = psmove_get_buttons(psmoves[i]); // Bitwise; tells if each button is down.
psmove_get_button_events(psmoves[i], &localControllerData.Pressed, &localControllerData.Released); // i.e., state change
// Get the controller trigger value (uint8; 0-255)
localControllerData.TriggerValue = psmove_get_trigger(psmoves[i]);
// Set the controller rumble (uint8; 0-255)
psmove_set_rumble(psmoves[i], localControllerData.RumbleRequest);
}
if (localControllerData.ResetPoseRequest)
{
psmove_reset_orientation(psmoves[i]);
//TODO: reset yaw only
localControllerData.PoseWasReset = true;
}
else {
localControllerData.PoseWasReset = false;
}
//--------------
// Publish the updated worker data to the component
//--------------
localControllerData.WorkerPost();
}
//Sleeping the thread seems to crash libusb.
//FPlatformProcess::Sleep(0.005);
}
// Delete the controllers
for (int i = 0; i<PSMoveCount; i++)
{
psmove_disconnect(psmoves[i]);
}
// Delete the fusion
if (psmove_fusion)
{
psmove_fusion_free(psmove_fusion);
}
// Delete the tracker
if (psmove_tracker)
{
psmove_tracker_free(psmove_tracker);
}
psmove_shutdown();
return 0;
}
