glcricket.c

无线传感器网络中的节点定位算法。详见ReadMe文件。在TinyOS上实现的节点定位算法。

{
    float y;
    glLineWidth(4.0);
    glBegin(GL_LINES);
    glColor4f(0.2, 0.2, 0.2, 1.0);
    y = ceil((-fov/aspect-origin_y)/50.0)*50.0;
    for ( ; y < fov/aspect-origin_y; y += 50) {
        glVertex3f(-fov-origin_x, y, 0.0);
        glVertex3f(fov-origin_x, y, 0.0);
    }
    y = ceil((-fov-origin_x)/50.0)*50.0;
    for ( ; y < fov-origin_x; y += 50) {
        glVertex3f(y, -fov/aspect-origin_y, 0.0);
        glVertex3f(y, fov/aspect-origin_y, 0.0);
    }
    glEnd();
}

#define EDGE_HIST 200
struct edge_len {
    float len, loc_len;
} edge[EDGE_HIST];
int edge_head = 0;
int edge_tail = 0;
struct timeval edgeupdate;

int new_edge = 0;
float new_edge_len;

void draw_edge_over_time()
{
    int i, c=0;
    float max = 0.1;
    float last_loc = 0.0;
    struct timeval tv;

    getcrickettime(&tv);
    if (tv.tv_sec > edgeupdate.tv_sec ||
            (tv.tv_sec == edgeupdate.tv_sec &&
             tv.tv_usec > edgeupdate.tv_usec)) {
        edge_head++;
        if (edge_head == EDGE_HIST)
            edge_head = 0;
        edge[edge_head].len = 0;
        edge[edge_head].loc_len = 0;
        
        if (new_edge) {
            edge[edge_head].len = new_edge_len;
        }
        new_edge = 0;

        if ((pos[edge_v[0]].stat & STAT_VISIBLE) &&
                (pos[edge_v[1]].stat & STAT_VISIBLE)) {
            edge[edge_head].loc_len =
                sqrt(square(pos[edge_v[0]].x - pos[edge_v[1]].x) + 
                        square(pos[edge_v[0]].y - pos[edge_v[1]].y));
        }

        if (edge_head == edge_tail) {
            edge_tail++;
            if (edge_tail == EDGE_HIST)
                edge_tail = 0;
        }

        edgeupdate.tv_sec += (edgeupdate.tv_usec+250000) / 1000000;
        edgeupdate.tv_usec = (edgeupdate.tv_usec+250000) % 1000000;
    }
    
    i = edge_head;
    while (i != edge_tail) {
        if (edge[i].len > max)
            max = edge[i].len;
        if (edge[i].loc_len > max)
            max = edge[i].loc_len;

        if (i==0)
            i = EDGE_HIST;
        i--;
    }

    glLineWidth(1.0);
    glBegin(GL_LINES);
    i = edge_head;
    while (i != edge_tail) {
        float x = -c/(float)EDGE_HIST;
        float lx = -(c-1)/(float)EDGE_HIST;

        if (edge[i].loc_len > 0.0 && last_loc > 0.0) {
            glColor4f(1.0, 1.0, 1.0, 1.0);
            glVertex3f(x, -0.95 + 0.20*edge[i].loc_len/max, 0.0);
            glVertex3f(lx, -0.95 + 0.20*last_loc/max, 0.0);
        }
        last_loc = edge[i].loc_len;
        if (edge[i].len > 0.0) {
            if (edge_v[0] == max_nodes)
                glColor4f(1.0, 1.0, 0.0, 1.0);
            else
                glColor4f(0.0, 1.0, 0.0, 1.0);
            glVertex3f(x, -0.95, 0.0);
            glVertex3f(x, -0.95 + 0.20*edge[i].len/max, 0.0);
        }

        if (i==0)
            i = EDGE_HIST;
        i--;
        c++;
    }
    glEnd();
}

