draw.c

利用极坐标系统对机器人所携带的激光传感器所测得的数据进行匹配

  
  double discrim, lambda;
  // first find largest eigenvalue
  discrim = (cxx- cyy)*(cxx- cyy)+4.0*cxy*cxy;
  if (discrim > 0)  // rounding errors can result in < 0
    discrim = sqrt(discrim)/2.0;
  else (discrim = 0.0);

  lambda = (cxx+cyy)/2.0 + discrim;
  if (cxy != 0)
    angle_major_rad = atan2(cxx-lambda, -cxy);
  else angle_major_rad = 0.0;
  major_stdev = sqrt(lambda);
  lambda = (cxx+cyy)/2.0 - discrim;
  if (lambda > 0)
    minor_stdev = sqrt(lambda);
  else
    minor_stdev = 0.0;


  const int ANGLE_DIVISIONS = 12;
  const double angle_inc = M_PI*2.0/(ANGLE_DIVISIONS-1.0);
  const double c = cos(angle_major_rad), s = sin(angle_major_rad);
  int i;

  float last_x = major_stdev*c+x, last_y=major_stdev*s+y;
  for (i=1; i<ANGLE_DIVISIONS; i++)
  {
    double cosa = cos(i*angle_inc), sina = sin(i*angle_inc);
    double xp = major_stdev*cosa*c-minor_stdev*sina*s + x;
    double yp = major_stdev*cosa*s+minor_stdev*sina*c + y;

    XDrawLine(dr_Display,dr_Window,dr_GC,
                  dr_dispx(last_x),dr_dispy(last_y),
                  dr_dispx(xp),dr_dispy(yp));
    
  //struct Line{ int t;  double x1,y1,x2,y2;  char* color;  };

    obj[obj_nmb].l.t = LINE;
    obj[obj_nmb].l.x1 = last_x;
    obj[obj_nmb].l.y1 = last_y;
    obj[obj_nmb].l.x2 = xp;
    obj[obj_nmb].l.y2 = yp;
    strcpy(obj[obj_nmb].l.color,color);
    obj_nmb++;
    if(obj_nmb>=MAX_OBJECTS)
    {
        fprintf (stderr, "draw.c: Max number of stored objects reached");
        obj_nmb--;
    }
    last_x = xp; last_y = yp;
  } // for
  XFlush(dr_Display);
  
}

void dr_marker(double x, double y, int type,char *color)
{
//  pl_pencolorname (color);
//  pl_fmarker(x,y,type,-1);
//struct Marker{int t;double x,y;char color[100];int type;double size;};



  obj[obj_nmb].m.t = MARKER;
  obj[obj_nmb].m.x = x;
  obj[obj_nmb].m.y = y;
  obj[obj_nmb].m.type = type;
  strcpy(obj[obj_nmb].m.color,color);
  obj_nmb++;
  if(obj_nmb>=MAX_OBJECTS)
  {
      fprintf (stderr, "draw.c: Max number of stored objects reached");
      obj_nmb--;
  }
}
//hz_al,int vr_al are not in use!
void dr_text(double x, double y, int hz_al,int vr_al,char *text, char* color)
{
  XColor col;
  XParseColor(dr_Display, dr_Colormap, color, &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);

  XDrawString(dr_Display, dr_Window, dr_GC,dr_dispx(x),
  dr_dispy(y), text,strlen(text));

//struct Text {int t;double x,y;char *text;int hz_al,vr_al;char color[100];};
  obj[obj_nmb].t.t = TEXT ;
  obj[obj_nmb].t.x = x;
  obj[obj_nmb].t.y = y;
  obj[obj_nmb].t.hz_al =hz_al ;
  obj[obj_nmb].t.vr_al = vr_al;
  strcpy(obj[obj_nmb].t.color,color);
  strcpy(obj[obj_nmb].t.text,text);
  obj_nmb++;
  if(obj_nmb>=MAX_OBJECTS)
  {
      fprintf (stderr, "draw.c: Max number of stored objects reached");
      obj_nmb--;
  }
  XFlush(dr_Display);
}

