// Source file for GAPS scalar grid class



////////////////////////////////////////////////////////////////////////
// NOTE:
// Grid values are defined as samples at the grid positions ranging from
// (0, 0) to (xres-1, yres-1).  Grid values outside this range are undefined.
////////////////////////////////////////////////////////////////////////



// Include files

#include "R2Shapes/R2Shapes.h"



R2Grid::
R2Grid(int xresolution, int yresolution)
{
  // Set grid resolution
  grid_resolution[0] = xresolution;
  grid_resolution[1] = yresolution;
  grid_row_size = xresolution;
  grid_size = grid_row_size * yresolution;

  // Allocate grid values
  if (grid_size == 0) grid_values = NULL;
  else grid_values = new RNScalar [ grid_size ];
  assert(!grid_size || grid_values);

  // Set transformations
  grid_to_world_transform = R2identity_affine;
  world_to_grid_transform = R2identity_affine;
  world_to_grid_scale_factor = 1.0;

  // Set all values to zero
  Clear(0);
}



R2Grid::
R2Grid(int xresolution, int yresolution, const R2Box& bbox)
{
  // Set grid resolution
  grid_resolution[0] = xresolution;
  grid_resolution[1] = yresolution;
  grid_row_size = xresolution;
  grid_size = grid_row_size * yresolution;

  // Allocate grid values
  if (grid_size == 0) grid_values = NULL;
  else grid_values = new RNScalar [ grid_size ];
  assert(!grid_size || grid_values);

  // Set transformation
  SetWorldToGridTransformation(bbox);

  // Set all values to zero
  Clear(0);
}



R2Grid::
R2Grid(const R2Grid& voxels)
  : grid_values(NULL)
{
  // Copy everything
  *this = voxels;
}



R2Grid::
~R2Grid(void)
{
  // Deallocate memory for grid values
  if (grid_values) delete [] grid_values;
}



RNInterval R2Grid::
Range(void) const
{
  // Find smallest and largest values
  RNScalar minimum = FLT_MAX;
  RNScalar maximum = -FLT_MAX;
  RNScalar *grid_valuep = grid_values;
  for (int i = 0; i < grid_size; i++) {
    if (*grid_valuep != R2_GRID_UNKNOWN_VALUE) {
      if (*grid_valuep < minimum) minimum = *grid_valuep;
      if (*grid_valuep > maximum) maximum = *grid_valuep;
    }
    grid_valuep++;
  }
  return RNInterval(minimum, maximum);
}



RNScalar R2Grid::
Median(void) const
{
  // Return median
  if (grid_size == 0) return 0.0;
  int tmp_count = 0;
  RNScalar *tmp_values = new RNScalar [ grid_size ];
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      tmp_values[tmp_count++] = grid_values[i];
    }
  }
  if (tmp_count == 0) return R2_GRID_UNKNOWN_VALUE;
  qsort(tmp_values, tmp_count, sizeof(RNScalar), RNCompareScalars);
  RNScalar median = tmp_values[tmp_count/2];
  delete [] tmp_values;
  return median;
}



RNScalar R2Grid::
L1Norm(void) const
{
  // Return L1 norm of grid
  RNScalar sum = 0.0;
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      sum += grid_values[i];
    }
  }
  return sum;
}



RNScalar R2Grid::
L2Norm(void) const
{
  // Return L2 norm of grid
  RNScalar sum = 0.0;
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      RNScalar value = grid_values[i];
      sum += value * value;
    }
  }
  return sqrt(sum);
}



int R2Grid::
Cardinality(void) const
{
  // Return number of non-zero grid values
  int count = 0;
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      if (grid_values[i] != 0) count++;
    }
  }
  return count;
}



RNScalar R2Grid::
GridValue(RNScalar x, RNScalar y) const
{
  // Check if within bounds
  if ((x < 0) || (x > grid_resolution[0]-1)) return 0.0;
  if ((y < 0) || (y > grid_resolution[1]-1)) return 0.0;

  // Bilinear interpolation
  int ix1 = (int) x;
  int iy1 = (int) y;
  int ix2 = ix1 + 1;
  int iy2 = iy1 + 1;
  if (ix2 >= grid_resolution[0]) ix2 = ix1;
  if (iy2 >= grid_resolution[1]) iy2 = iy1;
  RNScalar dx = x - ix1;
  RNScalar dy = y - iy1;
  RNScalar value11 = GridValue(ix1, iy1);
  RNScalar value12 = GridValue(ix1, iy2);
  RNScalar value21 = GridValue(ix2, iy1);
  RNScalar value22 = GridValue(ix2, iy2);
  RNScalar weight11 = (1.0-dx) * (1.0-dy);
  RNScalar weight12 = (1.0-dx) * dy;
  RNScalar weight21 = dx * (1.0-dy);
  RNScalar weight22 = dx * dy;
  RNScalar value = 0;
  RNScalar weight = 0;
  if (value11 != R2_GRID_UNKNOWN_VALUE) { value += weight11 * value11; weight += weight11; }
  if (value12 != R2_GRID_UNKNOWN_VALUE) { value += weight12 * value12; weight += weight12; }
  if (value21 != R2_GRID_UNKNOWN_VALUE) { value += weight21 * value21; weight += weight21; }
  if (value22 != R2_GRID_UNKNOWN_VALUE) { value += weight22 * value22; weight += weight22; }
  if (weight == 0) return R2_GRID_UNKNOWN_VALUE;
  return value / weight;
}



R2Grid& R2Grid::
operator=(const R2Grid& voxels) 
{
  // Copy grid resolution
  grid_resolution[0] = voxels.grid_resolution[0];
  grid_resolution[1] = voxels.grid_resolution[1];
  grid_row_size = voxels.grid_row_size;
  grid_size = voxels.grid_size;

  // Copy grid values
  if (grid_values) delete [] grid_values;
  if (grid_size == 0) grid_values = NULL;
  else grid_values = new RNScalar [ grid_size ];
  assert(!grid_size || grid_values);
  for (int i = 0; i < grid_size; i++) {
    grid_values[i] = voxels.grid_values[i];
  }

  // Copy transforms
  grid_to_world_transform = voxels.grid_to_world_transform;
  world_to_grid_transform = voxels.world_to_grid_transform;
  world_to_grid_scale_factor = voxels.world_to_grid_scale_factor;

  // Return this
  return *this;
}



void R2Grid::
Abs(void) 
{
  // Take absolute value of every grid value
  for (int i = 0; i < grid_size; i++) {
    RNScalar value = grid_values[i];
    if (value != R2_GRID_UNKNOWN_VALUE) grid_values[i] = fabs(value);
  }
}



void R2Grid::
Sqrt(void) 
{
  // Take sqrt of every grid value
  for (int i = 0; i < grid_size; i++) {
    RNScalar value = grid_values[i];
    if (value != R2_GRID_UNKNOWN_VALUE) grid_values[i] = sqrt(value);
  }
}



void R2Grid::
Square(void) 
{
  // Square every grid value
  for (int i = 0; i < grid_size; i++) {
    RNScalar value = grid_values[i];
    if (value != R2_GRID_UNKNOWN_VALUE) grid_values[i] *= value;
  }
}



void R2Grid::
Negate(void) 
{
  // Square every grid value
  for (int i = 0; i < grid_size; i++) {
    RNScalar value = grid_values[i];
    if (value != R2_GRID_UNKNOWN_VALUE) grid_values[i] = -value;
  }
}



void R2Grid::
Invert(void) 
{
  // Invert every grid value
  // Square every grid value
  for (int i = 0; i < grid_size; i++) {
    RNScalar value = grid_values[i];
    if ((value != R2_GRID_UNKNOWN_VALUE) && (value != 0)) {
      grid_values[i] = 1.0 / value;
    }
  }
}



void R2Grid::
Normalize(void) 
{
  // Scale so that length of "vector" is one
  Divide(L2Norm());
}



void R2Grid::
FillHoles(void) 
{
  // Build Gaussian filter
  RNScalar sigma = 1.5;
  double denom = -2.0 * sigma * sigma;
  RNScalar filter[5][5];
  for (int i = -2; i <= 2; i++) {
    for (int j = -2; j <= 2; j++) {
      filter[i+2][j+2] = exp((i*i+j*j)/denom);
    }
  }

  // Seed queue with border unknown values 
  RNQueue<RNScalar *> queue;
  const RNScalar on_queue_value = -46573822;
  for (int x = 0; x < grid_resolution[0]; x++) {
    for (int y = 0; y < grid_resolution[1]; y++) {
      if (GridValue(x, y) == R2_GRID_UNKNOWN_VALUE) continue;
      if (GridValue(x, y) == on_queue_value) continue;

      // Push neighbors with unknown values onto queue
      if ((x > 0) && (GridValue(x-1, y) == R2_GRID_UNKNOWN_VALUE)) {
        queue.Push(&grid_values[y*grid_row_size+(x-1)]);
        SetGridValue(x-1, y, on_queue_value);
      }
      if ((y > 0) && (GridValue(x, y-1) == R2_GRID_UNKNOWN_VALUE)) {
        queue.Push(&grid_values[(y-1)*grid_row_size+x]);
        SetGridValue(x, y-1, on_queue_value);
      }
      if ((x < grid_resolution[0]-1) && (GridValue(x+1, y) == R2_GRID_UNKNOWN_VALUE)) {
        queue.Push(&grid_values[y*grid_row_size+(x+1)]);
        SetGridValue(x+1, y, on_queue_value);
      }
      if ((y < grid_resolution[1]-1) && (GridValue(x, y+1) == R2_GRID_UNKNOWN_VALUE)) {
        queue.Push(&grid_values[(y+1)*grid_row_size+x]);
        SetGridValue(x, y+1, on_queue_value);
      }
    }
  }

  // Iteratively update border unknown values with blur of immediate neighbors
  while (!queue.IsEmpty()) {
    // Pop grid cell from queue
    RNScalar *valuep = queue.Pop();
    assert(*valuep == on_queue_value);
    int index = valuep - grid_values;
    assert((index >= 0) && (index < grid_size));
    int x, y; IndexToIndices(index, x, y);

    // Update value
    RNScalar sum = 0;
    RNScalar weight = 0;
    for (int i = -2; i <= 2; i++) {
      for (int j = -2; j <= 2; j++) {
        int nx = x + i;
        int ny = y + j;
        if ((nx < 0) || (nx >= grid_resolution[0])) continue;
        if ((ny < 0) || (ny >= grid_resolution[1])) continue;
        RNScalar value = GridValue(nx, ny);
        if (value == R2_GRID_UNKNOWN_VALUE) continue;
        if (value == on_queue_value) continue;
        RNScalar w = filter[i+2][j+2];
        sum += w * value;
        weight += w;
      }
    }

    // Divide by total weight
    if (weight > 0) *valuep = sum / weight;
    else fprintf(stderr, "Zero weight in fill holes\n"); 

    // Push neighbors with unknown values onto queue
    if ((x > 0) && (GridValue(x-1, y) == R2_GRID_UNKNOWN_VALUE)) {
      queue.Push(&grid_values[y*grid_row_size+(x-1)]);
      SetGridValue(x-1, y, on_queue_value);
    }
    if ((y > 0) && (GridValue(x, y-1) == R2_GRID_UNKNOWN_VALUE)) {
      queue.Push(&grid_values[(y-1)*grid_row_size+x]);
      SetGridValue(x, y-1, on_queue_value);
    }
    if ((x < grid_resolution[0]-1) && (GridValue(x+1, y) == R2_GRID_UNKNOWN_VALUE)) {
      queue.Push(&grid_values[y*grid_row_size+(x+1)]);
      SetGridValue(x+1, y, on_queue_value);
    }
    if ((y < grid_resolution[1]-1) && (GridValue(x, y+1) == R2_GRID_UNKNOWN_VALUE)) {
      queue.Push(&grid_values[(y+1)*grid_row_size+x]);
      SetGridValue(x, y+1, on_queue_value);
    }
  }
}



