/****************************************************************
3D Euclidean Distance Transform Class
Last modified: May 1, 2014 

Functions computing DT are derived from "dt.c" written by 
Alexander Vasilevskiy.

Jiaolong Yang <yangjiaolong@gmail.com>
****************************************************************/
#pragma once
#include <math.h>
#include <stdio.h>
#include <malloc.h>
#include <stdlib.h>
#include <memory.h>

#define FALSE 0
#define TRUE 1

#define sqrt1(x) sqrt((double)x)
#define ROUND(x) (int((x)+0.5))
#define FLOOR(x) (int((x)))
#define CEIL(x) (int((x+0.99999)))
#define infty 32767 // Max value for a signed short (2 bytes / 16 bits)

typedef struct DEucl
{
 int v,h;
 float distance;
}DEucl;

typedef struct DEucl3D
{
 short v,h,d;
 float distance;
}DEucl3D;


template <class T>
struct Array3d {
  int Xdim, Ydim, Zdim;
  T *** data;
  T * data_array;

  void Init(int x, int y, int z);

  // Call this printslice;
  void printArrayDE(int x);
  Array3d()
  {
	  data =  NULL;
	  data_array = NULL;
  }
  ~Array3d()
  {
   if (data && data[0])
	   delete(data[0]);
   if(data)
	   delete(data);
   if(data_array)
	   delete(data_array);
  }
};

template <class T>
void Array3d<T>::Init(int x, int y, int z)
{
	Xdim = x;
	Ydim = y;
	Zdim = z;
  // allocate the memory for the first level pointers.
  int n1 = z, n2 = y, n3 = x;

  data = new T** [n1];

  // set the first level pointers and allocate the memory for the second level pointers.
  {
    data[0] = new T* [n1 * n2];
    for (int row1_index = 0; row1_index < n1; row1_index++)
      data [row1_index] = data[0] + n2 * row1_index;
  }

  T* array_ptr = new T [n1*n2*n3];
  data_array = array_ptr;

  // set the second level pointers.
  for (int row1_index = 0; row1_index < n1; row1_index++)
    for (int row2_index = 0; row2_index < n2; row2_index++) {
      data [row1_index][row2_index] = array_ptr;
      array_ptr += n3;
    }
}

template <class T>
void Array3d<T>::printArrayDE(int x)
{
  int z,y;
  printf("slice %d",x);
  for (y=0;y<Ydim;y++) {
    printf("\n");
    for(z=0;z<Zdim;z++) 
      if (data[z][y][x].distance>=infty) 
	printf("(*,*,*)"); 
      else 
	printf("(%d,%d,%d)",data[z][y][x].v,data[z][y][x].h,data[z][y][x].d,data[z][y][x].distance);
  }
  printf("\nDONE\n");
}

typedef Array3d<DEucl3D> Array3dDEucl3D;
typedef Array3d<float> Array3dfloat;

class DT3D{
public:
	DT3D();
	int SIZE;
	double scale;
	double expandFactor;
	double xMin, xMax, yMin, yMax, zMin, zMax;
	void Build(double* x, double* y, double* z, int num);
	float Distance(double x, double y, double z);
private:
	Array3dDEucl3D A;
};


/********************************************************/


// ************************************
//
//       D-Euclidean BEGIN
//
// ************************************


void initDE(Array3dDEucl3D & inDE){
  int z,y,x;
  for (z=0;z<inDE.Zdim;z++)
    for(y=0;y<inDE.Ydim;y++)
      for(x=0;x<inDE.Xdim;x++) {
	inDE.data[z][y][x].v=infty;
	inDE.data[z][y][x].h=infty;
	inDE.data[z][y][x].d=infty;
	inDE.data[z][y][x].distance=infty;
      }
  inDE.data[2][2][3].v=0;
  inDE.data[2][2][3].h=0;
  inDE.data[2][2][3].d=0;
  inDE.data[2][2][3].distance=0.0;
}

