draw.c

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

/***************************************************************************
                          draw.c  -  simplistic drawing module
                             -------------------
    begin                : Sun Dec 21 2003
    version              : 0.1    
    copyright            : (C) 2005 by Albert Diosi and Lindsay Kleeman
    email                : [e-mail]
    change:           -14/03/2005 rewriting to use only X, instead of libplot
                            however, markers are not implemented! assumes equal
                            pixel size! saving of window as png added -slow though
                            added markers(circle,triangle and cross)
 ***************************************************************************/
 /****************************************************************************
    This file is part of polar_matching.

    polar_matching 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.

    polar_matching 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 polar_matching; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
****************************************************************************/

#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <math.h>
#include <X11/Xlib.h>
#include <X11/Xutil.h>


#define DR_BORDER_WIDTH 2

int handle;

#define LINE    1
#define MARKER  2
#define TEXT    3
#define CIRCLE  4

/*markers are not scaled*/
#define DR_MARKER_CIRCLE   1
#define DR_MARKER_TRIANGLE 2
#define DR_MARKER_CROSS    3

#define DR_MARKER_SIZE   3 //pixels


#define MAX_OBJECTS 200000 //maximum nuber of lines+markers+texts

/* Program wide globals */
Display       *dr_Display;
int           dr_Screen;
int           dr_Depth;
unsigned long dr_BlackPixel;
unsigned long dr_WhitePixel;
Window        dr_Window;//window id
GC            dr_GC;
Colormap      dr_Colormap;

char* dr_COLORS[] = {"red","green","blue","gray","yellow","magenta","pink","lightgray","brown"};
int   dr_COLORS_CNT = 9;

struct Axis{
  double xmin,ymin,xmax,ymax;
} axis; //the area represented buy the window
double dr_xm,dr_ym;//screen size in pixels - has to be updatet
double dr_kx,dr_ky;// kx=dr_xm/(axis.xmax-axis.xmin)

//transforms x coordinates from the
int inline dr_dispx(double x)
{
  return (int)((x-axis.xmin)*dr_kx);
}
int inline dr_dispy(double y)
{
  return (int)(dr_ym-(y-axis.ymin)*dr_ky);
}

struct Line{ int t;  double x1,y1,x2,y2;  char color[100];  };
struct Circle{int t;  double x,y,r;  char color[100];  };
struct Marker{int t;double x,y;char color[100];int type;};
struct Text {int t;double x,y;char text[1000];int hz_al,vr_al;char color[100];};
//t is the type

union Obj {struct Line l;struct Marker m;struct Text t;struct Circle c;};

union Obj obj[MAX_OBJECTS];//stores lines, texts ...
int  obj_nmb;//number of objects stored

int dr_axes_equal=1;

//making dx equal to dy
//dr_xm, dr_ym i.e. the window size must be updated prior calling this function
void axisEqual(double *xmin,double* ymin,double* xmax,double *ymax)
{

  double dx=(*xmax-*xmin)/2.0;
  double dy=(*ymax-*ymin)/2.0;
  double cx=(*xmax+*xmin)/2.0;
  double cy=(*ymax+*ymin)/2.0;

  XWindowAttributes win_attr;
  XGetWindowAttributes(dr_Display, dr_Window, &win_attr);
  dr_xm = win_attr.width;
  dr_ym = win_attr.height;
  if(dr_axes_equal)
    if(dx>dy)
    {
      *ymax = cy + (dx*dr_ym)/dr_xm;
      *ymin = cy - (dx*dr_ym)/dr_xm;
    }
    else
    {
      *xmax = cx + (dy*dr_xm)/dr_ym;
      *xmin = cx - (dy*dr_xm)/dr_ym;
    }
  dr_kx = dr_xm/(axis.xmax-axis.xmin);
  dr_ky = dr_ym/(axis.ymax-axis.ymin);
}