void R2Grid::
Clear(RNScalar value) 
{
  // Set all grid values to value
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    grid_values[i] = value;
  }
}



void R2Grid::
Dilate(RNLength grid_distance) 
{
  // Make copy so that can restore unknown values
  R2Grid copy(*this);

  // Set voxels (to one) within grid_distance from some non-zero voxel
  SquaredDistanceTransform();
  Threshold(grid_distance * grid_distance, 1, 0);

  // Restore unknown values
  for (int i = 0; i < grid_size; i++) {
    if (copy.grid_values[i] == R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] = R2_GRID_UNKNOWN_VALUE;
    }
  }
}



void R2Grid::
Erode(RNLength grid_distance) 
{
  // Make copy so that can restore unknown values
  R2Grid copy(*this);

  // Keep only voxels at least distance from some zero voxel
  Threshold(1.0E-20, 1, 0);
  SquaredDistanceTransform();
  Threshold(grid_distance * grid_distance, 0, 1);

  // Restore unknown values
  for (int i = 0; i < grid_size; i++) {
    if (copy.grid_values[i] == R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] = R2_GRID_UNKNOWN_VALUE;
    }
  }
}



void R2Grid::
Blur(RNLength grid_sigma) 
{
  // Build filter
  RNScalar sigma = grid_sigma;
  int filter_radius = (int) (3 * sigma + 0.5);
  RNScalar *filter = new RNScalar [ filter_radius + 1 ];
  assert(filter);

  // Make buffer for temporary copy of row
  int res = XResolution();
  if (res < YResolution()) res = YResolution();
  RNScalar *buffer = new RNScalar [ res ];
  assert(buffer);

  // Fill filter with Gaussian 
  const RNScalar sqrt_two_pi = sqrt(RN_TWO_PI);
  double a = sqrt_two_pi * sigma;
  double fac = 1.0 / (a * a * a);
  double denom = 2.0 * sigma * sigma;
  for (int i = 0; i <= filter_radius; i++) {
    filter[i] = fac * exp(-i * i / denom);
  }

  // Convolve grid with filter in X direction
  for (int j = 0; j < YResolution(); j++) { 
    for (int i = 0; i < XResolution(); i++) 
      buffer[i] = GridValue(i, j); 
    for (int i = 0; i < XResolution(); i++) { 
      if (buffer[i] == R2_GRID_UNKNOWN_VALUE) continue;
      RNScalar sum = 0;
      RNScalar weight = 0;
      RNScalar value = buffer[i];
      if (value != R2_GRID_UNKNOWN_VALUE) {
        sum += filter[0] * value;
        weight += filter[0];
      }
      int nsamples = i;
      if (nsamples > filter_radius) nsamples = filter_radius;
      for (int m = 1; m <= nsamples; m++) {
        RNScalar value = buffer[i - m];;
        if (value != R2_GRID_UNKNOWN_VALUE) {
          sum += filter[m] * value;
          weight += filter[m];
        }
      }
      nsamples = XResolution() - 1 - i;
      if (nsamples > filter_radius) nsamples = filter_radius;
      for (int m = 1; m <= nsamples; m++) {
        RNScalar value = buffer[i + m];
        if (value != R2_GRID_UNKNOWN_VALUE) {
          sum += filter[m] * value;
          weight += filter[m];
        }
      }
      if (weight > 0) SetGridValue(i, j, sum / weight);
    }
  }

  // Convolve grid with filter in Y direction
  for (int j = 0; j < XResolution(); j++) { 
    for (int i = 0; i < YResolution(); i++) 
      buffer[i] = GridValue(j, i); 
    for (int i = 0; i < YResolution(); i++) { 
      if (buffer[i] == R2_GRID_UNKNOWN_VALUE) continue;
      RNScalar sum = 0;
      RNScalar weight = 0;
      RNScalar value = buffer[i];
      if (value != R2_GRID_UNKNOWN_VALUE) {
        sum += filter[0] * value;
        weight += filter[0];
      }
      int nsamples = i;
      if (nsamples > filter_radius) nsamples = filter_radius;
      for (int m = 1; m <= nsamples; m++) {
        RNScalar value = buffer[i - m];
        if (value != R2_GRID_UNKNOWN_VALUE) {
          sum += filter[m] * value;
          weight += filter[m];
        }
      }
      nsamples = YResolution() - 1 - i;
      if (nsamples > filter_radius) nsamples = filter_radius;
      for (int m = 1; m <= nsamples; m++) {
        RNScalar value = buffer[i + m];
        if (value != R2_GRID_UNKNOWN_VALUE) {
          sum += filter[m] * value;
          weight += filter[m];
        }
      }
      if (weight > 0) SetGridValue(j, i, sum / weight);
    }
  }

  // Deallocate memory
  delete [] filter;
  delete [] buffer;
}



void R2Grid::
BilateralFilter(RNLength grid_sigma, RNLength value_sigma)
{
  // Make copy of grid
  R2Grid copy(*this);

  // Determine reasonable value sigma
  if (value_sigma == -1) {
    RNInterval range = Range();
    value_sigma = 0.01 * (range.Max() - range.Min());
  }

  // Get convenient variables
  double grid_denom = 2.0 * grid_sigma * grid_sigma;
  double value_denom = 2.0 * value_sigma * value_sigma;
  RNScalar grid_radius = 3 * grid_sigma;
  int r = (int) (grid_radius + 1);
  int r_squared = r * r;

  // Set every sample to be median of surrounding region in input grid
  for (int cy = 0; cy < YResolution(); cy++) {
    for (int cx = 0; cx < XResolution(); cx++) {
      // Check if current value is unknown - if so, don't update
      RNScalar value = copy.GridValue(cx, cy);
      if (value != R2_GRID_UNKNOWN_VALUE) {
        RNScalar sum = 0;
        RNScalar weight = 0;
        int ymin = cy - r;
        int ymax = cy + r;
        if (ymin < 0) ymin = 0;
        if (ymax >= YResolution()) ymax = YResolution() - 1;
        for (int y = ymin; y <= ymax; y++) {
          int xmin = cx - r;
          int xmax = cx + r;
          if (xmin < 0) xmin = 0;
          if (xmax >= XResolution()) xmax = XResolution() - 1;
          int dy = y - cy;
          for (int x = xmin; x <= xmax; x++) {
            int dx = x - cx;
            int grid_distance_squared = dx*dx + dy*dy;
            if (grid_distance_squared > r_squared) continue;
            RNScalar sample = copy.GridValue(x, y);
            if (sample == R2_GRID_UNKNOWN_VALUE) continue;
            RNScalar value_distance_squared = value - sample;
            value_distance_squared *= value_distance_squared;
            RNScalar w = exp(-grid_distance_squared/grid_denom) * exp(-value_distance_squared/value_denom);
            sum += w * sample;
            weight += w;
          }
        }

        // Set grid value
        if (weight == 0) SetGridValue(cx, cy, R2_GRID_UNKNOWN_VALUE);
        else SetGridValue(cx, cy, sum / weight);
      }
    }
  }
}



void R2Grid::
PercentileFilter(RNLength grid_radius, RNScalar percentile)
{
  // Make copy of grid
  R2Grid copy(*this);

  // Get convenient variables
  RNScalar grid_radius_squared = grid_radius * grid_radius;
  int r = (int) grid_radius;
  assert(r >= 0);
  int max_samples = (2*r+1) * (2*r+1);
  RNScalar *samples = new RNScalar [ max_samples ];
  assert(samples);

  // Set every sample to be Kth percentile of surrounding region in input grid
  for (int cy = 0; cy < YResolution(); cy++) {
    for (int cx = 0; cx < XResolution(); cx++) {
      // Check if current value is unknown - if so, don't update
      if (GridValue(cx, cy) != R2_GRID_UNKNOWN_VALUE) {
        // Build list of grid values in neighborhood
        int nsamples = 0;
        int ymin = cy - r;
        int ymax = cy + r;
        if (ymin < 0) ymin = 0;
        if (ymax >= YResolution()) ymax = YResolution() - 1;
        for (int y = ymin; y <= ymax; y++) {
          int xmin = cx - r;
          int xmax = cx + r;
          if (xmin < 0) xmin = 0;
          if (xmax >= XResolution()) xmax = XResolution() - 1;
          int dy = y - cy;
          for (int x = xmin; x <= xmax; x++) {
            int dx = x - cx;
            int d_squared = dx*dx + dy*dy;
            if (d_squared > grid_radius_squared) continue;
            RNScalar sample = copy.GridValue(x, y);
            if (sample == R2_GRID_UNKNOWN_VALUE) continue;
            samples[nsamples++] = sample;
          }
        }

        // Check number of grid values in neighborhood
        if (nsamples == 0) {
          SetGridValue(cx, cy, R2_GRID_UNKNOWN_VALUE);
        }
        else {
          // Sort samples found in neighborhood
          qsort(samples, nsamples, sizeof(RNScalar), RNCompareScalars);

          // Set grid value to percentile of neighborhood
          int index = (int) (percentile * nsamples);
          if (index < 0) index = 0;
          else if (index >= nsamples) index = nsamples-1;
          SetGridValue(cx, cy, samples[index]);
        }
      }
    }
  }

  // Delete temporary memory
  delete [] samples;
}