DEucl3D MINforwardDE3(Array3dDEucl3D& A, int z,int y,int x)
{
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  //              MASK:
  //
  //             (0,0,0)(0,0,1)
  //      (0,1,1)(0,1,0)(0,1,1)
  //

  DEucl3D mask[5];
  DEucl3D min;
  int looper;
  min.distance=infty;

  if (z<Zdim-1) {
    mask[0].v=inDE[z+1][y][x].v;
    mask[0].h=inDE[z+1][y][x].h;
    mask[0].d=inDE[z+1][y][x].d+1;
    mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d);
  }
  else {
    mask[0].v=infty;
    mask[0].h=infty;
    mask[0].d=infty;
    mask[0].distance=infty;
  }

  if ((y<Ydim-1)&&(z<Zdim-1)) {
    mask[1].v=inDE[z+1][y][x].v;
    mask[1].h=inDE[z+1][y][x].h+1;
    mask[1].d=inDE[z+1][y][x].d+1;
    mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d);
  }
  else {
    mask[1].v=infty;
    mask[1].h=infty;
    mask[1].d=infty;
    mask[1].distance=infty;

  }

  if (y<Ydim-1) {
    mask[2].v=inDE[z][y+1][x].v;
    mask[2].h=inDE[z][y+1][x].h+1;
    mask[2].d=inDE[z][y+1][x].d;
    mask[2].distance=sqrt1(mask[2].v*mask[2].v+mask[2].h*mask[2].h+mask[2].d*mask[2].d);
  }
  else {
    mask[2].v=infty;
    mask[2].h=infty;
    mask[2].d=infty;
    mask[2].distance=infty;
  }
  mask[3].v=inDE[z][y][x].v;
  mask[3].h=inDE[z][y][x].h;
  mask[3].d=inDE[z][y][x].d;
  mask[3].distance=sqrt1(mask[3].v*mask[3].v+mask[3].h*mask[3].h+mask[3].d*mask[3].d);
  if ((z>0)&&(y<Ydim-1)) {
    mask[4].v=inDE[z-1][y+1][x].v;
    mask[4].h=inDE[z-1][y+1][x].h+1;
    mask[4].d=inDE[z-1][y+1][x].d+1;
    mask[4].distance=sqrt1(mask[4].v*mask[4].v+mask[4].h*mask[4].h+mask[4].d*mask[4].d);
  }
 else {
   mask[4].v=infty;
   mask[4].h=infty;
   mask[4].d=infty;
   mask[4].distance=infty;
 }

  for(looper=0;looper<5;looper++) {
    if (mask[looper].distance<min.distance) min=mask[looper];
  }

  return min;
}


DEucl3D MINforwardDE4(Array3dDEucl3D& A, int z,int y,int x)
{
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  //
  //   MASK:   (0,0,1)(0,0,0)
  //
  DEucl3D mask[2];
  DEucl3D min;
  int looper;
  min.distance=infty;
  if (z>0) {
    mask[0].v=inDE[z-1][y][x].v;
    mask[0].h=inDE[z-1][y][x].h;
    mask[0].d=inDE[z-1][y][x].d+1;
    mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d);
  }
  else {
    mask[0].v=infty;
    mask[0].h=infty;
    mask[0].d=infty;
    mask[0].distance=infty;
  }
  mask[1].v=inDE[z][y][x].v;
  mask[1].h=inDE[z][y][x].h;
  mask[1].d=inDE[z][y][x].d;
  mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d);

  for(looper=0;looper<2;looper++) {
    if (mask[looper].distance<min.distance) min=mask[looper];
  }

  return min;
}

DEucl3D MINbackwardDE3(Array3dDEucl3D& A, int z,int y,int x)
{
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  //
  //                     MASK:
  //
  //             (0,1,1)(0,1,0)(0,1,1)
  //             (0,0,1)(0,0,0)
  //
  //


  DEucl3D mask[5];
  DEucl3D min;
  float temp;
  int looper;
  min.distance=infty;
  if ((z>0)&&(y>0)) {
    mask[0].v=inDE[z-1][y-1][x].v;
    mask[0].h=inDE[z-1][y-1][x].h+1;
    mask[0].d=inDE[z-1][y-1][x].d+1;
    mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d);
  }
  else {
    mask[0].v=infty;
    mask[0].h=infty;
    mask[0].d=infty;
    mask[0].distance=infty;
  }
  if (y>0){
    mask[1].v=inDE[z][y-1][x].v;
    mask[1].h=inDE[z][y-1][x].h+1;
    mask[1].d=inDE[z][y-1][x].d;
    mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d);
  }
  else {
    mask[1].v=infty;
    mask[1].h=infty;
    mask[1].d=infty;
    mask[1].distance=infty;

  }
  if ((z<Zdim-1)&&(y>0)) {
    mask[2].v=inDE[z+1][y-1][x].v;
    mask[2].h=inDE[z+1][y-1][x].h+1;
    mask[2].d=inDE[z+1][y-1][x].d+1;
    mask[2].distance=sqrt1(mask[2].v*mask[2].v+mask[2].h*mask[2].h+mask[2].d*mask[2].d);
  }
  else {
    mask[2].v=infty;
    mask[2].h=infty;
    mask[2].d=infty;
    mask[2].distance=infty;
  }
  mask[3].v=inDE[z][y][x].v;
  mask[3].h=inDE[z][y][x].h;
  mask[3].d=inDE[z][y][x].d;
  mask[3].distance=sqrt1(mask[3].v*mask[3].v+mask[3].h*mask[3].h+mask[3].d*mask[3].d);
  if (z>0) {
    mask[4].v=inDE[z-1][y][x].v;
    mask[4].h=inDE[z-1][y][x].h;
    mask[4].d=inDE[z-1][y][x].d+1;
    mask[4].distance=sqrt1(mask[4].v*mask[4].v+mask[4].h*mask[4].h+mask[4].d*mask[4].d);
  }
 else {
   mask[4].v=infty;
   mask[4].h=infty;
   mask[4].d=infty;
   mask[4].distance=infty;
 }

  for(looper=0;looper<5;looper++) {
    if (mask[looper].distance<min.distance) min=mask[looper];
  }

  return min;
}


