image.cc

一个机器人平台

/*************************************************************************
 * image.cc - bitmap image class Nimage with processing functions
 *            originally by Neil Sumpter and others at U.Leeds, UK.
 * RTV
 * $Id: image.cc,v 1.1.4.1 2003/01/02 22:36:34 rtv Exp $
 ************************************************************************/

#include <math.h>
#include <assert.h>

#include <iostream>
#include <fstream>

using namespace std;

#include "stage_types.hh"
#include "image.hh"

//#if INCLUDE_ZLIB
#ifdef HAVE_LIBZ
#include <zlib.h>
#endif


//#define DEBUG

typedef short           short_triple[3];

unsigned int RGB( int r, int g, int b )
{
  unsigned int res;
  //cout << "RGB!" << endl;
  res = (((r << 8) | g) << 8) | b;
  return res;
}

unsigned char aget_pixel(unsigned char* data, int width, int height, int x, int y)
{ 
  if (x<0 || x>=width || y<0 || y>=height) return 0;
  return (unsigned char)data[x+(y*width)]; 
}

void aset_pixel(unsigned char* data, int width, int height, int x, int y, unsigned char c)
{
  if (x<0 || x>=width || y<0 || y>=height) return;
  data[(x + y*width)] = c;
}

// Default constructor
Nimage::Nimage()
{
    width = 0;
    height = 0;
    data = NULL;
}


// constuct by copying an existing image
Nimage::Nimage(Nimage* img)
{
#ifdef DEBUG
  cout << "Image: " << width << 'x' << height << flush;
#endif

  width = img->get_width();
  height = img->get_height();
  data = new unsigned char[width*height];
  
  this->copy_from(img);
 
#ifdef DEBUG
  cout << '@' << data << endl;
#endif
}

// construct from width / height 
Nimage::Nimage(int w,int h)
{
#ifdef DEBUG
  cout << "Image: " << width << 'x' << height << flush;
#endif
  
  width = w; height = h;
  data 	= new unsigned char[width*height];

#ifdef DEBUG
  cout << "unsigned char: " << sizeof( unsigned char ) << endl;
  cout << '@' << data << endl;
#endif
  //cout << "constructed NImage" << endl;
}

// construct from width / height with existing data
Nimage::Nimage(unsigned char* array, int w,int h)
{
#ifdef DEBUG
  cout << "Image: " << width << 'x' << height << flush;
#endif

  width 	= w;
  height 	= h;
  data 		= array;

#ifdef DEBUG
  cout << '@' << data << endl;
#endif
}

//destruct
Nimage::~Nimage(void) 	
{
	if (data) delete [] data;
	data = NULL;
}

void Nimage::copy_from(Nimage* img)
{
  if ((width != img->width)||(height !=img->height))
    {
      fprintf(stderr,"Nimage:: mismatch in size (copy_from)\n");
      exit(0);
    }
  
  memcpy(data,img->data,width*height);
}



void Nimage::bgfill( int x, int y, unsigned char col )
{
  // simple recursive flood fill
  if( x < 0 || x >= width ) return;
  if( y < 0 || y >= height ) return;

  set_pixel( x,y,col );
 
  if( get_pixel( x,y-1 ) != col ) bgfill( x, y-1, col );
  if( get_pixel( x,y+1 ) != col ) bgfill( x, y+1, col );
  if( get_pixel( x+1,y ) != col ) bgfill( x+1, y, col );
  if( get_pixel( x-1,y ) != col ) bgfill( x-1, y, col );
}

 
void Nimage::draw_box(int x,int y,int w,int h,unsigned char col)
{
	int i;

	for (i=0;i<w;i++)
	{
		data[x+i+y*width]=col;
		data[x+i+(y+h)*width]=col;
	}

	for (i=0;i<h;i++)
	{
		data[x+(y+i)*width]=col;
		data[x+w+(y+i)*width]=col;
	}
}

void Nimage::save_raw(char* fname)
{
  // this will not write files readable by load_raw!
  FILE *stream = fopen( fname, "wb");
  fwrite( this->get_data(), 1, width*height, stream );
  fclose(stream);
}

void Nimage::load_raw(char* fname)
{
  int n, m;
  char p;

  FILE *stream = fopen( fname, "r");

  for( n=0; n<height; n++ )
    for( m=0; m<width; m++ )
      {
	fread( &p, 1, 1, stream );
	set_pixel( m,n, p );
      }

  fclose(stream);
}