void R2Grid::
MaskNonMinima(RNLength grid_radius)
{
  // Create grid with local minima
  R2Grid copy(*this);
  copy.MinFilter(grid_radius);

  // Mask values that are not minima
  for (int i = 0; i < grid_size; i++) {
    if ((grid_values[i] == R2_GRID_UNKNOWN_VALUE) || (copy.grid_values[i] == R2_GRID_UNKNOWN_VALUE)) continue;
    if (grid_values[i] > copy.grid_values[i]) grid_values[i] = 0;
  }
}



void R2Grid::
MaskNonMaxima(RNLength grid_radius)
{
  // Create grid with local maxima
  R2Grid copy(*this);
  copy.MaxFilter(grid_radius);

  // Mask values that are not maxima
  for (int i = 0; i < grid_size; i++) {
    if ((grid_values[i] == R2_GRID_UNKNOWN_VALUE) || (copy.grid_values[i] == R2_GRID_UNKNOWN_VALUE)) continue;
    if (grid_values[i] < copy.grid_values[i]) grid_values[i] = 0;
  }
}



void R2Grid::
Convolve(const RNScalar filter[3][3]) 
{
  // Make temporary copy of grid
  R2Grid copy(*this);

  // Mark boundaries unknown
  for (int i = 0; i < XResolution(); i++) { 
    SetGridValue(i, 0, R2_GRID_UNKNOWN_VALUE);
    SetGridValue(i, YResolution()-1, R2_GRID_UNKNOWN_VALUE);
  }
  for (int j = 0; j < YResolution(); j++) { 
    SetGridValue(0, j, R2_GRID_UNKNOWN_VALUE);
    SetGridValue(XResolution()-1, j, R2_GRID_UNKNOWN_VALUE);
  }

  // Convolve grid with 3x3 filter
  for (int j = 1; j < YResolution()-1; j++) { 
    for (int i = 1; i < XResolution()-1; i++) { 
      RNScalar value = copy.GridValue(i, j);
      if (value != R2_GRID_UNKNOWN_VALUE) {
        RNScalar sum = 0;
        RNBoolean unknown = FALSE;
        for (int dj = -1; dj <= 1; dj++) {
          for (int di = -1; di <= 1; di++) {
            value = copy.GridValue(i + di, j + dj);
            if (value == R2_GRID_UNKNOWN_VALUE) { unknown = TRUE; break; }
            else sum += filter[dj+1][di+1] * value;
          }
          if (unknown) break; 
        }
        if (unknown) SetGridValue(i, j, R2_GRID_UNKNOWN_VALUE);
        else SetGridValue(i, j, sum);
      }
    }
  }
}



void R2Grid::
Gradient(RNDimension dim)
{
  // Set up xfilter
  const RNScalar xfilter[3][3] = { 
    { -1, 0, 1 }, 
    { -2, 0, 2}, 
    { -1, 0, 1 } 
  };
  const RNScalar yfilter[3][3] = { 
    { -1, -2, -1 }, 
    {  0,  0,  0 }, 
    {  1,  2,  1 } 
  };

  // Convolve with filter
  if (dim == RN_X) Convolve(xfilter);
  else if (dim == RN_Y) Convolve(yfilter);
  else RNAbort("Invalid dimension");
}



void R2Grid::
Hessian(RNDimension dim1, RNDimension dim2)
{
  // Compute gradient twice
  Gradient(dim1);
  Gradient(dim2);
}



void R2Grid::
GradientAngle(void)
{
  // Compute direction of gradient
  R2Grid gx(*this); gx.Gradient(RN_X);
  R2Grid gy(*this); gy.Gradient(RN_Y);
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) {
      continue;
    }
    RNScalar x = gx.GridValue(i);
    if (x == R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] = R2_GRID_UNKNOWN_VALUE;
      continue;
    }
    RNScalar y = gy.GridValue(i);
    if (y == R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] = R2_GRID_UNKNOWN_VALUE;
      continue;
     }
    if (x == 0) {
      grid_values[i] = RN_PI_OVER_TWO;
      continue;
    }
    grid_values[i] = atan(y/x);
  }
}



void R2Grid::
GradientMagnitude(void)
{
  // Compute magnitude of gradient (Sobel operator)
  R2Grid gx(*this); gx.Gradient(RN_X);
  R2Grid gy(*this); gy.Gradient(RN_Y);
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) {
      continue;
    }
    RNScalar x = gx.GridValue(i);
    if (x == R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] = R2_GRID_UNKNOWN_VALUE;
      continue;
    }
    RNScalar y = gy.GridValue(i);
    if (y == R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] = R2_GRID_UNKNOWN_VALUE;
      continue;
    }
    grid_values[i] = sqrt(x*x + y*y);
  }
}



void R2Grid::
Laplacian(void)
{
  // Set up Laplacian filter
  const RNScalar filter[3][3] = { 
    { -1, -1, -1 }, 
    { -1,  8, -1 }, 
    { -1, -1, -1 } 
  };

  // Convolve with filter
  Convolve(filter);
}



void R2Grid::
Substitute(RNScalar old_value, RNScalar new_value) 
{
  // Replace all instances of old_value with new_value
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == old_value) {
      grid_values[i] = new_value;
    }
  }
}



void R2Grid::
Add(RNScalar value) 
{
  // Add value to all grid values 
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] += value;
    }
  }
}



void R2Grid::
Add(const R2Grid& voxels) 
{
  // Resolutions must be the same (for now)
  assert(grid_resolution[0] == voxels.grid_resolution[0]);
  assert(grid_resolution[1] == voxels.grid_resolution[1]);

  // Add passed grid values to corresponding entries of this grid
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      if (voxels.grid_values[i] == R2_GRID_UNKNOWN_VALUE) grid_values[i] = R2_GRID_UNKNOWN_VALUE;
      else grid_values[i] += voxels.grid_values[i];
    }
  }
}



void R2Grid::
Subtract(RNScalar value) 
{
  // Add the opposite
  Add(-value);
}



void R2Grid::
Subtract(const R2Grid& voxels) 
{
  // Resolutions must be the same (for now)
  assert(grid_resolution[0] == voxels.grid_resolution[0]);
  assert(grid_resolution[1] == voxels.grid_resolution[1]);

  // Subtract passed grid values from corresponding entries of this grid
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      if (voxels.grid_values[i] == R2_GRID_UNKNOWN_VALUE) grid_values[i] = R2_GRID_UNKNOWN_VALUE;
      else grid_values[i] -= voxels.grid_values[i];
    }
  }
}



void R2Grid::
Multiply(RNScalar value) 
{
  // Multiply grid values by value
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] *= value;
    }
  }
}



void R2Grid::
Multiply(const R2Grid& voxels) 
{
  // Resolutions must be the same (for now)
  assert(grid_resolution[0] == voxels.grid_resolution[0]);
  assert(grid_resolution[1] == voxels.grid_resolution[1]);

  // Multiply passed grid values by corresponding entries of this grid
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      if (voxels.grid_values[i] == R2_GRID_UNKNOWN_VALUE) grid_values[i] = R2_GRID_UNKNOWN_VALUE;
      else grid_values[i] *= voxels.grid_values[i];
    }
  }
}



void R2Grid::
Divide(RNScalar value) 
{
  // Just checking
  if (RNIsZero(value)) return;

  // Multiply by recipricol
  Multiply(1.0 / value);
}



void R2Grid::
Divide(const R2Grid& voxels) 
{
  // Resolutions must be the same (for now)
  assert(grid_resolution[0] == voxels.grid_resolution[0]);
  assert(grid_resolution[1] == voxels.grid_resolution[1]);

  // Divide entries of this grid by by corresponding entries of passed grid 
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      if (voxels.grid_values[i] == R2_GRID_UNKNOWN_VALUE) {
       grid_values[i] = R2_GRID_UNKNOWN_VALUE;
      }
      else if (voxels.grid_values[i] == 0) {
        if (grid_values[i] > 0) grid_values[i] = RN_INFINITY;
        else if (grid_values[i] < 0) grid_values[i] = -RN_INFINITY;
      }
      else {
        grid_values[i] /= voxels.grid_values[i];
      }
    }
  }
}



void R2Grid::
Pow(RNScalar exponent) 
{
  // Apply exponent to all grid values 
  for (int i = 0; i < grid_size; i++) {
    RNScalar value = grid_values[i];
    if (value != R2_GRID_UNKNOWN_VALUE) {
      grid_values[i] = pow(value, exponent);
    }
  }
}



void R2Grid::
Mask(const R2Grid& voxels) 
{
  // Resolutions must be the same (for now)
  assert(grid_resolution[0] == voxels.grid_resolution[0]);
  assert(grid_resolution[1] == voxels.grid_resolution[1]);

  // Mask entries of grid
  for (int i = 0; i < grid_size; i++) {
    if (voxels.grid_values[i] == 0) grid_values[i] = 0;
    else if (voxels.grid_values[i] == R2_GRID_UNKNOWN_VALUE) grid_values[i] = R2_GRID_UNKNOWN_VALUE;
  }
}



void R2Grid::
Threshold(RNScalar threshold, RNScalar low, RNScalar high) 
{
  // Set grid value to low (high) if less/equal (greater) than threshold
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] != R2_GRID_UNKNOWN_VALUE) {
      if (grid_values[i] <= threshold) {
        if (low != R2_GRID_KEEP_VALUE) grid_values[i] = low;
      }
      else {
        if (high != R2_GRID_KEEP_VALUE) grid_values[i] = high;
      }
    }
  }
}



void R2Grid::
Threshold(const R2Grid& threshold, RNScalar low, RNScalar high) 
{
  // Set grid value to low (high) if less/equal (greater) than threshold
  for (int i = 0; i < grid_size; i++) {
    if ((grid_values[i] != R2_GRID_UNKNOWN_VALUE) && (threshold.grid_values[i] != R2_GRID_UNKNOWN_VALUE)) {
      if (grid_values[i] <= threshold.grid_values[i]) {
        if (low != R2_GRID_KEEP_VALUE) grid_values[i] = low;
      }
      else {
        if (high != R2_GRID_KEEP_VALUE) grid_values[i] = high;
      }
    }
  }
}