DEucl3D MINforwardDE2(Array3dDEucl3D& A, int z,int y,int x)
{
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  //
  // MASK:   (0,0,0)(0,0,1)
  //

  DEucl3D mask[2];
  DEucl3D min;
  int looper;
  min.distance=infty;
  if (z<Zdim-1) {
    mask[0].v=inDE[z+1][y][x].v;
    mask[0].h=inDE[z+1][y][x].h;
    mask[0].d=inDE[z+1][y][x].d+1;
    mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d);
  }
  else {
    mask[0].v=infty;
    mask[0].h=infty;
    mask[0].d=infty;
    mask[0].distance=infty;
  }
  mask[1].v=inDE[z][y][x].v;
  mask[1].h=inDE[z][y][x].h;
  mask[1].d=inDE[z][y][x].d;
  mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d);

  for(looper=0;looper<2;looper++) {
    if (mask[looper].distance<min.distance) min=mask[looper];
  }


  return min;
}


DEucl3D MINbackwardDE1(Array3dDEucl3D& A, int z,int y,int x)
{
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  //
  //                     MASK:
  //
  //             (1,1,1)(1,1,0)(1,1,1)
  //             (1,0,1)(1,0,0)(1,0,1)
  //             (1,1,1)(1,1,0)(1,1,1)
  //


  DEucl3D mask[14];
  DEucl3D min;
  float temp;
  int looper;
  min.distance=infty;

 if ((z>0)&&(y>0)&&(x<Xdim-1)) {
    mask[0].v=inDE[z-1][y-1][x+1].v+1;
    mask[0].h=inDE[z-1][y-1][x+1].h+1;
    mask[0].d=inDE[z-1][y-1][x+1].d+1;
    mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d);
  }
  else {
    mask[0].v=infty;
    mask[0].h=infty;
    mask[0].d=infty;
    mask[0].distance=infty;
  }

  if ((y>0)&&(x<Xdim-1)){
    mask[1].v=inDE[z][y-1][x+1].v+1;
    mask[1].h=inDE[z][y-1][x+1].h+1;
    mask[1].d=inDE[z][y-1][x+1].d;
    mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d);
  }
  else {
    mask[1].v=infty;
    mask[1].h=infty;
    mask[1].d=infty;
    mask[1].distance=infty;

  }
   if ((z<Zdim-1)&&(y>0)&&(x<Xdim-1)) {
    mask[2].v=inDE[z+1][y-1][x+1].v+1;
    mask[2].h=inDE[z+1][y-1][x+1].h+1;
    mask[2].d=inDE[z+1][y-1][x+1].d+1;
    mask[2].distance=sqrt1(mask[2].v*mask[2].v+mask[2].h*mask[2].h+mask[2].d*mask[2].d);
  }
  else {
    mask[2].v=infty;
    mask[2].h=infty;
    mask[2].d=infty;
    mask[2].distance=infty;
  }

  if ((z>0)&&(x<Xdim-1)) {
    mask[3].v=inDE[z-1][y][x+1].v+1;
    mask[3].h=inDE[z-1][y][x+1].h;
    mask[3].d=inDE[z-1][y][x+1].d+1;
    mask[3].distance=sqrt1(mask[3].v*mask[3].v+mask[3].h*mask[3].h+mask[3].d*mask[3].d);
  }
 else {
   mask[3].v=infty;
   mask[3].h=infty;
   mask[3].d=infty;
   mask[3].distance=infty;
 }

 if (x<Xdim-1) {
    mask[4].v=inDE[z][y][x+1].v+1;
    mask[4].h=inDE[z][y][x+1].h;
    mask[4].d=inDE[z][y][x+1].d;
    mask[4].distance=sqrt1(mask[4].v*mask[4].v+mask[4].h*mask[4].h+mask[4].d*mask[4].d);
  }
 else {
   mask[4].v=infty;
   mask[4].h=infty;
   mask[4].d=infty;
   mask[4].distance=infty;
 }

 if ((x<Xdim-1)&&(z<Zdim-1)) {
    mask[5].v=inDE[z+1][y][x+1].v+1;
    mask[5].h=inDE[z+1][y][x+1].h;
    mask[5].d=inDE[z+1][y][x+1].d+1;
    mask[5].distance=sqrt1(mask[5].v*mask[5].v+mask[5].h*mask[5].h+mask[5].d*mask[5].d);
  }
 else {
   mask[5].v=infty;
   mask[5].h=infty;
   mask[5].d=infty;
   mask[5].distance=infty;
 }