void draw_grid(void)
{
  int     n,nx,ny;//how many intervals in each direction
  int     i;
  double  dx = (axis.xmax-axis.xmin);
  double  dy = (axis.ymax-axis.ymin);
  double  dinc,dincx,dincy,d;
  char s[1000];
  char *str,*strx, *stry;

  d   = dx;
  dinc= 1;
  n   = d/1.0;
  str = (char *)"1 cm";
  if(n>30)
  {  
    n=d/10.0;
    dinc=10;
    str = (char *)"10 cm";
    if(n>30)
    {
      n=d/100.0;
      dinc=100;
      str = (char *)"1 m";
      if(n>30)
      {
        n=d/100.0;
        dinc=1000;
        str = (char *)"10 m";
      }
    }
  }
  dincx = dinc;
  nx = n;
  strx = str;

  if(!dr_axes_equal)
  {
      d   = dy;
      dinc= 1;
      n   = d/1.0;
      str = (char *)"1 cm";
      if(n>30)
      {
        n=d/10.0;
        dinc=10;
        str = (char *)"10 cm";
        if(n>30)
        {
          n=d/100.0;
          dinc=100;
          str = (char *)"1 m";
          if(n>30)
          {
            n=d/100.0;
            dinc=1000;
            str = (char *)"10 m";
          }
        }
      }
  }
  dincy = dinc;
  ny = n;
  stry = str;
    
  //choose color for the mesh                
  XColor col;
//  XParseColor(dr_Display, dr_Colormap, "gray40", &col); //the original
  XParseColor(dr_Display, dr_Colormap, "black", &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);

  double xstart = (int)(axis.xmin/dincx)*dincx;
  double ystart = (int)(axis.ymin/dincy)*dincy;
  for(i=0;i<=nx;i++)
  {
    XDrawLine(dr_Display,dr_Window,dr_GC,dr_dispx(xstart+i*dincx),
              dr_dispy(axis.ymin),dr_dispx(xstart+i*dincx),dr_dispy(axis.ymax));
    //putting the numbers to the axes
    sprintf(s,"%.0f",xstart+i*dincx);
    XDrawString(dr_Display, dr_Window, dr_GC, dr_dispx(xstart+i*dincx),
      dr_dispy(axis.ymin), s,strlen(s));
  }
  for(i=0;i<=ny;i++)
  {
    XDrawLine(dr_Display,dr_Window,dr_GC,
        dr_dispx(axis.xmin),dr_dispy(ystart+i*dincy),
        dr_dispx(axis.xmax),dr_dispy(ystart+i*dincy));//horizontal
    //putting the numbers to the axes

    sprintf(s,"%.0f",ystart+i*dincy);
    XDrawString(dr_Display, dr_Window, dr_GC, dr_dispx(axis.xmin),
      dr_dispy(ystart+i*dincy), s,strlen(s));
  }
  //drawing the main axes
  XParseColor(dr_Display, dr_Colormap, /*"gray8"*/"black", &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);
  
  if(axis.xmin<=0 && axis.xmax>=0)
    XDrawLine(dr_Display,dr_Window,dr_GC,dr_dispx(0),dr_dispy(axis.ymin),
        dr_dispx(0),dr_dispy(axis.ymax));
  if(axis.ymin<=0 && axis.ymax>=0)
    XDrawLine(dr_Display,dr_Window,dr_GC,dr_dispx(axis.xmin),dr_dispy(0),
        dr_dispx(axis.xmax),dr_dispy(0));
}

int createGC(Window theNewWindow,GC *theNewGC){
        XGCValues theGCValues;
        *theNewGC = XCreateGC(dr_Display,theNewWindow,(unsigned long) 0,&theGCValues);
        if(*theNewGC == 0)
                return 0;
        else{
                XSetForeground(dr_Display,*theNewGC,dr_WhitePixel);
                XSetBackground(dr_Display,*theNewGC,dr_BlackPixel);
                return 1;
        }
}


Window OpenWindow(int x, int y, int width, int height, int flag,GC *theNewGC){
        XSetWindowAttributes theWindowAttributes;
        unsigned long theWindowMask;
        XSizeHints theSizeHints;
        XWMHints theWMHints;
        Window theNewWindow;


        /*Setting the attributes*/
        theWindowAttributes.border_pixel
                =BlackPixel(dr_Display,dr_Screen);
        theWindowAttributes.background_pixel
                = WhitePixel(dr_Display,dr_Screen);
        theWindowAttributes.override_redirect = False;



        theWindowMask = CWBackPixel|CWBorderPixel|CWOverrideRedirect;


        theNewWindow = XCreateWindow( dr_Display,
                        RootWindow(dr_Display,dr_Screen),
                        x,y,width,height,
                        DR_BORDER_WIDTH,dr_Depth,
                        InputOutput,
                        CopyFromParent,
                        theWindowMask,
                        &theWindowAttributes);


        theWMHints.initial_state = NormalState;
        theWMHints.flags = StateHint;


        XSetWMHints(dr_Display,theNewWindow,&theWMHints);

        theSizeHints.flags = PPosition | PSize;
        theSizeHints.x = x;
        theSizeHints.y = y;
        theSizeHints.width = width;
        theSizeHints.height = height;


        XSetNormalHints(dr_Display,theNewWindow,&theSizeHints);


        if( createGC(theNewWindow,theNewGC) == 0){
                XDestroyWindow(dr_Display,theNewWindow);
                return( (Window) 0);
                }


        XMapWindow(dr_Display,theNewWindow);
        XFlush(dr_Display);

        return theNewWindow;
}