void R2Grid::
SignedDistanceTransform(void)
{
  // Compute distance from boundary into interior (negative) and into exterior (positive)
  R2Grid copy(*this);
  SquaredDistanceTransform();
  Sqrt();
  copy.Threshold(0, 1, 0);
  copy.Substitute(R2_GRID_UNKNOWN_VALUE, 1);
  copy.SquaredDistanceTransform();
  copy.Sqrt();
  Subtract(copy);
}



void R2Grid::
SquaredDistanceTransform(void)
{
  int x,y,s,t;
  int dist,square,new_dist;
  int* oldBuffer;
  int* newBuffer;
  int first;
  int i;

  // Allocate temporary buffers
  int res = XResolution();
  if (res < YResolution()) res = YResolution();
  oldBuffer = new int[res];
  assert(oldBuffer);
  newBuffer = new int[res];
  assert(newBuffer);

  // Initalize values (0 if was set, max_value if not)
  RNScalar max_value = 2 * (res+1) * (res+1);
  RNScalar *grid_valuesp = grid_values;
  for (i = 0; i < grid_size; i++) {
    if (*grid_valuesp == 0.0) *grid_valuesp = max_value;
    else if (*grid_valuesp == R2_GRID_UNKNOWN_VALUE) *grid_valuesp = max_value;
    else *grid_valuesp = 0.0;
    grid_valuesp++;
  }

  // Scan along x axis
  for (y = 0; y < YResolution(); y++) {
    first = 1;
    dist = 0;
    for (x = 0; x < XResolution(); x++) {
      if (GridValue(x,y) == 0.0) {
        dist=0;
        first=0;
        SetGridValue(x, y, 0);
      }
      else if (first == 0) {
        dist++;
        square = dist*dist;
        SetGridValue(x, y, square);
      }
    }
      		
    // backward scan
    dist = 0;
    first = 1;
    for (x = XResolution()-1; x >= 0; x--) {
      if (GridValue(x,y) == 0.0) {
        dist = 0;
        first = 0;
        SetGridValue(x, y, 0);
      }
      else if (first == 0) {
        dist++;
        square = dist*dist;
        if (square < GridValue(x, y)) {
          SetGridValue(x, y, square);
        }
      }
    }
  }

  // Scan along y axis
  for (x = 0; x < XResolution(); x++) {
    // Copy grid values
    for (y = 0; y < YResolution(); y++) 
      oldBuffer[y] = (int) (GridValue(x, y) + 0.5);
      		
    // forward scan
    s = 0;
    for (y = 0; y < YResolution(); y++) {
      dist = oldBuffer[y];
      if (dist) {
        for (t = s; t <= y ; t++) {
          new_dist = oldBuffer[t] + (y - t) * (y - t);
          if (new_dist <= dist){
            dist = new_dist;
            s = t;
          }
        }
      }
      else { 
        s = y;
      }
      newBuffer[y] = dist;
    }
  
    // backward scan
    s = YResolution() - 1;
    for(y = YResolution()-1; y >=0 ; y--) {
      dist = newBuffer[y];
      if (dist) {
        for (t = s; t > y ; t--) {
          new_dist = oldBuffer[t] + (y - t) * (y - t);
          if (new_dist <= dist){
            dist = new_dist;
            s = t;
          }
        }
        SetGridValue(x, y, dist);
      }
      else { 
        s = y; 
      }
    }
  }
	
  // Delete temporary buffers
  delete[] oldBuffer;
  delete[] newBuffer;
}



void R2Grid::
PointSymmetryTransform(int radius)
{
  // Make copy of grid
  R2Grid copy(*this);
  copy.Normalize();

  // Brute force for now
  for (int j = 0; j < grid_resolution[1]; j++) {
    int y_radius = (j < grid_resolution[1]/2) ? j : grid_resolution[1]-1 - j;
    if ((radius > 0) && (y_radius > radius)) y_radius = radius;
    for (int i = 0; i < grid_resolution[0]; i++) {
      int x_radius = (i < grid_resolution[0]/2) ? i : grid_resolution[0]-1 - i;
      if ((radius > 0) && (x_radius > radius)) x_radius = radius;
      RNScalar dot = 0;
      RNScalar norm = 0;
      for (int t = -y_radius; t <= y_radius; t++) {
        for (int s = -x_radius; s <= x_radius; s++) {
          RNScalar value1 = GridValue(i-s, j-t);
          RNScalar value2 = GridValue(i+s, j+t);
          dot += 2 * value1 * value2;
          norm += value1 * value1; 
          norm += value2 * value2; 
        }
      }
      if (norm > 0) dot /= norm;
      SetGridValue(i, j, dot);
    }
  }
}



void R2Grid::
Resample(int xresolution, int yresolution)
{
  // Resample grid values at new resolution
  RNScalar *new_grid_values = NULL;
  int new_grid_size = xresolution * yresolution;
  if (new_grid_size > 0) {
    new_grid_values = new RNScalar [ new_grid_size ];
    assert(new_grid_values);
    RNScalar *new_grid_valuesp = new_grid_values;
    RNScalar xscale = (RNScalar) (grid_resolution[0]-1) / (RNScalar) (xresolution - 1);
    RNScalar yscale = (RNScalar) (grid_resolution[1]-1) / (RNScalar) (yresolution - 1);
    for (int j = 0; j < yresolution; j++) {
      RNScalar y = (j == yresolution-1) ? grid_resolution[1]-1 : j * yscale;
      for (int i = 0; i < xresolution; i++) {
        RNScalar x = (i == xresolution-1) ? grid_resolution[0]-1 : i * xscale;
        *(new_grid_valuesp++) = GridValue(x, y);
      }
    }
  }

  // Reset grid variables
  grid_resolution[0] = xresolution;
  grid_resolution[1] = yresolution;
  grid_row_size = xresolution;
  grid_size = grid_row_size * yresolution;
  if (grid_values) delete [] grid_values;
  grid_values = new_grid_values;
}



void R2Grid::
RasterizeGridPoint(RNScalar x, RNScalar y, RNScalar value)
{
  // Check if within bounds
  if ((x < 0) || (x > grid_resolution[0]-1)) return;
  if ((y < 0) || (y > grid_resolution[1]-1)) return;

  // Bilinear interpolation
  int ix1 = (int) x;
  int iy1 = (int) y;
  int ix2 = ix1 + 1;
  int iy2 = iy1 + 1;
  if (ix2 >= grid_resolution[0]) ix2 = ix1;
  if (iy2 >= grid_resolution[1]) iy2 = iy1;
  RNScalar dx = x - ix1;
  RNScalar dy = y - iy1;
  AddGridValue(ix1, iy1, value * (1.0-dx) * (1.0-dy));
  AddGridValue(ix1, iy2, value * (1.0-dx) * dy);
  AddGridValue(ix2, iy1, value * dx * (1.0-dy));
  AddGridValue(ix2, iy2, value * dx * dy);
}



void R2Grid::
RasterizeGridSpan(const int p1[2], const int p2[2], RNScalar value)
{
  // Get some convenient variables
  int d[2],p[2],dd[2],s[2];
  for (int i = 0; i < 2; i++) {
    d[i]= p2[i] - p1[i];
    if(d[i]<0){
      dd[i] = -d[i];
      s[i] = -1;
    }
    else{
      dd[i] = d[i];
      s[i] = 1;
    }
    p[i] = p1[i];
  }

  // Choose dimensions
  int i1=0;
  if(dd[1]>dd[i1]){i1=1;}
  int i2=(i1+1)%2;

  // Check span extent
  if(dd[i1]==0){
    // Span is a point - rasterize it
    AddGridValue(p[0], p[1], value);
  }
  else {
    // Step along span
    int off[2] = { 0, 0 };
    for (int i = 0; i <= dd[i1]; i++) {
      AddGridValue(p[0], p[1], value);
      off[i2]+=dd[i2];
      p[i1]+=s[i1];
      p[i2]+=s[i2]*off[i2]/dd[i1];
      off[i2]%=dd[i1];
    }
  }
}



void R2Grid::
RasterizeGridTriangle(const int p1[2], const int p2[2], const int p3[2], RNScalar value)
{
  R2Point points[3];
  points[0][0] = p1[0];
  points[0][1] = p1[1];
  points[1][0] = p2[0];
  points[1][1] = p2[1];
  points[2][0] = p3[0];
  points[2][1] = p3[1];
  RasterizeGridPolygon(R2Polygon(points,3), value);
}



void R2Grid::
RasterizeGridCircle(const R2Point& center, RNLength radius, RNScalar value)
{
  // Figure out the min and max in each dimension
  int mn[2], mx[2];
  for (int i = 0; i < 2; i++) {
    mx[i]= (int) (center[i]+radius);
    if (mx[i] < 0) return;
    if (mx[i] > Resolution(i)-1) mx[i] = Resolution(i)-1;
    mn[i]= (int) (center[i]-radius);
    if (mn[i] > Resolution(i)-1) return;
    if (mn[i] < 0) mn[i] = 0;
  }

  // Rasterize circle interior
  int y1 = (int) (center[1] - radius + 0.5);
  int y2 = (int) (center[1] + radius + 0.5);
  if (y1 < mn[1]) y1 = mn[1];
  if (y2 > mx[1]) y2 = mx[1];
  RNLength radius_squared = radius * radius;
  for (int j = y1; j <= y2; j++) {
    RNCoord y = j - center[1];
    RNCoord y_squared = y*y;
    RNLength x_squared = radius_squared - y_squared;
    RNLength x = sqrt(x_squared);
    int x1 = (int) (center[0] - x + 0.5);
    int x2 = (int) (center[0] + x + 0.5);
    if (x1 < mn[0]) x1 = mn[0];
    if (x2 > mx[0]) x2 = mx[0];
    for (int i = x1; i <= x2; i++) {
      AddGridValue(i, j, value);
    }
  }
}


// Used by RasterizeGridPolygon
struct ScanLineCrossing { RNCoord x; int side; };