if ((x<Xdim-1)&&(z>0)&&(y<Ydim-1)) {
    mask[6].v=inDE[z-1][y+1][x+1].v+1;
    mask[6].h=inDE[z-1][y+1][x+1].h+1;
    mask[6].d=inDE[z-1][y+1][x+1].d+1;
    mask[6].distance=sqrt1(mask[6].v*mask[6].v+mask[6].h*mask[6].h+mask[6].d*mask[6].d);
  }
 else {
   mask[6].v=infty;
   mask[6].h=infty;
   mask[6].d=infty;
   mask[6].distance=infty;
 }

if ((x<Xdim-1)&&(y<Ydim-1)) {
    mask[7].v=inDE[z][y+1][x+1].v+1;
    mask[7].h=inDE[z][y+1][x+1].h+1;
    mask[7].d=inDE[z][y+1][x+1].d;
    mask[7].distance=sqrt1(mask[7].v*mask[7].v+mask[7].h*mask[7].h+mask[7].d*mask[7].d);
  }
 else {
   mask[7].v=infty;
   mask[7].h=infty;
   mask[7].d=infty;
   mask[7].distance=infty;
 }

if ((x<Xdim-1)&&(y<Ydim-1)&&(z<Zdim-1)) {
    mask[8].v=inDE[z+1][y+1][x+1].v+1;
    mask[8].h=inDE[z+1][y+1][x+1].h+1;
    mask[8].d=inDE[z+1][y+1][x+1].d+1;
    mask[8].distance=sqrt1(mask[8].v*mask[8].v+mask[8].h*mask[8].h+mask[8].d*mask[8].d);
  }
 else {
   mask[8].v=infty;
   mask[8].h=infty;
   mask[8].d=infty;
   mask[8].distance=infty;
 }

  //              MASK :
  //
  //             (0,0,0)(0,0,1)
  //      (0,1,1)(0,1,0)(0,1,1)
  //

  if (z<Zdim-1) {
    mask[9].v=inDE[z+1][y][x].v;
    mask[9].h=inDE[z+1][y][x].h;
    mask[9].d=inDE[z+1][y][x].d+1;
    mask[9].distance=sqrt1(mask[9].v*mask[9].v+mask[9].h*mask[9].h+mask[9].d*mask[9].d);
  }
  else {
    mask[9].v=infty;
    mask[9].h=infty;
    mask[9].d=infty;
    mask[9].distance=infty;
  }

  if ((y<Ydim-1)&&(z<Zdim-1)) {
    mask[10].v=inDE[z+1][y][x].v;
    mask[10].h=inDE[z+1][y][x].h+1;
    mask[10].d=inDE[z+1][y][x].d+1;
    mask[10].distance=sqrt1(mask[10].v*mask[10].v+mask[10].h*mask[10].h+mask[10].d*mask[10].d);
  }
  else {
    mask[10].v=infty;
    mask[10].h=infty;
    mask[10].d=infty;
    mask[10].distance=infty;

  }
  if (y<Ydim-1) {
    mask[11].v=inDE[z][y+1][x].v;
    mask[11].h=inDE[z][y+1][x].h+1;
    mask[11].d=inDE[z][y+1][x].d;
    mask[11].distance=sqrt1(mask[11].v*mask[11].v+mask[11].h*mask[11].h+mask[11].d*mask[11].d);
  }
  else {
    mask[11].v=infty;
    mask[11].h=infty;
    mask[11].d=infty;
    mask[11].distance=infty;
  }
  mask[12].v=inDE[z][y][x].v;
  mask[12].h=inDE[z][y][x].h;
  mask[12].d=inDE[z][y][x].d;
  mask[12].distance=sqrt1(mask[12].v*mask[12].v+mask[12].h*mask[12].h+mask[12].d*mask[12].d);

  if ((z>0)&&(y<Ydim-1)) {
    mask[13].v=inDE[z-1][y+1][x].v;
    mask[13].h=inDE[z-1][y+1][x].h+1;
    mask[13].d=inDE[z-1][y+1][x].d+1;
    mask[13].distance=sqrt1(mask[13].v*mask[13].v+mask[13].h*mask[13].h+mask[13].d*mask[13].d);
  }
 else {
   mask[13].v=infty;
   mask[13].h=infty;
   mask[13].d=infty;
   mask[13].distance=infty;
 }

  for(looper=0;looper<14;looper++) {
    if (mask[looper].distance<min.distance) min=mask[looper];
  }
  return min;
}

