marching.c

Trung Nguyen 编写的一个简单的fast marching 算法

/*--------------------------- Commande MegaWave -----------------------------*/
/* mwcommand
   name = {marching};
   author = {"Trung Nguyen"};
   version = {"1.0"};
   function = {"Fast Marching Level Set"};
   usage = {   
     'l':[l=-1]->l               "level to print out (default: all levels)",
     'e':error<-error                  "image of error",
     in->in                      "input Fcurve",
     size->size                  "size of image output",
     out<-out                    "distance image"
   };
*/


#include <stdio.h>
#include <math.h>
#include <time.h>
#include "mw.h"

#define PI 3.141592
#define INFINITY 10000.0

#define max(x, y) (x > y ? x : y)
#define min(x, y) (x < y ? x : y)

#define ARRAY_2D_ALLOC(array, iWidth, iHeight, type) \
(array) = (type**) malloc((iHeight)*sizeof(type*)); \
if((array) == NULL) mwerror(FATAL, 1, "ARRAY_2D_ALLOC --> memory"); \
(array)[0] = (type*) calloc((iWidth)*(iHeight), sizeof(type)); \
if((array)[0] == NULL) mwerror(FATAL, 1, "ARRAY_2D_ALLOC --> memory"); \
for(i = 1; i < (iHeight); i++) \
  (array)[i] = (array)[i-1]+(iWidth);

enum Status {ALIVE, NARROW_BAND, FAR_AWAY};
int MAX;

typedef struct {
  int    x;
  int    y;
} Element;

Element*        heap;
int*            heap_size;
enum Status**   stat;
double**        T;        /* distance of points */
int**           pos;         /* position of a grid point in the heap */



float sqr(float x) { return (x * x); }


/*** Manipulate a heap at node i ***/
void Heapify(Element* h, int N, int i) {
  int       l, r, smallest;
  Element   tmp;

  l = 2 * i; 
  r = 2 * i + 1;

  /* Find the smallest value between 3 nodes */
  if (l <= *heap_size && T[h[l].x + MAX/2][h[l].y + MAX/2] < T[h[i].x + MAX/2][h[i].y + MAX/2])
    smallest = l;
  else smallest = i;  
  if (r <= *heap_size && T[h[r].x + MAX/2][h[r].y + MAX/2] < T[h[smallest].x + MAX/2][h[smallest].y + MAX/2]) 
    smallest = r;

  if (smallest != i) {    
    tmp = h[i];
    h[i] = h[smallest];
    h[smallest] = tmp;
    pos[h[i].x + MAX/2][h[i].y + MAX/2] = i;
    pos[h[smallest].x + MAX/2][h[smallest].y + MAX/2] = smallest;

    Heapify(h, N, smallest);
  }    
}


/* Extract the smallest value of a heap */
Element Heap_Extract_Min(Element* h, int *N) {
  Element min;
  
  if (*N < 1) {
    printf("error: heap underflow\n");
    return min;
  }
  
  min = h[1];
  pos[h[1].x + MAX/2][h[1].y + MAX/2] = 0;
  h[1] = h[*N];
  pos[h[1].x + MAX/2][h[1].y + MAX/2] = 1;
  
  (*N)--;

  Heapify(h, *N, 1);  
  return min;
}


/*** Insert an element into a heap ***/
void Heap_Insert(Element* h, int* N, Element e) {
  int i;

  i = (N != NULL) ? ++(*N) : 0;
  
  /* Find the best place for e */
  while (i > 1 && T[h[i / 2].x + MAX/2][h[i / 2].y + MAX/2] > T[e.x + MAX/2][e.y + MAX/2]) {
    h[i] = h[i / 2];
    pos[h[i].x + MAX/2][h[i].y + MAX/2] = i;
    i = i / 2;
  }
  
  h[i] = e;
  pos[e.x + MAX/2][e.y + MAX/2] = i;
}


/*** Return G(A, B, C, D) at the value T ***/
float G(float A, float B, float C, float D, float T) {
  return (sqr(max(max(T - B, 0), -min(D - T, 0))) +
    sqr(max(max(T - A, 0), -min(C - T, 0))));
}


/*** Approximate the gradient of a point (cf. Rouy and Tourin) ***/
float Distance(float A, float B, float C, float D, float F) {
  float low = -INFINITY;
  float upp = INFINITY;
  float time;
  time = (low + upp) / 2;
  while (low < upp &&  G(A, B, C, D, time) != F) 
    if (G(A, B, C, D, time) < F) {
      low = time + 0.0001;
      time = (low + upp) / 2;
    }
    else {
      upp = time - 0.0001;
      time = (low + upp) / 2;
    }      
  return time;  
}