// Used by RasterizeGridPolygon
static int CompareScanLineCrossings(const void *data1, const void *data2)
{
  ScanLineCrossing *crossing1 = (ScanLineCrossing *) data1;
  ScanLineCrossing *crossing2 = (ScanLineCrossing *) data2;
  if (RNIsLess(crossing1->x, crossing2->x)) return -1;
  else if (RNIsGreater(crossing1->x, crossing2->x)) return 1;
  else if (crossing1->side < crossing2->side) return -1;
  else if (crossing1->side > crossing2->side) return 1;
  else return 0;
}



void R2Grid::
RasterizeGridPolygon(const R2Polygon& polygon, RNScalar value) 
{
  // Clip polygon to grid bounding box
  R2Polygon clipped_polygon(polygon);
  clipped_polygon.Clip(GridBox());
  if (clipped_polygon.NPoints() == 0) return; 

  // Check if only one point
  if (clipped_polygon.NPoints() == 1) { 
    RasterizeGridPoint(clipped_polygon.Point(0), value); 
    return; 
  }

  // Check if only two points
  if (clipped_polygon.NPoints() == 2) { 
    RasterizeGridSpan(clipped_polygon.Point(0), clipped_polygon.Point(1), value); 
    return; 
  }

  // Allocate list of scan line crossings
  int max_crossings = 2 * YResolution() * clipped_polygon.NPoints();
  ScanLineCrossing *crossings = new ScanLineCrossing [max_crossings];
  int ncrossings = 0;

  // Scan convert polygon
  for (int iy = 0; iy < YResolution(); iy++) {
    // Build list of scan line crossings
    ncrossings = 0;    
    const R2Point *p1 = &clipped_polygon.Point(clipped_polygon.NPoints()-1);
    RNScalar d1 = p1->Y() - iy;
    for (int i = 0; i < clipped_polygon.NPoints(); i++) {
      if (ncrossings >= max_crossings) break;
      const R2Point *p2 = &clipped_polygon.Point(i);
      RNScalar d2 = p2->Y() - iy;
      if ((d1 < 0) && (d2 >= 0)) {
        RNScalar t = d2 / (d2 - d1);
        crossings[ncrossings].x = p2->X() + t * (p1->X() - p2->X());
        crossings[ncrossings].side = -1;
        ncrossings++;
      }
      else if ((d2 < 0) && (d1 >= 0)) {
        RNScalar t = d1 / (d1 - d2);
        crossings[ncrossings].x = p1->X() + t * (p2->X() - p1->X());
        crossings[ncrossings].side = 1;
        ncrossings++;
      }
      p1 = p2;
      d1 = d2;
    }

    // Check number of crossings
    if (ncrossings < 2) continue;

    // Sort scan line crossings by x coordinate
    qsort(crossings, ncrossings, sizeof(ScanLineCrossing), CompareScanLineCrossings);

    // Rasterize scan line
    int index = 0;
    while (index < ncrossings-1) {
      // Find two crossings
      ScanLineCrossing *crossing1 = &crossings[index++];
      ScanLineCrossing *crossing2 = &crossings[index++];
      while ((index < ncrossings) && (crossing2->side == crossing1->side)) crossing2 = &crossings[index++];
      if (crossing1->side == crossing2->side) break;

      // Fill pixels between crossings
      int ix1 = (int) (crossing1->x + 0.5);
      int ix2 = (int) (crossing2->x + 0.5);
      for (int ix = ix1; ix <= ix2; ix++) {
        AddGridValue(ix, iy, value);
      }
    }
  }

  // Delete crossings
  delete [] crossings;
}



void R2Grid::
RasterizeWorldPolygon(const R2Polygon& polygon, RNScalar value) 
{
  // Rasterize polygon into grid coordinates
  R2Polygon transformed_polygon(polygon);
  transformed_polygon.Transform(WorldToGridTransformation());
  RasterizeGridPolygon(transformed_polygon, value);
}



RNScalar R2Grid::
Dot(const R2Grid& voxels) const
{
  // Resolutions and transforms must be the same (for now)
  assert(grid_resolution[0] == voxels.grid_resolution[0]);
  assert(grid_resolution[1] == voxels.grid_resolution[1]);

  // Compute dot product between this and grid
  RNScalar dot = 0.0;
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    if (voxels.grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    dot += grid_values[i] * voxels.grid_values[i];
  }
  return dot;
}



RNScalar R2Grid::
L1Distance(const R2Grid& voxels) const
{
  // Compute distance between this and grid
  RNScalar distance = 0.0;
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    if (voxels.grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    distance += fabs(grid_values[i] - voxels.grid_values[i]);
  }
  return distance;
}



RNScalar R2Grid::
L2DistanceSquared(const R2Grid& voxels) const
{
  // Compute distance between this and grid
  RNScalar distance_squared = 0.0;
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    if (voxels.grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    RNScalar delta = (grid_values[i] - voxels.grid_values[i]);
    distance_squared += delta * delta;
  }

  // Return result
  return distance_squared;
}



void R2Grid::
SetWorldToGridTransformation(const R2Affine& affine)
{
  // Set transformations
  world_to_grid_transform = affine;
  grid_to_world_transform = affine.Inverse();
  world_to_grid_scale_factor = affine.ScaleFactor();
}



void R2Grid::
SetWorldToGridTransformation(const R2Box& world_box)
{
  // Just checking
  if (grid_size == 0) return;
  if (world_box.NDimensions() < 2) return;

  // Compute grid origin
  R2Vector grid_diagonal(XResolution()-1, YResolution()-1);
  R2Vector grid_origin = 0.5 * grid_diagonal;

  // Compute world origin
  R2Vector world_diagonal(world_box.XLength(), world_box.YLength());
  R2Vector world_origin = world_box.Centroid().Vector();

  // Compute scale
  RNScalar scale = FLT_MAX;
  RNScalar xscale = grid_diagonal[0] / world_diagonal[0];
  if (xscale < scale) scale = xscale;
  RNScalar yscale = grid_diagonal[1] / world_diagonal[1];
  if (yscale < scale) scale = yscale;

  // Compute world-to-grid transformation
  R2Affine affine(R2identity_affine);
  affine.Translate(grid_origin);
  affine.Scale(scale);
  affine.Translate(-world_origin);

  // Set transformations
  SetWorldToGridTransformation(affine);
}



void R2Grid::
SetWorldToGridTransformation(const R2Point& world_origin, const R2Vector& world_xaxis, RNLength world_radius)
{
  // Just checking
  if (grid_size == 0) return;

  // Compute grid origin
  R2Vector grid_diagonal(XResolution()-1, YResolution()-1);
  R2Vector grid_origin = 0.5 * grid_diagonal;
  RNScalar grid_radius = grid_origin[0];
  if (grid_origin[1] < grid_radius) grid_radius = grid_origin[1];

  // Compute scale
  if (RNIsZero(world_radius)) return;
  if (RNIsZero(grid_radius)) return;
  RNScalar scale = grid_radius / world_radius;

  // Compute rotation
  RNAngle rotation = R2InteriorAngle(world_xaxis, R2posx_vector);
  if (world_xaxis.Y() < 0.0) rotation = RN_TWO_PI - rotation;

  // Compute world-to-grid transformation
  R2Affine affine(R2identity_affine);
  affine.Translate(grid_origin);
  affine.Rotate(-rotation);
  affine.Scale(scale);
  affine.Translate(-(world_origin.Vector()));

  // Set transformations
  SetWorldToGridTransformation(affine);
}



R2Point R2Grid::
WorldPosition(RNCoord x, RNCoord y) const
{
  // Transform point from grid coordinates to world coordinates
  R2Point world_point(x, y);
  world_point.Transform(grid_to_world_transform);
  return world_point;

}



R2Point R2Grid::
GridPosition(RNCoord x, RNCoord y) const
{
  // Transform point from world coordinates to grid coordinates
  R2Point grid_point(x, y);
  grid_point.Transform(world_to_grid_transform);
  return grid_point;
}



void R2Grid::
Capture(void)
{
  // Check image size
  GLint viewport[4];
  glGetIntegerv(GL_VIEWPORT, viewport);
  assert(grid_resolution[0] >= viewport[2]);
  assert(grid_resolution[1] >= viewport[3]);
  assert(grid_values);

  // Allocate pixels
  float *pixels = new float [ grid_size ];

  // Read pixels from frame buffer 
  glReadPixels(0, 0, grid_resolution[0], grid_resolution[1], GL_LUMINANCE, GL_FLOAT, pixels); 

  // Copy pixels
  for (int i = 0; i < grid_size; i++) grid_values[i] = pixels[i];

  // Delete pixels
  delete [] pixels;
}



void R2Grid::
DrawMesh(void) const
{
  // Push transformation
  grid_to_world_transform.Push();

  // Draw triangle strip
  for (int j = 1; j < grid_resolution[1]; j++) {
    glBegin(GL_TRIANGLE_STRIP);
    for (int i = 0; i < grid_resolution[0]; i++) {
      R3LoadPoint(i, j, GridValue(i, j));
      R3LoadPoint(i, j-1, GridValue(i, j-1));
    }
    glEnd();
  }

  // Pop transformation
  grid_to_world_transform.Pop();
}



void R2Grid::
DrawImage(int x, int y) const
{
  // Set projection matrix
  glMatrixMode(GL_PROJECTION);  
  glPushMatrix();
  glLoadIdentity();
  gluOrtho2D(0, grid_resolution[0], 0, grid_resolution[1]);

  // Set model view matrix
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glLoadIdentity();

  // Set position for image
  glRasterPos2i(x, y);

  // Allocate pixels
  float *pixels = new float [ grid_size ];

  // Copy pixels
  for (int i = 0; i < grid_size; i++) pixels[i] = (float) grid_values[i];

  // Draw pixels
  glDrawPixels(grid_resolution[0], grid_resolution[1], GL_LUMINANCE, GL_FLOAT, pixels); 

  // Delete pixels
  delete [] pixels;

  // Reset projection matrix
  glMatrixMode(GL_PROJECTION);
  glPopMatrix();

  // Reset model view matrix
  glMatrixMode(GL_MODELVIEW);
  glPopMatrix();
}