DEucl3D MINforwardDE1(Array3dDEucl3D& A, int z,int y,int x)
{
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  DEucl3D mask[14];
  DEucl3D min;
  float temp;
  int looper;
  min.distance=infty;


  //                     MASK:
  //
  //             (1,1,1)(1,1,0)(1,1,1)
  //             (1,0,1)(1,0,0)(1,0,1)
  //             (1,1,1)(1,1,0)(1,1,1)
  //
 if ((z>0)&&(y>0)&&(x>0)) {
    mask[0].v=inDE[z-1][y-1][x-1].v+1;
    mask[0].h=inDE[z-1][y-1][x-1].h+1;
    mask[0].d=inDE[z-1][y-1][x-1].d+1;
    mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d);
  }
  else {
    mask[0].v=infty;
    mask[0].h=infty;
    mask[0].d=infty;
    mask[0].distance=infty;
  }
  if ((y>0)&&(x>0)){
    mask[1].v=inDE[z][y-1][x-1].v+1;
    mask[1].h=inDE[z][y-1][x-1].h+1;
    mask[1].d=inDE[z][y-1][x-1].d;
    mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d);
  }
  else {
    mask[1].v=infty;
    mask[1].h=infty;
    mask[1].d=infty;
    mask[1].distance=infty;

  }
  if ((z<Zdim-1)&&(y>0)&&(x>0)) {
    mask[2].v=inDE[z+1][y-1][x-1].v+1;
    mask[2].h=inDE[z+1][y-1][x-1].h+1;
    mask[2].d=inDE[z+1][y-1][x-1].d+1;
    mask[2].distance=sqrt1(mask[2].v*mask[2].v+mask[2].h*mask[2].h+mask[2].d*mask[2].d);
  }
  else {
    mask[2].v=infty;
    mask[2].h=infty;
    mask[2].d=infty;
    mask[2].distance=infty;
  }
  if ((z>0)&&(x>0)) {
    mask[3].v=inDE[z-1][y][x-1].v+1;
    mask[3].h=inDE[z-1][y][x-1].h;
    mask[3].d=inDE[z-1][y][x-1].d+1;
    mask[3].distance=sqrt1(mask[3].v*mask[3].v+mask[3].h*mask[3].h+mask[3].d*mask[3].d);
  }
 else {
   mask[3].v=infty;
   mask[3].h=infty;
   mask[3].d=infty;
   mask[3].distance=infty;
 }
 if (x>0) {
    mask[4].v=inDE[z][y][x-1].v+1;
    mask[4].h=inDE[z][y][x-1].h;
    mask[4].d=inDE[z][y][x-1].d;
    mask[4].distance=sqrt1(mask[4].v*mask[4].v+mask[4].h*mask[4].h+mask[4].d*mask[4].d);
  }
 else {
   mask[4].v=infty;
   mask[4].h=infty;
   mask[4].d=infty;
   mask[4].distance=infty;
 }
 if ((x>0)&&(z<Zdim-1)) {
    mask[5].v=inDE[z+1][y][x-1].v+1;
    mask[5].h=inDE[z+1][y][x-1].h;
    mask[5].d=inDE[z+1][y][x-1].d+1;
    mask[5].distance=sqrt1(mask[5].v*mask[5].v+mask[5].h*mask[5].h+mask[5].d*mask[5].d);
  }
 else {
   mask[5].v=infty;
   mask[5].h=infty;
   mask[5].d=infty;
   mask[5].distance=infty;
 }
if ((x>0)&&(z>0)&&(y<Ydim-1)) {
    mask[6].v=inDE[z-1][y+1][x-1].v+1;
    mask[6].h=inDE[z-1][y+1][x-1].h+1;
    mask[6].d=inDE[z-1][y+1][x-1].d+1;
    mask[6].distance=sqrt1(mask[6].v*mask[6].v+mask[6].h*mask[6].h+mask[6].d*mask[6].d);
  }
 else {
   mask[6].v=infty;
   mask[6].h=infty;
   mask[6].d=infty;
   mask[6].distance=infty;
 }
if ((x>0)&&(y<Ydim-1)) {
    mask[7].v=inDE[z][y+1][x-1].v+1;
    mask[7].h=inDE[z][y+1][x-1].h+1;
    mask[7].d=inDE[z][y+1][x-1].d;
    mask[7].distance=sqrt1(mask[7].v*mask[7].v+mask[7].h*mask[7].h+mask[7].d*mask[7].d);
  }
 else {
   mask[7].v=infty;
   mask[7].h=infty;
   mask[7].d=infty;
   mask[7].distance=infty;
 }