//temporary code to output text in screen coordinates
void dr_text_pix(int px, int py,char *text, char* color)
{
  XColor col;
  XParseColor(dr_Display, dr_Colormap, color, &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);

  XDrawString(dr_Display, dr_Window, dr_GC,px,py,text,strlen(text));

  XFlush(dr_Display);
}
void dr_replot(void)
{
  int i;
  XColor col;
  double x,y,x1,x2,x3,y1,y2,y3;


  XParseColor(dr_Display, dr_Colormap, "white", &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);
  XFillRectangle(dr_Display, dr_Window, dr_GC, 0, 0, (int)dr_xm, (int)dr_ym);

  draw_grid();

  for(i=0;i<obj_nmb;i++)
  {
    
    switch(obj[i].l.t){
      case LINE:
          XParseColor(dr_Display, dr_Colormap, obj[i].l.color, &col);
          XAllocColor(dr_Display, dr_Colormap, &col);
          XSetForeground(dr_Display, dr_GC, col.pixel);
      
          XDrawLine(dr_Display,dr_Window,dr_GC,
                  dr_dispx(obj[i].l.x1),dr_dispy(obj[i].l.y1),
                  dr_dispx(obj[i].l.x2),dr_dispy(obj[i].l.y2));

        break;
      case MARKER:
          XParseColor(dr_Display, dr_Colormap, obj[i].m.color, &col);
          XAllocColor(dr_Display, dr_Colormap, &col);
          XSetForeground(dr_Display, dr_GC, col.pixel);
          switch(obj[i].m.type)
          {
            case DR_MARKER_CIRCLE:            
               XDrawArc(dr_Display, dr_Window, dr_GC,dr_dispx(obj[i].m.x)-DR_MARKER_SIZE,
                 dr_dispy(obj[i].m.y)-DR_MARKER_SIZE, 2.0*DR_MARKER_SIZE, 2.0*DR_MARKER_SIZE, 0, 23040);            
              break;
            case DR_MARKER_TRIANGLE:
              x = dr_dispx(obj[i].m.x);
              y = dr_dispy(obj[i].m.y);
              x1 = x;
              y1 = y-DR_MARKER_SIZE;
              x2 = x-DR_MARKER_SIZE;
              y2 = y+DR_MARKER_SIZE;
              x3 = x+DR_MARKER_SIZE;
              y3 = y+DR_MARKER_SIZE;
              XDrawLine(dr_Display,dr_Window,dr_GC,x1,y1,x2,y2);
              XDrawLine(dr_Display,dr_Window,dr_GC,x2,y2,x3,y3);
              XDrawLine(dr_Display,dr_Window,dr_GC,x3,y3,x1,y1);
              break;            
            case DR_MARKER_CROSS:
              x = dr_dispx(obj[i].m.x);
              y = dr_dispy(obj[i].m.y);
              XDrawLine(dr_Display,dr_Window,dr_GC,x,y-DR_MARKER_SIZE,x,y+DR_MARKER_SIZE);
              XDrawLine(dr_Display,dr_Window,dr_GC,x-DR_MARKER_SIZE,y,x+DR_MARKER_SIZE,y);
              break;            
          }
        break;
      case TEXT:
          XParseColor(dr_Display, dr_Colormap, obj[i].t.color, &col);
          XAllocColor(dr_Display, dr_Colormap, &col);
          XSetForeground(dr_Display, dr_GC, col.pixel);
          XDrawString(dr_Display, dr_Window, dr_GC,dr_dispx(obj[i].t.x),
          dr_dispy(obj[i].t.y), obj[i].t.text,strlen(obj[i].t.text));
        break;
      case CIRCLE:
          if(axis.xmin<obj[i].c.x && axis.xmax>obj[i].c.x
          && axis.ymin<obj[i].c.y && axis.ymax>obj[i].c.y)
          {
             XParseColor(dr_Display, dr_Colormap, obj[i].c.color, &col);
             XAllocColor(dr_Display, dr_Colormap, &col);
             XSetForeground(dr_Display, dr_GC, col.pixel);

             if(obj[i].c.r*dr_ky>1)
             {
               XDrawArc(dr_Display, dr_Window, dr_GC,dr_dispx(obj[i].c.x-obj[i].c.r),
               dr_dispy(obj[i].c.y+obj[i].c.r), 2.0*obj[i].c.r*dr_kx, 2.0*obj[i].c.r*dr_ky, 0, 23040);
             }
             else
               XDrawPoint(dr_Display, dr_Window, dr_GC,dr_dispx(obj[i].c.x),dr_dispy(obj[i].c.y) );
             
          }
        break;        
    }
  }
  XFlush(dr_Display);
}


