image.cc

一个机器人平台

  }
  
  fclose(file);
  return true;
}


// Read in a polyline figure
bool Nimage::load_fig_polyline(FILE *file, char *line, int size, double scale)
{
  // Read object header and discard most of it  
  strtok(line, " \t");
  for (int i = 0; i < 14; i++)
    strtok(NULL, " \t");
  int npoints = atoi(strtok(NULL, " \t"));

  for (int i = 0; i < npoints;)
  {
    if (fgets(line, size, file) == NULL)
      break;
    if (line[0] == '#')
      continue;

    char *t1, *t2;
    int ax=0, ay=0, bx=0, by=0;
    
    t1 = strtok(line, " \t\n");
    while (i < npoints)
    {
      t2 = strtok(NULL, " \t\n");      
      bx = (int) (atoi(t1) * scale);
      by = (int) (atoi(t2) * scale);
      i++;
      if (i >= 2)
          draw_line(ax, ay, bx, by, 1);
      ax = bx;
      ay = by;
      t1 = strtok(NULL, " \t\n");
      if (!t1)
          break;
    }
  }

  return true;
}


void Nimage::draw_big(void)
{
  N_draw_big = true;
}

void Nimage::draw_small(void)
{
  N_draw_big = false;
}

void Nimage::draw_circle(int x,int y,int r,unsigned char c)
{
  double i,cx,cy;
  int x1,y1,x2,y2;
  
  x1=x;y1=y+r;	
  
  for (i=0;i<2.0*M_PI;i+=0.1)
    {
      cx = (double) x + (double (r) * sin(i));
      cy = (double) y + (double (r) * cos(i));
      x2=(int)cx;y2=(int)cy;
      draw_line(x1,y1,x2,y2,c);
      x1=x2;y1=y2;
    }	
  
  draw_line(x1,y1,x,y+r,c);
}

void Nimage::draw_rect( const Rect t, unsigned char c)
{
  draw_line( t.toplx, t.toply, t.toprx, t.topry, c );
  draw_line( t.toprx, t.topry, t.botlx, t.botly, c );
  draw_line( t.botlx, t.botly, t.botrx, t.botry, c );
  draw_line( t.botrx, t.botry, t.toplx, t.toply, c );
}


void Nimage::draw_line(int x1,int y1,int x2,int y2,unsigned char col)
{
  int delta_x, delta_y;
  int delta, incE, incNE;
  int x, y;
  int neg_slope = 0;
  
  if (x1 > x2)
    {
      delta_x = x1 - x2;
      if (y1 > y2)	delta_y = y1 - y2;
      else		delta_y = y2 - y1;
      
      if (delta_y <= delta_x)	draw_line(x2, y2, x1, y1, col);
    }
  if (y1 > y2)
    {
      delta_y = y1 - y2;
      if (x1 > x2)	delta_x = x1 - x2;
      else		delta_x = x2 - x1;
      
      if (delta_y > delta_x)	draw_line(x2, y2, x1, y1, col);
    }
  
  if (x1 > x2)
    {
      neg_slope = 1;
      delta_x = x1 - x2;
    }
  else
    delta_x = x2 - x1;
  
  if (y1 > y2)
    {
      neg_slope = 1;
      delta_y = y1 - y2;
    }
  else
    delta_y = y2 - y1;
  
  x = x1;
  y = y1;
  
  set_pixel(x,y,col);
  
  if (delta_y <= delta_x)
    {
      delta = 2 * delta_y - delta_x;
      incE = 2 * delta_y;
      incNE = 2 * (delta_y - delta_x);
      
      while (x < x2)
	{
	  if (delta <= 0)
	    {
	      delta = delta + incE;
	      x++;
	    }
	  else
	    {
	      delta = delta + incNE;
	      x++;
	      if (neg_slope)	y--;
	      else		y++;
	    }
	  set_pixel(x,y,col);
	}
    }
  else
    {
      delta = 2 * delta_x - delta_y;
      incE = 2 * delta_x;
      incNE = 2 * (delta_x - delta_y);
      
      while (y < y2) 
	{
	  if (delta <= 0)
	    {
	      delta = delta + incE;
	      y++;
	    }
	  else
	    {
	      delta = delta + incNE;
	      y++;
	      if (neg_slope)	x--;
	      else		x++;
	    }
	  set_pixel(x,y,col);
	}
    }
}

unsigned char Nimage::rect_detect( const Rect& r)
{
  unsigned char hit;
  
  if( (hit = line_detect( r.toplx, r.toply, r.toprx, r.topry)) > 0 ) 
    return hit;
  
  if( (hit = line_detect( r.toprx, r.topry, r.botlx, r.botly)) > 0 )
    return hit;
  
  if( (hit = line_detect( r.botlx, r.botly, r.botrx, r.botry)) > 0 )
    return hit;
  
  if( (hit = line_detect( r.botrx, r.botry, r.toplx, r.toply)) > 0 )
    return hit;

  return 0;
}