if ((x>0)&&(y<Ydim-1)&&(z<Zdim-1)) {
    mask[8].v=inDE[z+1][y+1][x-1].v+1;
    mask[8].h=inDE[z+1][y+1][x-1].h+1;
    mask[8].d=inDE[z+1][y+1][x-1].d+1;
    mask[8].distance=sqrt1(mask[8].v*mask[8].v+mask[8].h*mask[8].h+mask[8].d*mask[8].d);
  }
 else {
   mask[8].v=infty;
   mask[8].h=infty;
   mask[8].d=infty;
   mask[8].distance=infty;
 }

  //
  //                     MASK:
  //
  //             (0,1,1)(0,1,0)(0,1,1)
  //             (0,0,1)(0,0,0)
  //
  //


  if ((z>0)&&(y>0)) {
    mask[9].v=inDE[z-1][y-1][x].v;
    mask[9].h=inDE[z-1][y-1][x].h+1;
    mask[9].d=inDE[z-1][y-1][x].d+1;
    mask[9].distance=sqrt1(mask[9].v*mask[9].v+mask[9].h*mask[9].h+mask[9].d*mask[9].d);
  }
  else {
    mask[9].v=infty;
    mask[9].h=infty;
    mask[9].d=infty;
    mask[9].distance=infty;
  }


  if (y>0){
    mask[10].v=inDE[z][y-1][x].v;
    mask[10].h=inDE[z][y-1][x].h+1;
    mask[10].d=inDE[z][y-1][x].d;
    mask[10].distance=sqrt1(mask[10].v*mask[10].v+mask[10].h*mask[10].h+mask[10].d*mask[10].d);
  }
  else {
    mask[10].v=infty;
    mask[10].h=infty;
    mask[10].d=infty;
    mask[10].distance=infty;

  }


  if ((z<Zdim-1)&&(y>0)) {
    mask[11].v=inDE[z+1][y-1][x].v;
    mask[11].h=inDE[z+1][y-1][x].h+1;
    mask[11].d=inDE[z+1][y-1][x].d+1;
    mask[11].distance=sqrt1(mask[11].v*mask[11].v+mask[11].h*mask[11].h+mask[11].d*mask[11].d);
  }
  else {
    mask[11].v=infty;
    mask[11].h=infty;
    mask[11].d=infty;
    mask[11].distance=infty;
  }

  mask[12].v=inDE[z][y][x].v;
  mask[12].h=inDE[z][y][x].h;
  mask[12].d=inDE[z][y][x].d;
  mask[12].distance=sqrt1(mask[12].v*mask[12].v+mask[12].h*mask[12].h+mask[12].d*mask[12].d);

  if (z>0) {
    mask[13].v=inDE[z-1][y][x].v;
    mask[13].h=inDE[z-1][y][x].h;
    mask[13].d=inDE[z-1][y][x].d+1;
    mask[13].distance=sqrt1(mask[13].v*mask[13].v+mask[13].h*mask[13].h+mask[13].d*mask[13].d);
  }
 else {
   mask[13].v=infty;
   mask[13].h=infty;
   mask[13].d=infty;
   mask[13].distance=infty;
 }

  for(looper=0;looper<14;looper++) {
    if (mask[looper].distance<min.distance) min=mask[looper];
  }

  return min;
}

const bool Debug= false;

void DEuclidean(Array3dDEucl3D& A)
{
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  int y,x,z;
  if (Debug) printf("DEuclidean started ...\n");
  for (x=0;x<Xdim;x++) {
    for(y=0;y<Ydim;y++){
      for(z=0;z<Zdim;z++) inDE[z][y][x]=MINforwardDE1(A, z,y,x);
      for(z=Zdim-1;z>-1;z--) inDE[z][y][x]=MINforwardDE2(A, z,y,x);
    }
    for(y=Ydim-1;y>-1;y--) {
      for(z=Zdim-1;z>-1;z--) inDE[z][y][x]=MINforwardDE3(A, z,y,x);
      for(z=0;z<Zdim;z++) inDE[z][y][x]=MINforwardDE4(A, z,y,x);
    }
  }
  for (x=Xdim-1;x>-1;x--) {
    for(y=Ydim-1;y>-1;y--){
      for(z=Zdim-1;z>-1;z--) inDE[z][y][x]=MINbackwardDE1(A, z,y,x);
      for(z=0;z<Zdim;z++) inDE[z][y][x]=MINforwardDE4(A, z,y,x);
    }
    for(y=0;y<Ydim;y++) {
      for(z=0;z<Zdim;z++) inDE[z][y][x]=MINbackwardDE3(A, z,y,x);
      for(z=Zdim-1;z>-1;z--) inDE[z][y][x]=MINforwardDE2(A, z,y,x);

    }
  }
  if (Debug) printf("DEuclidean finished\n");

}

// ************************************
//
//       D-Euclidean END
//
// ***********************************

void printArray3D(const Array3dDEucl3D& A, int x, char type)
{
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  int z,y;
  printf("\nslice x= %d\n",x);
  for (y=0;y<Ydim;y++) {
    printf("\n");
    for(z=0;z<Zdim;z++)
      if (type=='f') {
	if (inDE[z][y][x].distance==infty)
	  printf("* ");
	else
	  printf("%f ",inDE[z][y][x].distance);
      }
  }
  printf("\n");
}

