glcricket.c

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

        glVertex3f(pos[i].x + xdif*(spc+2*as)/dist - ydif*as/dist,
                pos[i].y + ydif*(spc+2*as)/dist + xdif*as/dist, 0.0);
        glEnd();
    }

    dist2 = meas/30.0;
    if (fabs(dist2-dist) > spc) {
        float sig = 1;
        dist2 -= dist;
        if (dist2 < -spc) {
            glLineWidth(4.0);
            glLineStipple(1, 0xff00);
            glEnable(GL_LINE_STIPPLE);
            sig = -1;
        }
        glBegin(GL_LINE_STRIP);
        glVertex3f(pos[j].x + xdif*dist2/dist - ydif*as/dist,
                pos[j].y + ydif*dist2/dist + xdif*as/dist, 0.0);
        glVertex3f(pos[j].x + sig*xdif*spc/dist - ydif*as/dist,
                pos[j].y + sig*ydif*spc/dist + xdif*as/dist, 0.0);
        glEnd();
        if (dist2 < -spc)
            glDisable(GL_LINE_STIPPLE);
    }
}

void draw_node_vel(int i)
{
    if (!pos[i].vel_set)
        return;

    glLineWidth(4.0);
    glBegin(GL_LINES);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3f(pos[i].x, pos[i].y, 0.0);
    glVertex3f(pos[i].x + pos[i].vel_x, pos[i].y + pos[i].vel_y, 0.0);
    glEnd();
    
}

void draw_mob_dist()
{
    int i, j;

    for (i=0; i<=max_nodes; i++) {
        if (!(pos[i].stat & STAT_MOVING) || !(pos[i].stat & STAT_VISIBLE))
            continue;
        for (j=0; j<=max_nodes; j++) {
            if (i != j && mob_dist[i][j] != 0)
                draw_node_ping(j, i, NULL, mob_dist[i][j]);
        }
    }
}

void draw_pings()
{
    int i, j;

    if (show_vertex != -1) {
        for (j=0; j<=max_nodes; j++) {
            if (j != show_vertex) {
                draw_node_ping(show_vertex, j, NULL,
                        pings_summary[show_vertex][j]);
                draw_node_ping(j, show_vertex, NULL,
                        pings_summary[j][show_vertex]);
            }
        }
        return;
    }

    for (i=0; i<=max_nodes; i++) {
        if (!(pos[i].stat & STAT_VISIBLE))
            continue;
        for (j=0; j<=max_nodes; j++) {
            if (j != i)
                draw_node_ping(i, j, &pings[i][j], pings_d[i][j]);
        }
    }
}

void draw_edge(int a, int b)
{
    glVertex3f(pos[a].x, pos[a].y, 0.0);
    glVertex3f(pos[b].x, pos[b].y, 0.0);
}

#define square(a)   ((a)*(a))

int get_normalized_dist(int i, int j)
{
    if (pos[i].stat & STAT_MOVING)
        return pings_summary[j][i];
    if (pos[j].stat & STAT_MOVING)
        return pings_summary[i][j];

    return (pings_summary[i][j] + pings_summary[j][i]) >> 1;
}

void trilaterate1(int i1, int i2, int i3, int i,
        float * x, float * y)
{
    float dsq, gam, xa, ya, xb, yb, xt1, xt2, yt1, yt2, d1, d2;
    float x1, y1, x2, y2, x3, y3;
    int r1, r2, r3;

    x1 = pos[i1].x*30;
    y1 = pos[i1].y*30;

    x2 = pos[i2].x*30;
    y2 = pos[i2].y*30;

    x3 = pos[i3].x*30;
    y3 = pos[i3].y*30;

    /*
    r1 = (pings_summary[i1][i] + pings_summary[i][i1]) >> 1;
    r2 = (pings_summary[i2][i] + pings_summary[i][i2]) >> 1;
    r3 = (pings_summary[i3][i] + pings_summary[i][i3]) >> 1;
    */
    r1 = get_normalized_dist(i1, i);
    r2 = get_normalized_dist(i2, i);
    r3 = get_normalized_dist(i3, i);

    dsq = square(x2-x1) + square(y2-y1);
    gam = sqrt((square((float)r2+r1) - dsq)*(dsq - square((float)r2-r1)));
    xa = -(square(r2)-square(r1))*(x2-x1)/(2.0*dsq) + (x1+x2)*0.5;
    ya = -(square(r2)-square(r1))*(y2-y1)/(2.0*dsq) + (y1+y2)*0.5;
    xb = (y2-y1)*gam/(2.0*dsq);
    yb = (x2-x1)*gam/(2.0*dsq);
    xt1 = xa - xb;
    xt2 = xa + xb;
    yt1 = ya + yb;
    yt2 = ya - yb;
    
    d1 = sqrt(square(xt1-x3) + square(yt1-y3));
    d2 = sqrt(square(xt2-x3) + square(yt2-y3));
    if (fabs(d1-r3) < fabs(d2-r3)) {
        *x = xt1/30;
        *y = yt1/30;
    }
    else {
        *x = xt2/30;
        *y = yt2/30;
    }
}