unsigned char Nimage::line_detect(int x1,int y1,int x2,int y2)
{
  int delta_x, delta_y;
  int delta, incE, incNE;
  int x, y;
  int neg_slope = 0;
  unsigned char pixel;
  

  if (x1 > x2)
    {
      delta_x = x1 - x2;
      if (y1 > y2)	delta_y = y1 - y2;
      else		delta_y = y2 - y1;
      
      if (delta_y <= delta_x) return( line_detect(x2, y2, x1, y1));
    }
  if (y1 > y2)
    {
      delta_y = y1 - y2;
      if (x1 > x2)	delta_x = x1 - x2;
      else		delta_x = x2 - x1;
      
      if (delta_y > delta_x) return( line_detect(x2, y2, x1, y1));
    }
  
  if (x1 > x2)
    {
      neg_slope = 1;
      delta_x = x1 - x2;
    }
  else
    delta_x = x2 - x1;
  
  if (y1 > y2)
    {
      neg_slope = 1;
      delta_y = y1 - y2;
    }
  else
    delta_y = y2 - y1;
  
  x = x1;
  y = y1;
  
  //check to see if this pixel is an obstacle
  pixel = get_pixel( x,y );
  if( pixel != 0)
   { 
     return pixel;
   }

  //set_pixel(x,y,col);
  
  if (delta_y <= delta_x)
    {
      delta = 2 * delta_y - delta_x;
      incE = 2 * delta_y;
      incNE = 2 * (delta_y - delta_x);
      
      while (x < x2)
	{
	  if (delta <= 0)
	    {
	      delta = delta + incE;
	      x++;
	    }
	  else
	    {
	      delta = delta + incNE;
	      x++;
	      if (neg_slope)	y--;
	      else		y++;
	    }

	  //check to see if this pixel is an obstacle
	  pixel = get_pixel( x,y );
	  if( pixel != 0)
	    { 
	      return pixel;
	    }
	  
	  //set_pixel(x,y,col);
	}
    }
  else
    {
      delta = 2 * delta_x - delta_y;
      incE = 2 * delta_x;
      incNE = 2 * (delta_x - delta_y);
      
      while (y < y2) 
	{
	  if (delta <= 0)
	    {
	      delta = delta + incE;
	      y++;
	    }
	  else
	    {
	      delta = delta + incNE;
	      y++;
	      if (neg_slope)	x--;
	      else		x++;
	    }
	  //check to see if this pixel is an obstacle
	  pixel = get_pixel( x,y );
	  if( pixel != 0)
	    { 
	      return pixel;
	    }
	  //set_pixel(x,y,col);
	}
    }
  return 0;
}


void Nimage::clear(unsigned char col)
{
  //cout << "Clear: " << data << ',' << col << ',' << width*height << endl;
  memset(data,col,width*height*sizeof(unsigned char));
}


/* save out image data to a ppm (colour pbm) image file */
/* Parameters: IN fname, the name of the file to save the image data to */
/*                cmap, file containing the mapping of grey scale to colour. */
// default value cmap = "/home/dream/derek/bin/high_col";

void Nimage::save_image_as_ppm(char *fname, char *cmap)
{
    FILE *f_out_ptr;
	int bytes_written;
	unsigned char *im_ptr;
	short_triple *the_cmap;

	f_out_ptr = fopen(fname, "wb");

	if (!f_out_ptr)
	{
	fprintf(stderr,"::save_ppm_data, cannot write to file %s\n", fname);
	fflush(stderr);
	exit(0);
	}

	bytes_written = 256;
	the_cmap = new short_triple [bytes_written];

	for (int i = 0; i < bytes_written; i++)
	{
			the_cmap[i][0]=i;
			the_cmap[i][1]=i;
			the_cmap[i][2]=i;
	}

	the_cmap[1][0]=255;the_cmap[1][1]=0;the_cmap[1][2]=0;
	the_cmap[2][0]=0;the_cmap[2][1]=255;the_cmap[2][2]=0;
	the_cmap[3][0]=255;the_cmap[3][1]=255;the_cmap[3][2]=0;
	the_cmap[4][0]=0;the_cmap[4][1]=0;the_cmap[4][2]=255;
	the_cmap[5][0]=255;the_cmap[5][1]=0;the_cmap[5][2]=255;
	the_cmap[6][0]=0;the_cmap[6][1]=255;the_cmap[6][2]=255;
	the_cmap[7][0]=255;the_cmap[7][1]=255;the_cmap[7][2]=255;

  /***** First write the PPM header data *******/
  /* need to save these variables */
  fprintf(f_out_ptr, "P6\n");
  fprintf(f_out_ptr, "# PPM raw bitmap image file format\n");
  fprintf(f_out_ptr, "#%s\n", cmap);    // To indicate where to find colour set

  fprintf(f_out_ptr, "# Width x Height\n");
  fprintf(f_out_ptr, "%d %d\n", width, height);

  fprintf(f_out_ptr, "# Max Grey number\n");
  fprintf(f_out_ptr, "255\n");

/*** u_char is defnd in the read_pgm_data fn (makes fwrite line shorter). ***/

  /***** Then write out the raw image data *****/

  bytes_written = 0;

  for (im_ptr = data; im_ptr < data + (width*height); im_ptr++)
  {
    fputc(char(the_cmap[*im_ptr][0]), f_out_ptr);
    fputc(char(the_cmap[*im_ptr][1]), f_out_ptr);
    fputc(char(the_cmap[*im_ptr][2]), f_out_ptr);
  }

  fflush(f_out_ptr); fclose(f_out_ptr);
}