/* Here goes our drawing code */
int drawGLScene(GLvoid)
{
    float m[] = {ui_flip, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
    float m2[] = {comp_flip, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
    float t;
    struct timeval tv;
    //glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glClear(GL_COLOR_BUFFER_BIT);
    glLoadIdentity();

    if (edge_picked) {
        draw_edge_over_time();
    }
    glScalef(1.0/fov, 1.0*aspect/fov, 1.0);
    glTranslatef(origin_x, origin_y, 0);
    draw_grid();
    
    if (truth) {
        glPushMatrix();
        glRotatef(trot, 0.0, 0.0, 1.0);
        draw_truth();
        glPopMatrix();
    }

    glRotatef(ui_rot, 0.0, 0.0, 1.0);
    glPushMatrix();
    glMultMatrixf(m);
    draw_history();
    glPopMatrix();
    glRotatef(comp_rot, 0.0, 0.0, 1.0);
    glMultMatrixf(m);
    glMultMatrixf(m2);
    draw_nodes();

    glLoadIdentity();
    glTranslatef(-1.0, 1.0, 0.0);
    glScalef(2.0/GLWin.width, -2.0/GLWin.height, 1.0);
    glColor3f(0.92, 0.92, 0.92);
    glRasterPos2f(10, 30);
    getcrickettime(&tv);
    t = sec_diff(tv, starttime);
    glPrintf(1, "t = %7.2f", t);
    
    if (GLWin.doubleBuffered)
    {
        glXSwapBuffers(GLWin.dpy, GLWin.win);
    }
    return True;    
}

int find_node_by_xy(int x, int y)
{
    float nx, ny, ox;
    int i, closest;
    float dist, cdist = HUGE_VAL;

    nx = (x*2/(float)GLWin.width - 1.0) * fov - origin_x;
    ny = -(y*2/(float)GLWin.height - 1.0) * fov/aspect - origin_y;

    ox = nx;
    nx = ox*cos(-(ui_rot+comp_rot)*M_PI/180) - ny*sin(-(ui_rot+comp_rot)*M_PI/180);
    ny = ox*sin(-(ui_rot+comp_rot)*M_PI/180) + ny*cos(-(ui_rot+comp_rot)*M_PI/180);
    nx = ui_flip * comp_flip * nx;

    
    for (i=0; i<=max_nodes; i++) {
        if (i != max_nodes && !(pos[i].stat & STAT_VISIBLE))
            continue;

        dist = hypot(pos[i].x - nx, pos[i].y - ny);
        if (dist < cdist) {
            cdist = dist;
            closest = i;
        }
    }

    if (cdist < 10.0)
        return closest;

    return -1;
}

/* function to release/destroy our resources and restoring the old desktop */
GLvoid killGLWindow(GLvoid)
{
    if (GLWin.ctx)
    {
        if (!glXMakeCurrent(GLWin.dpy, None, NULL))
        {
            printf("Could not release drawing context.\n");
        }
        glXDestroyContext(GLWin.dpy, GLWin.ctx);
        GLWin.ctx = NULL;
    }
    /* switch back to original desktop resolution if we were in fs */
    if (GLWin.fs)
    {
        XF86VidModeSwitchToMode(GLWin.dpy, GLWin.screen, &GLWin.deskMode);
        XF86VidModeSetViewPort(GLWin.dpy, GLWin.screen, 0, 0);
    }
    deleteFont();
    XCloseDisplay(GLWin.dpy);
}

/* this function creates our window and sets it up properly */
/* FIXME: bits is currently unused */
Bool createGLWindow(char* title, int width, int height, int bits,
                    Bool fullscreenflag)
{
    XVisualInfo *vi;
    Colormap cmap;
    int dpyWidth, dpyHeight;
    int i;
    int glxMajorVersion, glxMinorVersion;
    int vidModeMajorVersion, vidModeMinorVersion;
    XF86VidModeModeInfo **modes;
    int modeNum;
    int bestMode;
    Atom wmDelete;
    Window winDummy;
    unsigned int borderDummy;
    
    GLWin.fs = fullscreenflag;
    /* set best mode to current */
    bestMode = 0;
    /* get a connection */
    GLWin.dpy = XOpenDisplay(0);
    GLWin.screen = DefaultScreen(GLWin.dpy);
    XF86VidModeQueryVersion(GLWin.dpy, &vidModeMajorVersion,
        &vidModeMinorVersion);
    printf("XF86VidModeExtension-Version %d.%d\n", vidModeMajorVersion,
        vidModeMinorVersion);
    XF86VidModeGetAllModeLines(GLWin.dpy, GLWin.screen, &modeNum, &modes);
    /* save desktop-resolution before switching modes */
    GLWin.deskMode = *modes[0];
    /* look for mode with requested resolution */
    for (i = 0; i < modeNum; i++)
    {
        if ((modes[i]->hdisplay == width) && (modes[i]->vdisplay == height))
        {
            bestMode = i;
        }
    }
    /* get an appropriate visual */
    vi = glXChooseVisual(GLWin.dpy, GLWin.screen, attrListDbl);
    if (vi == NULL)
    {
        vi = glXChooseVisual(GLWin.dpy, GLWin.screen, attrListSgl);
        GLWin.doubleBuffered = False;
        printf("Only Singlebuffered Visual!\n");
    }
    else
    {
        GLWin.doubleBuffered = True;
        printf("Got Doublebuffered Visual!\n");
    }
    glXQueryVersion(GLWin.dpy, &glxMajorVersion, &glxMinorVersion);
    printf("glX-Version %d.%d\n", glxMajorVersion, glxMinorVersion);
    /* create a GLX context */
    GLWin.ctx = glXCreateContext(GLWin.dpy, vi, 0, GL_TRUE);
    /* create a color map */
    cmap = XCreateColormap(GLWin.dpy, RootWindow(GLWin.dpy, vi->screen),
        vi->visual, AllocNone);
    GLWin.attr.colormap = cmap;
    GLWin.attr.border_pixel = 0;

    if (GLWin.fs)
    {
        XF86VidModeSwitchToMode(GLWin.dpy, GLWin.screen, modes[bestMode]);
        XF86VidModeSetViewPort(GLWin.dpy, GLWin.screen, 0, 0);
        dpyWidth = modes[bestMode]->hdisplay;
        dpyHeight = modes[bestMode]->vdisplay;
        printf("Resolution %dx%d\n", dpyWidth, dpyHeight);
        XFree(modes);
    
        /* create a fullscreen window */
        GLWin.attr.override_redirect = True;
        GLWin.attr.event_mask = ExposureMask | KeyPressMask | ButtonPressMask |
            StructureNotifyMask;
        GLWin.win = XCreateWindow(GLWin.dpy, RootWindow(GLWin.dpy, vi->screen),
            0, 0, dpyWidth, dpyHeight, 0, vi->depth, InputOutput, vi->visual,
            CWBorderPixel | CWColormap | CWEventMask | CWOverrideRedirect,
            &GLWin.attr);
        XWarpPointer(GLWin.dpy, None, GLWin.win, 0, 0, 0, 0, 0, 0);
		XMapRaised(GLWin.dpy, GLWin.win);
        XGrabKeyboard(GLWin.dpy, GLWin.win, True, GrabModeAsync,
            GrabModeAsync, CurrentTime);
        XGrabPointer(GLWin.dpy, GLWin.win, True, ButtonPressMask,
            GrabModeAsync, GrabModeAsync, GLWin.win, None, CurrentTime);
    }
    else
    {
        /* create a window in window mode*/
        GLWin.attr.event_mask = ExposureMask | KeyPressMask | ButtonPressMask |
            StructureNotifyMask;
        GLWin.win = XCreateWindow(GLWin.dpy, RootWindow(GLWin.dpy, vi->screen),
            0, 0, width, height, 0, vi->depth, InputOutput, vi->visual,
            CWBorderPixel | CWColormap | CWEventMask, &GLWin.attr);
        /* only set window title and handle wm_delete_events if in windowed mode */
        wmDelete = XInternAtom(GLWin.dpy, "WM_DELETE_WINDOW", True);
        XSetWMProtocols(GLWin.dpy, GLWin.win, &wmDelete, 1);
        XSetStandardProperties(GLWin.dpy, GLWin.win, title,
            title, None, NULL, 0, NULL);
        XMapRaised(GLWin.dpy, GLWin.win);
    }       
    /* connect the glx-context to the window */
    glXMakeCurrent(GLWin.dpy, GLWin.win, GLWin.ctx);
    XGetGeometry(GLWin.dpy, GLWin.win, &winDummy, &GLWin.x, &GLWin.y,
        &GLWin.width, &GLWin.height, &borderDummy, &GLWin.depth);
    printf("Depth %d\n", GLWin.depth);
    printf("%dx%d\n", GLWin.width, GLWin.height);
    if (glXIsDirect(GLWin.dpy, GLWin.ctx)) 
        printf("Congrats, you have Direct Rendering!\n");
    else
        printf("Sorry, no Direct Rendering possible!\n");
    initGL();
    return True;    
}

GLvoid glPrintf(int font, const char * fmt, ...)
{
    va_list ap;
    char text[256];
    if (fmt == NULL)
        return;

    va_start(ap, fmt);
    vsprintf(text, fmt, ap);
    va_end(ap);
    glPushAttrib(GL_LIST_BIT);
    glListBase(base + font*96 - 32);
    glCallLists(strlen(text), GL_UNSIGNED_BYTE, text);
    glPopAttrib();
}

int serial_open(char * dev)
{
    struct termios tio;
    int fd;
    char * dbg_on = "DEBUG ON\n";
    char * ln_on = "LOCALIZATION ON\n";
    char * cn_on = "CONES ON\n";

    fprintf(stderr, "Using %s\n", dev);
                                                                                
    fd = open(dev, O_RDWR);
    if (fd == -1) {
        perror("open");
        return -1;
    }
                                                                                
    /* Set the serial port attributes */
    tcgetattr(fd, &tio);
                                                                                
    tio.c_cflag = CS8 | CLOCAL | CREAD;
    tio.c_iflag = IGNPAR | IGNCR;
    cfmakeraw(&tio);
                                                                                
    cfsetospeed(&tio, B115200);
    cfsetispeed(&tio, B115200);
                                                                                
    tcflush(fd, TCIFLUSH);
    if (tcsetattr(fd, TCSANOW, &tio) == -1) {
        perror("tcsetattr");
        return -1;
    }

    write(fd, dbg_on, strlen(dbg_on));
    usleep(300000);
    write(fd, ln_on, strlen(ln_on));
    if (do_cones) {
        usleep(300000);
        write(fd, cn_on, strlen(cn_on));
    }

    return fd;
}

int node_find_v1(int id)
{
    int i;
    for (i=0; i<=max_nodes; i++) {
        if ((pos[i].stat & STAT_VALID) && pos[i].id[3] == id)
            return i;
    }
    return -1;
}

int id_match(int * id1, int * id2)
{
    int i;
    for (i=0; i<4; i++) {
        if (id1[i] != id2[i])
            return 0;
    }
    return 1;
}

void node_new(int i, int * id)
{
    int j;
    char buf[8];
    pos[i].stat = STAT_VALID;
    pos[i].newstat = pos[i].stat;
    pos[i].id[0] = id[0];
    pos[i].id[1] = id[1];
    pos[i].id[2] = id[2];
    pos[i].id[3] = id[3];
    pos[i].x = 0;
    pos[i].y = 0;
    pos[i].last_x = 0;
    pos[i].last_y = 0;
    pos[i].hist = NULL;
    pos[i].hist_size = 0;
    pos[i].hist_num = 0;
    pos[i].vel_set = 0;
    for (j=0; j<=max_nodes; j++) {
        pings[i][j].tv_sec = 0;
        pings[j][i].tv_sec = 0;
    }
    for (j=0; j<NUM_NAMES; j++) {
        if (id_match(names[j].id, id)) {
            pos[i].name = names[j].name;
            printf("# node %d is called \"%s\"\n", i, names[j].name);
            break;
        }
    }
    if (j == NUM_NAMES) {
        sprintf(buf, "0x%02x", id[3]);
        pos[i].name = strdup(buf);
    }

    if (do_flip && !strcmp(flipname, pos[i].name)) {
        do_flip = 2;
        flip_node = i;
        printf("Flip node is %d\n", i);
    }
    if (do_rot && !strcmp(rotname, pos[i].name)) {
        do_rot = 2;
        rot_node = i;
        printf("Rot node is %d\n", i);
    }
}

void print_matrix(float * m, int r, int c)
{
    int i, j;
    printf("[ ");
    for (i=0; i<r; i++) {
        for (j=0; j<c; j++) {
            printf("%f ", m[j*r+i]);