// ****************************
//
//  float functions BEGIN
//
// ****************************
float MINforward3Dfloat(Array3dDEucl3D& A, float d1,float d2,float d3,int z,int y,int x){
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  float mask[14];
  float min=infty;
  int looper;

  // MASK
  //
  //   (+d3)(+d2)(+d3)   (+d2)(+d1)(+d2)
  //   (+d2)(+d1)(+d2)   (+d1)( 0 )
  //   (+d3)(+d2)(+d3)


  // apply the mask
  if ((z>0)&&(y>0)) mask[0]=inDE[z-1][y-1][x].distance+d2; else mask[0]=infty;
  if (y>0) mask[1]=inDE[z][y-1][x].distance+d1; else mask[1]=infty;
  if (( z<Zdim-1)&&(y>0)) mask[2]=inDE[z+1][y-1][x].distance+d2; else mask[2]=infty;
  mask[3]=inDE[z][y][x].distance;
  if (z>0) mask[4]=inDE[z-1][y][x].distance+d1; else mask[4]=infty;

  if ((x>0)&&(z>0)&&(y>0)) mask[5]=inDE[z-1][y-1][x-1].distance+d3; else mask[5]=infty;
  if ((x>0)&&(y>0)) mask[6]=inDE[z][y-1][x-1].distance+d2; else mask[6]=infty;
  if ((x>0)&&(z<Zdim-1)&&(y>0)) mask[7]=inDE[z+1][y-1][x-1].distance+d3; else mask[7]=infty;
  if ((x>0)&&(z>0)) mask[8]=inDE[z-1][y][x-1].distance+d2; else mask[8]=infty;
  if (x>0) mask[9]=inDE[z][y][x-1].distance+d1; else mask[9]=infty;
  if ((x>0)&&(z<Zdim-1)) mask[10]=inDE[z+1][y][x-1].distance+d2; else mask[10]=infty;
  if ((x>0)&&(z>0)&&(y<Ydim-1)) mask[11]=inDE[z-1][y+1][x-1].distance+d3; else mask[11]=infty;
  if ((x>0)&&(y<Ydim-1)) mask[12]=inDE[z][y+1][x-1].distance+d2; else mask[12]=infty;
  if ((x>0)&&(z<Zdim-1)&&(y<Ydim-1)) mask[13]=inDE[z+1][y+1][x-1].distance+d3; else mask[13]=infty;
  // find minimum
  for(looper=0;looper<14;looper++) if (mask[looper]<min) min=mask[looper];


  return min;
}

float MINbackward3Dfloat(Array3dDEucl3D& A, float d1,float d2,float d3,int z,int y,int x){
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  float mask[14];
  float min=infty;
  int looper;

  //                MASK:
  //
  //                       (+d3)(+d2)(+d3)
  //          ( 0 )(+d1)   (+d2)(+d1)(+d2)
  //     (+d2)(+d1)(+d2)   (+d3)(+d2)(+d3)

  // apply the mask

  mask[0]=inDE[z][y][x].distance;
  if (z<Zdim-1) mask[1]=inDE[z+1][y][x].distance+d1; else mask[1]=infty;
  if ((y<Ydim-1)&&(z<Zdim-1)) mask[2]=inDE[z+1][y+1][x].distance+d2; else mask[2]=infty;
  if (y<Ydim-1) mask[3]=inDE[z][y+1][x].distance+d1; else mask[3]=infty;
  if ((z>0)&&(y<Ydim-1)) mask[4]=inDE[z-1][y+1][x].distance+d2; else mask[4]=infty;

  if ((x<Xdim-1)&&(z>0)&&(y>0)) mask[5]=inDE[z-1][y-1][x+1].distance+d3; else mask[5]=infty;
  if ((x<Xdim-1)&&(y>0)) mask[6]=inDE[z][y-1][x+1].distance+d2; else mask[6]=infty;
  if ((x<Xdim-1)&&(z<Zdim-1)&&(y>0)) mask[7]=inDE[z+1][y-1][x+1].distance+d3; else mask[7]=infty;
  if ((x<Xdim-1)&&(z>0)) mask[8]=inDE[z-1][y][x+1].distance+d2; else mask[8]=infty;
  if (x<Xdim-1) mask[9]=inDE[z][y][x+1].distance+d1; else mask[9]=infty;
  if ((x<Xdim-1)&&(z<Zdim-1)) mask[10]=inDE[z+1][y][x+1].distance+d2; else mask[10]=infty;
  if ((x<Xdim-1)&&(z>0)&&(y<Ydim-1)) mask[11]=inDE[z-1][y+1][x+1].distance+d3; else mask[11]=infty;
  if ((x<Xdim-1)&&(y<Ydim-1)) mask[12]=inDE[z][y+1][x+1].distance+d2; else mask[12]=infty;
  if ((x<Xdim-1)&&(z<Zdim-1)&&(y<Ydim-1)) mask[13]=inDE[z+1][y+1][x+1].distance+d3; else mask[13]=infty;
  // find minimum
  for(looper=0;looper<14;looper++) if (mask[looper]<min) min=mask[looper];


  return min;
}

void DistanceTransform3Dfloat(Array3dDEucl3D& A, float d1,float d2,float d3){
  DEucl3D*** inDE = A.data;
  int Xdim = A.Xdim;
  int Ydim = A.Ydim;
  int Zdim = A.Zdim;

  int z,y,x;
  if (Debug) printf("chamfer float started ...\n");

 for (x=0;x<Xdim;x++)
   for (y=0;y<Ydim;y++)
     for(z=0;z<Zdim;z++) inDE[z][y][x].distance=MINforward3Dfloat(A, d1,d2,d3,z,y,x);


 for (x=Xdim-1;x>-1;x--)
   for (y=Ydim-1;y>-1;y--)
     for(z=Zdim-1;z>-1;z--) inDE[z][y][x].distance=MINbackward3Dfloat(A, d1,d2,d3,z,y,x);
  if (Debug) printf("chamfer float finished\n");

}