void dr_scale(double xmin, double ymin, double xmax, double ymax)
{
  axisEqual(&xmin, &ymin, &xmax, &ymax);
  axis.xmin=xmin;axis.xmax=xmax;axis.ymin=ymin;axis.ymax=ymax;
  dr_replot();
}
void dr_erase(void)
{
  XColor col;

  XParseColor(dr_Display, dr_Colormap, "white", &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);
  XFillRectangle(dr_Display, dr_Window, dr_GC, 0, 0, (int)dr_xm, (int)dr_ym);
  obj_nmb=0;
  draw_grid();  
}

static struct termios tty_orig;

void RestoreTty(void)
{
        tcsetattr(fileno(stdin), TCSANOW, &tty_orig);
//        printf("restored original settings\n");
}//RestoreTty

int Setcbreak (void)
{
     struct termios tty_set;
     int STDIN_F = fileno ( stdin );
     if ( tcgetattr ( STDIN_F, &tty_orig )) return -1;
     atexit(RestoreTty); // clean up when finished
     tty_set = tty_orig;
//     tty_set.c_lflag &= ~ICANON & ~ECHO;
     tty_set.c_lflag &= ~ICANON;
     tty_set.c_cc[VMIN] = 1;
     tty_set.c_cc[VTIME] = 0;
     return tcsetattr ( STDIN_F, TCSANOW, &tty_set );
} //Setcbreak

void dr_zoom(void)//interactive zooming until enter is hit
{
  int ch=0;
  double dx,dy,cx,cy;
  static int first=1;
  struct Axis oldax = axis;
  if(first)
  { first=0;
    Setcbreak();
  }
  while(ch!='q')
  {
          ch = getchar();          
          switch(ch)
          {
            case '6': // right
                  dx = axis.xmax-axis.xmin;
                  axis.xmax +=  dx/4.0;
                  axis.xmin +=  dx/4.0;
                     break;
            case '4': //left 
                  dx = axis.xmax-axis.xmin;
                  axis.xmax -=  dx/4.0;
                  axis.xmin -=  dx/4.0;
                     break;
            case '2': //down
                  dy = axis.ymax-axis.ymin;
                  axis.ymax -=  dy/4.0;
                  axis.ymin -=  dy/4.0;
                     break;
            case '8': // up
                  dy = axis.ymax-axis.ymin;
                  axis.ymax +=  dy/4.0;
                  axis.ymin +=  dy/4.0;
                     break;
            case '9':
                  dx = axis.xmax-axis.xmin;
                  dy = axis.ymax-axis.ymin;
                  cx = (axis.xmax+axis.xmin)/2.0;
                  cy = (axis.ymax+axis.ymin)/2.0;
                  axis.xmin = cx-dx/3.6;
                  axis.xmax = cx+dx/3.6;
                  axis.ymin = cy-dy/3.6;
                  axis.ymax = cy+dy/3.6;
                break;
            case '3':
                  dx = axis.xmax-axis.xmin;
                  dy = axis.ymax-axis.ymin;
                  cx = (axis.xmax+axis.xmin)/2.0;
                  cy = (axis.ymax+axis.ymin)/2.0;
            
                  axis.xmin = cx-dx*0.7;
                  axis.xmax = cx+dx*0.7;
                  axis.ymin = cy-dy*0.7;
                  axis.ymax = cy+dy*0.7;
                break;
            case 'c' : axis=oldax;
              break;
                                              
            case ' ':case 'q': case 10: // abort
                      ch ='q';
                     break;
          } // switch
          if(ch!='q')
          {
            axisEqual(&axis.xmin, &axis.ymin, &axis.xmax, &axis.ymax);
            dr_replot();
          }
   }//while
}//void dr_zoom(void)//interactive zooming until enter is hit

