glcricket.c

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

        }
        printf("; ");
    }
    printf("]\n");
}


void matrix_mult(float * dst, float * m1, float * m2, int r1, int c1, int c2)
{
    int i, j, k;
    //printf("srcs mult: ");
    //print_matrix(m1, r1, c1);
    //print_matrix(m2, c1, c2);
    for (i=0; i<c2; i++) {
        for (j=0; j<r1; j++) {
            float * m = m1+j;
            *dst = 0;
            for (k=0; k<c1; k++) {
                *dst += m2[k] * m[k*r1];
            }
            dst++;
        }
        m2 += c1;
    }
    //printf("dst ");
    //print_matrix(dst, r1, c2);
}

void matrix_mult_trans_2(float * dst, float * m1, float * m2, int r1, int c1, int r2)
{
    int i, j, k;
    //printf("srcs mult_trans: ");
    //print_matrix(m1, r1, c1);
    //print_matrix(m2, r2, c1);
    for (i=0; i<r2; i++) {
        for (j=0; j<r1; j++) {
            float * a = m1+j;
            float * b = m2+i;
            *dst = 0;
            for (k=0; k<c1; k++) {
                *dst += b[k*r2] * a[k*r1];
            }
            dst++;
        }
    }
    //printf("dst ");
    //print_matrix(dst, r1, r2);
}

void matrix_inv_2x2(float * dst, float * m)
{
    float div = 1.0 / (m[0]*m[3] - m[2]*m[1]);
    dst[0] = m[3] * div;
    dst[1] = -m[1] * div;
    dst[2] = -m[2] * div;
    dst[3] = m[0] * div;
}

void update_vels(int i, char * str) {
    struct node_pos * p = pos+i;
    int j, n;
    float vs[max_nodes];
    float xy[max_nodes*2];
    float B[4];
    float C[4];
    float v[2];

    if (p->vel_set && do_smoothing) {
        float t;
        struct timeval tv;
        getcrickettime(&tv);
        t = sec_diff(tv, p->vel_time);
        p->last_x += p->vel_x * t;
        p->last_y += p->vel_y * t;
        p->x = p->last_x;
        p->y = p->last_y;
        //printf("new xy %f %f t %f\n", p->x, p->y, t);
    }
    n = 0;
    while (1) {
        float xdif, ydif, dist;
        int ret, vel, c;
        ret = sscanf(str, "%d v=%d;%n", &j, &vel, &c);
        if (ret != 2)
            break;
        str += c;
        if (!(pos[j].stat & STAT_VISIBLE))
            continue;
        if (vel > 1000 || vel < -1000)
            continue;
        vs[n] = vel / 30.0;
        xdif = pos[i].x - pos[j].x;
        ydif = pos[i].y - pos[j].y;
        //printf("xy %f %f xy %f %f\n", pos[i].x, pos[i].y, pos[j].x, pos[j].y);
        dist = sqrt(xdif*xdif + ydif*ydif);
        if (dist == 0.0)
            continue;
        //printf("xdif %f ydif %f dist %f\n", xdif, ydif, dist);
        xy[n<<1]     = xdif / dist;
        xy[(n<<1)+1] = ydif / dist;
        n++;
    }
    if (n < 2) {
        p->vel_set = 0;
        return;
    }
    matrix_mult_trans_2(B, xy, xy, 2, n, 2);
    matrix_inv_2x2(C, B);
    matrix_mult(B, xy, vs, 2, n, 1);
    matrix_mult(v, C, B, 2, 2, 1);
    p->vel_set = 1;
    p->vel_x = v[0];
    p->vel_y = v[1];
    //printf("vel is %f %f\n", v[0], v[1]);
    getcrickettime(&p->vel_time);
}

void update_history(int i)
{
    struct node_pos * p = pos+i;

    if (!(p->stat & STAT_MOVING)) {
        if (p->hist) {
            free(p->hist);
            p->hist_num = 0;
            p->hist_size = 0;
        }
        return;
    }

    if (p->hist_num == p->hist_size) {
        p->hist_size += 20;
        p->hist = realloc(p->hist, p->hist_size * sizeof(struct hist_pos));
    }

    p->hist[p->hist_num].x = p->x;
    p->hist[p->hist_num].y = p->y;
    getcrickettime(&p->hist[p->hist_num].time);
    p->hist_num++;
}

int node_new_v1(int id)
{
    int i, j;
    for (i=0; i<max_nodes; i++) {
        if (pos[i].stat == STAT_EMPTY) {
            pos[i].stat = STAT_VALID;
            pos[i].id[3] = id;
            for (j=0; j<=max_nodes; j++) {
                pings[i][j].tv_sec = 0;
                pings[j][i].tv_sec = 0;
            }
            pos[i].name = "Unk";
            for (j=0; j<NUM_NAMES; j++) {
                if (names[j].crc == id)
                    pos[i].name = names[j].name;
            }
            return i;
        }
    }
    return -1;
}

