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