//screen_y_max,screen_x_max ... size of window in pixels
//xmin, ymin, xmax, ymax the displayed area in measurement space
void dr_init(int screen_x_max,int screen_y_max,double xmin, double ymin, double xmax, double ymax)
{
    int i;
    dr_Display = XOpenDisplay(NULL);

    dr_Screen = DefaultScreen(dr_Display);
    dr_Depth = DefaultDepth(dr_Display,dr_Screen);
    dr_BlackPixel = WhitePixel(dr_Display,dr_Screen);
    dr_WhitePixel = BlackPixel(dr_Display,dr_Screen);
    dr_Colormap  = DefaultColormap(dr_Display,dr_Screen);

    dr_Window = OpenWindow(0,0,screen_x_max, screen_y_max,0,&dr_GC);


    axis.xmin=xmin; axis.xmax=xmax; axis.ymin=ymin; axis.ymax=ymax;
    axisEqual(&axis.xmin, &axis.ymin, &axis.xmax, &axis.ymax);

    draw_grid();
    obj_nmb=0;

//    printf("screen %i Window %i\n",dr_Screen,dr_Window);

}
void dr_close(void)
{
    XDestroyWindow(dr_Display,dr_Window);
}

void dr_line(double x1, double y1, double x2, double y2,char *color)
{
  XColor col;
  XParseColor(dr_Display, dr_Colormap, color, &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);
  
  XDrawLine(dr_Display,dr_Window,dr_GC,
                  dr_dispx(x1),dr_dispy(y1),
                  dr_dispx(x2),dr_dispy(y2));
  XFlush(dr_Display);
                  
  //struct Line{ int t;  double x1,y1,x2,y2;  char* color;  };

  obj[obj_nmb].l.t = LINE;
  obj[obj_nmb].l.x1 = x1;
  obj[obj_nmb].l.y1 = y1;
  obj[obj_nmb].l.x2 = x2;
  obj[obj_nmb].l.y2 = y2;
  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--;
  }
}
//r is the radius!
void dr_circle(double x, double y, double r,char *color)
{ int i;
  XColor col;

  XParseColor(dr_Display, dr_Colormap, color, &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);

  if( (2.0*r*dr_kx)>2 &&  (2.0*r*dr_ky)>2 ) // check if larger than 2 pixels
    XDrawArc(dr_Display, dr_Window, dr_GC,dr_dispx(x-r),
             dr_dispy(y+r), 2.0*r*dr_kx, 2.0*r*dr_ky, 0, 23040);
  else
    XDrawPoint(dr_Display, dr_Window, dr_GC,dr_dispx(x-r),dr_dispy(y+r));
  

  XFlush(dr_Display);
  
  
  obj[obj_nmb].c.t = CIRCLE;
  obj[obj_nmb].c.x = x;
  obj[obj_nmb].c.y = y;
  obj[obj_nmb].c.r = r;
  strcpy(obj[obj_nmb].c.color,color);
  obj_nmb++;
  if(obj_nmb>=MAX_OBJECTS)
  {
      fprintf (stderr, "draw.c: Max number of stored objects reached");
      obj_nmb--;
  }
}
// plotting a cov ellipse by breaking it up into line segments
// expensive a bit
// parts lifted from lindsays kzoom
void dr_cov_ellipse(double x, double y, double cxx,double cxy,double cyy,char *color)
{
  double angle_major_rad, major_stdev, minor_stdev;
  XColor col;

  XParseColor(dr_Display, dr_Colormap, color, &col);
  XAllocColor(dr_Display, dr_Colormap, &col);
  XSetForeground(dr_Display, dr_GC, col.pixel);