void update_mob_dist(char * str)
{
    int j, c;
    int ret, dist;

    while (1) {
        ret = sscanf(str, " %d d=%d ;%n", &j, &dist, &c);
        str += c;
        if (ret != 2)
            break;
        if (do_cones) {
            dist -= 180;
        }
        mob_dist_new[j] = dist;
    }
}

void process_dists_v2(int node, char * str)
{
    int j, c;
    int ret, dist;

    while (1) {
        ret = sscanf(str, " %d d=%d ;%n", &j, &dist, &c);
        str += c;
        if (ret != 2)
            break;
//        getcrickettime(&pings[node][j]);
        if (do_cones) {
            dist -= 180;
        }
        pings_summary_new[node][j] = dist;
        if (input_version == 3)
            pings_summary_new[j][node] = dist;
        if (edge_picked && node != max_nodes && node == edge_v[0] &&
                j == edge_v[1] && dist != pings_summary[node][j]) {
            new_edge = 1;
            new_edge_len = dist/(float)30;
        }
//        printf("%02d %02x %05d\n", node, id, dist);
    }
}

void process_dists_v1(int node, char * str)
{
    int j, c;
    int id, dist;

    while (1) {
        j = sscanf(str, " 0x%x d=%d ;%n", &id, &dist, &c);
        str += c;
        if (j != 2)
            break;
        j = node_find_v1(id);
        if (j == -1)
            continue;
//        getcrickettime(&pings[node][j]);
        pings_summary_new[node][j] = dist;
        if (edge_picked && node != max_nodes && node == edge_v[0] &&
                j == edge_v[1] && dist != pings_summary[node][j]) {
            new_edge = 1;
            new_edge_len = dist/(float)30;
        }
//        printf("%02d %02x %05d\n", node, id, dist);
    }
}

int solve_read = 0;
int opt_read = 0;
int last_node;