// Load a pnm image.
bool Nimage::load_pnm(const char* fname)
{
  char magicNumber[10];
  //char comment[512];
  int whiteNum;

  ifstream source( fname );

  if( !source )
  {
    PRINT_ERR1("unable to open image file [%s]", fname);
    return false;
  }

  source >> magicNumber;

  if( strcmp(magicNumber, "P5" ) != 0 )
  {
    PRINT_ERR("image file is of incorrect type:"
              " should be pnm, binary, monochrome (magic number P5)");
    return false; 
  }
  
  // ignore the end of this line
  source.ignore( 1024, '\n' );
  
  // ignore comment lines
  while( source.peek() == '#' )
    source.ignore( 1024, '\n' );  

  // get the width, height and white value  
  source >> width >> height >> whiteNum;

  // skip to the end of the line again
  source.ignore( 1024, '\n' );

  unsigned int numPixels = width * height;

  // make space for the pixels
  if (data) delete[] data;
  data = new unsigned char[ numPixels ];
  
  source.read( (char*)data, numPixels );

  // check that we read the right amount of data
  assert( (unsigned int)(source.gcount()) == numPixels );

  source.close();

  return true;
}


// Load a gzipped pnm file
bool Nimage::load_pnm_gz(const char* fname)
{
//#ifndef INCLUDE_ZLIB
#ifndef HAVE_LIBZ
  printf("opening %s\n", fname);
  printf("gzipped files not supported\n");
  return false;
#else
    
  char line[1024];
  char magicNumber[10];
  //  char comment[256];
  int whiteNum; 

#ifdef DEBUG
  printf("opening %s\n", fname);
#endif

  gzFile file = gzopen(fname, "r");
  if (file == NULL)
  {
    PRINT_ERR1("unable to open image file [%s]", fname);
    return false;
  }

  // Extremely crude and ugly file parsing! Fix sometime.  Andrew.
  // yeah - and it had a yukky bug. fixed that but it's still a crude
  // parser - still the format definition for PNMs is strict, so we're
  // almost certainly OK. - rtv

  // (some weeks later) ok, so some programs don't insert a comment line into
  // pnm files, so I'm detecting comment lines now.
  // should change this to use libpnm in the near future - rtv

  // Read and check the magic number
  //
  gzgets(file, magicNumber, sizeof(magicNumber));
  if (strcmp(magicNumber, "P5\n") != 0)
  {
    PRINT_ERR("image file is of incorrect type:"
              " should be pnm, binary, monochrome (magic number P5)");
    return false;
  }
  
  //gzgets(file, comment, sizeof(comment));
  
  do{
    gzgets(file, line, sizeof(line)); 
  } while( line[0] == '#' );
      
  sscanf(line, "%d %d", &width, &height );

  gzgets(file, line, sizeof(line));
  sscanf(line, "%d", &whiteNum);

  if (data)
    delete[] data;
  data = new unsigned char[ width * height ];
  
  // zero the image
  memset( data, 0, width * height * sizeof( data[0] ) );

  for(int n=0; n<height; n++ )
  {
    for(int m=0; m<width; m++ )
    {
      unsigned char a = gzgetc(file);
      if (a > 0)
	set_pixel( m,n, 1 );
    }
  }

  gzclose(file);

  return true;
#endif
}


// Load from an xfig file
// Note that we need both the pixels-per-meter and the figure scale
// to create an image of the correct size and scale.
bool Nimage::load_fig(const char* filename, double ppm, double scale)
{
  FILE *file = fopen(filename, "r");
  if (file == NULL)
  {
    printf("unable to open image file\n");
    return false;
  }

  char line[1024];
  int format;

  if (!fgets(line, sizeof(line), file))
  {
    printf("unexpected end-of-file\n");
    fclose(file);
    return false;
  }

  // Read the file format
  if (strncmp(line, "#FIG 3.2", 8) == 0)
    format = 32;
  else
  {
    printf("unrecognized file format\n");
    fclose(file);
    return false;
  }

  // Read the header and discard most of it
  // The <units> variable is used to handle imperial vs metric units.
  // When using imperial units, xfig will use 1200 pixels/inch.
  // When using metric units, xfig will use 450 pixels/cm.
  // This latter figure is completely undocumented; its just a magic
  // number I derived empirically.  ahoward
  double units = 1200 / 0.0254;
  for (int i = 0; i < 8;)
  {
    if (fgets(line, sizeof(line), file) == NULL)
      break;
    if (line[0] == '#')
      continue;
    if (i == 2)
    {
      if (strncmp(line, "Metric", 6) == 0)
        units = 450 / 0.01;
    }
    i++;
  }

  // Create the figure
  // We assume the whole page should be included,
  // and that we are using US letter in landscape mode.
  this->width = (int) (ppm * 11.5 * 0.0254 * scale);
  this->height = (int) (ppm * 8.5 * 0.0254 * scale);
  if (this->data)
      delete[] this->data;
  this->data = new unsigned char[ this->width * this->height ];

  // Read the objects
  while (true)
  {
    if (fgets(line, sizeof(line), file) == NULL)
      break;
    if (line[0] == '#')
      continue;

    int code = atoi(line);
    if (code == 2)
        load_fig_polyline(file, line, sizeof(line), ppm / units * scale);
    else
        printf("warning : unsupported object type in figure\n");