/*--------------------------- 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);}