void dr_fit(void)//scale so that everything fits
{
  int i;
  axis.xmin = 1000000;
  axis.xmax = -10000000;
  axis.ymin = 1000000;
  axis.ymax = -10000000;

    
  for(i=0;i<obj_nmb;i++)
  {
    switch(obj[i].l.t){
      case LINE:
          if(obj[i].l.x1 <axis.xmin)
            axis.xmin = obj[i].l.x1;
          if(obj[i].l.x1 >axis.xmax)
            axis.xmax = obj[i].l.x1;
          if(obj[i].l.y1 <axis.ymin)
            axis.ymin = obj[i].l.y1;
          if(obj[i].l.y1 >axis.ymax)
            axis.ymax = obj[i].l.y1;
            
          if(obj[i].l.x2 <axis.xmin)
            axis.xmin = obj[i].l.x2;
          if(obj[i].l.x2 >axis.xmax)
            axis.xmax = obj[i].l.x2;
          if(obj[i].l.y2 <axis.ymin)
            axis.ymin = obj[i].l.y2;
          if(obj[i].l.y2 >axis.ymax)
            axis.ymax = obj[i].l.y2;
         break;
      case MARKER:
          if(obj[i].m.x <axis.xmin)
            axis.xmin = obj[i].m.x;
          if(obj[i].m.x >axis.xmax)
            axis.xmax = obj[i].m.x;
          if(obj[i].m.y <axis.ymin)
            axis.ymin = obj[i].m.y;
          if(obj[i].m.y >axis.ymax)
            axis.ymax = obj[i].m.y;
        break;
      case TEXT:
          if(obj[i].t.x <axis.xmin)
            axis.xmin = obj[i].t.x;
          if(obj[i].t.x >axis.xmax)
            axis.xmax = obj[i].t.x;
          if(obj[i].t.y <axis.ymin)
            axis.ymin = obj[i].t.y;
          if(obj[i].t.y >axis.ymax)
            axis.ymax = obj[i].t.y;
        break;
      case CIRCLE:
          if(obj[i].c.x <axis.xmin)
            axis.xmin = obj[i].c.x;
          if(obj[i].c.x >axis.xmax)
            axis.xmax = obj[i].c.x;
          if(obj[i].c.y <axis.ymin)
            axis.ymin = obj[i].c.y;
          if(obj[i].c.y >axis.ymax)
            axis.ymax = obj[i].c.y;
        break;
    }//switch
  }//for
  double dx,dy;
  dx= axis.xmax-axis.xmin;
  dy= axis.ymax-axis.ymin;
  axis.xmin -= dx/20.0;
  axis.xmax += dx/20.0;
  axis.ymin -= dy/20.0;
  axis.ymax += dy/20.0;
  
  axisEqual(&axis.xmin, &axis.ymin, &axis.xmax, &axis.ymax);
  dr_replot();
}

void dr_save(char *filename)//save screen into a file
{
  char s[1000];
//  sprintf(s," xwd -id %i -out %s -nobdrs -silent ",dr_Window,filename);
  sprintf(s," xwd -id %i -nobdrs -silent | xwdtopnm | pnmtopng > %s",dr_Window,filename);
  if(system(s)==-1)
   printf("dr_save: ERROR could not save\n");
}

void dr_equal(int equal)//true -> equal axes, false->non equal axes
{
  dr_axes_equal = equal;
}