glcricket.c

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

/*
 * glcricket.c
 * David Moore <[e-mail]>
 *
 * Visualization tool for Crickets running the robust quad localization
 * algorithm.  Displays their positions and debugging information about
 * the network.
 *
 * TODO: This code is a horrible mess.  A new architecture with
 * event-driven parsing is currently being worked on.
 *
 *
 * Copyright (C) 2004  Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */


#define __USE_ISOC99 1
 
#include <stdio.h>
#include <GL/glx.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <X11/extensions/xf86vmode.h>
#include <X11/keysym.h>
#include <png.h>


#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/time.h>
#include <string.h>
#include <math.h>
#include <stdarg.h>
#include <stdlib.h>

#define WIDTH   1280
#define HEIGHT  960

/* stuff about our window grouped together */
typedef struct {
    Display *dpy;
    int screen;
    Window win;
    GLXContext ctx;
    XSetWindowAttributes attr;
    Bool fs;
    Bool doubleBuffered;
    XF86VidModeModeInfo deskMode;
    int x, y;
    unsigned int width, height;
    unsigned int depth;    
} GLWindow;

#define STAT_EMPTY      0x0
#define STAT_VALID      0x1
#define STAT_VISIBLE    0x2
#define STAT_OPTIMIZED  0x4
#define STAT_MARKED     0x8
#define STAT_WEAK       0x10
#define STAT_MOVING     0x20
#define MAX_NODES 40
#define MAX_QUADS 500
struct hist_pos {
    float x, y;
    struct timeval time;
};
struct node_pos {
    int stat, newstat;
    float x, y;
    float last_x, last_y;
    float ox, oy;
    int id[4];
    char * name;
    struct hist_pos * hist;
    int hist_size;
    int hist_num;
    int vel_set;
    struct timeval vel_time;
    float vel_x, vel_y;
} pos[MAX_NODES+1];

struct quad_t {
    int v[3];
} quads[MAX_QUADS];
struct quad_t quads_new[MAX_QUADS];
int num_quads;
int num_quads_new;

struct timeval pings[MAX_NODES+1][MAX_NODES+1];
int pings_d[MAX_NODES+1][MAX_NODES+1];
int pings_summary[MAX_NODES+1][MAX_NODES+1];
int pings_summary_new[MAX_NODES+1][MAX_NODES+1];
int mob_dist[MAX_NODES+1][MAX_NODES+1];
int mob_dist_new[MAX_NODES+1];
int max_nodes = 40;

int num_truth = 0;
struct truth_pos {
    float x, y;
    char * name;
};
struct truth_pos * truth = NULL;

float fov = 100.0;
float target_fov = 100.0;
float origin_x = 0.0;
float target_ox = 0.0;
float origin_y = 0.0;
float target_oy = 0.0;
float ui_rot = 0.0;
float comp_rot = 0.0;
float trot = 0.0;
float aspect;
int ui_flip = 1;
int comp_flip = 1;
int show_quads = 0;
int show_pings = 1;
int show_all_history = 0;
int auto_zoom = 1;
int show_vertex = -1;
int summary_complete = 1;
int picking_edge = 0;
int edge_v[2];
int edge_picked = 0;
float opt_err;

int input_version = 2;
int do_cones = 0;

int do_rot = 0;
int do_flip = 0;
char rotname[16];
char flipname[16];
int rot_node = 0;
int flip_node = 0;
float last_rot = 0;
float last_cross = 0;

int paused = 0;
int pause_on_loc = 0;

int do_smoothing = 0;

void process_line_v1(char * str);
void process_line_v2(char * str);

void (*process_line)(char *) = process_line_v2;