// ***************************
//
//  float functions END
//
// ***************************

DT3D::DT3D()
{
	A.data = NULL;
}

void DT3D::Build(double* _x, double* _y, double* _z, int num)
{
	xMin=_x[0]; xMax=_x[0]; yMin=_y[0]; yMax=_y[0]; zMin=_z[0]; zMax=_z[0];
	int i;
	for(i = 1; i < num; i ++)
	{
		if(xMin > _x[i]) xMin = _x[i];
		if(xMax < _x[i]) xMax = _x[i];
		if(yMin > _y[i]) yMin = _y[i];
		if(yMax < _y[i]) yMax = _y[i];
		if(zMin > _z[i]) zMin = _z[i];
		if(zMax < _z[i]) zMax = _z[i];
	}

	double xCenter = (xMin+xMax)/2;
	double yCenter = (yMin+yMax)/2;
	double zCenter = (zMin+zMax)/2;
	xMin = xCenter - expandFactor*(xMax-xCenter);
	xMax = xCenter + expandFactor*(xMax-xCenter);
	yMin = yCenter - expandFactor*(yMax-yCenter);
	yMax = yCenter + expandFactor*(yMax-yCenter);
	zMin = zCenter - expandFactor*(zMax-zCenter);
	zMax = zCenter + expandFactor*(zMax-zCenter);

	double max = xMax-xMin > yMax-yMin ? xMax-xMin : yMax-yMin;
	max = max > zMax-zMin ? max : zMax-zMin;

	xMin = xCenter - max/2;
	xMax = xCenter + max/2;
	yMin = yCenter - max/2;
	yMax = yCenter + max/2;
	zMin = zCenter - max/2;
	zMax = zCenter + max/2;

	scale = SIZE/max;

	//printf("DTaccu:%lf\n",sqrt(3.0)/2/scale);

	if(A.data == NULL)
	{
		A.Init(SIZE, SIZE, SIZE);
	}

	int x,y,z;

	DEucl3D*** inDE = A.data;
	int Xdim = A.Xdim;
	int Ydim = A.Ydim;
	int Zdim = A.Zdim;

	for(z=0; z<Zdim; z++) {
		//	printf("x=%d y=%d z=%d\n",x,y,z);
		for(y=0; y<Ydim; y++) {
			//	printf("x=%d y=%d z=%d\n",x,y,z);
			for(x=0; x<Xdim; x++) {
				//	printf("x=%d y=%d z=%d\n",x,y,z);
					inDE[z][y][x].distance=infty;
					inDE[z][y][x].h=infty;
					inDE[z][y][x].v=infty;
					inDE[z][y][x].d=infty;
			}
		}
	}
	for(i = 0; i < num; i++)
	{
		x = ROUND((_x[i]-xMin)*scale);
		y = ROUND((_y[i]-yMin)*scale);
		z = ROUND((_z[i]-zMin)*scale);

		if(x<0 || x>=Xdim || y<0 || y>=Ydim || z<0 || z>=Zdim)
			continue;

		inDE[z][y][x].distance=0;
		inDE[z][y][x].h=0;
		inDE[z][y][x].v=0;
		inDE[z][y][x].d=0;
	}

	DEuclidean(A);

	for(z=0; z<Zdim; z++) {
		for(y=0; y<Ydim; y++) {
			for(x=0; x<Xdim; x++) {
				//inDE[z][y][x].distance = (inDE[z][y][x].distance  - sqrt(3.0))/scale;
				inDE[z][y][x].distance = (inDE[z][y][x].distance)/scale;
				if(inDE[z][y][x].distance < 0)
					inDE[z][y][x].distance = 0;
			}
		}
	}
}

float DT3D::Distance(double _x, double _y, double _z)
{
	int x, y, z;
	x = ROUND((_x-xMin)*scale);
	y = ROUND((_y-yMin)*scale);
	z = ROUND((_z-zMin)*scale);

	if(x > -1 && x < SIZE && y > -1 && y < SIZE && z > -1 && z < SIZE)
		return A.data[z][y][x].distance;

	float a = 0, b = 0, c = 0;
	if(x < 0)
	{
		a = x;
		x = 0;
	}
	else if(x >= SIZE)
	{
		a = x-SIZE+1;
		x = SIZE-1;
	}

	if(y < 0)
	{
		b = y;
		y = 0;
	}
	else if(y >= SIZE)
	{
		b = y-SIZE+1;
		y = SIZE-1;
	}

	if(z < 0)
	{
		c = z;
		z = 0;
	}
	else if(z >= SIZE)
	{
		c = z-SIZE+1;
		z = SIZE-1;
	}

	return sqrt(a*a+b*b+c*c)/scale + A.data[z][y][x].distance;
}