void process_line_v2(char * str)
{
    int i;
    int ret;
    float x, y;
    struct timeval tv;
    
    getcrickettime(&tv);
    printf("%d.%03d: %s\n", tv.tv_sec, tv.tv_usec/1000, str);

    if (!strncmp(str, "Localized ", 10)) {
        solve_read = 1;
        for (i=0; i<max_nodes; i++) {
            if (pos[i].stat & STAT_VALID) {
#if 0
                if (pos[i].stat & STAT_MARKED) {
                    pos[i].stat &= ~STAT_VISIBLE;
                    pos[i].stat &= ~STAT_OPTIMIZED;
                }
                pos[i].stat |= STAT_MARKED;
#endif
                pos[i].newstat = pos[i].stat;
                if (!(pos[i].stat & STAT_MOVING))
                    pos[i].newstat &= ~STAT_VISIBLE;
                pos[i].newstat &= ~STAT_OPTIMIZED;
            }
        }
    }
    else if (!strncmp(str, "Optimized,", 10)) {
        float err;
        ret = sscanf(str, "Optimized, sumsq=%f", &err);
        if (ret == 1) {
            opt_err = err;
        }
        opt_read = 1;
        solve_read = 0;
    }
    else if (!strcmp(str, "Localizing...")) {
        num_quads_new = 0;
    }
    else if (!strncmp(str, "Rigid Quad:", 11)) {
        int ids[3];
        sscanf(str, "Rigid Quad: %d %d %d", ids, ids+1, ids+2);
        for (i=0; i<3; i++)
            quads_new[num_quads_new].v[i] = ids[i];
        num_quads_new++;
    }
    else if (!strncmp(str, "RQ:", 3)) {
        int ids[3];
        sscanf(str, "RQ: %d %d %d", ids, ids+1, ids+2);
        for (i=0; i<3; i++)
            quads_new[num_quads_new].v[i] = ids[i];
        num_quads_new++;
    }
    else if (sscanf(str, "  %d: x=%f y=%f", &i, &x, &y) == 3) {
        if (solve_read) {
            pos[i].newstat |= STAT_VISIBLE;
            //pos[i].stat &= ~STAT_MARKED;
            if (strchr(str, '*'))
                pos[i].newstat |= STAT_WEAK;
            else
                pos[i].newstat &= ~STAT_WEAK;
            if (strchr(str, 'm')) {
                pos[i].newstat |= STAT_MOVING;
                pos[i].ox = x;
                pos[i].oy = y;
            }
            else {
                pos[i].newstat &= ~STAT_MOVING;
                pos[i].x = x;
                pos[i].y = y;
                pos[i].last_x = x;
                pos[i].last_y = y;
                getcrickettime(&pos[i].vel_time);
            }
        }
        else if (opt_read) {
           /* if (opt_err > 1000000) {
                x = pos[i].ox;
                y = pos[i].oy;
            }
            else {
                pos[i].newstat |= STAT_OPTIMIZED;
            }
            */
            if (opt_err < 1000000) {
                pos[i].x = x;
                pos[i].y = y;
                pos[i].last_x = x;
                pos[i].last_y = y;
                getcrickettime(&pos[i].vel_time);
                update_history(i);
                memcpy(mob_dist[i], mob_dist_new, sizeof(mob_dist_new));
            }
        }
    }
    else if (!strcmp(str, "done")) {
        opt_read = solve_read = 0;
        memcpy(quads, quads_new, sizeof(quads));
        num_quads = num_quads_new;
        memcpy(pings_summary, pings_summary_new, sizeof(pings_summary));
        for (i=0; i<max_nodes; i++) {
            if (pos[i].stat & STAT_VALID) {
                pos[i].stat = pos[i].newstat;
            }
        }
        //drawGLScene();
    }
    else if (!strcmp(str, "opt done")) {
        opt_read = 0;
        if (pause_on_loc)
            paused = 1;
    }
    else if (!strncmp(str, "beacon:", 7)) {
        int dist, time;
        ret = sscanf(str, "beacon:%d,dist=%d,time=%d", &i, &dist, &time);
        if (ret != 3)
            return;
        if (i == 0xff)
            return;
        getcrickettime(&pings[max_nodes][i]);
        if (time/dist == 28)
            pings_d[max_nodes][i] = time * 1.08;
        else
            pings_d[max_nodes][i] = time;
        if (do_cones)
            pings_d[max_nodes][i] -= 180;
        if (edge_picked && edge_v[0] == max_nodes && edge_v[1] == i) {
            new_edge = 1;
            new_edge_len = pings_d[max_nodes][i]/(float)30;
        }
        //drawGLScene();
    }
    else if (!strncmp(str, "node ", 5)) {
        int id[4];
        char info[64];
        info[0] = '\0';
        if (sscanf(str, "node %d (self) ID %x:%x:%x:%x %64s",
                    &i, id, id+1, id+2, id+3, info) >= 5) {
            max_nodes = i;
            node_new(max_nodes, id);
            pos[max_nodes].stat |= STAT_VISIBLE;
        }
        else if (sscanf(str, "node %d ID %x:%x:%x:%x %64s",
                    &i, id, id+1, id+2, id+3, info) >= 5) {
            node_new(i, id);
        }
    }
    else if (!strncmp(str, "  from ", 7)) {
        char buf[16];
        ret = sscanf(str, " from %d%7c:", &i, buf);
        buf[7] = '\0';
        if (ret == 2) {
            if (summary_complete) {
                bzero(pings_summary_new, sizeof(pings_summary_new));
                summary_complete = 0;
            }
            if (!strcmp(buf, " (self)")) {
                summary_complete = 1;
            }
            last_node = i;
            process_dists_v2(i, str+17);
        }
    }
    else if (!strncmp(str, "          ", 10)) {
        process_dists_v2(last_node, str);
    }
    else if (!strncmp(str, "  vels to", 9)) {
        int c;
        ret = sscanf(str, " vels to %d:%n", &i, &c);
        if (ret != 1)
            return;
        str += c;
        update_vels(i, str);
    }
    else if (!strncmp(str, "  dist", 6)) {
        bzero(mob_dist_new, sizeof(mob_dist_new));
        update_mob_dist(str+6);
    }
    else if (!strncmp(str, "Pred:", 5)) {
        int dist;
        ret = sscanf(str, "Pred:%d,dist=%d", &i, &dist);
        if (ret != 2)
            return;
        pings_summary_new[max_nodes][i] = dist;
    }
}

void process_line_v1(char * str)
{
    int i;
    int id;
    float x, y;
    struct timeval tv;
    
    getcrickettime(&tv);
    printf("%d.%03d: %s\n", tv.tv_sec, tv.tv_usec/1000, str);

    if (!strncmp(str, "Localized ", 10)) {
        solve_read = 1;
        for (i=0; i<max_nodes; i++) {
            if (pos[i].stat & STAT_VALID) {
                if (pos[i].stat & STAT_MARKED) {
                    pos[i].stat = STAT_EMPTY;
                }
                else {
                    pos[i].stat |= STAT_MARKED;
                }
            }
        }
    }
    else if (!strncmp(str, "Optimized,", 10)) {
        opt_read = 1;
    }
    else if (!strcmp(str, "Localizing...")) {
        num_quads_new = 0;
    }
    else if (!strncmp(str, "Rigid Quad:", 11)) {
        int ids[3];
        sscanf(str, "Rigid Quad: %x %x %x", ids, ids+1, ids+2);
        for (i=0; i<3; i++)
            quads_new[num_quads_new].v[i] = node_find_v1(ids[i]);
        num_quads_new++;
    }
    else if (!strncmp(str, "  0x", 4)) {
        sscanf(str, "  0x%x: x=%f y=%f", &id, &x, &y);
        i = node_find_v1(id);
        if (i == -1)