/*** Marching algorithm (cf. Sethian) ***/
void Marching() {
  Element p, e;
  int i, j, k, fix;

  while (*heap_size > 0) {
    p = Heap_Extract_Min(heap, heap_size);
    stat[p.x + MAX/2][p.y + MAX/2] = ALIVE;
    
    for (k = 0; k < 4; k++) {
      i = (k < 2 ? (k * 2 - 1) : 0); 
      j = (k < 2 ? 0 : k * 2 - 5);  
      if (p.x + i - 1 > -MAX/2 && p.x + i + 1 < MAX/2 && 
	  p.y + j - 1 > -MAX/2 && p.y + j  + 1< MAX/2 &&
	  stat[p.x + i + MAX/2][p.y + j + MAX/2] != ALIVE) {
	
	T[p.x + i + MAX/2][p.y + j + MAX/2] 
	  = Distance(T[p.x + i + MAX/2][p.y + j - 1 + MAX/2],
		     T[p.x + i - 1 + MAX/2][p.y + j + MAX/2],
		     T[p.x + i + MAX/2][p.y + j + 1 + MAX/2],
		     T[p.x + i + 1 + MAX/2][p.y + j + MAX/2], 1.0);	

	for (fix = pos[p.x + i + MAX/2][p.y + j + MAX/2] / 2; fix >= 1; fix = fix / 2) 
	  Heapify(heap, *heap_size, fix);

	if (stat[p.x + i + MAX/2][p.y + j + MAX/2] == FAR_AWAY) {
	  stat[p.x + i + MAX/2][p.y + j + MAX/2] = NARROW_BAND;
	  e.x = p.x + i;
	  e.y = p.y + j;
	  Heap_Insert(heap, heap_size, e);
	}	
      }
    } 
  }
}


/*** MAIN PROGRAM ***/
void marching(in, l, size, out, error) 
     Fcurve in;
     Fimage out, error;
     int size;
     int *l;
{ 
  int x, y, d, i, j, nb_points = 0;
  double medx = 0, medy = 0;
  float sum_error = 0;
  Point_fcurve p;
  Element e;
  int print_error;
  clock_t tick;

  if (error) print_error = 1; else print_error = 0;

  MAX = size + 2;

  ARRAY_2D_ALLOC(T, MAX, MAX, double);
  ARRAY_2D_ALLOC(pos, MAX, MAX, int);
  ARRAY_2D_ALLOC(stat, MAX, MAX, enum Status);


  for (i = 0; i < MAX; i++)
    for (j = 0; j < MAX; j++) {
      stat[i][j] = FAR_AWAY;
      T[i][j] = INFINITY;
    }  

  heap = (Element*) malloc(MAX * MAX * sizeof(Element));
  heap_size = (int*) malloc(sizeof(int));
  *heap_size = 0;
  
  for (p = in->first; p; p = p->next) {
    medx += p->x;
    medy += p->y;
    nb_points++;
  }

  medx /= nb_points;
  medy /= nb_points;

  for (p = in->first; p; p = p->next) {
    x = (int)(p->x - medx);
    y = (int)(p->y - medy);
    if (x + MAX/2 < 0 || y + MAX /2 < 0 || x + MAX/2 > MAX || y + MAX/2 > MAX)
      mwerror(FATAL, 1, "Output image size is too small");
    stat[x + MAX/2][y + MAX/2] = ALIVE;
    T[x + MAX/2][y + MAX/2] = 0.0;
  }
  
  for (x = -MAX/2; x < MAX/2; x++)
    for (y = -MAX/2; y < MAX/2; y++) 
      if (stat[x + MAX/2][y + MAX/2] == ALIVE) {
	for (i = -1; i <= 1; i++)
	  for (j = -1; j <= 1; j++)
	    if (stat[x + i + MAX/2][y + j + MAX/2] == FAR_AWAY)
	      if (x * x + y * y < (x + i) * (x + i) + (y + j) * (y + j)) {
		stat[x + i + MAX/2][y + j + MAX/2] = NARROW_BAND;
		T[x + i + MAX/2][y + j + MAX/2] = i * i + j * j;
		e.x = x + i;
		e.y = y + j;
		Heap_Insert(heap, heap_size, e);
	      }	    
      }
  
  Marching();

  if  ((out = mw_change_fimage(out ,size, size)) == NULL)
   mwerror(FATAL, 1, "Not enough memory !\n");

  if  ((error = mw_change_fimage(error, size, size)) == NULL)
   mwerror(FATAL, 1, "Not enough memory !\n");
  
  for (x = 1; x < MAX - 1; x++)
    for (y = 1; y < MAX - 1; y++) {
      error->gray[(MAX - y - 2) * size + x - 1] = (T[x][y] == INFINITY) ? 0 :  fabs(T[x][y] - sqrt(sqr(x - MAX/2) + sqr(y - MAX/2)));
      sum_error += sqr(error->gray[(MAX - y - 2) * size + x - 1]);

      if (*l == -1)
	out->gray[(MAX - y - 2) * size + x - 1] = (T[x][y] == INFINITY) ? 0 : T[x][y];
      else
	if (((int)(T[x][y])) == *l) 
	  out->gray[(MAX - y - 2) * size + x - 1] = 255;
	else
	  out->gray[(MAX - y - 2) * size + x - 1] = 0;
    }

  if (print_error)
    printf("Total squared error: %f\n", sum_error / (size * size));

  free(heap);
  free(heap_size);
  free(stat[0]);
  free(stat);
  free(T[0]);
  free(T);
  free(pos[0]);
  free(pos);

  printf("%f seconds\n", (float)clock() / CLOCKS_PER_SEC);
}