static struct board_names {
    int id[4];
    int crc;
    char * name;
} names[] = {
    { {0x09, 0x9e, 0x84, 0xdb}, 0, "01" },
    { {0x09, 0x9e, 0x8e, 0xad}, 0, "02" },
    { {0x09, 0x9e, 0x88, 0x83}, 0, "06" },
    { {0x09, 0x9e, 0x9b, 0x96}, 0, "07" },
    { {0x09, 0x9e, 0x91, 0x26}, 0, "08" },
    { {0x09, 0x9e, 0xa5, 0x45}, 0, "10" },
    { {0x09, 0x9e, 0xb0, 0x6d}, 0x81, "11" },
    { {0x09, 0x9e, 0x9a, 0x3c}, 0xdd, "12" },
    { {0x09, 0x9e, 0x98, 0x80}, 0, "13" },
    { {0x09, 0x9e, 0xae, 0x73}, 0, "14" },
    { {0x09, 0x9e, 0x85, 0x2f}, 0, "15" },
    { {0x09, 0x9e, 0x9c, 0x63}, 0x4b, "16" },
    { {0x09, 0x9e, 0x94, 0xe1}, 0xf1, "17" },
    { {0x09, 0x9e, 0x9a, 0x01}, 0x7a, "18" },
    { {0x09, 0x9e, 0x88, 0xaf}, 0xca, "19" },
    { {0x09, 0x9e, 0x96, 0x6a}, 0x60, "20" },
    { {0x09, 0x9e, 0xb2, 0x3a}, 0, "21" },
    { {0x09, 0x9e, 0x8b, 0xd9}, 0, "22" },
    { {0x09, 0x9e, 0x86, 0xb8}, 0, "23" },
    { {0x09, 0x9e, 0x8f, 0x3b}, 0, "24" },
    { {0x09, 0x9e, 0x86, 0xc6}, 0, "25" },

    { {0x0a, 0x3c, 0x67, 0xfb}, 0, "030" },
    { {0x0a, 0x3c, 0x35, 0x30}, 0, "034" },


    { {0x09, 0x9e, 0x87, 0xee}, 0, "C" },
    { {0x09, 0x9e, 0x93, 0xdc}, 0, "E" },
    { {0x09, 0x9e, 0x9b, 0x74}, 0, "G" },
    { {0x09, 0x9e, 0xaa, 0x1e}, 0, "H" },
    { {0x09, 0x9e, 0x88, 0x2a}, 0xcb, "B03" },
    { {0x09, 0x9e, 0x90, 0x34}, 0, "9AL" },
    { {0x09, 0x9e, 0x8b, 0x99}, 0, "B08" },
    { {0x09, 0x9e, 0x8c, 0xfb}, 0, "B09" },
    { {0x09, 0x9e, 0xb5, 0x6f}, 0, "B07" },
    { {0x09, 0x9e, 0xa6, 0x23}, 0, "6A" },
    { {0x09, 0x9e, 0xac, 0x1f}, 0, "14P" },

    { {0x0a, 0x9e, 0x88, 0x83}, 0, "06b" },
    { {0x0a, 0x9e, 0x91, 0x26}, 0, "08b" },
    { {0x0a, 0x9e, 0x84, 0xdb}, 0, "01b" },
    { {0x0a, 0x9e, 0x8e, 0xad}, 0, "02b" },
    { {0x0a, 0x9e, 0x9b, 0x96}, 0, "07b" },
    { {0x0a, 0x9e, 0xa5, 0x45}, 0, "10b" },
    { {0x0a, 0x9e, 0xb0, 0x6d}, 0x81, "11b" },
    { {0x0a, 0x9e, 0x9a, 0x3c}, 0xdd, "12b" },
    { {0x0a, 0x9e, 0xae, 0x73}, 0, "14b" },
    { {0x0a, 0x9e, 0x85, 0x2f}, 0, "15b" },
    { {0x0a, 0x9e, 0x9c, 0x63}, 0x4b, "16b" },
    { {0x0a, 0x9e, 0x9a, 0x01}, 0x7a, "18b" },
    { {0x0a, 0x9e, 0x88, 0xaf}, 0xca, "19b" },
    { {0x0a, 0x9e, 0xb2, 0x3a}, 0, "21b" },
    { {0x0a, 0x9e, 0x8b, 0xd9}, 0, "22b" },
    { {0x0b, 0x9e, 0x84, 0xdb}, 0, "01c" },
    { {0x0b, 0x9e, 0x8e, 0xad}, 0, "02c" },
    { {0x0b, 0x9e, 0x88, 0x83}, 0, "06c" },
    { {0x0b, 0x9e, 0xae, 0x73}, 0, "14c" },
    { {0x0b, 0x9e, 0x9a, 0x01}, 0x7a, "18c" },
    { {0x0b, 0x9e, 0x88, 0xaf}, 0xca, "19c" },
    { {0x0c, 0x9e, 0x84, 0xdb}, 0, "01d" },
};
#define NUM_NAMES   (sizeof(names)/sizeof(struct board_names))