double rigidity(int a, int b, int c)
{
    double min, d, e;
    double costh;

    if (a <= b && a <= c)
        min = a, d = b, e = c;
    else if (b <= a && b <= c)
        min = b, d = a, e = c;
    else
        min = c, d = a, e = b;

    costh = (square(d)+square(e)-square(min))/(2.0*d*e);
    return min*(1-square(costh));
}

void print_rigidity(int vertex)
{
    int i, j, k, l;
    int dij, dik, dil, djk, dkl, dlj;

    for (i=0; i<num_quads; i++) {
        if (vertex == quads[i].v[0] || vertex == quads[i].v[1] ||
                vertex == quads[i].v[2])
            break;
    }
    if (i == num_quads)
        return;

    j = quads[i].v[0];
    k = quads[i].v[1];
    l = quads[i].v[2];
    i = max_nodes;

    dij = (pings_summary[j][i] + pings_summary[i][j]) >> 1;
    dik = (pings_summary[k][i] + pings_summary[i][k]) >> 1;
    dil = (pings_summary[l][i] + pings_summary[i][l]) >> 1;
    dkl = (pings_summary[l][k] + pings_summary[k][l]) >> 1;
    djk = (pings_summary[j][k] + pings_summary[k][j]) >> 1;
    dlj = (pings_summary[j][l] + pings_summary[l][j]) >> 1;

    printf("Rigidity of Quad %d %d %d %d: %.1f %.1f %.1f %.1f\n",
            i, j, k, l, rigidity(dij,dik,djk), rigidity(dij,dil,dlj),
            rigidity(dik,dil,dkl), rigidity(djk,dkl,dlj));
    printf("(%d %d %d) (%d %d %d) (%d %d %d) (%d %d %d)\n",
            dij, dik, djk, dij, dil, dlj, dik, dil, dkl, djk, dkl, dlj);
}

void draw_quads()
{
    int i;

    glColor4f(0.0, 0.0, 1.0, 1.0);
    glLineWidth(4.0);
    for (i=0; i<num_quads; i++) {
        if (show_vertex != -1 && show_vertex != quads[i].v[0] &&
                    show_vertex != quads[i].v[1] && show_vertex != quads[i].v[2])
            continue;
        glBegin(GL_LINES);
        draw_edge(quads[i].v[1], quads[i].v[2]);
        draw_edge(quads[i].v[0], quads[i].v[2]);
        draw_edge(max_nodes, quads[i].v[2]);
        glEnd();
        if (show_vertex != -1)
            glLineWidth(8.0);
        glBegin(GL_LINES);
        draw_edge(quads[i].v[0], quads[i].v[1]);
        draw_edge(max_nodes, quads[i].v[0]);
        draw_edge(max_nodes, quads[i].v[1]);
        glEnd();
        if (show_vertex != -1)
            break;
    }

    /* Draw the three possible trilaterated points */
    if (i != num_quads && show_vertex == quads[i].v[2]) {
        float x, y;
        glLineWidth(4.0);
        glColor4f(1.0, 1.0, 0.0, 1.0);
        glBegin(GL_LINES);
        trilaterate1(max_nodes, quads[i].v[0], quads[i].v[1], show_vertex,
                &x, &y);
        glVertex3f(x-4.0, y, 0.0);
        glVertex3f(x+4.0, y, 0.0);
        glVertex3f(x, y-4.0, 0.0);
        glVertex3f(x, y+4.0, 0.0);
        trilaterate1(max_nodes, quads[i].v[1], quads[i].v[0], show_vertex,
                &x, &y);
        glVertex3f(x-4.0, y, 0.0);
        glVertex3f(x+4.0, y, 0.0);
        glVertex3f(x, y-4.0, 0.0);
        glVertex3f(x, y+4.0, 0.0);
        trilaterate1(quads[i].v[0], quads[i].v[1], max_nodes, show_vertex,
                &x, &y);
        glVertex3f(x-4.0, y, 0.0);
        glVertex3f(x+4.0, y, 0.0);
        glVertex3f(x, y-4.0, 0.0);
        glVertex3f(x, y+4.0, 0.0);
        glEnd();
    }
}

