void psmove_fusion_get_position(PSMoveFusion *fusion, PSMove *move, float *x, float *y, float *z) { psmove_return_if_fail(fusion != NULL); psmove_return_if_fail(move != NULL); float camX, camY, camR; psmove_tracker_get_position(fusion->tracker, move, &camX, &camY, &camR); float winX = (float)camX; float winY = fusion->height - (float)camY; float winZ = .5; /* start value for binary search */ float targetWidth = 2.*camR; glm::vec3 obj; /* Binary search for the best distance based on the current projection */ float step = .25; while (step > PSMOVE_FUSION_STEP_EPSILON) { /* Calculate center position of sphere */ obj = glm::unProject(glm::vec3(winX, winY, winZ), glm::mat4(), fusion->projection, fusion->viewport); /* Project left edge center of sphere */ glm::vec3 left = glm::project(glm::vec3(obj.x - .5, obj.y, obj.z), glm::mat4(), fusion->projection, fusion->viewport); /* Project right edge center of sphere */ glm::vec3 right = glm::project(glm::vec3(obj.x + .5, obj.y, obj.z), glm::mat4(), fusion->projection, fusion->viewport); float width = (right.x - left.x); if (width > targetWidth) { /* Too near */ winZ += step; } else if (width < targetWidth) { /* Too far away */ winZ -= step; } else { /* Perfect fit */ break; } step *= .5; } if (x != NULL) { *x = obj.x; } if (y != NULL) { *y = obj.y; } if (z != NULL) { *z = obj.z; } }
void MainWindow::timeout() { unsigned char r, g, b; psmove_tracker_get_color(m_tracker, m_move, &r, &g, &b); psmove_set_leds(m_move, r, g, b); psmove_update_leds(m_move); psmove_tracker_update_image(m_tracker); psmove_tracker_update(m_tracker, m_move); float x, y; if (psmove_tracker_get_position(m_tracker, m_move, &x, &y, NULL)) { QPointF currentPos(x, y); unsigned int buttons = 0; while (psmove_poll(m_move)) { unsigned int pressed; psmove_get_button_events(m_move, &pressed, NULL); if (pressed & Btn_MOVE) { m_points[m_pointsOffset] = currentPos; m_pointsOffset = (m_pointsOffset + 1) % 4; m_mapping.set(m_points); } if (pressed & Btn_T) { m_path.moveTo(m_mapping.map(currentPos)); } if (pressed & Btn_CIRCLE) { m_path = QPainterPath(); } if (pressed & Btn_PS) { QApplication::quit(); } buttons = psmove_get_buttons(m_move); } if (buttons & Btn_T) { m_path.lineTo(m_mapping.map(currentPos)); } m_mousePos = QPointF(x, y); update(); } }
void MoveHandler::updateTracker() { float xPos = 0.0f; float yPos = 0.0f; float ledRadius = 0.0f; psmove_tracker_update_image(this->tracker); psmove_tracker_update(this->tracker, NULL); #if DEBUG psmove_tracker_annotate(this->tracker); frame = psmove_tracker_get_frame(this->tracker); if (frame) { cvShowImage("Live camera feed", frame); } #endif psmove_tracker_get_position(this->tracker, this->USBController, &xPos, &yPos, &ledRadius); this->x = xPos; this->y = yPos; this->radius = ledRadius; }
int main(int arg, char** args) { measurement measurements[MEASUREMENTS]; float distance = MEASUREMENTS_CM_START; int pos = 0; PSMove *move = psmove_connect(); if (move == NULL) { printf("Could not connect to the default controller.\n"); return 1; } PSMoveTracker* tracker = psmove_tracker_new(); if (tracker == NULL) { printf("Could not create tracker.\n"); return 2; } printf("Calibrating controller...\n"); while (psmove_tracker_enable(tracker, move) != Tracker_CALIBRATED); while (cvWaitKey(1) != 27 && pos < MEASUREMENTS) { psmove_tracker_update_image(tracker); psmove_tracker_update(tracker, NULL); printf("Distance: %.2f cm\n", distance); void *frame = psmove_tracker_get_image(tracker); cvShowImage("Camera", frame); unsigned char r, g, b; psmove_tracker_get_color(tracker, move, &r, &g, &b); psmove_set_leds(move, r, g, b); psmove_update_leds(move); float x, y, radius; psmove_tracker_get_position(tracker, move, &x, &y, &radius); unsigned int pressed, released; while (psmove_poll(move)); psmove_get_button_events(move, &pressed, &released); if (pressed & Btn_CROSS) { // Save current measurement save(frame, (int)distance); measurements[pos].distance_cm = distance; measurements[pos].radius_px = radius; distance += MEASUREMENTS_CM_STEP; pos++; } else if (pressed & Btn_CIRCLE && pos > 0) { // Go back and retry previous measurement distance -= MEASUREMENTS_CM_STEP; pos--; } } int i; FILE *fp = fopen("distance.csv", "w"); fprintf(fp, "distance,radius\n"); for (i=0; i<pos; i++) { fprintf(fp, "%.5f,%.5f\n", measurements[i].distance_cm, measurements[i].radius_px); } fclose(fp); psmove_tracker_free(tracker); psmove_disconnect(move); return 0; }
int main(int arg, char** args) { int i; int count = psmove_count_connected(); PSMove* controllers[count]; printf("%s", "### Trying to init PSMoveTracker..."); PSMoveTracker* tracker = psmove_tracker_new(); printf("%s\n", "OK"); printf("### Found %d controllers.\n", count); void *frame; unsigned char r, g, b; int result; for (i=0; i<count; i++) { printf("Opening controller %d\n", i); controllers[i] = psmove_connect_by_id(i); assert(controllers[i] != NULL); while (1) { printf("Calibrating controller %d...", i); fflush(stdout); result = psmove_tracker_enable(tracker, controllers[i]); if (result == Tracker_CALIBRATED) { printf("OK\n"); break; } else { printf("ERROR - retrying\n"); } } } while ((cvWaitKey(1) & 0xFF) != 27) { psmove_tracker_update_image(tracker); psmove_tracker_update(tracker, NULL); frame = psmove_tracker_get_image(tracker); if (frame) { cvShowImage("live camera feed", frame); } for (i=0; i<count; i++) { psmove_tracker_get_color(tracker, controllers[i], &r, &g, &b); psmove_set_leds(controllers[i], r, g, b); psmove_update_leds(controllers[i]); float x, y, r; psmove_tracker_get_position(tracker, controllers[i], &x, &y, &r); printf("x: %10.2f, y: %10.2f, r: %10.2f\n", x, y, r); } } for (i=0; i<count; i++) { psmove_disconnect(controllers[i]); } psmove_tracker_free(tracker); return 0; }