/* attributes for a single buffered visual in RGBA format with at least
 * 4 bits per color and a 16 bit depth buffer */
static int attrListSgl[] = {GLX_RGBA, GLX_RED_SIZE, 4, 
    GLX_GREEN_SIZE, 4, 
    GLX_BLUE_SIZE, 4, 
//    GLX_DEPTH_SIZE, 16,
    None};

/* attributes for a double buffered visual in RGBA format with at least
 * 4 bits per color and a 16 bit depth buffer */
static int attrListDbl[] = { GLX_RGBA, GLX_DOUBLEBUFFER, 
    GLX_RED_SIZE, 4, 
    GLX_GREEN_SIZE, 4, 
    GLX_BLUE_SIZE, 4, 
    GLX_ALPHA_SIZE, 8,
    GLX_DEPTH_SIZE, 16,
    None };

GLWindow GLWin;
GLuint base;
GLvoid glPrintf(int font, const char * fmt, ...);

int isrecorded = 0;
struct timeval crickettime;
struct timeval starttime;

void getcrickettime(struct timeval * tv)
{
    if (isrecorded) {
        tv->tv_sec = crickettime.tv_sec;
        tv->tv_usec = crickettime.tv_usec;
    }
    else
        gettimeofday(tv, NULL);
}

unsigned char outbuf[WIDTH*HEIGHT*3];

int write_png(char * filename, int w, int h, int s, unsigned char * buf)
{
  FILE *f=fopen(filename,"wb");
  int i;
  png_structp png_ptr;
  png_infop info_ptr;
  png_byte * row_pointers[h];

  if (!f)
    return -1;

  png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
  if (!png_ptr)
    return -1;

  png_set_compression_level(png_ptr, 2);
  info_ptr = png_create_info_struct(png_ptr);
  if (!png_ptr)
    {
      png_destroy_write_struct(&png_ptr, NULL);
      return -1;
    }
  
  png_init_io(png_ptr,f);
  png_set_IHDR(png_ptr, info_ptr, w, h, 8, PNG_COLOR_TYPE_RGB,
               PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT,
               PNG_FILTER_TYPE_DEFAULT);
  
  for (i=0; i<h; i++)
    {
      row_pointers[i] = buf + (h-i-1)*s;
    }
  png_set_rows(png_ptr, info_ptr, row_pointers);
  png_write_png(png_ptr, info_ptr, PNG_TRANSFORM_IDENTITY, NULL);
  png_write_end(png_ptr, info_ptr);
  png_destroy_write_struct(&png_ptr, &info_ptr);
  fclose(f);
  return 0;
}


void write_frame(char * filename)
{
    glReadPixels(0, 0, GLWin.width, GLWin.height,
            GL_RGB, GL_UNSIGNED_BYTE, outbuf);
    write_png(filename, GLWin.width, GLWin.height, GLWin.width*3, outbuf);
}

/* function called when our window is resized (should only happen in window mode) */
void resizeGLScene(unsigned int width, unsigned int height)
{
    unsigned int size;
    if (height == 0)    /* Prevent A Divide By Zero If The Window Is Too Small */
        height = 1;

    size = (width<height)?width:height;
   // glViewport((width-size)/2, (height-size)/2, size, size);    /* Reset The Current Viewport And Perspective Transformation */
    glViewport(0, 0, width, height);    /* Reset The Current Viewport And Perspective Transformation */
    aspect = width/(float)height;
    //glMatrixMode(GL_PROJECTION);
    //glLoadIdentity();
    //gluPerspective(45.0f, (GLfloat)width / (GLfloat)height, 0.1f, 100.0f);
    //glMatrixMode(GL_MODELVIEW);
}

GLvoid buildFont(GLvoid)
{
    XFontStruct *font1, *font2;
    base = glGenLists(192);
    font1 = XLoadQueryFont(GLWin.dpy, "-*-helvetica-bold-r-normal--34-*-*-*-p-*-iso8859-1");
    font2 = XLoadQueryFont(GLWin.dpy, "-*-courier-bold-r-normal--34-*-*-*-m-*-iso8859-1");
    if (font1 == NULL) {
        font1 = XLoadQueryFont(GLWin.dpy, "fixed");
        if (font1 == NULL) {
            fprintf(stderr, "Could not load font\n");
            exit(1);
        }
    }

    glXUseXFont(font1->fid, 32, 96, base);
    glXUseXFont(font2->fid, 32, 96, base+96);
    XFreeFont(GLWin.dpy, font1);
    XFreeFont(GLWin.dpy, font2);
}