void draw_circle(float x, float y, float z, float r)
{
    float th;
    for (th=0; th < 2*M_PI; th += 0.4) {
        glVertex3f(x + r*cos(th), y + r*sin(th), z);
    }
}

void draw_truth()
{
    int i, match=-1;
    float ox = 0;
    float oy = 0;

    for (i=0; i<num_truth; i++) {
        if (!strcmp(truth[i].name, pos[max_nodes].name)) {
            ox = -truth[i].x;
            oy = -truth[i].y;
            match = i;
            break;
        }
    }
    for (i=0; i<num_truth; i++) {
        if (i == match)
            continue;
        glBegin(GL_POLYGON);
        glColor4f(0.0, 1.0, 0.0, 0.5);
        draw_circle(truth[i].x+ox, truth[i].y+oy, 0.0, 3.0);
        glEnd();
        glRasterPos2f(truth[i].x+ox+3.5*cos(trot*M_PI/180),
                truth[i].y+oy-3.5*sin(trot*M_PI/180));
        glPrintf(0, "%s", truth[i].name);
    }
}

void draw_history()
{
    int i, j;
    float th = 1.0;
    struct timeval tv;
    float exp = 60.0;
    getcrickettime(&tv);
    glClearDepth(0.0);
    glDepthFunc(GL_NOTEQUAL);
    glEnable(GL_DEPTH_TEST);
    glDepthMask(GL_TRUE);
    for (i=0; i<=max_nodes; i++) {
        float x1, y1, x2, y2, xdif, ydif, dist, t1, t2;
        if (pos[i].hist_num == 0)
            continue;

        glClear(GL_DEPTH_BUFFER_BIT);
        glColor3f(0.5, 0.5, 0.92);
        x2 = pos[i].x;
        y2 = pos[i].y;
        t2 = 0;
        for (j=pos[i].hist_num-1; j>=0; j--) {
            x1 = pos[i].hist[j].x;
            y1 = pos[i].hist[j].y;
            if (show_all_history)
                t1 = 0;
            else
                t1 = sec_diff(tv, pos[i].hist[j].time);
            ydif = y2 - y1;
            xdif = x2 - x1;
            dist = hypot(ydif, xdif);
            if (t2 > exp) {
                break;
            }
            else if (t1 > exp) {
                x1 = x2 - xdif * (exp-t2)/(t1-t2);
                y1 = y2 - ydif * (exp-t2)/(t1-t2);
            }
            glBegin(GL_QUADS);
            glColor4f(0.5, 0.5, 0.92, (exp-t2)/exp);
            glVertex3f(x2 - ydif*th/dist, y2 + xdif*th/dist, 1.0);
            glVertex3f(x2 + ydif*th/dist, y2 - xdif*th/dist, 1.0);
            glColor4f(0.5, 0.5, 0.92, (exp-t1)/exp);
            glVertex3f(x1 + ydif*th/dist, y1 - xdif*th/dist, 1.0);
            glVertex3f(x1 - ydif*th/dist, y1 + xdif*th/dist, 1.0);
            glEnd();
            glBegin(GL_POLYGON);
            glColor4f(0.5, 0.5, 0.92, (exp-t1)/exp);
            draw_circle(x1, y1, 1.0, th);
            glEnd();
            x2 = x1;
            y2 = y1;
            t2 = t1;
        }
    }
    glDisable(GL_DEPTH_TEST);
}