RNScalar R2Grid::
GridValue(RNScalar x, RNScalar y, RNLength sigma) const
{
  // Check if within bounds
  if ((x < 0) || (x > grid_resolution[0]-1)) return 0.0;
  if ((y < 0) || (y > grid_resolution[1]-1)) return 0.0;

  // Determine Gaussian filter extent
  int f = (int) (3 * sigma + 0.5);
  int xmin = (int) (x - f + 0.5);
  int xmax = (int) (x + f + 0.5);
  int ymin = (int) (y - f + 0.5);
  int ymax = (int) (y + f + 0.5);
  if (xmin < 0) xmin = 0;
  if (xmax > XResolution()-1) xmax = XResolution()-1;
  if (ymin < 0) ymin = 0;
  if (ymax > YResolution()-1) ymax = YResolution()-1;

  // Filter sample with Gaussian
  RNScalar sum = 0.0;
  RNScalar weight = 0.0;
  double denom = 2.0 * sigma * sigma;
  for (int j = ymin; j <= ymax; j++) {
    for (int i = xmin; i <= xmax; i++) {
      RNScalar value = GridValue(i, j);
      if (value != R2_GRID_UNKNOWN_VALUE) { 
        RNScalar dx = x - i;
        RNScalar dy = y - j;
        RNScalar d = dx*dx + dy*dy;
        RNScalar w = exp(-d / denom);
        sum += w * value;
        weight += w;
      }
    }
  }

  // Normalize value based on total weight of Gaussian samples
  if (weight > 0) return sum / weight;
  else return R2_GRID_UNKNOWN_VALUE;
}



void R2Grid::
ConnectedComponentCentroidFilter(RNScalar isolevel)
{
  // Compute connected components
  int *sizes = new int [ grid_size ];
  int *components = new int [ grid_size ];
  int ncomponents = ConnectedComponents(isolevel, grid_size, NULL, sizes, components);
  if (ncomponents == 0) { Clear(0); return; }

  // Allocate and initalize centroids
  int *centroid_counts = new int [ ncomponents ];
  R2Point *centroid_positions = new R2Point [ ncomponents ];
  RNScalar *centroid_values = new RNScalar [ ncomponents ];
  for (int i = 0; i < ncomponents; i++) {
    centroid_counts[i] = 0;
    centroid_positions[i] = R2zero_point;
    centroid_values[i] = 0;
  }

  // Sum positions in every connected component
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    int component = components[i];
    if ((component < 0) || (component >= ncomponents)) continue;
    int ix, iy;
    IndexToIndices(i, ix, iy);
    centroid_counts[component]++;
    centroid_positions[component][0] += ix;
    centroid_positions[component][1] += iy;
  }

  // Divide by counts
  for (int i = 0; i < ncomponents; i++) {
    if (centroid_counts[i] == 0) continue;
    centroid_positions[i] /= centroid_counts[i];
  }

  // Remember original values at centroids in grid
  for (int i = 0; i < ncomponents; i++) {
    if (centroid_counts[i] == 0) continue;
    int ix = (int) (centroid_positions[i][0] + 0.5);
    int iy = (int) (centroid_positions[i][1] + 0.5);
    if ((ix < 0) || (ix >= grid_resolution[0])) continue;
    if ((iy < 0) || (iy >= grid_resolution[1])) continue;
    centroid_values[i] = GridValue(ix, iy);
  }

  // Clear out entire grid
  Clear(0);

  // Set values at centroids of connected components
  for (int i = 0; i < ncomponents; i++) {
    if (centroid_counts[i] == 0) continue;
    int ix = (int) (centroid_positions[i][0] + 0.5);
    int iy = (int) (centroid_positions[i][1] + 0.5);
    if ((ix < 0) || (ix >= grid_resolution[0])) continue;
    if ((iy < 0) || (iy >= grid_resolution[1])) continue;
    SetGridValue(ix, iy, centroid_values[i]);
  }

  // Delete temporary memory
  delete [] centroid_counts;
  delete [] centroid_positions;
  delete [] centroid_values;
  delete [] components;
  delete [] sizes;
}



void R2Grid::
ConnectedComponentSizeFilter(RNScalar isolevel)
{
  // Compute connected components
  int *sizes = new int [ grid_size ];
  int *components = new int [ grid_size ];
  int ncomponents = ConnectedComponents(isolevel, grid_size, NULL, sizes, components);

  // Assign component sizes
  for (int i = 0; i < grid_size; i++) {
    if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
    int component = components[i];
    if ((component < 0) || (component >= ncomponents)) grid_values[i] = 0;
    else grid_values[i] = sizes[component];
  }

  // Delete temporary memory
  delete [] components;
  delete [] sizes;
}



void R2Grid::
ConnectedComponentFilter(RNScalar isolevel, RNArea min_grid_area, RNArea max_grid_area, 
  RNScalar under_isolevel_value, RNScalar too_small_value, RNScalar too_large_value)
{
  // Compute connected components
  int *sizes = new int [ grid_size ];
  int *components = new int [ grid_size ];
  int ncomponents = ConnectedComponents(isolevel, grid_size, NULL, sizes, components);

  // Mask values under isolevel
  if (under_isolevel_value != R2_GRID_KEEP_VALUE) {
    for (int i = 0; i < grid_size; i++) {
      if (grid_values[i] == R2_GRID_UNKNOWN_VALUE) continue;
      if (grid_values[i] < isolevel) grid_values[i] = under_isolevel_value;
    }
  }

  // Mask components with areas too small 
  if (too_small_value != R2_GRID_KEEP_VALUE) {
    for (int component = 0; component < ncomponents; component++) {
      int area = sizes[component];
      if (area < min_grid_area) {
        for (int i = 0; i < grid_size; i++) {
          if (components[i] == component) grid_values[i] = too_small_value;
        }
      }
    }
  }

  // Mask components with areas too large 
  if (too_large_value != R2_GRID_KEEP_VALUE) {
    for (int component = 0; component < ncomponents; component++) {
      int area = sizes[component];
      if (area > max_grid_area) {
        for (int i = 0; i < grid_size; i++) {
          if (components[i] == component) grid_values[i] = too_large_value;
        }
      }
    }
  }

  // Delete temporary memory
  delete [] components;
  delete [] sizes;
}



int R2Grid::
ConnectedComponents(RNScalar isolevel, int max_components, int *seeds, int *sizes, int *grid_components)
{
  // Allocate array of component identifiers
  int *components = grid_components;
  if (!grid_components) {
    components = new int [ grid_size ];
    assert(components);
  }

  // Initialize array of components
  for (int i = 0; i < grid_size; i++) 
    components[i] = -1;

  // Find connected components
  int ncomponents = 0;
  int next_seed = 0;
  while (TRUE){
    // Find unmarked seed
    int seed = next_seed;
    while (seed < grid_size) { 
      if ((components[seed] < 0) && (grid_values[seed] != R2_GRID_UNKNOWN_VALUE)&& (grid_values[seed] > isolevel)) break;
      seed++;
    }

    // Check if found a seed
    if (seed >= grid_size) break;

    // Flood fill marking all grid entries 6-connected to seed
    int x, y, neighbor;
    int size = 0;
    RNArray<RNScalar *> stack;
    stack.Insert(&grid_values[seed]);
    components[seed] = ncomponents;
    while (!stack.IsEmpty()) {
      // Pop top of stack
      RNScalar *c = stack.Tail();
      stack.RemoveTail();

      // Add grid entry to component
      int index = c - grid_values;
      assert(index >= 0);
      assert(index < grid_size);
      assert(components[index] == ncomponents);
      assert(grid_values[index] > isolevel);

      // Add neighbors to stack
      IndexToIndices(index, x, y);
      if (x > 0) {
        IndicesToIndex(x-1, y, neighbor);
        if ((components[neighbor] < 0) && (grid_values[neighbor] != R2_GRID_UNKNOWN_VALUE) && (grid_values[neighbor] > isolevel)) {
          components[neighbor] = ncomponents;
          stack.Insert(&grid_values[neighbor]);
        }
      }
      if (x < XResolution()-1) {
        IndicesToIndex(x+1, y, neighbor);
        if ((components[neighbor] < 0) && (grid_values[neighbor] != R2_GRID_UNKNOWN_VALUE)&& (grid_values[neighbor] > isolevel)) {
          components[neighbor] = ncomponents;
          stack.Insert(&grid_values[neighbor]);
        }
      }
      if (y > 0) {
        IndicesToIndex(x, y-1, neighbor);
        if ((components[neighbor] < 0) && (grid_values[neighbor] != R2_GRID_UNKNOWN_VALUE)&& (grid_values[neighbor] > isolevel)) {
          components[neighbor] = ncomponents;
          stack.Insert(&grid_values[neighbor]);
        }
      }
      if (y < YResolution()-1) {
        IndicesToIndex(x, y+1, neighbor);
        if ((components[neighbor] < 0) && (grid_values[neighbor] != R2_GRID_UNKNOWN_VALUE)&& (grid_values[neighbor] > isolevel)) { 
          components[neighbor] = ncomponents;
          stack.Insert(&grid_values[neighbor]);
        }
      }

      // Increment component_size
      size++;
    }

    // Update output variables
    if (ncomponents < max_components) {
      if (seeds) seeds[ncomponents] = seed;
      if (sizes) sizes[ncomponents] = size;
    }

    // Increment the number of components
    ncomponents++;
  }


  // Delete components
  if (!grid_components) delete [] components;

  // Return number of connected components found
  return ncomponents;
}



#if 0

static int
NextIndexCCW(const R2Grid *grid, int cur, int prev)
{
  // Get next index in CCW direction (used for generating isocontour)
  int next, next_x, next_y;
  int cur_x, cur_y, prev_x, prev_y;
  IndexToIndices(cur, cur_x, cur_y);
  IndexToIndices(prev, prev_x, prev_y);
  int dx = cur_x - prev_x;
  int dy = cur_y - prev_y;
  if (dx == 1) { next_x = cur_x; next_y = cur_y + 1; } 
  else if (dx == -1) { next_x = cur_x; next_y = cur_y - 1; } 
  else if (dy == 1) { next_x = cur_x - 1; next_y = cur_y; } 
  else if (dy == -1) { next_x = cur_x + 1; next_y = cur_y; } 
  else { next_x = cur_x + 1; next_y = cur_y; } 
  if ((next_x < 0) || (next_x >= grid->XResolution())) return -1;
  if ((next_y < 0) || (next_y >= grid->YResolution())) return -1;
  IndicesToIndex(next_x, next_y, next);
  return next;
}



struct ContourVertex {
  ContourVertex(int x, int y, int dim, int dir, RNScalar t) : x(x), y(y), dim(dim), dir(dir), t(t), mark(0) {};
  int x, y;
  int dim;
  int dir; 
  RNScalar t;
  int mark;
};



