Beispiel #1
0
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;
    }
}
Beispiel #2
0
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();
    }

}
Beispiel #3
0
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;
}
Beispiel #5
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;
}