void draw_nodes()
{
    int i;

    if (show_quads)
        draw_quads();
    for (i=0; i<=max_nodes; i++) {
        float x, y;
        glBegin(GL_POLYGON);
        if (i == max_nodes) {
            glColor3f(0.9, 0.1, 0.1);
        }
        else if (pos[i].stat & STAT_VISIBLE) {
            if (pos[i].stat & STAT_WEAK)
                glColor3f(0.92, 0.5, 0.5);
            else
                glColor3f(0.92, 0.92, 0.92);
        }
        else {
            continue;
        }
        x = pos[i].last_x;
        y = pos[i].last_y;
        //printf("x = %f y = %f\n", x, y);
        if ((pos[i].stat & STAT_MOVING) && pos[i].vel_set && do_smoothing) {
            float t;
            struct timeval tv;

            getcrickettime(&tv);
            t = sec_diff(tv, pos[i].vel_time);
            x += pos[i].vel_x * t;
            y += pos[i].vel_y * t;
            pos[i].x = x;
            pos[i].y = y;
            //printf("draw x = %f y = %f t = %f\n", x, y, t);
        }
        draw_circle(x, y, 0.0, 5.0);
        glEnd();
        if (pos[i].stat & STAT_MOVING) {
            glBegin(GL_POLYGON);
            glColor3f(0.0, 0.0, 0.0);
            draw_circle(x, y, 0.0, 1.6);
            glEnd();
        }
        if (pos[i].stat & STAT_OPTIMIZED) {
            glBegin(GL_POLYGON);
            glColor4f(0.0, 0.0, 1.0, 0.5);
            draw_circle(pos[i].ox, pos[i].oy, 0.0, 5.0);
            glEnd();
        }
        glColor3f(0.92, 0.92, 0.92);
        glRasterPos2f(x+ui_flip*comp_flip*6*sin((ui_rot+comp_rot)*M_PI/180),
                y+6*cos((ui_rot+comp_rot)*M_PI/180));
        glPrintf(0, "%s", pos[i].name);
      //  if (pos[i].stat & STAT_MOVING)
      //      draw_node_vel(i);
    }
    if (show_pings == 1)
        draw_pings();
    else if (show_pings == 2)
        draw_mob_dist();
}

#define MAX(a,b) ((a)>(b)?(a):(b))

void recompute_bounding(float * ox, float * oy, float * fov,
        float cox, float coy, float cfov)
{
    float minx = HUGE_VAL;
    float maxx = -HUGE_VAL;
    float miny = HUGE_VAL;
    float maxy = -HUGE_VAL;
    float x, y;
    float rot;
    float new_fov;
    int i, refresh = 0;

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

        x = ui_flip * comp_flip * pos[i].x;
        rot = ui_rot + comp_rot;
        y = x * sin(rot*M_PI/180) + pos[i].y * cos(rot*M_PI/180);
        x = x * cos(rot*M_PI/180) - pos[i].y * sin(rot*M_PI/180);
        if (y > maxy)
            maxy = y;
        if (y < miny)
            miny = y;
        if (x > maxx)
            maxx = x;
        if (x < minx)
            minx = x;
    }
    maxy += 20;
    miny -= 20;
    maxx += 20;
    minx -= 20;

    /*
    if (maxx + *ox > *fov || minx + *ox < -*fov)
        refresh = 1;
    if (maxy + *oy > *fov/aspect || miny + *oy < -*fov/aspect)
        refresh = 1;

    if (!refresh)
        return;
*/
    new_fov = MAX((maxx - minx) / 2, (maxy - miny) * aspect / 2);
    if (((new_fov/cfov) > 1.1 || (new_fov/cfov) < 0.9) && new_fov > 100)
        *fov = new_fov;

    maxx = -(minx + maxx) / 2;
    if (fabs(maxx - cox) > 10)
        *ox = maxx;
    maxy = -(miny + maxy) / 2;
    if (fabs(maxy - coy) > 10)
        *oy = maxy;
}

void draw_grid()