GLvoid deleteFont(GLvoid)
{
    glDeleteLists(base, 96);
}

/* general OpenGL initialization function */
int initGL(GLvoid)
{
    //glShadeModel(GL_SMOOTH);
    glClearColor(0.0, 0.0, 0.0, 0.0);
//    glClearDepth(1.0f);
//    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//    glEnable(GL_POLYGON_SMOOTH);
    glEnable(GL_LINE_SMOOTH);
    glEnable(GL_POINT_SMOOTH);
    //glDepthFunc(GL_LEQUAL);
    //glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
    /* we use resizeGLScene once to set up our initial perspective */
    buildFont();
    resizeGLScene(GLWin.width, GLWin.height);
    glFlush();
    return True;
}

void init_pos()
{
    int i;
    for (i=0; i<max_nodes; i++) {
        pos[i].stat = STAT_EMPTY;
        pos[i].newstat = pos[i].stat;
    }
    pos[max_nodes].stat = STAT_VALID | STAT_VISIBLE;
    pos[max_nodes].newstat = pos[max_nodes].stat;
    pos[max_nodes].x = 0;
    pos[max_nodes].y = 0;
    pos[max_nodes].last_x = 0;
    pos[max_nodes].last_y = 0;
    pos[max_nodes].name = "Unknown";

    num_quads = 0;
}

float sec_diff(struct timeval t1, struct timeval t2)
{
    return (t1.tv_sec-t2.tv_sec) + ((float)t1.tv_usec-(float)t2.tv_usec)/1000000.0;
}

void draw_arc(float x, float y, float r, float th1, float th2)
{
    float th;
    for (th=th1; th < th2; th += 0.02) {
        glVertex3f(x + r*cos(th),
                y + r*sin(th), 0.0);
    }
    glVertex3f(x + r*cos(th2),
            y + r*sin(th2), 0.0);
}

void draw_node_ping(int i, int j, struct timeval * pt, int meas)
{
    struct timeval tv;
    float diff, dist, xdif, ydif, dist2;
    float as = 4.0;
    float spc = 6.5;

    getcrickettime(&tv);
    if (!(pos[j].stat & STAT_VISIBLE) ||
            !(pos[i].stat & STAT_VISIBLE) || meas == 0)
        return;
    if (pt) {
        if (pt->tv_sec == 0)
            return;
        diff = sec_diff(tv, *pt);
        if (diff >= 1.0) {
            pt->tv_sec = 0;
            return;
        }
        glColor4f(1.0, 1.0, 0.0, 1.0-diff);
    }
    else if (meas == 1) {
        glColor4f(1.0, 0.0, 0.0, 1.0);
    }
    else {
        glColor4f(0.0, 1.0, 0.0, 1.0);
    }
    glLineWidth(4.0);
    xdif = pos[j].x - pos[i].x;
    ydif = pos[j].y - pos[i].y;
    dist = sqrt(xdif*xdif+ydif*ydif);
    dist2 = meas/30.0;
    if (dist2-dist > -spc)
        dist2 = dist - spc;
//        glVertex3f(pos[j].x - xdif*4.0/dist - ydif*2.0/dist,
//                pos[j].y - ydif*4.0/dist + xdif*2.0/dist, 0.0);
    if (dist2 > spc) {
        glBegin(GL_LINE_STRIP);
        glVertex3f(pos[i].x + xdif*dist2/dist - ydif*as/dist,
                pos[i].y + ydif*dist2/dist + xdif*as/dist, 0.0);
        glVertex3f(pos[i].x + xdif*spc/dist - ydif*as/dist,
                pos[i].y + ydif*spc/dist + xdif*as/dist, 0.0);
        glEnd();
        glBegin(GL_TRIANGLES);
        glVertex3f(pos[i].x + xdif*spc/dist - ydif*as/dist,
                pos[i].y + ydif*spc/dist + xdif*as/dist, 0.0);
        glVertex3f(pos[i].x + xdif*(spc+2*as)/dist - ydif*2*as/dist,
                pos[i].y + ydif*(spc+2*as)/dist + xdif*2*as/dist, 0.0);