int R2Grid::
GenerateIsoContour(RNScalar isolevel, R2Point *points, int max_points) const
{
  // Initialize array of all vertices
  ContourVertex **vertices = new ContourVertex * [ 2 * grid_size ];
  for (int i = 0; i < 2 * grid_size; i++) vertices[i] = NULL;

  // Create vertices on horizontal edges
  for (int j = 0; j < grid_resolution[1]; j++) {
    for (int i = 0; i < grid_resolution[0]-1; i++) {
      RNScalar value1 = GridValue(i, j);
      RNScalar value2 = GridValue(i+1, j);
      RNScalar delta1 = isolevel - value1;
      RNScalar delta2 = isolevel - value2;
      if (delta1 < 0) {
        if (delta2 > 0) {
          RNScalar t = -delta1 / (delta2 - delta1);
          ContourVertex *vertex = new ContourVertex(i+1,j,RN_X,-1,t);
          vertices[j*grid_resolution[0]+i] = vertex;
        }
      }
      else if (delta1 > 0) {
        if (delta2 < 0) {
          RNScalar t = delta1 / (delta1 - delta2);
          ContourVertex *vertex = new ContourVertex(i,j,RN_X,1,t);
          vertices[j*grid_resolution[0]+i] = vertex;
        }
      }
    }
  }

  // Create vertices on vertical edges
  ContourVertex **vverts = new ContourVertex * [ grid_size ];
  for (int i = 0; i < grid_resolution[0]; i++) {
    for (int j = 0; j < grid_resolution[1]-1; j++) {
      RNScalar value1 = GridValue(i, j);
      RNScalar value2 = GridValue(i, j+1);
      RNScalar delta1 = isolevel - value1;
      RNScalar delta2 = isolevel - value2;
      if (delta1 < 0) {
        if (delta2 > 0) {
          RNScalar t = -delta1 / (delta2 - delta1);
          ContourVertex *vertex = new ContourVertex(i,j+1,RN_Y,-1,t);
          vertices[grid_size + j*grid_resolution[0]+i] = vertex;
        }
      }
      else if (delta1 > 0) {
        if (delta2 < 0) {
          RNScalar t = delta1 / (delta1 - delta2);
          ContourVertex *vertex = new ContourVertex(i,j,RN_Y,1,t);
          vertices[grid_size + j*grid_resolution[0]+i] = vertex;
        }
      }
    }
  }

  // Trace contour boundaries
  for (int start = 0; start < 2 * grid_size; start++) {
    if (vertices[start] == NULL) continue;
    if (vertices[start]->mark) continue;

    // Insert start vertex 
    RNArray<ContourVertex *> contour;
    ContourVertex *start_vertex = vertices[start];
    contour.Insert(start_vertex);
    start_vertex->mark = 1;

    // Traverse boundary
    ContourVertex *vertex = start_vertex;
    do {
      // Get convenient variables
      int x = vertex->x;
      int y = vertex->y;
      int dim = vertex->dim;
      int dir = vertex->dir;

      // Get neighbor vertex
      ContourVertex *neighbor = NULL;
      if (dim == RN_X) {
        if (dir == -1) {
          assert((GridValue(x-1,y) < isolevel) && (GridValue(x,y) > isolevel));
          neighbor1 = vertices[ grid_size + (y
        }
        else {
        }
      }
      int x1 = (vertex->dim == RN_X) ? vertex->x + vertex->dir : vertex->x;
      int y1 = (vertex->dim == RN_Y) ? vertex->y + vertex->dir : vertex->y;



    } while(vertex != vertices[start]);
          
    int prev = marks[start];
    for (int i = 0; i < 4; i++) {
      int index = NextIndexCCW(this, start, prev);
      if (index < 0) break;
      if (components[index] != c) break; 
      prev = index;
    }

    // Check if found previous neighbor
    if (prev == -1) continue;
    if (prev == start) continue;

    // Walk along boundary counter-clockwise outputing points
    do {
      int neighbor = prev;
      for (int i = 0; i < 4; i++) {
        neighbor = NextIndexCCW(this, current, neighbor);
        if (components[index] != c) {
          // Output a point
        }
        else {
          // Move to next grid cell
          prev = current;
          current = neighbor;
          break;
        }
      }
    } while (current != start);
  }

  // Return number of points
  return npoints;
}

#endif



int R2Grid::
Read(const char *filename)
{
  // Parse input filename extension
  const char *input_extension;
  if (!(input_extension = strrchr(filename, '.'))) {
    fprintf(stderr, "Input file has no extension (e.g., .pfm).\n");
    return 0;
  }
  
  // Read file of appropriate type
  if (!strncmp(input_extension, ".raw", 4)) return ReadRAW(filename);
  else if (!strncmp(input_extension, ".pfm", 4)) return ReadPFM(filename);
  else if (!strncmp(input_extension, ".grd", 4)) return ReadGRD(filename);
  else return ReadImage(filename);
  
  // Should never get here
  fprintf(stderr, "Unrecognized image file extension");
  return 0;
}



int R2Grid::
Write(const char *filename) const
{
  // Parse input filename extension
  const char *input_extension;
  if (!(input_extension = strrchr(filename, '.'))) {
    fprintf(stderr, "Input file has no extension (e.g., .pfm).\n");
    return 0;
  }
  
  // Write file of appropriate type
  if (!strncmp(input_extension, ".raw", 4)) return WriteRAW(filename);
  else if (!strncmp(input_extension, ".pfm", 4)) return WritePFM(filename);
  else if (!strncmp(input_extension, ".grd", 4)) return WriteGRD(filename);
  else return WriteImage(filename);

  // Should never get here
  fprintf(stderr, "Unrecognized image file extension");
  return 0;
}



////////////////////////////////////////////////////////////////////////
// RAW FORMAT READ/WRITE
////////////////////////////////////////////////////////////////////////

int R2Grid::
ReadRAW(const char *filename)
{
  // Open file
  FILE *fp = fopen(filename, "rb");
  if (!fp) {
    fprintf(stderr, "Unable to open raw image file %s", filename);
    return 0;
  }

  // Read header
  unsigned int header[4];
  if (fread(header, sizeof(unsigned int), 4, fp) != 4) {
    fprintf(stderr, "Unable to header to raw image file %s", filename);
    return -1;
  }

  // Check magic number
  const unsigned int magic = 54321;
  if (header[0] != magic) {
    fprintf(stderr, "Invalid header in raw image file %s", filename);
    return -1;
  }

  // Check number of channels
  if (header[3] != 1) {
    fprintf(stderr, "Invalid number of channels (%d)in raw image file %s", header[3], filename);
    return -1;
  }

  // Parse header
  int width = header[1];
  int height = header[2];

  // Allocate pixels
  unsigned int npixels = width * height;
  float *pixels = new float [ npixels ];
  if (!pixels) {
    fprintf(stderr, "Unable to allocate data for raw image file %s", filename);
    return -1;
  }

  // Read pixels
  int count = npixels;
  while (count > 0) {
    int n = fread(&pixels[npixels-count], sizeof(float), count, fp);
    if (n < 0) { fprintf(stderr, "Unable to read pixels from %s\n", filename); return 0; }
    count -= n;
  }

  // Fill in grid info
  grid_resolution[0] = width;
  grid_resolution[1] = height;
  grid_row_size = width;
  grid_size = npixels;
  world_to_grid_scale_factor = 1;
  world_to_grid_transform = R2identity_affine;
  grid_to_world_transform = R2identity_affine;
  if (grid_values) delete [] grid_values;
  grid_values = new RNScalar [ grid_size ];
  for (int i = 0; i < grid_size; i++) {
    if (RNIsEqual(pixels[i], R2_GRID_UNKNOWN_VALUE)) grid_values[i] = R2_GRID_UNKNOWN_VALUE;
    else grid_values[i] = pixels[i];
  }

  // Delete pixels
  delete [] pixels;

  // Close file
  fclose(fp);

  // Return success
  return grid_size;
}


int R2Grid::
WriteRAW(const char *filename) const
{
  // Open file
  FILE *fp = fopen(filename, "wb");
  if (!fp) {
    fprintf(stderr, "Unable to open raw image file %s", filename);
    return 0;
  }

  // Write header
  const unsigned int magic = 54321;
  unsigned int header[4] = { magic, grid_resolution[0], grid_resolution[1], 1 };
  if (fwrite(header, sizeof(unsigned int), 4, fp) != 4) {
    fprintf(stderr, "Unable to header to raw image file %s", filename);
    return -1;
  }

  // Allocate pixels
  float *pixels = new float [ grid_row_size ];

  // Write pixels (row by row to avoid large buffers)
  for (int i = 0; i < grid_resolution[1]; i++) {
    RNScalar *valuesp = &grid_values[i * grid_resolution[0]];
    for (int i = 0; i < grid_resolution[0]; i++) pixels[i] = (float) valuesp[i];
    if (fwrite(pixels, sizeof(float), grid_resolution[0], fp) != (unsigned int) grid_resolution[0]) {
      fprintf(stderr, "Unable to write grid values to file %s\n", filename);
      return 0;
    }
  }

  // Delete pixels
  delete [] pixels;

  // Close file
  fclose(fp);

  // Return success
  return grid_size;
}




////////////////////////////////////////////////////////////////////////
// PFM FORMAT READ/WRITE
////////////////////////////////////////////////////////////////////////

int R2Grid::
ReadPFM(const char *filename)
{
  // Open file
  FILE *fp = fopen(filename, "rb");
  if (!fp) {
    fprintf(stderr, "Unable to open image: %s\n", filename);
    return 0;
  }

  // Read header
  int width, height;
  float endian;

#if 0
  char magic[256];
  fscanf(fp, "%s", magic);
  fscanf(fp, "%d%d", &width, &height);
  fscanf(fp, "%f\n", &endian);

  // Check magic keyword
  if (strcmp(magic, "Pf")) {
    fprintf(stderr, "Bad magic keyword in %s\n", filename);
    return 0;
  }
#else
  // Read magic header
  int c;
  c = fgetc(fp); if (c != 'P') { fprintf(stderr, "Bad magic keyword in %s\n", filename); return 0; }
  c = fgetc(fp); if (c != 'f') { fprintf(stderr, "Bad magic keyword in %s\n", filename); return 0; }
  c = fgetc(fp); if (c != '\n') { fprintf(stderr, "Bad magic keyword in %s\n", filename); return 0; }

  // Read width
  int width_count = 0;
  char width_string[256];
  for (int i = 0; i < 256; i++) { 
    c = fgetc(fp); 
    if ((c == ' ') && (width_count == 0)) { continue; }
    else if ((c == ' ') || (c == '\n')) { width_string[width_count] = '\0'; break; }
    else if (!isdigit(c)) { fprintf(stderr, "Bad width character %c in %s\n", c, filename); return 0; }
    else width_string[width_count++] = c;
  }

  // Check width
  if ((width_count == 0) || (width_count > 128)) {
    fprintf(stderr, "Error reading width in %s\n", filename); 
    return 0; 
  }

  // Read height
  int height_count = 0;
  char height_string[256];
  for (int i = 0; i < 256; i++) { 
    c = fgetc(fp); 
    if ((c == ' ') && (height_count == 0)) { continue; }
    else if ((c == ' ') || (c == '\n')) { height_string[height_count] = '\0'; break; }
    else if (!isdigit(c)) { fprintf(stderr, "Bad height character %c in %s\n", c, filename); return 0; }
    else height_string[height_count++] = c;
  }

  // Check height
  if ((height_count == 0) || (height_count > 128)) {
    fprintf(stderr, "Error reading height in %s\n", filename); 
    return 0; 
  }

  // Read endian
  int endian_count = 0;
  char endian_string[256];
  for (int i = 0; i < 256; i++) { 
    c = fgetc(fp); 
    if ((c == ' ') && (endian_count == 0)) { continue; }
    else if ((c == ' ') || (c == '\n')) { endian_string[endian_count] = '\0'; break; }
    if (!isdigit(c) && (c != '.') && (c != '-')) { fprintf(stderr, "Bad endian character %c in %s\n", c, filename); return 0; }
    endian_string[endian_count++] = c;
  }

  // Check endian
  if ((endian_count == 0) || (endian_count > 128)) {
    fprintf(stderr, "Error reading endian in %s\n", filename); 
    return 0; 
  }

  // Parse values
  width = atoi(width_string);
  height = atoi(height_string);
  endian = (float) atof(endian_string);
#endif

  // Allocate pixels
  int npixels = width * height;
  float *pixels = new float [ npixels ];
  if (!pixels) {
    fprintf(stderr, "Unable to allocate pixels for %s\n", filename);
    return 0;
  }

  // Read pixels
  int count = npixels;
  while (count > 0) {
    int n = fread(&pixels[npixels-count], sizeof(float), count, fp);
    if (n <= 0) { fprintf(stderr, "Unable to read pixels from %s\n", filename); return 0; }
    printf("%d %d\n", n, count);
    count -= n;
  }

  // Fill in grid info
  grid_resolution[0] = width;
  grid_resolution[1] = height;
  grid_row_size = width;
  grid_size = npixels;
  world_to_grid_scale_factor = 1;
  world_to_grid_transform = R2identity_affine;
  grid_to_world_transform = R2identity_affine;
  if (grid_values) delete [] grid_values;
  grid_values = new RNScalar [ grid_size ];
  for (int i = 0; i < grid_size; i++) {
    if (RNIsEqual(pixels[i], R2_GRID_UNKNOWN_VALUE)) grid_values[i] = R2_GRID_UNKNOWN_VALUE;
    else grid_values[i] = pixels[i];
  }

  // Delete pixels
  delete [] pixels;

  // Close image
  fclose(fp);

  // Return success
  return grid_size;
}



int R2Grid::
WritePFM(const char *filename) const
{
  // Open file
  FILE *fp = fopen(filename, "wb");
  if (!fp) {
    fprintf(stderr, "Unable to open pfm image file %s", filename);
    return 0;
  }

  // Write header
  fprintf(fp, "Pf\n");
  fprintf(fp, "%d %d\n", grid_resolution[0], grid_resolution[1]);
  fprintf(fp, "-1.0\n");

  // Allocate pixels
  float *pixels = new float [ grid_resolution[0] ];

  // Write pixels (row by row to avoid large buffers)
  for (int i = 0; i < grid_resolution[1]; i++) {
    RNScalar *valuesp = &grid_values[i * grid_resolution[0]];
    for (int i = 0; i < grid_resolution[0]; i++) pixels[i] = valuesp[i];
    if (fwrite(pixels, sizeof(float), grid_resolution[0], fp) != (unsigned int) grid_resolution[0]) {
      fprintf(stderr, "Unable to write grid values to file %s\n", filename);
      return 0;
    }
  }

  // Delete pixels
  delete [] pixels;

  // Close file
  fclose(fp);

  // Return success
  return grid_size;
}




////////////////////////////////////////////////////////////////////////
// GRD FORMAT READ/WRITE
////////////////////////////////////////////////////////////////////////

int R2Grid::
ReadGRD(const char *filename)
{
  // Open file
  FILE *fp = fopen(filename, "rb");
  if (!fp) {
    RNFail("Unable to open file %s", filename);
    return 0;
  }

  // Read grid resolution from file
  if (fread(&grid_resolution, sizeof(int), 2, fp) != 2) {
    RNFail("Unable to read resolution from grid file");
    return 0;
  }

  // Update grid resolution variables
  grid_row_size = grid_resolution[0];
  grid_size = grid_row_size * grid_resolution[1];
  if (grid_size <= 0) {
    RNFail("Invalid grid size (%d) in file", grid_size);
    return 0;
  }

  // Read world_to_grid transformation from file
  RNScalar m[9];
  if (fread(m, sizeof(RNScalar), 9, fp) != 9) {
    RNFail("Unable to read transformation matrix from file");
    return 0;
  }

  // Update transformation variables
  world_to_grid_transform.Reset(R3Matrix(m));
  world_to_grid_scale_factor = world_to_grid_transform.ScaleFactor();
  grid_to_world_transform = world_to_grid_transform.Inverse();

  // Allocate grid values
  grid_values = new RNScalar [ grid_size ];
  assert(grid_values);

  // Read values
  int count = grid_size;
  while (count > 0) {
    int n = fread(&grid_values[grid_size-count], sizeof(RNScalar), count, fp);
    if (n < 0) { fprintf(stderr, "Unable to read pixels of %s\n", filename); return 0; }
    count -= n;
  }

  // Close file
  fclose(fp);

  // Just to be sure
  for (int i = 0; i < grid_size; i++) {
    if (RNIsEqual(grid_values[i], R2_GRID_UNKNOWN_VALUE)) grid_values[i] = R2_GRID_UNKNOWN_VALUE;
  }

  // Return number of grid values read
  return grid_size;
}



int R2Grid::
WriteGRD(const char *filename) const
{
  // Open file
  FILE *fp = fopen(filename, "wb");
  if (!fp) {
    RNFail("Unable to open file %s", filename);
    return 0;
  }

  // Write grid resolution from file
  if (fwrite(&grid_resolution, sizeof(int), 2, fp) != 2) {
    RNFail("Unable to write resolution to file");
    return 0;
  }

  // Write world_to_grid transformation to file
  const RNScalar *m = &(world_to_grid_transform.Matrix()[0][0]);
  if (fwrite(m, sizeof(RNScalar), 9, fp) != 9) {
    RNFail("Unable to write transformation matrix to file");
    return 0;
  }

  // Write grid values (row by row to avoid large buffers)
  for (int i = 0; i < grid_resolution[1]; i++) {
    RNScalar *row_values = &grid_values[i * grid_resolution[0]];
    if (fwrite(row_values, sizeof(RNScalar), grid_resolution[0], fp) != (unsigned int) grid_resolution[0]) {
      RNFail("Unable to write grid values to file %s\n", filename);
      return 0;
    }
  }

  // Close file
  fclose(fp);

  // Return number of grid values written
  return grid_size;
}



////////////////////////////////////////////////////////////////////////
// IMAGE READ/WRITE
////////////////////////////////////////////////////////////////////////

int R2Grid::
ReadImage(const char *filename)
{
  // Allocate image
  R2Image *image = new R2Image();
  if (!image) {
    fprintf(stderr, "Unable to allocate image for %s\n", filename);
    return 0;
  }

  // Read image
  if (!image->Read(filename)) { 
    // fprintf(stderr, "Unable to read image from %s\n", filename);
    delete image; 
    return 0; 
  }

  // Fill in grid info
  grid_resolution[0] = image->Width();
  grid_resolution[1] = image->Height();
  grid_row_size = image->Width();
  grid_size = image->Width() * image->Height();
  world_to_grid_scale_factor = 1;
  world_to_grid_transform = R2identity_affine;
  grid_to_world_transform = R2identity_affine;
  if (grid_values) delete [] grid_values;
  grid_values = new RNScalar [ grid_size ];
  for (int j = 0; j < image->Height(); j++) {
    for (int i = 0; i < image->Width(); i++) {
      SetGridValue(i, j, image->PixelRGB(i, j).Luminance());
    }
  }

  // Delete image
  delete image;

  // Return success
  return 1;
}



int R2Grid::
WriteImage(const char *filename) const
{
  // Allocate image
  R2Image *image = new R2Image(grid_resolution[0], grid_resolution[1], 3);
  if (!image) {
    fprintf(stderr, "Unable to allocate image for %s\n", filename);
    return 0;
  }

  // Fill image pixel values
  RNInterval range = Range();
  if (range.Diameter() > 0) {
    for (int j = 0; j < image->Height(); j++) {
      for (int i = 0; i < image->Width(); i++) {
        RNScalar value = (GridValue(i, j) - range.Min()) / (range.Max() - range.Min());
        image->SetPixelRGB(i, j, RNRgb(value, value, value));
      }
    }
  }

  // Write image
  if (!image->Write(filename)) { 
    // fprintf(stderr, "Unable to write image to %s\n", filename);
    delete image; 
    return 0; 
  }

  // Delete image
  delete image;

  // Return success
  return 1;
}



////////////////////////////////////////////////////////////////////////
// OTHER I/O FUNCTIONS
////////////////////////////////////////////////////////////////////////

int R2Grid::
Print(FILE *fp) const
{
  // Check file
  if (!fp) fp = stdout;

  // Print values
  for (int j = 0; j < YResolution(); j++) {
    for (int i = 0; i < XResolution(); i++) {
      fprintf(fp, "%g ", GridValue(i, j));
    }
    fprintf(fp, "\n");
  }

  // Return number of grid values written
  return grid_size;
}




