/******************************************************************************/
/*  Changed from Version 2.0                                                  */
/*  1. Subvolume matching is also added for subject driving voxels            */
/*      during correspondence detection.                                      */
/*  2. samll testing                                                          */
/*     (a) Binarize the tissue matching results  -> Good for Young Brain only */
/*     (b) The background effect in subvolume matching is also considered ->  */
/*                                                          NOT GOOD FOR ALL  */
/*  Search 'March ?? 2003' for these changes.                                 */
/******************************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <strings.h>
#include <mvcdDRTG.h>
//#include <cres.h>
#include <matrixSHENDRTGX.h>  /*by SHEN*/

//// for tensor structural feature
#include "TensorField.h"

//// for orientation feature
#include "SalientRegionFeature.h"

#define MaxOfUC  255

#define X .525731112119133606
#define Z .850650808352039932
#define ADD     1
#define NOTADD  0
#define YYES    1
#define NNO     0
#define OOK     1
#define FFAIL   0
#define NEGINT    -1000
#define BOUNDERY  -10000

/* features for vertices */
#define BG    0            /* background */
#define CSF   10
#define VN    50
#define GM    150
#define WM    250
#define GMVN_EDGE     100   
#define GMCSFBG_EDGE  120  /* added on June 4, 2001*/
#define WMVN_EDGE     180
#define WMGM_EDGE     255

 
typedef struct  
{ 
  unsigned char prolateMean;  // - prolate (lambda1-lambda2) /lambda1 - mean  // -FA mean
  unsigned char prolateVar;  // - prolate (lambda1-lambda2) /lambda1 - variance  // -FA var
  
  unsigned char oblateMean; // - oblate (lambda2-lambda3)/lambda1 - mean
  unsigned char oblateVar; // - oblate (lambda2-lambda3)/lambda1 - variance
  
  unsigned char sphereMean; //- sphere lambda3/lambda1 - mean
  unsigned char sphereVar; //- sphere lambda3/lambda1 - variance
  
  unsigned char faTiss;  // - FA intensity
  unsigned char faEdge; //- FA Canny Edge
  unsigned char traceEdge; // - Trace Canny Edge // -FA contrast
} DTIImgAttribute ;  
  

typedef struct 
{ 
  unsigned char Variance_UP; // FA value
  unsigned char CannyEdge_UP;
  unsigned char CannyMinimalRequiredEdgeValue ;
} Threshold ; 


////////// trigger for FA feature computation////////
bool       IsFAFeature;
////////////////////////////////////////////////

//////////////////Orientation feature//////////////

bool       bOrientationFeature; /* trigger for the orientation feature */

////threshold of FA value to consider the orientation feature. Above the threshold to be considered
float      faThresholdForOrientationFeature = 0.25;
int        nPDHistBins = 3; /*histogram bins for each direction of PD*/

int        nMdlRegionScale = 3;
int        nObjRegionScale = 3;

ByteVectorField MdlPDColorMap; /* Model PD color map */
ByteVectorField ObjPDColorMap; /* Object PD color map */
//////////////////////////////////////////////////


//// decrease the weight of trace edge when choosing the focus points
float      TraceEdgeConfidence = 0.80;

Fvector3d  *MdlVer;       /* focused edge points from MODEL image */
Ivector3d  *FixedMdlVer;  /* focused edge points from MODEL image */ 
int        MdlVer_Num ;   /* total number of the focused edge points */
Ivector3d  *ObjVer;       /* focused edge points from OBJECT image */ 
int        ObjVer_Num ;   /* The number of focused edge points */
DTIImgAttribute *MdlVer_Fea, *ObjVer_Fea ;
Threshold  MdlThreshold, ObjThreshold ;
float      PercentOfLowCannyEdge ;
int        SmoothingTimes ; /* for edge preserving smoothing */
int        BigVN ;        /* This is used to act "pull force" from the subject's ventrical, on the model's ventricle. */
float      SMR_THR=0.8 ;  /* A threshold for similarity */
float      DIF_THR ;      /* A threshold for difference */
DTIImgAttribute MaxAttribute ;  /* Maximal vaules in each attribute, used for normalization */


/* surface model, to be carved to the subject's space */
Fvector3d  *SurfVer, *SurfNorms;       /* focused edge points from OBJECT image */ 
int        SurfVer_Num, **SurfNbr, max_nbr=13 ;   /* The number of focused edge points */


/*int    FeaNUM=4 ;*/  /* the size of attributes in each voxel/vertex */
Matrix *Transform ;
int  XYZres ;

void MarcMain(int,char *[]);


void ShenMain(int argc,char *argv[]) ;
void show_usage_SHEN() ;
void AffinDeform3D(Fvector3d *MdlVer, Ivector3d *FixedMdlVer, int MdlVer_Num, DTIImgAttribute *MdlVer_Fea, Fvector3d ***DeformFld, DTIImgAttribute ***MdlImg, Ivector3d *ObjVer, int ObjVer_Num, DTIImgAttribute *ObjVer_Fea, DTIImgAttribute ***ObjImg, int image_size, int z_size, int resolution) ;
/*void ShiftByGuassianWeight(Fvector3d In, Fvector3d *Tmp, Fvector3d Search, float level, int max) ;*/
void ShiftByGuassianWeight(Fvector3d In, Fvector3d *Tmp, Fvector3d Search, float level, float sigma) ;
float GuassianWeight(float level, float sigma) ;
void GlobalAffineUpdatePlusLocalDeformation( Fvector3d *verObject, Fvector3d *verModel, int MdlVer_Num ) ;
void SaveCurrentWarpingResultOnSegmentedObj( DTIImgAttribute ***ObjImg, Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder ) ; // save the FA image...
void SaveCurrentWarpingResultOnOriginalObjImg( unsigned char ***ObjOriginalImg, Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder ) ;
void EstimatingScalingAndShiftOf3DSurfaceFromImg(Fvector3d *MdlVer, Ivector3d *FixedMdlVer, DTIImgAttribute *MdlVer_Fea, int MdlVer_Num, Fvector3d ***DeformFld, DTIImgAttribute ***ObjImg, int image_size, int z_size );
void SmoothDeformationField(Fvector3d ***DeformFld, int image_size, int z_size, DTIImgAttribute ***MdlImg, int SmoothingTimes) ;
void SmoothDeformationFieldWithEdgePreserving( Ivector3d *FixedMdlVer, int MdlVer_Num, Fvector3d ***DeformFld, DTIImgAttribute ***MdlImg, int image_size, int z_size, float ratio_iteration, int SmoothingTimes ) ;

/* statistics */
void StatisticalMapping(Fvector3d *ver, int MdlVer_Num, float alpha) ;

/*  options */
static float Deform_RATE=0.04, Affine_Degree=0.2, Statistical_factor=0.1, MatchingDegreeOfMdlOnImgEdge=0.1, smoothFactor, DfmSmthCoeff, ConfidenceOnLastDeformation=0, StartPercent=0.10, MaxPercentOfAllowedDrivingVoxels  ;
static int   search_resolution, last_order, AddMthdOfAvoidingContraction=NOTADD, EstimateTransformOrNot=NOTADD, StatisticsEmployedOrNot=NOTADD, YoungBrain=NNO, DfmSmthTimes;
double CannyGuassianSigma=1.5 ;
static int   Start_Search_Point=0 ;
char   MdlDirtory[180] ;
int UsingForcesFromObject ; /* Feb 22, 2002 */

/* Dec 28, 1999 for adaptive-focus model */
static float ratio_iteration=0 ;
int   image_size, z_size;

/* To stack the 3D landmarks as 1D. July 2000 */
Ivector3d    *MyHood ;
int          MyHoodSize, pixelNumIn_MyHood, Orig_pixelNumIn_MyHood ;
Fvector3d    *forceByFocusedPntInObj ;
int          *multiple ;
void Select3DEdgePointsInObjectTO1D(int *ObjVer_Num, DTIImgAttribute ***ObjImg, int image_size, int z_size) ;
void Select3DEdgePointsInModelTO1D(int *MdlVer_Num, DTIImgAttribute ***MdlImg, Fvector3d ***DeformFld, int image_size, int z_size) ;
void SearchTheNearestVertexForEachLandmark( Ivector3d *ObjVer, int ObjVer_Num, DTIImgAttribute *ObjVer_Fea, Fvector3d *MdlVer, int MdlVer_Num, DTIImgAttribute *MdlVer_Fea, int image_size, int z_size ) ;
void SearchTheNearestVertexForEachLandmark_NewFeb2002(Ivector3d *ObjVer, int ObjVer_Num, DTIImgAttribute *ObjVer_Fea, DTIImgAttribute ***ObjImg, Fvector3d *MdlVer, int MdlVer_Num, DTIImgAttribute *MdlVer_Fea, DTIImgAttribute ***MdlImg, Fvector3d ***DeformFld, int image_size, int z_size) ;
void SearchTheNearestVertexForEachLandmark_March2003(Ivector3d *ObjVer, int ObjVer_Num, DTIImgAttribute *ObjVer_Fea, DTIImgAttribute ***ObjImg, 
						     Fvector3d *MdlVer, int MdlVer_Num, DTIImgAttribute *MdlVer_Fea, Ivector3d *FixedMdlVer, DTIImgAttribute ***MdlImg, Fvector3d ***DeformFld,   int image_size, int z_size,  int max) ;
void Cubic_edgeDetectionBy2DcannyMethod(unsigned char ***Img, unsigned char ***Img_edge) ;
void Cubic_edgeDetectionBythree2DcannyMethod(unsigned char ***Img, unsigned char ***Img_edge) ;
void SearchThresholdForObtainingTopXPercentOfDrivingVoxels(DTIImgAttribute ***Img, Threshold *FeatureThreshold, float CurrentRequirePercent) ;


//************ DTI Image Attribute Memory operation ***************//
DTIImgAttribute ***DTIImgAttributealloc3d(int i_size,int j_size,int k_size)
{
  DTIImgAttribute ***array;
  int i,j,k;

  array=(DTIImgAttribute ***) calloc(k_size,sizeof(DTIImgAttribute **));

  for(k=0;k<k_size;k++)
    array[k]=(DTIImgAttribute **) calloc(i_size,sizeof(DTIImgAttribute *));

  for(k=0;k<k_size;k++)
    for(i=0;i<i_size;i++)
      array[k][i]=(DTIImgAttribute *) calloc(j_size,sizeof(DTIImgAttribute ));
	
  return(array);
}


DTIImgAttribute *DTIImgAttributealloc1d(int k_size)
{
  DTIImgAttribute *array;
  int i,j,k;

  array=(DTIImgAttribute *) calloc(k_size,sizeof(DTIImgAttribute));

  return(array);
}

/*free*/
void DTIImgAttributefree3d(DTIImgAttribute ***array,int k_size,int i_size)
{
  int k,i;

  for(k=0;k<k_size;k++)
    for(i=0;i<i_size;i++)
      free(array[k][i]);

  for(k=0;k<k_size;k++)
    free(array[k]);

  free(array);
}
//************ End ofDTI Image Attribute Memory operation ************//


void WriteImg(char filename[180], unsigned char ***data, int image_size, int z_size)
{
  FILE  *fp;
  int   i, k ;

  /* write the smoothed image */
  fp=myopen(filename,"w");
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      fwrite(data[k][i],1,image_size,fp);
  fclose(fp);
}void WriteImgAttribute(char filename[180], DTIImgAttribute ***Img, int image_size, int z_size)
{
  FILE  *fp;
  int   i, j, k ;
  unsigned char ***TempImg;
  char fn[180] ;

  TempImg = UCalloc3d(image_size,image_size,z_size);
	
  /* FA tissue */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].faTiss ;
  sprintf(fn, "%s_faTissue", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ;  
  
  /* Trace Edge */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].traceEdge ;
  sprintf(fn, "%s_traceEdge", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ;  

  /* FA Edge */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].faEdge ;
  sprintf(fn, "%s_faEdge", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 
   
  /* prolate mean */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].prolateMean ;
  sprintf(fn, "%s_prolateMean", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 
  
  /* prolate variance */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].prolateVar ;
  sprintf(fn, "%s_prolateVar", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 

  
  if (!IsFAFeature)
  {
  /* oblate mean */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].oblateMean ;
  sprintf(fn, "%s_oblateMean", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 
  
  /* oblate variance */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].oblateVar ;
  sprintf(fn, "%s_oblateVar", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 
  
  /* sphere mean */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].sphereMean ;
  sprintf(fn, "%s_sphereMean", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 
  
  /* sphere variance */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	TempImg[k][i][j] = Img[k][i][j].sphereVar ;
  sprintf(fn, "%s_sphereVar", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 
  }
  
  UCfree3d(TempImg, z_size, image_size) ;
}

void WriteDeformationField(char filename[180], Fvector3d ***DeformFld, int image_size, int z_size)
{
  FILE  *fp;
  int   i, k ;

  /* write the smoothed image */
  fp=myopen(filename,"w");
  fwrite(&image_size,sizeof(int),1,fp);
  fwrite(&z_size,sizeof(int),1,fp);
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      fwrite(DeformFld[k][i],sizeof(Fvector3d),image_size,fp);
  fclose(fp);
}
       
void OpenDeformationField(char filename[180], Fvector3d ***DeformFld, int image_size, int z_size)
{
  FILE  *fp;
  Fvector3d ***Last_DeformFld ;
  int Last_image_size, Last_z_size ;
  float XYZ_ratio, deform_ratio, dx, dy, dz ;
  /* for linear interpolation */
  float ii,jj,kk, b,c,d, b1,c1,d1;
  int   i,j,k,ni,nj,nk, niP1,njP1,nkP1, GreyValue ;
  float alpha ;

  /* open deformation field in the last resolution */
  fp=myopen(filename,"r");
  fread(&Last_image_size,sizeof(int),1,fp); printf("Last_image_size=%d ", Last_image_size) ;
  fread(&Last_z_size,sizeof(int),1,fp);     printf("Last_z_size=%d\n", Last_z_size) ;
     
  Last_DeformFld = Fvector3dalloc3d(Last_image_size,Last_image_size,Last_z_size);
  
  for(k=0;k<Last_z_size;k++)
    for(i=0;i<Last_image_size;i++)
      fread(Last_DeformFld[k][i],sizeof(Fvector3d),Last_image_size,fp);
  fclose(fp);


  /* interpolation */
  XYZ_ratio = (float)Last_z_size/(float)z_size ;

  deform_ratio = 1.0/XYZ_ratio ;
  alpha        = 1.0-ConfidenceOnLastDeformation ; /* default: ConfidenceOnLastDeformation=0 *//* it was directly 1.0 at June 18 2001 ~ July 2003; theoretically it should be 0. */
  deform_ratio = ( 1.0*alpha + deform_ratio*(1.0-alpha) ) ;  /* changed back June 18, 2001 */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  ii = i*XYZ_ratio ;
	  jj = j*XYZ_ratio ;
	  kk = k*XYZ_ratio ;

	  ni = (int)ii ;
	  nj = (int)jj ;
	  nk = (int)kk ;

	  niP1 = ni+1 ;
	  njP1 = nj+1 ;
	  nkP1 = nk+1 ;

	  if(ni>=0 && ni<Last_image_size-1  &&  nj>=0 && nj<Last_image_size-1  &&  nk>=0 && nk<Last_z_size-1 )
	    {
	      b = ii-ni ;        b1 = 1.-b ;
	      c = jj-nj ;        c1 = 1.-c ;
	      d = kk-nk ;        d1 = 1.-d ;

	      dx = ( d1*(Last_DeformFld[nk][ni][nj].x*(b1*c1) + Last_DeformFld[nk][niP1][nj].x*(b*c1) + Last_DeformFld[nk][ni][njP1].x*(b1*c) + Last_DeformFld[nk][niP1][njP1].x*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].x*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].x*(b*c1) + Last_DeformFld[nkP1][ni][njP1].x*(b1*c) + Last_DeformFld[nkP1][niP1][njP1].x*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 
	      dy = ( d1*(Last_DeformFld[nk][ni][nj].y*(b1*c1) + Last_DeformFld[nk][niP1][nj].y*(b*c1) + Last_DeformFld[nk][ni][njP1].y*(b1*c) + Last_DeformFld[nk][niP1][njP1].y*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].y*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].y*(b*c1) + Last_DeformFld[nkP1][ni][njP1].y*(b1*c) + Last_DeformFld[nkP1][niP1][njP1].y*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 
	      dz = ( d1*(Last_DeformFld[nk][ni][nj].z*(b1*c1) + Last_DeformFld[nk][niP1][nj].z*(b*c1) + Last_DeformFld[nk][ni][njP1].z*(b1*c) + Last_DeformFld[nk][niP1][njP1].z*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].z*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].z*(b*c1) + Last_DeformFld[nkP1][ni][njP1].z*(b1*c) + Last_DeformFld[nkP1][niP1][njP1].z*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 

	      DeformFld[k][i][j].x = dx*deform_ratio ; /* *deform_ratio, June 14, 2001*/
	      DeformFld[k][i][j].y = dy*deform_ratio ;
	      DeformFld[k][i][j].z = dz*deform_ratio ;
	  }

	  if(ni==Last_image_size-1 && nj>=0 && nj<Last_image_size-1 && nk>=0 && nk<Last_z_size-1 || ni>=0 && ni<Last_image_size-1 && nj==Last_image_size-1 && nk>=0 && nk<Last_z_size-1  || ni>=0 && ni<Last_image_size-1 && nj>=0 && nj<Last_image_size-1 && nk==Last_z_size-1)
	    {
	      DeformFld[k][i][j].x = Last_DeformFld[nk][ni][nj].x*deform_ratio ; /* *deform_ratio, June 14, 2001*/
	      DeformFld[k][i][j].y = Last_DeformFld[nk][ni][nj].y*deform_ratio ;
	      DeformFld[k][i][j].z = Last_DeformFld[nk][ni][nj].z*deform_ratio ;
	    }

	  /* boundary constraints */
	  if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
	  if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
	  if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;

	  if(DeformFld[k][i][j].x+i>=(image_size-1)) DeformFld[k][i][j].x=(image_size-1)-i ;
	  if(DeformFld[k][i][j].y+j>=(image_size-1)) DeformFld[k][i][j].y=(image_size-1)-j ;
	  if(DeformFld[k][i][j].z+k>=(z_size-1))     DeformFld[k][i][j].z=(z_size-1)-k ;
	}

  /* free */
  Fvector3dfree3d(Last_DeformFld, Last_z_size, Last_image_size) ;
 }


#define BSIC_RAD 1 /* 1 */
void calculate_hood(Fvector3d **hood,int *nbr_size,float xres,float yres,float zres)
{
  int i,j,k,s,t,rad_sqrd,radius;

  /*  printf("xres=%f yres=%f zres=%f\n",xres,yres,zres); */
  for(s=0;s<10;s++)
    {
      t=0;
      radius=s+BSIC_RAD;
      rad_sqrd=radius*radius;
      for (i=-radius;i<=radius;i++)
	for(j=-radius;j<=radius;j++)
	  for(k=-radius;k<=radius;k++)
	    {
	      if((i*i+j*j+k*k)<=rad_sqrd)
		{
		  hood[s][t].x=((float) i)/xres;
		  hood[s][t].y=((float) j)/yres;
		  hood[s][t].z=((float) k)/zres;
		  t++;
		}
	    }
      nbr_size[s]=t;
    }

}

#define BASIC_R 0
void calculate_hood_special(Fvector3d *bubble,int bubble_size, int *pixelNumInBubble, float xres,float yres,float zres)
{
  int i,j,k,t,rad_sqrd,radius;

  t=0;
  radius=bubble_size+BASIC_R;
  rad_sqrd=radius*radius;
  for(i=-radius;i<=radius;i++)
    for(j=-radius;j<=radius;j++)
      for(k=-radius;k<=radius;k++)
	if((i*i+j*j+k*k)<=rad_sqrd)
	  {
	    bubble[t].x=((float) i)/xres;
	    bubble[t].y=((float) j)/yres;
	    bubble[t].z=((float) k)/zres;
	    t++;
	  }
  (*pixelNumInBubble) = t;
}

void calculate_hood_byIncreasingRadius(Ivector3d *Hood, int HoodSize, int *pixelNumInHood, float xres,float yres,float zres)
{
  int        i, j, k, l ;
  int        x, y, z, t, r  ;
  int        half ; 


  t = 0 ;  Hood[0].x = 0 ;  Hood[0].y = 0 ;  Hood[0].z = 0 ;
  t++ ;
  half = HoodSize/2;
  for(r=1; r<=half; r++)
    {
      for(z=-r; z<=r; z+=2*r)
	for(x=-r; x<=r; x++)
	  for(y=-r; y<=r; y++)
	    {
	      Hood[t].x = (int)(x/xres) ;
	      Hood[t].y = (int)(y/yres) ;
	      Hood[t].z = (int)(z/zres) ;
	      
	      t++ ;
	      /*printf("t=%d: (%d,%d,%d)\n", t,x,y,z) ;*/
	    }
      for(x=-r; x<=r; x+=2*r)
	for(z=-r+1; z<=r-1; z++) 
	  for(y=-r; y<=r; y++)
	    {
	      Hood[t].x = (int)(x/xres) ;
	      Hood[t].y = (int)(y/yres) ;
	      Hood[t].z = (int)(z/zres) ;
	      
	      t++ ;
	      /*printf("t=%d: (%d,%d,%d)\n", t,x,y,z) ;*/
	    }
      for(y=-r; y<=r; y+=2*r)
	for(z=-r+1; z<=r-1; z++) 
	  for(x=-r+1; x<=r-1; x++) 
	    {
	      Hood[t].x = (int)(x/xres) ;
	      Hood[t].y = (int)(y/yres) ;
	      Hood[t].z = (int)(z/zres) ;
	      
	      t++ ;
	      /*printf("t=%d: (%d,%d,%d)\n", t,x,y,z) ;*/
	    }
    }
  (*pixelNumInHood) = t ;
  printf("total %d in the search area!\n", t) ;
}


void Compute_GeometricFAFeatures(TensorField& Tensor, DTIImgAttribute ***Img, int scale, float Xres, float Yres, float Zres, Threshold *FeatureThreshold, float CurrentRequirePercent) 
{
  int i,j,k, tempk, tempi, tempj, t, l,m,n ;
  float total, subTotal, diff, Cluster_Low, Cluster_High ;
  int   Num_Cluster_Low, Num_Cluster_High ;
  int   mean, variance, contrast, XYscale, Zscale;
  int   Hist[MaxOfUC] ;

  int       bubble_size, pixelNumInBubble;
  Fvector3d *bubble ;

  /* temporary memory */
  unsigned char ***GreyImg, ***CannyImg ;

  
  /*** tensor decomposition ***/
  printf("Decompose tensor to obtain required structures.\n");
  
  int nSelectedStruct = FA_STRUCT;
  Tensor.initStruct(nSelectedStruct);
  Tensor.decomposeStructure();
  Tensor.normalizeTensorStructField(FA_STRUCT);
  Tensor.smoothTensorStructField(nSelectedStruct, 1.0f);  
  /*************************/

  /*** orientation feature ***/
  if (bOrientationFeature)
  {
    printf("Orientation feature is set. Compute PD color map...\n");
    int nSelectedPD = PD_FIRST;
    Tensor.initColorPD(nSelectedPD);
    Tensor.computeColorPD(true);
    Tensor.smoothColorPDField(nSelectedPD, 1.0f);
  }
  
  /*************************/
  
  ByteScalarField fa = Tensor.getFAStructure();
  unsigned char *datFA = fa.getDataPtr();
    
  /* fa tissue feature */
  for (k=0; k<z_size; k++)
    for (i=0; i<image_size; i++)
      for (j=0; j<image_size; j++)
      {
        int idx = k*image_size*image_size+i*image_size+j;
        Img[k][i][j].faTiss = datFA[idx];
      }
  std::cout << "FA tissue feature extracted!" << std::endl; 
  
  /////////////////////////////
  printf("Compute geometric features for each edge voxel.\n") ;
  
  
  /* neighborhood */
  bubble = Fvector3dalloc1d(10000);
  bubble_size = scale ;
  calculate_hood_special(bubble, bubble_size, &pixelNumInBubble, Xres, Yres, Zres);
  printf("bubble_size=%d, pixelNumInBubble=%d\n", bubble_size, pixelNumInBubble) ;


  Zscale  = (int)(scale/Zres) ;
  XYscale = (int)(scale/Xres) ;

  /* action */
  for(k=Zscale;k<z_size-Zscale;k++)
    for(i=XYscale;i<image_size-XYscale;i++)
      for(j=XYscale;j<image_size-XYscale;j++)
	if( Img[k][i][j].faTiss>0 )
	{
	  /* get mean intensity around (i,j,k) */
	  total = 0 ;
	  for(t=0; t<pixelNumInBubble; t++)
	    {
	      l = (int)bubble[t].x ;
	      m = (int)bubble[t].y ;
	      n = (int)bubble[t].z ;

	      total += Img[k+n][i+l][j+m].faTiss ;
	    }
	  mean = (int)(total/pixelNumInBubble) ;
	  if( mean>255 )  
	    Img[k][i][j].prolateMean = 255 ;
	  else
	    Img[k][i][j].prolateMean = mean ;

	  /* get variance in the neighborhood */
	  total = 0 ;
	  for(t=0; t<pixelNumInBubble; t++)
	    {
	      l = (int)bubble[t].x ;
	      m = (int)bubble[t].y ;
	      n = (int)bubble[t].z ;

	      diff = Img[k+n][i+l][j+m].faTiss - mean ;
	      total += diff*diff ;
	    }
	  variance = (int)(sqrt(total/pixelNumInBubble)) ;
	  if( variance>255 )  
	    Img[k][i][j].prolateVar = 255 ;
	  else
	    Img[k][i][j].prolateVar = variance ;


	  /* get contrast */
	  Cluster_Low = 0 ;      Cluster_High = 0 ;
	  Num_Cluster_Low = 0 ;  Num_Cluster_High = 0;
	  total = 0 ;
	  for(t=0; t<pixelNumInBubble; t++)
	    {
	      l = (int)bubble[t].x ;
	      m = (int)bubble[t].y ;
	      n = (int)bubble[t].z ;

	      if( Img[k+n][i+l][j+m].faTiss > mean )
		{ /* high */
		  Num_Cluster_High ++ ;
		  Cluster_High += Img[k+n][i+l][j+m].faTiss ;
		}
	      else
		{ /* low */
		  Num_Cluster_Low ++ ;
		  Cluster_Low += Img[k+n][i+l][j+m].faTiss ;
		}
	    }
	  if( Num_Cluster_High>0 )
	    Cluster_High /= Num_Cluster_High ;
	  if( Num_Cluster_Low>0 )
	    Cluster_Low /= Num_Cluster_Low ;
	  contrast = (int)(Cluster_High - Cluster_Low) ;
	  if( contrast>255 )  
	    Img[k][i][j].traceEdge = 255 ;
	  else
	    Img[k][i][j].traceEdge = contrast ;
	}

  /* 3D imgs */
  GreyImg  = UCalloc3d(image_size,image_size,z_size);
  CannyImg = UCalloc3d(image_size,image_size,z_size);

  /* FA canny edge ... */  
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
      {
        int idx = k*image_size*image_size+i*image_size+j;
        GreyImg[k][i][j] = datFA[idx];
        CannyImg[k][i][j] = 0;
      }
  /*Cubic_edgeDetectionBy2DcannyMethod(GreyImg, CannyImg) ; use this before Sept 18 */
  Cubic_edgeDetectionBythree2DcannyMethod(GreyImg, CannyImg) ;
  
  /* put edges as attribute */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	    Img[k][i][j].faEdge = CannyImg[k][i][j] ;
        
        
    /* get histogram of edge map (added on Sept 16, 2003) */
   for(k=0; k<=MaxOfUC; k++) Hist[k] = 0 ;
   total = 0 ;
   for(k=0; k<z_size; k++)
     for(i=0; i<image_size; i++)
       for(j=0; j<image_size; j++)
	 {
	   Hist[ CannyImg[k][i][j] ] += 1 ;
	   if( CannyImg[k][i][j]!=0 )   total ++ ;
	 }
   subTotal = 0 ;  (*FeatureThreshold).CannyMinimalRequiredEdgeValue = 0 ;
   for(k=1; k<=MaxOfUC; k++)
     {
       subTotal += Hist[k] ;
       if( subTotal/total>=PercentOfLowCannyEdge/*0.25*/ ) /* threshold for getting strong edges */
	 {
	   (*FeatureThreshold).CannyMinimalRequiredEdgeValue = k ;
	   break ;
	 }
     }
     
  if( MaxAttribute.faTiss == 0 ) /* this is global value, which will used to normalize attributes of model and subject */
  {
      /* find max */
      MaxAttribute.faTiss = 1; /*0*/ /* for avoiding the case that the final Max equals to 0*/
      MaxAttribute.prolateVar = 1; /*0*/
      MaxAttribute.prolateMean = 1; /*0*/
      MaxAttribute.oblateVar = 1;
      MaxAttribute.oblateMean = 1;
      MaxAttribute.sphereVar = 1;
      MaxAttribute.sphereMean = 1;
      MaxAttribute.traceEdge= 1; /*0*/ 
      MaxAttribute.faEdge= 1; /*0*/ 
      
      for(k=Zscale;k<z_size-Zscale;k++)
	    for(i=XYscale;i<image_size-XYscale;i++)
	      for(j=XYscale;j<image_size-XYscale;j++)
	        if( Img[k][i][j].faTiss>0 )
	        {
		       if( Img[k][i][j].faTiss>MaxAttribute.faTiss )   MaxAttribute.faTiss=Img[k][i][j].faTiss ;
		       if( Img[k][i][j].prolateVar>MaxAttribute.prolateVar )   MaxAttribute.prolateVar=Img[k][i][j].prolateVar ;
		       if( Img[k][i][j].prolateMean>MaxAttribute.prolateMean )   MaxAttribute.prolateMean=Img[k][i][j].prolateMean ;
		       if( Img[k][i][j].oblateVar>MaxAttribute.oblateVar )   MaxAttribute.oblateVar=Img[k][i][j].oblateVar ;
		       if( Img[k][i][j].oblateMean>MaxAttribute.oblateMean )   MaxAttribute.oblateMean=Img[k][i][j].oblateMean ;
		       if( Img[k][i][j].sphereVar>MaxAttribute.sphereVar )   MaxAttribute.sphereVar=Img[k][i][j].sphereVar ;
		       if( Img[k][i][j].sphereMean>MaxAttribute.sphereMean )   MaxAttribute.sphereMean=Img[k][i][j].sphereMean ;
		       if( Img[k][i][j].traceEdge>MaxAttribute.traceEdge ) MaxAttribute.traceEdge=Img[k][i][j].traceEdge ;
		       if( Img[k][i][j].faEdge>MaxAttribute.faEdge ) MaxAttribute.faEdge=Img[k][i][j].faEdge ;
	        }
            
      printf("MaxAttribute.faTiss=%d\n",  MaxAttribute.faTiss) ; 
      printf("MaxAttribute.prolateVar=%d\n",  MaxAttribute.prolateVar) ; 
      printf("MaxAttribute.prolateMean=%d\n",  MaxAttribute.prolateMean) ; 
      printf("MaxAttribute.oblateVar=%d\n",  MaxAttribute.oblateVar) ; 
      printf("MaxAttribute.oblateMean=%d\n",  MaxAttribute.prolateMean) ; 
      printf("MaxAttribute.sphereVar=%d\n",  MaxAttribute.sphereVar) ; 
      printf("MaxAttribute.sphereMean=%d\n",  MaxAttribute.sphereMean) ; 
      printf("MaxAttribute.traceEdge=%d\n", MaxAttribute.traceEdge) ; 
      printf("MaxAttribute.faEdge=%d\n", MaxAttribute.faEdge) ; 
  }

  /* free */
  free(bubble);
  UCfree3d(GreyImg,  z_size, image_size) ;
  UCfree3d(CannyImg, z_size, image_size) ;  
}


void Compute_GeometricFeatures(TensorField& Tensor, DTIImgAttribute ***Img, int scale, float Xres, float Yres, float Zres, Threshold *FeatureThreshold, float CurrentRequirePercent) 
{
   
  int i,j,k, tempk, tempi, tempj, t, l,m,n ;
  float total, total_fa, total_prolate, total_oblate, total_sphere, subTotal, diff, Cluster_Low, Cluster_High ;
  int   Num_Cluster_Low, Num_Cluster_High ;
  int   mean, variance, contrast, XYscale, Zscale;
  int   Hist[MaxOfUC] ;

  int       bubble_size, pixelNumInBubble;
  Fvector3d *bubble ;

  /* temporary memory */
  unsigned char ***GreyImg, ***CannyImg ;

  
  /*** tensor decomposition ***/
  printf("Decompose tensor to obtain required structures.\n");
  
  int nSelectedStruct = PROLATE_STRUCT + OBLATE_STRUCT + SPHERE_STRUCT + FA_STRUCT + TRACE_STRUCT;
  Tensor.initStruct(nSelectedStruct);
  Tensor.decomposeStructure();
  Tensor.normalizeTensorStructField(FA_STRUCT);
  Tensor.smoothTensorStructField(nSelectedStruct, 1.0f);  
  /*************************/

  /*** orientation feature ***/
  if (bOrientationFeature)
  {
    printf("Orientation feature is set. Compute PD color map...\n");
    int nSelectedPD = PD_FIRST;
    Tensor.initColorPD(nSelectedPD);
    Tensor.computeColorPD(true);
    Tensor.smoothColorPDField(nSelectedPD, 1.0f);
  }
  
  /*************************/
  
  ByteScalarField trace = Tensor.getTraceStructure();
  ByteScalarField fa = Tensor.getFAStructure();
  ByteScalarField prolate = Tensor.getProlateStructure();
  ByteScalarField oblate = Tensor.getOblateStructure();
  ByteScalarField sphere = Tensor.getSphereStructure();
  
  unsigned char *datTrace = trace.getDataPtr();
  unsigned char *datFA = fa.getDataPtr();
  unsigned char *datProlate = prolate.getDataPtr();
  unsigned char *datOblate = oblate.getDataPtr();
  unsigned char *datSphere = sphere.getDataPtr();
    
  /* fa tissue feature */
  for (k=0; k<z_size; k++)
    for (i=0; i<image_size; i++)
      for (j=0; j<image_size; j++)
      {
        int idx = k*image_size*image_size+i*image_size+j;
        Img[k][i][j].faTiss = datFA[idx];
      }
  std::cout << "FA tissue feature extracted!" << std::endl; 
  
  /////////////////////////////
  printf("Compute geometric features for each edge voxel.\n") ;

  /* neighborhood */
  bubble = Fvector3dalloc1d(10000);
  bubble_size = scale ;
  calculate_hood_special(bubble, bubble_size, &pixelNumInBubble, Xres, Yres, Zres);
  printf("bubble_size=%d, pixelNumInBubble=%d\n", bubble_size, pixelNumInBubble) ;


  Zscale  = (int)(scale/Zres) ;
  XYscale = (int)(scale/Xres) ;

  /* action */
  for(k=Zscale;k<z_size-Zscale;k++)
    for(i=XYscale;i<image_size-XYscale;i++)
      for(j=XYscale;j<image_size-XYscale;j++)
      {
        if( Img[k][i][j].faTiss>0 ) //skip backgroud voxels
	    {
  	      /* get mean intensity around (i,j,k) */
          total_fa = 0 ; total_prolate = 0; 
          total_oblate = 0; total_sphere = 0;
      
	      for(t=0; t<pixelNumInBubble; t++)
	      {
	        l = (int)bubble[t].x ;
	        m = (int)bubble[t].y ;
	        n = (int)bubble[t].z ;

	        total_fa += Img[k+n][i+l][j+m].faTiss ;
          
            int idx = (k+n)*image_size*image_size+(i+l)*image_size+(j+m);
          
            total_prolate += datProlate[idx];
            total_oblate += datOblate[idx];
            total_sphere += datSphere[idx];
	      }
     
          /// mean of each structure
          mean = (int)(total_prolate/pixelNumInBubble);
	      if( mean>255 )  
	        Img[k][i][j].prolateMean = 255 ;
	      else
	        Img[k][i][j].prolateMean = mean ;
        
          mean = (int)(total_oblate/pixelNumInBubble);
	      if( mean>255 )  
	        Img[k][i][j].oblateMean = 255 ;
	      else
	        Img[k][i][j].oblateMean = mean ;
        
          mean = (int)(total_sphere/pixelNumInBubble);
	      if( mean>255 )  
	        Img[k][i][j].sphereMean = 255 ;
	      else
	        Img[k][i][j].sphereMean = mean ;
        
	      /* get variance in the neighborhood */
	      total_prolate = 0 ; total_oblate = 0; total_sphere = 0;
	      for(t=0; t<pixelNumInBubble; t++)
	      {
	        l = (int)bubble[t].x ;
	        m = (int)bubble[t].y ;
	        n = (int)bubble[t].z ;

            int idx = (k+n)*image_size*image_size+(i+l)*image_size+(j+m);
	        diff = datProlate[idx] - Img[k][i][j].prolateMean ;
	        total_prolate += diff*diff ;
          
            diff = datOblate[idx] - Img[k][i][j].oblateMean ;
	        total_oblate += diff*diff ;
          
            diff = datSphere[idx] - Img[k][i][j].sphereMean ;
	        total_sphere += diff*diff ;
	      }
          /// variance of each structure
	      variance = (int)(sqrt(total_prolate/pixelNumInBubble)) ;
	      if( variance>255 )  
	        Img[k][i][j].prolateVar = 255 ;
	      else
	        Img[k][i][j].prolateVar = variance ;

	      variance = (int)(sqrt(total_oblate/pixelNumInBubble)) ;
	      if( variance>255 )  
	        Img[k][i][j].oblateVar = 255 ;
	      else
	        Img[k][i][j].oblateVar = variance ;
        
	      variance = (int)(sqrt(total_sphere/pixelNumInBubble)) ;
	      if( variance>255 )  
	        Img[k][i][j].sphereVar = 255 ;
	      else
	        Img[k][i][j].sphereVar = variance ;
	    }
        else
        {          
          Img[k][i][j].prolateMean = 0;
          Img[k][i][j].prolateVar = 0;

          Img[k][i][j].oblateMean = 0;
          Img[k][i][j].oblateVar = 0;

          Img[k][i][j].sphereMean = 0;
          Img[k][i][j].sphereVar = 0;
        }
      }
    
  std::cout << "Tensor structure features extracted!" << std::endl;
    
  /* 3D imgs */
  GreyImg  = UCalloc3d(image_size,image_size,z_size);
  CannyImg = UCalloc3d(image_size,image_size,z_size);

  /* FA canny edge ... */  
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
      {
        int idx = k*image_size*image_size+i*image_size+j;
        GreyImg[k][i][j] = datFA[idx];
        CannyImg[k][i][j] = 0;
      }
  /*Cubic_edgeDetectionBy2DcannyMethod(GreyImg, CannyImg) ; use this before Sept 18 */
  Cubic_edgeDetectionBythree2DcannyMethod(GreyImg, CannyImg) ;
  
  /* put edges as attribute */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	    Img[k][i][j].faEdge = CannyImg[k][i][j] ;
     
  /* Trace canny edge ... */  
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
      {
        int idx = k*image_size*image_size+i*image_size+j;
        GreyImg[k][i][j] = datTrace[idx];
        CannyImg[k][i][j] = 0;
      }
  /*Cubic_edgeDetectionBy2DcannyMethod(GreyImg, CannyImg) ; use this before Sept 18 */
  Cubic_edgeDetectionBythree2DcannyMethod(GreyImg, CannyImg) ;
  
  /* put edges as attribute */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	    Img[k][i][j].traceEdge = CannyImg[k][i][j] ;
        
  /* get histogram of both edge maps */
  for(k=0; k<=MaxOfUC; k++) Hist[k] = 0 ;
  total = 0 ;
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	  {
        unsigned char max_edge = (Img[k][i][j].faEdge>(int)(TraceEdgeConfidence*Img[k][i][j].traceEdge))?Img[k][i][j].faEdge:(int)(TraceEdgeConfidence*Img[k][i][j].traceEdge);
	    Hist[max_edge] += 1 ;       
	    if (max_edge!=0) total++ ;
	  }
  
  subTotal = 0 ;  (*FeatureThreshold).CannyMinimalRequiredEdgeValue = 0 ;
  for(k=1; k<=MaxOfUC; k++)
  {
    subTotal += Hist[k] ;
    if( subTotal/total>=PercentOfLowCannyEdge/*0.25*/ ) /* threshold for getting strong edges */
	{
	  (*FeatureThreshold).CannyMinimalRequiredEdgeValue = k ;
	  break ;
	}
  }

  if( MaxAttribute.faTiss == 0 ) /* this is global value, which will used to normalize attributes of model and subject */
  {
      /* find max */
      MaxAttribute.faTiss = 1; /*0*/ /* for avoiding the case that the final Max equals to 0*/
      MaxAttribute.prolateVar = 1; /*0*/
      MaxAttribute.prolateMean = 1; /*0*/
      MaxAttribute.oblateVar = 1;
      MaxAttribute.oblateMean = 1;
      MaxAttribute.sphereVar = 1;
      MaxAttribute.sphereMean = 1;
      MaxAttribute.traceEdge= 1; /*0*/ 
      MaxAttribute.faEdge= 1; /*0*/ 
      
      for(k=Zscale;k<z_size-Zscale;k++)
	    for(i=XYscale;i<image_size-XYscale;i++)
	      for(j=XYscale;j<image_size-XYscale;j++)
	        if( Img[k][i][j].faTiss>0 )
	        {
		       if( Img[k][i][j].faTiss>MaxAttribute.faTiss )   MaxAttribute.faTiss=Img[k][i][j].faTiss ;
		       if( Img[k][i][j].prolateVar>MaxAttribute.prolateVar )   MaxAttribute.prolateVar=Img[k][i][j].prolateVar ;
		       if( Img[k][i][j].prolateMean>MaxAttribute.prolateMean )   MaxAttribute.prolateMean=Img[k][i][j].prolateMean ;
		       if( Img[k][i][j].oblateVar>MaxAttribute.oblateVar )   MaxAttribute.oblateVar=Img[k][i][j].oblateVar ;
		       if( Img[k][i][j].oblateMean>MaxAttribute.oblateMean )   MaxAttribute.oblateMean=Img[k][i][j].oblateMean ;
		       if( Img[k][i][j].sphereVar>MaxAttribute.sphereVar )   MaxAttribute.sphereVar=Img[k][i][j].sphereVar ;
		       if( Img[k][i][j].sphereMean>MaxAttribute.sphereMean )   MaxAttribute.sphereMean=Img[k][i][j].sphereMean ;
		       if( Img[k][i][j].traceEdge>MaxAttribute.traceEdge ) MaxAttribute.traceEdge=Img[k][i][j].traceEdge ;
		       if( Img[k][i][j].faEdge>MaxAttribute.faEdge ) MaxAttribute.faEdge=Img[k][i][j].faEdge ;
	        }
            
      printf("MaxAttribute.faTiss=%d\n",  MaxAttribute.faTiss) ; 
      printf("MaxAttribute.prolateVar=%d\n",  MaxAttribute.prolateVar) ; 
      printf("MaxAttribute.prolateMean=%d\n",  MaxAttribute.prolateMean) ; 
      printf("MaxAttribute.oblateVar=%d\n",  MaxAttribute.oblateVar) ; 
      printf("MaxAttribute.oblateMean=%d\n",  MaxAttribute.prolateMean) ; 
      printf("MaxAttribute.sphereVar=%d\n",  MaxAttribute.sphereVar) ; 
      printf("MaxAttribute.sphereMean=%d\n",  MaxAttribute.sphereMean) ; 
      printf("MaxAttribute.traceEdge=%d\n", MaxAttribute.traceEdge) ; 
      printf("MaxAttribute.faEdge=%d\n", MaxAttribute.faEdge) ; 
  }
  
  /* free */
  free(bubble);
  UCfree3d(GreyImg,  z_size, image_size) ;
  UCfree3d(CannyImg, z_size, image_size) ;
}


void Normalize_GeometricFeatures(DTIImgAttribute ***Img, int scale, float Xres, float Yres, float Zres, Threshold *FeatureThreshold, float CurrentRequirePercent)
{
  float temp;
  int XYscale, Zscale;
  int i,j,k;
  
  Zscale  = (int)(scale/Zres) ;
  XYscale = (int)(scale/Xres) ;
  
  /* normalization */
  for(k=Zscale;k<z_size-Zscale;k++)
    for(i=XYscale;i<image_size-XYscale;i++)
      for(j=XYscale;j<image_size-XYscale;j++) 
	if( Img[k][i][j].faTiss>0 )
	  {
	    temp = Img[k][i][j].faTiss*255.0/MaxAttribute.faTiss ;    Img[k][i][j].faTiss = (unsigned char)temp ;
	    temp = Img[k][i][j].prolateVar*255.0/MaxAttribute.prolateVar ;    Img[k][i][j].prolateVar = (unsigned char)temp ;
	    temp = Img[k][i][j].prolateMean*255.0/MaxAttribute.prolateMean ;    Img[k][i][j].prolateMean = (unsigned char)temp ;
	    temp = Img[k][i][j].oblateVar*255.0/MaxAttribute.oblateVar ;    Img[k][i][j].oblateVar = (unsigned char)temp ;
	    temp = Img[k][i][j].oblateMean*255.0/MaxAttribute.oblateMean ;    Img[k][i][j].oblateMean = (unsigned char)temp ;
	    temp = Img[k][i][j].sphereVar*255.0/MaxAttribute.sphereVar ;    Img[k][i][j].sphereVar = (unsigned char)temp ;
	    temp = Img[k][i][j].sphereMean*255.0/MaxAttribute.sphereMean ;    Img[k][i][j].sphereMean = (unsigned char)temp ;
	    temp = Img[k][i][j].traceEdge*255.0/MaxAttribute.traceEdge ;  Img[k][i][j].traceEdge = (unsigned char)temp; 
	    temp = Img[k][i][j].faEdge*255.0/MaxAttribute.faEdge ;  Img[k][i][j].faEdge = (unsigned char)temp;
	  }

  /* get threshols for driving voxels */
  SearchThresholdForObtainingTopXPercentOfDrivingVoxels(Img, FeatureThreshold, CurrentRequirePercent) ;
  
}


void SearchThresholdForObtainingTopXPercentOfDrivingVoxels(DTIImgAttribute ***Img, Threshold *FeatureThreshold, float CurrentRequirePercent) 
{
  int   m, i, j, k, VoxNum ;
  float totalCanny, totalVariance ;
  int   Hist[MaxOfUC] ;


  /* get the number of non-zero voxels */
  VoxNum = 0;
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	if( Img[k][i][j].faTiss>0 )
	  VoxNum ++ ;


  /* get the histogram of edge map, for fast determination of edge threshold */
  for(k=0; k<=MaxOfUC; k++) Hist[k] = 0 ;
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
      {
        if (IsFAFeature)
          Hist[Img[k][i][j].faEdge] += 1;
        else
        {
          unsigned char max_edge = (Img[k][i][j].faEdge>(int)(TraceEdgeConfidence*Img[k][i][j].traceEdge))?Img[k][i][j].faEdge:(int)(TraceEdgeConfidence*Img[k][i][j].traceEdge);
          Hist[max_edge] += 1 ;    
        }
      }
      
  /* get driving points from canny edge map first*/
  totalCanny = 0 ;
  for(m=MaxOfUC-1; m>=(*FeatureThreshold).CannyMinimalRequiredEdgeValue; m--)  /*m>=0*/
  {
      /*totalCanny = 0 ;
      for(k=0;k<z_size;k++) 
	for(i=0;i<image_size;i++) 
	  for(j=0;j<image_size;j++) 
	    if( Img[k][i][j].CSFBG>m ) 
	    totalCanny += 1 ;*/
      totalCanny += Hist[m+1] ;
      
      (*FeatureThreshold).CannyEdge_UP = m ;	
      if( totalCanny/VoxNum > CurrentRequirePercent)
	    m = -111111 ; /* stop */	  
  }

  /* points in the canny edge map are not enough */
  (*FeatureThreshold).Variance_UP = 255 ;	
  if( totalCanny/VoxNum < CurrentRequirePercent ) 
    {
      /* get the histogram of variance map, only for the points not selected as driving voxels */
      for(k=0; k<=MaxOfUC; k++) Hist[k] = 0 ;
      for(k=0; k<z_size; k++)
	for(i=0; i<image_size; i++)
	  for(j=0; j<image_size; j++)
	    if( Img[k][i][j].faEdge<=(*FeatureThreshold).CannyEdge_UP )
	      Hist[ Img[k][i][j].prolateVar ] += 1 ;
      /* get driving points from variance map */
      totalVariance = 0 ;
      for(m=MaxOfUC-1; m>=0; m--)
	{
	  /*totalVariance = 0 ;
	  for(k=0;k<z_size;k++) 
	    for(i=0;i<image_size;i++) 
	      for(j=0;j<image_size;j++) 
		if( Img[k][i][j].Edge>m && Img[k][i][j].CSFBG<=(*FeatureThreshold).CannyEdge_UP ) 
		totalVariance += 1 ;*/
	  totalVariance += Hist[m+1] ;
	  
	  (*FeatureThreshold).Variance_UP = m ;	
	  if( (totalCanny+totalVariance)/VoxNum > CurrentRequirePercent )
	    m = -111111 ; /* stop */	  
	}
    }
}


void CannyEdgePercentInImg(DTIImgAttribute ***Img, float *CannyEdgePercent)
{
  int   m, i, j, k, VoxNum ;
  float totalCanny ;

  VoxNum = 0;
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	if( Img[k][i][j].faTiss>0 )
	  VoxNum ++ ;

  /* total non-zero edge points */
  totalCanny = 0 ;
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
      
	if( Img[k][i][j].faEdge!=0 || (!IsFAFeature&&Img[k][i][j].traceEdge!=0))
	  totalCanny += 1 ;

  *CannyEdgePercent = totalCanny/VoxNum ; 
}

void BluringCannyEdge(unsigned char ***CannyImg) 
{
  int   i, j, k, l,m,n ;
  unsigned char ***TempImg ; 
  int   total, num ;

  TempImg = UCalloc3d(image_size,image_size,z_size);

  for(k=1;k<z_size-1;k++)
    for(i=1;i<image_size-1;i++)
      for(j=1;j<image_size-1;j++)
	{
	  if( CannyImg[k][i][j]==0 ) 
	    {
	      total = 0 ; num=0;
	      for(l=-1;l<=1;l++)
		for(m=-1;m<=1;m++)
		  for(n=-1;n<=1;n++)
		    if( CannyImg[k+n][i+l][j+m]!=0 ) 
		      {
			total += CannyImg[k+n][i+l][j+m] ;
			num ++ ;
		      }
	      if( num>0 )
		TempImg[k][i][j] = (unsigned char)(total/num) ;
	    }
	}

  /* merge */
  for(k=1;k<z_size-1;k++)
    for(i=1;i<image_size-1;i++)
      for(j=1;j<image_size-1;j++)
	if( CannyImg[k][i][j]==0 ) 
	  CannyImg[k][i][j] = TempImg[k][i][j] ;

  /* free */
  UCfree3d(TempImg, z_size, image_size) ;
}


double hypotenuse(double x,double y)
{
    if (x==0.0 && y==0.0) return(0.0);
    else return(hypot(x,y));
}
float gaussian(float x, float s)
{
  return(exp((-x*x)/(2*s*s)));
}

#define ORIENT_SCALE  24
void canny_core(double s,int cols,int rows,unsigned char *data,unsigned char *derivative_mag,unsigned char *magnitude,unsigned char *orientation)
{
  int      filter_width;            /* length of 1-D gaussian mask */
  float    *gsmooth_x,*gsmooth_y;
  float    *derivative_x,*derivative_y;
  int      i,k,n;                   /* counters */
  int      t;                       /* temp. grad magnitude variable */
  double   a,b,c,d,g0;              /* mask generation intermediate vars*/
  double   ux,uy;
  double   t1,t2;
  double   G[20],dG[20],D2G[20];    /*gaussian & derivative filter masks*/
  double   gc,gn,gs,gw,ge,gnw,gne,gsw,gse;
  int      picsize,jstart,jlimit;
  int      ilimit;
  int      jfactor;
  int      kfactor1,kfactor2;
  int      kfactor;
  int       cindex,nindex,sindex,windex,eindex,nwindex,neindex,swindex,seindex;
  int      low=1,high=255;          /* tracker hysteresis parameters */
  int      mag_overflow_count=0;    /* used to measure how oft mag array overflows */
  int      mag_underflow_count=0;   /* used to measure how oft mag array underflows */

  double weightG, weightDG ;  /*normalizing ...*/



  picsize=cols*rows;    /* picture area */

  /* calc coeffs for 1-dimensional G, dG/dn and Delta-squared G filters */
  for(n=0; n<20; ++n)
    {
      a=gaussian((double)n,s);
      if (a>0.005 || n<2)
        {
	  b=gaussian((double)n-0.5,s);
	  c=gaussian((double)n+0.5,s);
	  d=gaussian((double)n,s*0.5);
	  /*fprintf(stderr,"a,b,c: %lf,%lf,%lf\n",a,b,c);*/
	  G[n]=(a+b+c)/3/(6.283185*s*s);
	  dG[n]=c-b;
	  D2G[n]=1.6*d-a; /* DOG */
	  /* fprintf(stderr,"G[%d]: %lf\n",n,G[n]);
	  fprintf(stderr,"dG[%d]: %lf\n",n,dG[n]); 
	  fprintf(stderr,"D2G[%d]: %lf\n",n,D2G[n]);*/	  
        }
      else break;
    }
  filter_width=n; /* printf("n=%d\n", n); */


  /*normalizing ...*/
  weightG  = 0.5*G[0] ; 
  weightDG = 0 ; 
  for(k=1;k<filter_width;++k)
    {
      weightG  +=  G[k] ; 
      weightDG += fabs(dG[k]) ;
    }
  for(k=0;k<filter_width;++k)
    {
      G[k]  /= weightG ; 
      dG[k] /= weightDG ; 
      /*fprintf(stderr,"G[%d]: %lf\n",k,G[k]);
	fprintf(stderr,"dG[%d]: %lf\n",k,dG[k]);*/
    }


  /*fprintf(stderr,"canny_core: smooth pic\n");*/
  /* allocate space for gaussian smoothing arrays */
  if ((gsmooth_x=(float *)calloc(picsize,sizeof(float)))==(float *)NULL)
    {
      fprintf(stderr,"can't alloc gsmooth_x\n");
      exit(0);
    }
  if ((gsmooth_y=(float *)calloc(picsize,sizeof(float)))==(float *)NULL)
    {
      fprintf(stderr,"can't alloc gsmooth_y\n");
      exit(0);
    }


  /* produce x- and y- convolutions with gaussian */
  ilimit=cols-(filter_width-1);
  jstart=cols*(filter_width-1);
  jlimit=cols*(rows-(filter_width-1));
  for (i=filter_width-1;i<ilimit;++i)
    {
      for(jfactor=jstart;
	  jfactor<jlimit;
	  jfactor+=cols)
        {
	  cindex=i+jfactor;
	  t1=data[cindex]*G[0];
	  t2=t1;
	  for(k=1,kfactor1=cindex-cols,
                kfactor2=cindex+cols;
	      k<filter_width;
	      k++,
                kfactor1-=cols,
                kfactor2+=cols)
            {
	      t1+=G[k]*(data[kfactor1]+
			data[kfactor2]);
	      t2+=G[k]*(data[cindex-k]+
			data[cindex+k]);
            }
	  gsmooth_x[cindex]=(float)t1;
	  gsmooth_y[cindex]=(float)t2;
        }
    }
    
  /* allocate space for gradient arrays */
  /*fprintf(stderr,"canny_core: find grad\n");*/


  if ((derivative_x=(float *)calloc(picsize,sizeof(float)))==(float *)NULL)
    {
      fprintf(stderr,"can't alloc x\n");
      exit(0);
    }
  /* produce x and y convolutions with derivative of gaussian */

  for (i=filter_width-1;i<ilimit;++i)
    {
      for(jfactor=jstart;
	  jfactor<jlimit;
	  jfactor+=cols)
        {
	  t1=0;
	  cindex=i+jfactor;
	  for (k=1;k<filter_width;++k)
	    t1+=dG[k]*(gsmooth_x[cindex-k]-
                       gsmooth_x[cindex+k]);
	  derivative_x[cindex] = (float)t1;
        }
    }
  free(gsmooth_x);
  if ((derivative_y=(float *)calloc(picsize,sizeof(float)))==(float *)NULL)
    {
      fprintf(stderr,"can't alloc y\n");
      exit(0);
    }
  
  for (i=n;i<cols-n;++i)
    {
      for(jfactor=jstart;jfactor<jlimit;jfactor+=cols)
        {
	  t2=0;
	  cindex=i+jfactor;
	  for (k=1,kfactor=cols;
	       k<filter_width;
	       k++,kfactor+=cols)
	    t2+=dG[k]*(gsmooth_y[cindex-kfactor]-gsmooth_y[cindex+kfactor]);
	  derivative_y[cindex] = (float)t2;
        }
    }
  free(gsmooth_y);
    

  /* non-maximum suppression (4 cases for orientation of line of max slope) */
  /* fprintf(stderr,"canny_core: non-maximum suppression\n"); */
  ilimit=cols-filter_width;
  jstart=cols*filter_width;
  jlimit=cols*(rows-filter_width);

  for (i=filter_width;i<ilimit;++i)
    {
      for (jfactor=jstart;
	   jfactor<jlimit;
	   jfactor+=cols)
        {
				/* calculate current indeces */
	  cindex=i+jfactor;
	  nindex=cindex-cols;
	  sindex=cindex+cols;
	  windex=cindex-1;
	  eindex=cindex+1;
	  nwindex=nindex-1;
	  neindex=nindex+1;
	  swindex=sindex-1;
	  seindex=sindex+1;
	  ux=derivative_x[cindex];
	  uy=derivative_y[cindex];
	  gc=hypotenuse(ux,uy);
	  /* scale gc to fit into an unsigned char array */
	  t=(int)(gc*1.5); /* 3.0 July 26, 1999 */
	  /*fprintf(stderr,"canny_core: i,j=(%d,%d), t=%lf\n",i,jfactor/cols,t);*/
	  derivative_mag[cindex]=(t<256 ? t : 255);
	  gn=hypotenuse(derivative_x[nindex],derivative_y[nindex]);
	  gs=hypotenuse(derivative_x[sindex],derivative_y[sindex]);
	  gw=hypotenuse(derivative_x[windex],derivative_y[windex]);
	  ge=hypotenuse(derivative_x[eindex],derivative_y[eindex]);
	  gne=hypotenuse(derivative_x[neindex],derivative_y[neindex]);
	  gse=hypotenuse(derivative_x[seindex],derivative_y[seindex]);
	  gsw=hypotenuse(derivative_x[swindex],derivative_y[swindex]);
	  gnw=hypotenuse(derivative_x[nwindex],derivative_y[nwindex]);
	  if (ux*uy>0)
            {
	      if(fabs(ux)<fabs(uy))
                {
		  if((g0=fabs(uy*gc))
                     < fabs(ux*gse+(uy-ux)*gs) ||
                     g0<=fabs(ux*gnw+(uy-ux)*gn))
                    continue;
                }
	      else
                {
		  if((g0=fabs(ux*gc))
                     < fabs(uy*gse+(ux-uy)*ge) ||
                     g0<=fabs(uy*gnw+(ux-uy)*gw))
                    continue;
                }
            }
	  else
            {
	      if(fabs(ux)<fabs(uy))
                {
		  if((g0=fabs(uy*gc))
                     < fabs(ux*gne-(uy+ux)*gn) ||
                     g0<=fabs(ux*gsw-(uy+ux)*gs))
		    continue;
                }
	      else
                {
		  if((g0=fabs(ux*gc))
                     < fabs(uy*gne-(ux+uy)*ge) ||
                     g0<=fabs(uy*gsw-(ux+uy)*gw))
		    continue;
                }
            }
	  /* seems to be a good scale factor */
	  magnitude[cindex]=derivative_mag[cindex];
	  /* pi*40 ~= 128 - direction is (thought of as) a signed byte */
	  orientation[cindex]=(unsigned char)((PI+atan2(ux, uy))*ORIENT_SCALE);
/*		if (orientation[cindex]>(3.1416*ORIENT_SCALE)) 
			orientation[cindex]=orientation[cindex];
		if (orientation[cindex]<0)
			orientation[cindex]=orientation[cindex];
	  float t1,t2,t3,t4;
	  unsigned char ut1,ut2,ut3,ut4;

	  t1=atan2(1,2);
	  t2=atan2(-1,2);
	  t3=atan2(1,-2);
	  t4=atan2(-1,-2);
	  	  ut1=(unsigned char)(t1*ORIENT_SCALE);	 
	  ut2=(unsigned char)(t2*ORIENT_SCALE);	
	  ut3=(unsigned char)(t3*ORIENT_SCALE);	
	  ut4=(unsigned char)(t4*ORIENT_SCALE);	 
*/ 
	   }
    } 

  free(derivative_x);
  free(derivative_y);
}


void Cubic_edgeDetectionBy2DcannyMethod(unsigned char ***Img, unsigned char ***Img_edge)
{
  /* Canny */
  unsigned char *XY_img, *XY_magnitude, *XY_orient, *XY_derivative_mag; /* mag of del G before non-maximum suppression */
  int  low=10, high=16 ; /* for track */

  /* 3D */
  int  i,j,k ;
  FILE *fp;
  int ii,jj,kk ;
  int GreyValue ;


  printf("\n\n2D Canny edge detector for 3D application, with Guassian sigma=%f\n\n", CannyGuassianSigma) ;


  /* Canny part: memory applciation*/
  XY_img           = (unsigned char *)malloc(image_size*image_size*sizeof(unsigned char));
  XY_magnitude     = (unsigned char *)calloc(image_size*image_size,1) ;
  XY_orient        = (unsigned char *)calloc(image_size*image_size,1) ;
  XY_derivative_mag= (unsigned char *)calloc(image_size*image_size,1) ;


  /* edge detection*/
   for(k=0;k<z_size;k++)
   {
     if(k%10==0)
       printf("%dth slice\n", k) ;

     for(i=0;i<image_size;i++)
       for(j=0;j<image_size;j++)
	 XY_img[i*image_size+j] = Img[k][i][j] ;

     /* Canny */
     canny_core(CannyGuassianSigma, image_size, image_size, XY_img, XY_derivative_mag, XY_magnitude, XY_orient);

     for(i=0;i<image_size;i++)
       for(j=0;j<image_size;j++)
	 {
	   Img_edge[k][i][j] = XY_magnitude[i*image_size+j] ;
	   XY_magnitude[i*image_size+j] = 0 ; /* reset */
	 }
   }
 
  /* 2D: free */
   free(XY_img) ;
   free(XY_orient) ;
   free(XY_derivative_mag) ;
   free(XY_magnitude) ;
}



void Cubic_edgeDetectionBythree2DcannyMethod(unsigned char ***Img, unsigned char ***Img_edge)
{
  /* Canny */
  unsigned char *XY_img, *XY_magnitude, *XY_orient, *XY_derivative_mag; /* mag of del G before non-maximum suppression */
  int  low=10, high=16 ; /* for track */

  /* 3D */
  int  i,j,k ;
  FILE *fp;
  int ii,jj,kk ;
  int GreyValue ;


  printf("\n\nthree 2D Canny edge detector for 3D application, with Guassian sigma=%f\n\n", CannyGuassianSigma) ;


  /* Z dimension: */
   printf("\n\nZ dimension\n") ;
   /* Canny part--memory applciation*/
    XY_img           = (unsigned char *)malloc(image_size*image_size*sizeof(unsigned char));
    XY_magnitude     = (unsigned char *)calloc(image_size*image_size,1) ;
    XY_orient        = (unsigned char *)calloc(image_size*image_size,1) ;
    XY_derivative_mag= (unsigned char *)calloc(image_size*image_size,1) ;

    /* edge detection*/
    for(k=0;k<z_size;k++)
      {
	if(k%10==0)
	  printf("%dth slice\n", k) ;
	
	for(i=0;i<image_size;i++)
	  for(j=0;j<image_size;j++)
	    XY_img[i*image_size+j] = Img[k][i][j] ;
	
	/* Canny */
	canny_core(CannyGuassianSigma, image_size, image_size, XY_img, XY_derivative_mag, XY_magnitude, XY_orient);
	
	for(i=0;i<image_size;i++)
	  for(j=0;j<image_size;j++)
	    {
	      Img_edge[k][i][j] = XY_magnitude[i*image_size+j] ;
	      XY_magnitude[i*image_size+j] = 0 ; /* reset */
	    }
      }
    /* 2D: free */
     free(XY_img) ;
     free(XY_orient) ;
     free(XY_derivative_mag) ;
     free(XY_magnitude) ;

  /* X dimension: */
   printf("\n\nX dimension\n") ;
   /* Canny part--memory applciation*/
    XY_img           = (unsigned char *)malloc(z_size*image_size*sizeof(unsigned char));
    XY_magnitude     = (unsigned char *)calloc(z_size*image_size,1) ;
    XY_orient        = (unsigned char *)calloc(z_size*image_size,1) ;
    XY_derivative_mag= (unsigned char *)calloc(z_size*image_size,1) ;

    /* edge detection*/
    for(i=0;i<image_size;i++)
      {
	if(i%10==0)
	  printf("%dth slice\n", i) ;
	
	for(k=0;k<z_size;k++)
	  for(j=0;j<image_size;j++)
	    XY_img[k*image_size+j] = Img[k][i][j] ;
	
	/* Canny */
	canny_core(CannyGuassianSigma, image_size, z_size, XY_img, XY_derivative_mag, XY_magnitude, XY_orient);
	
	for(k=0;k<z_size;k++)
	  for(j=0;j<image_size;j++)
	    {
	      if( Img_edge[k][i][j]<XY_magnitude[k*image_size+j] )
		Img_edge[k][i][j] = XY_magnitude[k*image_size+j] ;
	      XY_magnitude[k*image_size+j] = 0 ; /* reset */
	    }
      }
    /* 2D: free */
     free(XY_img) ;
     free(XY_orient) ;
     free(XY_derivative_mag) ;
     free(XY_magnitude) ;


  /* Y dimension: */
   printf("\n\nY dimension\n") ;
   /* Canny part--memory applciation*/
    XY_img           = (unsigned char *)malloc(z_size*image_size*sizeof(unsigned char));
    XY_magnitude     = (unsigned char *)calloc(z_size*image_size,1) ;
    XY_orient        = (unsigned char *)calloc(z_size*image_size,1) ;
    XY_derivative_mag= (unsigned char *)calloc(z_size*image_size,1) ;

    /* edge detection*/
    for(j=0;j<image_size;j++)
      {
	if(j%10==0)
	  printf("%dth slice\n", j) ;
	
	for(k=0;k<z_size;k++)
	  for(i=0;i<image_size;i++)
	    XY_img[k*image_size+i] = Img[k][i][j] ;
	
	/* Canny */
	canny_core(CannyGuassianSigma, image_size, z_size, XY_img, XY_derivative_mag, XY_magnitude, XY_orient);
	
	for(k=0;k<z_size;k++)
	  for(i=0;i<image_size;i++)
	    {
	      /*if( Img_edge[k][i][j]<XY_magnitude[k*image_size+i] )
		Img_edge[k][i][j] = XY_magnitude[k*image_size+i] ;*/
	      XY_magnitude[k*image_size+i] = 0 ; /* reset */
	    }
      }
    /* 2D: free */
     free(XY_img) ;
     free(XY_orient) ;
     free(XY_derivative_mag) ;
     free(XY_magnitude) ;
}


void ReadModelSurfVer(char SurfVerFileName[180], int *InputSurfVer)
{
  int i ;
  FILE  *fp;


  /***** allocate memory for surface and initialize arrays *****/
   printf("%s: max_nbr=%d\n\n\n", SurfVerFileName, max_nbr) ; /* a file name likes "MDL.ver.norm.nbr" */

   if( (fp=fopen(SurfVerFileName,"r"))==NULL )
     {
       printf("\n\nThis is invalidate surface model. \nProgram will not output any deformation result on the surface model.\n\n");
       (*InputSurfVer) = NNO ;
     }
   else
     {
       fseek(fp,0,SEEK_END);
       SurfVer_Num = ftell(fp)/(6*4+(max_nbr+1)*4); /*48*/
       rewind(fp);
       
       SurfVer   = Fvector3dalloc1d(SurfVer_Num);
       SurfNorms = Fvector3dalloc1d(SurfVer_Num);
       SurfNbr   = Ialloc2d(SurfVer_Num,max_nbr+1); /* 6+1 */
       
       fread(SurfVer,  sizeof(Fvector3d), SurfVer_Num,fp);
       fread(SurfNorms,sizeof(Fvector3d), SurfVer_Num,fp);
       for(i=0;i<SurfVer_Num;i++)
	 fread(SurfNbr[i],sizeof(int),max_nbr+1,fp);
       fclose(fp);
     }
}

void SaveDeformedModelSurfVer(Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder)
{

  int        s, x, y, z, i ;
  Fvector3d  *TempVer ;
  FILE  *fp;
  char  filename[180] ;

  /*space*/
  TempVer = Fvector3dalloc1d(SurfVer_Num);

  for(s=0; s<SurfVer_Num; s++)
      {
	x = (int)(SurfVer[s].x + 0.5) ;
	y = (int)(SurfVer[s].y + 0.5) ;
	z = (int)(SurfVer[s].z + 0.5) ;

	if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
	  {		
	    TempVer[s].x = SurfVer[s].x + DeformFld[z][x][y].x ;
	    TempVer[s].y = SurfVer[s].y + DeformFld[z][x][y].y ;
	    TempVer[s].z = SurfVer[s].z + DeformFld[z][x][y].z ;
	  }
	else
	  {
	    TempVer[s].x = SurfVer[s].x ;
	    TempVer[s].y = SurfVer[s].y ;
	    TempVer[s].z = SurfVer[s].z ;
	  }
      }

  /* save */
  sprintf(filename, "SurfaceMdl.Deformed.ver.norm.nbr.sample%d.%d", XYZres, fileOrder);    
  fp=myopen(filename,"w");
   fwrite(TempVer,sizeof(Fvector3d),SurfVer_Num,fp);
   fwrite(SurfNorms,sizeof(Fvector3d),SurfVer_Num,fp);
   for(i=0;i<SurfVer_Num;i++)
     fwrite(SurfNbr[i],sizeof(int),max_nbr+1,fp);
  fclose(fp);

  /* free */
  free(TempVer) ;
}




main(int argc,char *argv[])
{
  ShenMain(argc,argv);    
}


/* the following is edited by SHEN */
void ShenMain(int argc,char *argv[])
{
  //unsigned char ***Img;
  int           Img_XY, Img_Z ;
  DTIImgAttribute  ***MdlImg ;
  DTIImgAttribute  ***ObjImg ;
  unsigned char ***ObjOriginalImg ;
  Fvector3d     ***DeformFld ;
  float         total;
  int           i,j,k,iter,c,num,resolution,OpenDeformationFieldFromLastScale;
  FILE          *fp;
  extern char   *optarg;
  char          filename[180] ;
  int           scale ;
  char          ObjOriginalImgFile[180], SurfVerFileName[180] ;
  int           InputOriginalObjImg, InputSurfVer ;
  int           s, sx, sy, sz, InitialUpdate ;
  float         CurrentRequirePercent, CannyEdgePercent ;
  int           MdlCannyMinRecord ;

  num=3;
  iter=25;
  XYZres = 1 ;
  search_resolution= 6 ;
  last_order       = 0 ;
  Deform_RATE      = 0.025 ;
  Affine_Degree    = 0.2 ; 
  Statistical_factor = 0.1 ; 
  MatchingDegreeOfMdlOnImgEdge = 0.1 ;
  AddMthdOfAvoidingContraction = NOTADD;
  EstimateTransformOrNot       = NOTADD ;
  StatisticsEmployedOrNot      = NOTADD ; 
  smoothFactor = 0.01 ;
  OpenDeformationFieldFromLastScale=NNO;
  sprintf(MdlDirtory, "./");
  InputOriginalObjImg = NNO ;
  InputSurfVer = NNO ;
  SmoothingTimes=0 ;
  UsingForcesFromObject = YYES ;
  BigVN = NNO ;
  InitialUpdate = NNO ;
  Img_XY = 256 ;
  YoungBrain = NNO ;   /*March 31 2003*/
  DfmSmthCoeff = 0.5;  /* Dec 19, 2003 */
  DfmSmthTimes = 1 ;   /* Dec 19, 2003 */
  ConfidenceOnLastDeformation = 0.0 ; /* Aug 2003 */
  StartPercent=0.26  ; /* 0.30 09-17-2003 */
  CannyGuassianSigma = 1.5 ;
  PercentOfLowCannyEdge = 0.25 ; 
  MaxPercentOfAllowedDrivingVoxels = 0.65 ;

  IsFAFeature = true;
  
  bOrientationFeature=false; /*default: do not consider orientation feature*/
  
  if(argc<num)
    show_usage_SHEN() ;

  while((c=getopt(argc-2,argv+2,"fR:l:r:i:s:a:b:m:tedc:D:FO:S:h:p:E:NBUX:YC:P:G:M:A:Zo:")) != -1)
    {
      switch(c)
	{
    case 'f':
      IsFAFeature = true;
      break;
	case 'R':
	  Deform_RATE=atof(optarg);
	  break;
	case 'l':
	  last_order=atoi(optarg);
	  break;
	case 'r':
	  search_resolution=atoi(optarg);
	  break;
	case 'i':
	  iter=atoi(optarg);
	  break;
	case 's':
	  XYZres=atoi(optarg);
	  break;
	case 'a':
	  Affine_Degree=atof(optarg);
	  break;
	case 'b':
	  Statistical_factor=atof(optarg);
	  break;
	case 'm':
	  MatchingDegreeOfMdlOnImgEdge=atof(optarg) ;
	  break ;
	case 't':
	  AddMthdOfAvoidingContraction=ADD ;
	  break ;
	case 'e':
	  EstimateTransformOrNot=ADD ;
	  break ;
	case 'd':
	  StatisticsEmployedOrNot=ADD ; 
	  break ;
	case 'c':
	  smoothFactor =atof(optarg);
	  break ;
	case 'D':
	  sscanf(optarg, "%s", MdlDirtory) ;
	  break ;
	case 'F':
	  OpenDeformationFieldFromLastScale=YYES ;
	  break ;
	case 'O':
	  sscanf(optarg, "%s", ObjOriginalImgFile) ;
	  InputOriginalObjImg = YYES ;
	  break ;
	case 'S':
	  sscanf(optarg, "%s", SurfVerFileName) ;
	  InputSurfVer = YYES ;
	  break ;
	case 'h':
	  max_nbr=atoi(optarg);
	  break;
	case 'E':
	  SmoothingTimes=atoi(optarg);
	  break;
	case 'N':
	  UsingForcesFromObject=NNO ;
	  break;
	case 'B':
	  BigVN = YYES ;
	  break;
	case 'p':
	  sscanf(optarg, "%f,%d", &DfmSmthCoeff, &DfmSmthTimes) ;   /*Dec 19, 2003*/
	  break;
	case 'U':
	  InitialUpdate = YYES ;
	  break;
	case 'X':
	  Img_XY=atoi(optarg);
	  break;

	case 'Y':
	  YoungBrain = YYES ;   /*March 31 2003*/
	  break;

	case 'C':
	  ConfidenceOnLastDeformation=atof(optarg) ;
	  InitialUpdate = YYES ; /* since deformation may be large, this updating stage be done together */
	  break ;
	case 'P':
	  StartPercent=atof(optarg) ;
	  if( StartPercent<0.10 )  StartPercent=0.10 ;
	  if( StartPercent>0.50 )  StartPercent=0.50 ;
	  break ;
	case 'M':
	  PercentOfLowCannyEdge=atof(optarg) ;
	  break ;

	case 'G':
	  CannyGuassianSigma=atof(optarg) ;
	  break ;

	case 'A':
	  MaxPercentOfAllowedDrivingVoxels=atof(optarg) ;
	  break ;

    case 'o':
      bOrientationFeature = true; // set the orientation feature
      faThresholdForOrientationFeature = atof(optarg);
      break;
      
	default:
	  break;
	}
    }

  printf("\n\n search_resolution=%d\n iter=%d\n Deform_RATE=%f\n Affine_Degree=%f\n Statistical_factor=%f\n MatchingDegreeOfMdlOnImgEdge=%f\n AddMthdOfAvoidingContraction=%d\n", search_resolution, iter, Deform_RATE, Affine_Degree, Statistical_factor, MatchingDegreeOfMdlOnImgEdge, AddMthdOfAvoidingContraction) ;
  printf(" smoothFactor=%f\n", smoothFactor) ;
  printf(" Model directory: %s\n", MdlDirtory) ;
  printf(" Sampling rate: %f\n", XYZres) ;
  printf(" InputOriginalObjImg=%d\n\n", InputOriginalObjImg) ;
  if( InputOriginalObjImg==YYES ) printf(" Warping result on orignal image will be saved!\n") ;
  printf("\n\n SmoothingTimes=%d\n UsingForcesFromObject=%d\n", SmoothingTimes, UsingForcesFromObject) ;
  if(BigVN==YYES) { printf("\n\n *************\n"); printf("  big ventricle"); printf("\n *************\n");}
  printf(" InitialUpdate=%d\n", InitialUpdate) ;
  printf(" Img_XY=%d\n", Img_XY) ;
  if(  YoungBrain==YYES )  printf("\n\n ... YoungBrain!\n\n") ; /*March 31 2003*/
  printf(" DfmSmthCoeff=%f, DfmSmthTimes=%d\n\n", DfmSmthCoeff, DfmSmthTimes) ;
  printf(" ConfidenceOnLastDeformation=%f\n", ConfidenceOnLastDeformation) ; 
  printf(" StartPercent=%f\n", StartPercent) ;
  printf(" CannyGuassianSigma=%f\n", CannyGuassianSigma) ;
  printf(" PercentOfLowCannyEdge=%f\n", PercentOfLowCannyEdge) ;
  printf(" MaxPercentOfAllowedDrivingVoxels=%f\n", MaxPercentOfAllowedDrivingVoxels) ;
  printf(" Orientation feature= %d\n", bOrientationFeature);
  printf(" Is FA feature? = %d\n", IsFAFeature);

     TensorField Img, scalImg;
     
  /***** Model image, segmented *****/
     /*Img_XY = 256 ; obtain Img_Z*/	 
     sprintf(filename, "%s%s", MdlDirtory, argv[1]);    printf("\n\nmodel image : %s\n", filename) ;  
     fp=myopen(filename,"r");
     fseek(fp,0,SEEK_END);
     Img_Z=ftell(fp)/(Img_XY*Img_XY*sizeof(float)*6);
     fclose(fp);
     //======================//
     
     Img.init(Img_XY,Img_XY,Img_Z);
     Img.importRawFieldFromFile(filename);     
     printf("\n\n^^^^ Segmented modle image input!\n") ;
     
     image_size = Img_XY/XYZres ;
     z_size = Img_Z/XYZres ;
     printf("Im_XY=%d\n Img_Z=%d\n", Img_XY, Img_Z ) ;
     printf("image_size=%d\n z_size=%d\n", image_size, z_size) ;

     scalImg.init(image_size, image_size, z_size);
     MdlImg    = DTIImgAttributealloc3d(image_size,image_size,z_size); 
     DeformFld = Fvector3dalloc3d(image_size,image_size,z_size);
     
     /* resampling DTI*/
     float * datImg = Img.getDataPtr();
     float * datScalImg = scalImg.getDataPtr();
          
     for(k=0; k<z_size; k++)
       for(i=0; i<image_size; i++)
	 for(j=0; j<image_size; j++)
	 {
	   int idxs = 6*(k*image_size*image_size+i*image_size+j);
	   int idxo = 6*(k*XYZres*Img_XY*Img_XY+i*XYZres*Img_XY+j*XYZres);
	   
	   datScalImg[idxs] = datImg[idxo];
	   datScalImg[idxs+1] = datImg[idxo+1];
	   datScalImg[idxs+2] = datImg[idxo+2];
	   datScalImg[idxs+3] = datImg[idxo+3];
	   datScalImg[idxs+4] = datImg[idxo+4];
	   datScalImg[idxs+5] = datImg[idxo+5];
	   
	   //MdlImg[k][i][j].Tiss=Img[(int)(k*XYZres)][(int)(i*XYZres)][(int)(j*XYZres)] ;
	   DeformFld[k][i][j].x = 0 ;
	   DeformFld[k][i][j].y = 0 ;
	   DeformFld[k][i][j].z = 0 ;
	 }
     //UCfree3d(Img, Img_Z, Img_XY) ;
     Img.clear();

     /* get its edge map, geometric features */
     /*Get_WMedge_GmVnEdge_FromSegmentedImg(MdlImg) ; no use here - July 2003 */
     scale = (int)(/*7 5*/3*(1./XYZres)) ; if(scale<3) scale=3; printf("scale=%d\n", scale) ;
     MaxAttribute.faTiss = 0; /* This global value will be used to normalize the attribute vector.*/ 
     if (IsFAFeature)
       Compute_GeometricFAFeatures(scalImg, MdlImg, scale, 1., 1., 1.5,  &MdlThreshold, StartPercent);
     else
       Compute_GeometricFeatures(scalImg, MdlImg, scale, 1., 1., 1.5,  &MdlThreshold, StartPercent);

     WriteImgAttribute("MdlImg", MdlImg, image_size, z_size) ;
     scalImg.saveTensorStructField(15, "MdlImg");

     if (bOrientationFeature)
     {
       nMdlRegionScale = scale;
       
       MdlPDColorMap.copy(scalImg.getColorPD1());
       
       scalImg.saveColorPDField(1, "MdlImg");
       scalImg.cleanColorPD();
     }
     
     scalImg.clear();
     scalImg.cleanStruct();
     
     
  /***** Object image, segmented  *****/
     /*Img_XY = 256 ;	 */
     fp=myopen(argv[2],"r");
     fseek(fp,0,SEEK_END);
     Img_Z=ftell(fp)/(Img_XY*Img_XY*sizeof(float)*6);
     fclose(fp);
     //====================//
     
     //Img = UCalloc3d(Img_XY,Img_XY,Img_Z);
     Img.init(Img_XY,Img_XY,Img_Z);
     Img.importRawFieldFromFile(argv[2]);
     printf("\n\n^^^^ Segmented object image input!\n") ;
     
     /*image_size = Img_XY/XYZres ;
       z_size = Img_Z/XYZres ; */
     
     scalImg.init(image_size,image_size,z_size);
     ObjImg = DTIImgAttributealloc3d(image_size,image_size,z_size); 
     
     datImg = Img.getDataPtr();
     datScalImg = scalImg.getDataPtr();
     
     // resampling DTI
     for(k=0; k<z_size; k++)
     {
	 if(k<Img_Z/XYZres) 
	   for(i=0; i<image_size; i++)
	     for(j=0; j<image_size; j++)
         {
	       //ObjImg[k][i][j].faTiss=Img[(int)(k*XYZres)][(int)(i*XYZres)][(int)(j*XYZres)] ;
	       int idxs = 6*(k*image_size*image_size+i*image_size+j);
	       int idxo = 6*(k*XYZres*Img_XY*Img_XY+i*XYZres*Img_XY+j*XYZres);  

	       datScalImg[idxs] = datImg[idxo];
	       datScalImg[idxs+1] = datImg[idxo+1];
	       datScalImg[idxs+2] = datImg[idxo+2];
	       datScalImg[idxs+3] = datImg[idxo+3];
	       datScalImg[idxs+4] = datImg[idxo+4];
	       datScalImg[idxs+5] = datImg[idxo+5];           
         }
	 else
	   for(i=0; i<image_size; i++)
	     for(j=0; j<image_size; j++)
         {
	       //ObjImg[k][i][j].faTiss=0;
           int idxs = 6*(k*image_size*image_size+i*image_size+j);
           datScalImg[idxs] = 0;
	       datScalImg[idxs+1] = 0;
	       datScalImg[idxs+2] = 0;
	       datScalImg[idxs+3] = 0;
	       datScalImg[idxs+4] = 0;
	       datScalImg[idxs+5] = 0; 
         }
     }
     //UCfree3d(Img, Img_Z, Img_XY) ;
     Img.clear();
     
     /* get its edge map, geometric features */
     /*Get_WMedge_GmVnEdge_FromSegmentedImg(ObjImg) ; no use here - July 2003 */
     scale = (int)(/*7 5*/3*(1./XYZres)) ; if(scale<3) scale=3; printf("scale=%d\n", scale) ;
     if (IsFAFeature)
       Compute_GeometricFAFeatures(scalImg, ObjImg, scale, 1., 1., 1.5, &ObjThreshold, StartPercent); 
     else
       Compute_GeometricFeatures(scalImg, ObjImg, scale, 1., 1., 1.5, &ObjThreshold, StartPercent); 
     
     WriteImgAttribute("ObjImg", ObjImg, image_size, z_size) ;
     scalImg.saveTensorStructField(15, "ObjImg");
     
     if (bOrientationFeature)
     {
       nObjRegionScale = scale;
       
       ObjPDColorMap.copy(scalImg.getColorPD1());
       
       scalImg.saveColorPDField(1, "ObjImg");
       scalImg.cleanColorPD();
     } 
     
     scalImg.clear();
     scalImg.cleanStruct();
    
     //////// normalize features ////////
     Normalize_GeometricFeatures(MdlImg, scale, 1., 1., 1.5, &MdlThreshold, StartPercent);
     printf("MdlThreshold.Variance_UP=%d, MdlThreshold.CannyEdge_UP=%d, MdlThreshold.CannyMinimalRequiredEdgeValue=%d\n", MdlThreshold.Variance_UP, MdlThreshold.CannyEdge_UP, MdlThreshold.CannyMinimalRequiredEdgeValue) ;
     Normalize_GeometricFeatures(ObjImg, scale, 1., 1., 1.5, &ObjThreshold, StartPercent);
     printf("ObjThreshold.Variance_UP=%d, ObjThreshold.CannyEdge_UP=%d, ObjThreshold.CannyMinimalRequiredEdgeValue=%d\n", ObjThreshold.Variance_UP, ObjThreshold.CannyEdge_UP, ObjThreshold.CannyMinimalRequiredEdgeValue) ;
     
     /* Select edge points in the model image to be focused */
     Select3DEdgePointsInModelTO1D( &MdlVer_Num, MdlImg, DeformFld, image_size, z_size) ; /* get  MdlVer, FixedMdlVer, MdlVer_Fea*/
     /* Select edge points in the object image to be used as landmarks */
     Select3DEdgePointsInObjectTO1D(&ObjVer_Num, ObjImg, image_size, z_size) ; /* got ObjVer, ObjVer_Fea*/
     
     
     ////////////////////////////////
     
     //==== init force ====//
     forceByFocusedPntInObj = Fvector3dalloc1d(MdlVer_Num); 
     multiple         = Ialloc1d(MdlVer_Num); 
     MyHoodSize       = search_resolution/2*2+12+1;  /*20+1 ;*/
     MyHood           = Ivector3dalloc1d(MyHoodSize*MyHoodSize*MyHoodSize) ;
     calculate_hood_byIncreasingRadius(MyHood, MyHoodSize, &pixelNumIn_MyHood, 1., 1., 1.) ;
     Orig_pixelNumIn_MyHood = pixelNumIn_MyHood ;


  /***** Check here whether to input original Object image for warping it *****/
     if( InputOriginalObjImg==YYES )
       {
	 /*Img_XY = 256 ;  */
	 fp=myopen(ObjOriginalImgFile,"r");
	 fseek(fp,0,SEEK_END);
	 Img_Z=ftell(fp)/(Img_XY*Img_XY);
	 rewind(fp);
     unsigned char ***tImg;
            tImg = UCalloc3d(Img_XY,Img_XY,Img_Z);
	 for(k=0;k<Img_Z;k++)
	   for(i=0;i<Img_XY;i++)
	     fread(tImg[k][i],1,Img_XY,fp);
	 fclose(fp);
	   printf("\n\n^^^^  Original object image input!\n") ;
	   /*image_size = Img_XY/XYZres ; z_size = Img_Z/XYZres ; */
     
	 ObjOriginalImg = UCalloc3d(image_size,image_size,z_size); 
     
	 for(k=0; k<z_size; k++)
	   {
	     if(k<Img_Z/XYZres) 
	       for(i=0; i<image_size; i++)
		 for(j=0; j<image_size; j++)
		   ObjOriginalImg[k][i][j]=tImg[(int)(k*XYZres)][(int)(i*XYZres)][(int)(j*XYZres)] ;
	     else
	       for(i=0; i<image_size; i++)
		 for(j=0; j<image_size; j++)
		   ObjOriginalImg[k][i][j]=0;
	   }
	 UCfree3d(tImg, Img_Z, Img_XY) ;
       }

  /* Inout model's surface model for deforming and carving it into subject's space */
     if( InputSurfVer==YYES && XYZres==1.0 )
       {
	 printf("\n\n^^^^  Surface model input!\n") ;
	 ReadModelSurfVer(SurfVerFileName, &InputSurfVer) ;
       }



  /* estimate the global shift and scaling */
     if( EstimateTransformOrNot==ADD )
       {
	 EstimatingScalingAndShiftOf3DSurfaceFromImg(MdlVer, FixedMdlVer, MdlVer_Fea, MdlVer_Num, DeformFld, ObjImg, image_size, z_size ) ;
	 SaveCurrentWarpingResultOnSegmentedObj( ObjImg, DeformFld, image_size, z_size, (int)XYZres, 999 ) ;

	 if( InputOriginalObjImg==YYES )
	   SaveCurrentWarpingResultOnOriginalObjImg( ObjOriginalImg, DeformFld, image_size, z_size, (int)XYZres, 999 ) ;

	 if( InputSurfVer==YYES && XYZres==1.0 ) /* only on sample rate 1 */
	   SaveDeformedModelSurfVer(DeformFld, image_size, z_size, XYZres, 999) ;
       }

     if( OpenDeformationFieldFromLastScale==YYES )     
       { 
	 OpenDeformationField("DeformationField.float.img", DeformFld, image_size, z_size) ;    
	 SaveCurrentWarpingResultOnSegmentedObj( ObjImg, DeformFld, image_size, z_size, (int)XYZres, 999 ) ;

	 if( InputOriginalObjImg==YYES )
	   SaveCurrentWarpingResultOnOriginalObjImg( ObjOriginalImg, DeformFld, image_size, z_size, (int)XYZres, 999 ) ;

	 if( InputSurfVer==YYES && XYZres==1.0 ) /* only on sample rate 1 */
	   SaveDeformedModelSurfVer(DeformFld, image_size, z_size, XYZres, 999) ;

	 /* update the initial deformations in MdlVer; Added on Oct 23, 2002  */
         /* Theoretically, we need to update MdlVer by initial input. But it is removed, for robustness issues */
	 if( InitialUpdate==YYES )
	   {
	     printf("Update VerMdl by the result from the last resolution.") ;
	     for(s=0; s<MdlVer_Num; s++)
	       {
		 sx = FixedMdlVer[s].x ;
		 sy = FixedMdlVer[s].y ; 
		 sz = FixedMdlVer[s].z ; 
		 
		 MdlVer[s].x = DeformFld[sz][sx][sy].x + sx ; 
		 MdlVer[s].y = DeformFld[sz][sx][sy].y + sy ; 
		 MdlVer[s].z = DeformFld[sz][sx][sy].z + sz ; 
	       }
	   }
	 else
	   printf("No initial update for VerMdl!\n") ;

      }


     /* global transformation matrix */
     CreateMatrix(&Transform,    4, 4);
     
  /* deform */
     CannyEdgePercentInImg(MdlImg, &CannyEdgePercent) ; printf("Percent of Canny edge points in Mdl image %f\n", CannyEdgePercent ) ;
     ratio_iteration=0;
     MdlCannyMinRecord = MdlThreshold.CannyMinimalRequiredEdgeValue ;
     for(i=0;i<iter;i++)
     {
	   if(ratio_iteration>0.5) /*0.5*/ 
	   {
	     printf("focus change!\n") ;
	     /* focuse on more edge points */
	     free(MdlVer) ;
	     free(FixedMdlVer) ;
	     free(MdlVer_Fea) ;
	     free(forceByFocusedPntInObj) ;
	     free(multiple) ;

	      CurrentRequirePercent = StartPercent+(1-StartPercent)*pow(ratio_iteration, 2.0) ;  /* 2.0 */
	       /*if( CurrentRequirePercent>1.2*CannyEdgePercent ) CurrentRequirePercent=1.2*CannyEdgePercent ;*/
	      if( CurrentRequirePercent>MaxPercentOfAllowedDrivingVoxels ) CurrentRequirePercent=MaxPercentOfAllowedDrivingVoxels ; 
	      MdlThreshold.CannyMinimalRequiredEdgeValue = (int)(MdlCannyMinRecord*(1.-pow(ratio_iteration, 16.0))) ;
	     SearchThresholdForObtainingTopXPercentOfDrivingVoxels(MdlImg, &MdlThreshold, CurrentRequirePercent) ;
	     
	     Select3DEdgePointsInModelTO1D( &MdlVer_Num, MdlImg, DeformFld, image_size, z_size) ; /* get  MdlVer, FixedMdlVer, MdlVer_Fea*/

	     forceByFocusedPntInObj = Fvector3dalloc1d(MdlVer_Num); 
	     multiple               = Ialloc1d(MdlVer_Num); 
	   }
	 printf("CurrentRequirePercent=%f\n", CurrentRequirePercent) ;
	 printf("MdlThreshold.Variance_UP=%d, MdlThreshold.CannyEdge_UP=%d\n", MdlThreshold.Variance_UP, MdlThreshold.CannyEdge_UP) ;


	 /* for search match from object to Mdl */
	 pixelNumIn_MyHood = (int)pow( (int)(MyHoodSize*exp(-ratio_iteration*ratio_iteration/2./0.36)), 3.0) ;
 	  /*pixelNumIn_MyHood = Orig_pixelNumIn_MyHood*exp(-ratio_iteration*ratio_iteration/2./0.16);*/
	 if(pixelNumIn_MyHood<125 && Orig_pixelNumIn_MyHood>=125) pixelNumIn_MyHood = 125 ; /* for 5x5x5 neighborhood */

	 
	 ratio_iteration = (float)i/(float)iter ;
	 resolution = (int)(search_resolution*exp(-ratio_iteration*ratio_iteration/2./0.36)) ;  if(resolution<1) resolution=1 ;
	 printf("\n\n\n****************i=%d:   resolution=%d  ratio_iteration=%f************\n", i, resolution, ratio_iteration) ;
	 
	 //if(ratio_iteration<0.5) /* Added on Sept 17 2003, to deal with the cases of small VN */ 
	 //  SmoothDeformationField(DeformFld, image_size, z_size,MdlImg,SmoothingTimes) ;

	 AffinDeform3D(MdlVer, FixedMdlVer, MdlVer_Num, MdlVer_Fea, DeformFld, MdlImg, ObjVer, ObjVer_Num, 
         ObjVer_Fea, ObjImg, image_size, z_size, resolution) ;  


	 /* save temporary results */
	 SaveCurrentWarpingResultOnSegmentedObj( ObjImg, DeformFld, image_size, z_size, (int)XYZres, i ) ;
	 if( InputOriginalObjImg==YYES )
	   SaveCurrentWarpingResultOnOriginalObjImg( ObjOriginalImg, DeformFld, image_size, z_size, (int)XYZres, i ) ;
	 if( InputSurfVer==YYES && XYZres==1.0 ) /* only on sample rate 1 */
	   SaveDeformedModelSurfVer(DeformFld, image_size, z_size, XYZres, i) ;
	 
	 /* June 28, 2001: for movie */
	 /*sprintf(filename, "DeformationField.float.img%d.%d", (int)XYZres, i); 
	   WriteDeformationField(filename, DeformFld, image_size, z_size) ;*/
	 /*if( XYZres>1.0 ) WriteDeformationField(filename, DeformFld, image_size, z_size) ;*/
     }

     /* smooth */
     SmoothDeformationField(DeformFld, image_size, z_size, MdlImg, SmoothingTimes) ;

     if( XYZres>=1.0 )
       WriteDeformationField("DeformationField.float.img", DeformFld, image_size, z_size) ;
     
     printf("Deformation step on scale %d has been finished!\n", (int)XYZres) ;

}


void show_usage_SHEN()
{
  printf("USAGE: AdaptiveBrainMappingUsingGridMesh <model.original.dt.image> <obj.original.dt.image> \n\
\t -R <int>               : deformation rate, i.e. 0.05\n\
\t -l <int>               : last interation order\n\
\t -r <int>               : search resolution\n\
\t -i <int>               : # of initial iterations (18)\n\
\t -s <float>             : sampling rate (default 1 )\n\
\t -a <float>             : degree for expressing local affine transformation (0.2)\n\
\t -b <float>             : statistical factor (0.1)\n\
\t -m <float>             : Matching degree between model and image edge (0.1)\n\
\t -v <int>               : max neighbor size to describe the local connection of the current point (8)\n\
\t -t                     : add the method of stopping surface constraction\n\
\t -e                     : add estimation procedure for estimating scaling and shift of 3D model from image data\n\
\t -d                     : add statistics information to constrain deformation procedure\n\
\t -c <float>             : smooth factor (i.e. 0.01)\n\
\t -D <string>            : director where model is saved\n\
\t -F                     : read deformation field from file in the local directory\n\
\t -O <string>            : file name for orignal object image\n\
\t -S <string>            : model surface file name (dir/MDL.ver.norm.nbr).\n\
\t -h <int>               : number of maximum neighborhood (13)\n\
\t -E <int>               : Edge preserving smoothing times, i.e. 1,2 \n\
\t -N                     : new force from object \n\
\t -B                     : big ventricle case\n\
\t -p <float,int>         : smoothing factor for deformation fields, range [0,1], 1->smoothest, (default: 0.50)), \n\
\t -U                     : update initial VerMdl by the result from the last resolution\n\
\t -X <int>               : size of XY slice image; default:256\n\
\t -Y                     : Young Brain (less CSF)\n\
\t -C <float>             : Confidence on deformation field calculated in the last resolution (0~1.0); default: 0.0\n\
\t -P <float>             : The starting percent of driving voxles in non-zero voxel (0.1~0.5); default: 0.10\n\
\t -M <float>             : percentage of weak Canny edges, to be discard for driving defromation; default: 0.25\n\
\t -G <float>             : Gaussian standard deviation for Canny edge detector; default: 1.5\n\
\t -A <float>             : maximal percentage of voxels allowed to be driving voxels; default: 0.65\n\
\t -o <float>             : use orientation feature, the parameter is the threshold of FA value for considering the orientation, ~ 0.25.\n\
");
  exit(1);
}

unsigned char InterpolatedIntensity(float ii, float jj, float kk, unsigned char ***Img, int image_size, int z_size)
{
  float CurrentV ;
  float b,c,d, b1,c1,d1;
  int   ni,nj,nk, niP1,njP1,nkP1, GreyValue ;


  ni = (int)ii ;
  nj = (int)jj ;
  nk = (int)kk ;
  
  niP1 = ni+1 ;
  njP1 = nj+1 ;
  nkP1 = nk+1 ;
  
  if(ni>=0 && ni<image_size-1  &&  nj>=0 && nj<image_size-1  &&  nk>=0 && nk<z_size-1 )
    {
      b = ii-ni ;        b1 = 1.-b ;
      c = jj-nj ;        c1 = 1.-c ;
      d = kk-nk ;        d1 = 1.-d ;

      CurrentV = ( d1*(Img[nk][ni][nj]*(b1*c1) + Img[nk][niP1][nj]*(b*c1) + Img[nk][ni][njP1]*(b1*c) + Img[nk][niP1][njP1]*(b*c)) + d*(Img[nkP1][ni][nj]*(b1*c1) + Img[nkP1][niP1][nj]*(b*c1) + Img[nkP1][ni][njP1]*(b1*c) + Img[nkP1][niP1][njP1]*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 

      if( CurrentV>255 )
	GreyValue = 255 ;
      else
	GreyValue = (int)CurrentV ;
    }
  
  if(ni==image_size-1 && nj>=0 && nj<image_size-1 && nk>=0 && nk<z_size-1 || ni>=0 && ni<image_size-1 && nj==image_size-1 && nk>=0 && nk<z_size-1  || ni>=0 && ni<image_size-1 && nj>=0 && nj<image_size-1 && nk==z_size-1)
    GreyValue = Img[nk][ni][nj] ;

  return GreyValue ;
}

int INTERPOLATE=YYES ;
void SaveCurrentWarpingResultOnSegmentedObj( DTIImgAttribute ***ObjImg, Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder ) 
{
  int s, i, j, k, x, y, z ;
  FILE  *fp;
  char  filename[180] ;
  unsigned char ***resImg, ***Img ;
  float  ii, jj, kk ;
  unsigned char GreyValue, MappedV ;


  Img    = UCalloc3d(image_size,image_size,z_size); 
  resImg = UCalloc3d(image_size,image_size,z_size); 

  /* get intensity image in ObjImg */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	Img[k][i][j] = ObjImg[k][i][j].faTiss ;


  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  if(INTERPOLATE==YYES)
	    {
	      ii = i + DeformFld[k][i][j].x ;
	      jj = j + DeformFld[k][i][j].y ;
	      kk = k + DeformFld[k][i][j].z ;

	      GreyValue = InterpolatedIntensity(ii, jj, kk, Img, image_size, z_size) ;
	      resImg[k][i][j] = GreyValue ;

	      /*	      if( GreyValue>=BG          && GreyValue<=(BG+CSF)/2 )
		MappedV = BG ;	       
	      if( GreyValue>=(BG+CSF)/2  && GreyValue<=(CSF+VN)/2 ) 
		MappedV = CSF ;	       
	      if( GreyValue>=(CSF+VN)/2  && GreyValue<=(VN+GM)/2 )  
		MappedV = VN ; 
	      if( GreyValue>(VN+GM)/2    && GreyValue<=(GM+WM)/2 ) 
		MappedV = GM ; ; 
	      if( GreyValue>(GM+WM)/2) 
	      MappedV = WM ; 
	       
	      resImg[k][i][j] = MappedV ; */ 
	    }
	  else
	    {
	      x = (int)(i + DeformFld[k][i][j].x + 0.5) ;
	      y = (int)(j + DeformFld[k][i][j].y + 0.5) ;
	      z = (int)(k + DeformFld[k][i][j].z + 0.5) ;
	      
	      if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
		resImg[k][i][j] = Img[z][x][y] ; 
	      else 
		resImg[k][i][j] = 0 ;
	    }
	}

  /* save */
  sprintf(filename, "Res.img.sample%d.%d", XYZres, fileOrder);    
  WriteImg(filename, resImg, image_size, z_size) ;

  /* free */
  UCfree3d(resImg, z_size, image_size) ;
  UCfree3d(Img,    z_size, image_size) ;
}


void SaveCurrentWarpingResultOnOriginalObjImg( unsigned char ***ObjOriginalImg, Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder ) 
{
  int s, i, j, k, x, y, z ;
  FILE  *fp;
  char  filename[180] ;
  unsigned char ***resImg ;
  float  ii, jj, kk ;

  resImg = UCalloc3d(image_size,image_size,z_size); 

  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  if(INTERPOLATE==YYES)
	    {
	      ii = i + DeformFld[k][i][j].x ;
	      jj = j + DeformFld[k][i][j].y ;
	      kk = k + DeformFld[k][i][j].z ;

	      resImg[k][i][j] = InterpolatedIntensity(ii, jj, kk, ObjOriginalImg, image_size, z_size) ;
	    }
	  else
	    {
	      x = (int)(i + DeformFld[k][i][j].x + 0.5) ;
	      y = (int)(j + DeformFld[k][i][j].y + 0.5) ;
	      z = (int)(k + DeformFld[k][i][j].z + 0.5) ;
	      
	      if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
		resImg[k][i][j] = ObjOriginalImg[z][x][y] ;
	      else
		resImg[k][i][j] = 0 ;
	    }
	}

  /* save */
  sprintf(filename, "Warped.OriginalImg.sample%d.%d", XYZres, fileOrder);    
  WriteImg(filename, resImg, image_size, z_size) ;

  /* free */
  UCfree3d(resImg, z_size, image_size) ;
}

float computeSimilarityOfColorPDMaps(int Mdl_x, int Mdl_y, int Mdl_z, int Obj_x, int Obj_y, int Obj_z)
{
  VectorCircularRegionFeature3D MdlRegionFeature, ObjRegionFeature;
  
  MdlRegionFeature.setCircularCenter(Mdl_x, Mdl_y, Mdl_z);
  MdlRegionFeature.setCircularScale(nMdlRegionScale);
  MdlRegionFeature.init(MdlPDColorMap);
  MdlRegionFeature.setHistBins(nPDHistBins);
  
  ObjRegionFeature.setCircularCenter(Obj_x, Obj_y, Obj_z);
  ObjRegionFeature.setCircularScale(nObjRegionScale);
  ObjRegionFeature.init(ObjPDColorMap);
  ObjRegionFeature.setHistBins(nPDHistBins);
  
  float mi = (float)MdlRegionFeature.NMI(ObjRegionFeature);
  
  //printf("mutual information: %f\n", mi);
  
  //exit(0);
  
  return mi;
}

float SimilarityBetweenFocusedPointsInMdlObjImgs( DTIImgAttribute *ObjVer_Fea, int l, DTIImgAttribute *MdlVer_Fea, int s)
{
  float Degree ;
  

  Degree = 0.0f ;
  if( fabs(MdlVer_Fea[s].faEdge-ObjVer_Fea[l].faEdge)<DIF_THR )
    {
      Degree = (255.0 - fabs(MdlVer_Fea[s].prolateMean-ObjVer_Fea[l].prolateMean))/255;
      if (!IsFAFeature)
      {
        Degree *= (255.0 - fabs(MdlVer_Fea[s].oblateMean-ObjVer_Fea[l].oblateMean))/255;
        Degree *= (255.0 - fabs(MdlVer_Fea[s].sphereMean-ObjVer_Fea[l].sphereMean))/255;
      }
      
      Degree *= (255.0 - fabs(MdlVer_Fea[s].prolateVar-ObjVer_Fea[l].prolateVar))/255;
      if (!IsFAFeature)
      {
        Degree *= (255.0 - fabs(MdlVer_Fea[s].oblateVar-ObjVer_Fea[l].oblateVar))/255;
        Degree *= (255.0 - fabs(MdlVer_Fea[s].sphereVar-ObjVer_Fea[l].sphereVar))/255;
      }
      
      Degree *= (255.0-fabs(MdlVer_Fea[s].faEdge-ObjVer_Fea[l].faEdge))/255 ;
      Degree *= (255.0-fabs(MdlVer_Fea[s].traceEdge-ObjVer_Fea[l].traceEdge))/255 ;
      if (IsFAFeature)
        Degree *= (255.0-fabs(MdlVer_Fea[s].faTiss -ObjVer_Fea[l].faTiss))/255 ; /* the positions of the corresponding edge points across the sbjects may vary for a voxel. To avoid this, we just ignore the intensity similarity on edge position*/
        
      
      
      if ( (Degree > SMR_THR) && bOrientationFeature && MdlVer_Fea[s].faTiss > (int)(faThresholdForOrientationFeature*255) 
          && ObjVer_Fea[l].faTiss > (int)((faThresholdForOrientationFeature-0.08)*255) )
      {
          //printf("case 1: before counting orientation feature - %f, fa=%d\n", Degree, MdlVer_Fea[s].faTiss);
          
          Degree *=computeSimilarityOfColorPDMaps(FixedMdlVer[s].x, FixedMdlVer[s].y, FixedMdlVer[s].z, ObjVer[l].x, ObjVer[l].y, ObjVer[l].z);
           
          //printf("case 1: after counting orientation feature - %f\n", Degree);
      }
     
    }

  return Degree ;
}

float SimilarityBetweenFeaturesInImgAndFeaturesInMdl( DTIImgAttribute ***ObjImg, int x, int y, int z, DTIImgAttribute *MdlVer_Fea, int s)
{
  float Degree ;

  Degree = 0.0f ;
  if( fabs(MdlVer_Fea[s].faEdge-ObjImg[z][x][y].faEdge)<DIF_THR )
    {
      Degree = (255.0 - fabs(MdlVer_Fea[s].prolateMean-ObjImg[z][x][y].prolateMean))/255;
      if (!IsFAFeature)
      {
        Degree *= (255.0 - fabs(MdlVer_Fea[s].oblateMean-ObjImg[z][x][y].oblateMean))/255;
        Degree *= (255.0 - fabs(MdlVer_Fea[s].sphereMean-ObjImg[z][x][y].sphereMean))/255;
      }
      
      Degree *= (255.0 - fabs(MdlVer_Fea[s].prolateVar-ObjImg[z][x][y].prolateVar))/255;
      if (!IsFAFeature)
      {
        Degree *= (255.0 - fabs(MdlVer_Fea[s].oblateVar-ObjImg[z][x][y].oblateVar))/255;
        Degree *= (255.0 - fabs(MdlVer_Fea[s].sphereVar-ObjImg[z][x][y].sphereVar))/255;
      }
      
      Degree *= (255.0-fabs(MdlVer_Fea[s].faEdge-ObjImg[z][x][y].faEdge))/255 ;
      Degree *= (255.0-fabs(MdlVer_Fea[s].traceEdge-ObjImg[z][x][y].traceEdge))/255 ;
      if (IsFAFeature)
        Degree *= (255.0-fabs(MdlVer_Fea[s].faTiss -ObjImg[z][x][y].faTiss))/255 ; /* the positions of the corresponding edge points across the sbjects may vary for a voxel. To avoid this, we just ignore the intensity similarity on edge position*/
        
      
      
      if ( (Degree > SMR_THR) && bOrientationFeature && MdlVer_Fea[s].faTiss > (int)(faThresholdForOrientationFeature*255) 
          && ObjImg[z][x][y].faTiss > (int)((faThresholdForOrientationFeature-0.08)*255) )
      {
          //printf("case 2: before counting orientation feature - %f\n", Degree);  
          
          Degree *=computeSimilarityOfColorPDMaps(FixedMdlVer[s].x, FixedMdlVer[s].y, FixedMdlVer[s].z, x, y, z);
      
          //printf("case 2: after counting orientation feature - %f\n", Degree);      
      }       
      
    }

  return Degree ;
}

float SimilarityBetweenFeaturesInMdlAndObjImgs( DTIImgAttribute ***ObjImg, int x, int y, int z, DTIImgAttribute ***MdlImg, int i, int j, int k )
{
  float Degree ;

      Degree = (255.0 - fabs(MdlImg[k][i][j].prolateMean-ObjImg[z][x][y].prolateMean))/255;
      if (!IsFAFeature)
      {
        Degree *= (255.0 - fabs(MdlImg[k][i][j].oblateMean-ObjImg[z][x][y].oblateMean))/255;
        Degree *= (255.0 - fabs(MdlImg[k][i][j].sphereMean-ObjImg[z][x][y].sphereMean))/255;
      }
      
      Degree *= (255.0 - fabs(MdlImg[k][i][j].prolateVar-ObjImg[z][x][y].prolateVar))/255;
      if (!IsFAFeature)
      {
        Degree *= (255.0 - fabs(MdlImg[k][i][j].oblateVar-ObjImg[z][x][y].oblateVar))/255;
        Degree *= (255.0 - fabs(MdlImg[k][i][j].sphereVar-ObjImg[z][x][y].sphereVar))/255;
      }
      
      Degree *= (255.0-fabs(MdlImg[k][i][j].faEdge-ObjImg[z][x][y].faEdge))/255 ;
      Degree *= (255.0-fabs(MdlImg[k][i][j].traceEdge-ObjImg[z][x][y].traceEdge))/255;
      if (IsFAFeature)
        Degree *= (255.0-fabs(MdlImg[k][i][j].faTiss -ObjImg[z][x][y].faTiss))/255 ;

      
      
      if ( (Degree > 0.7) && bOrientationFeature && MdlImg[k][i][j].faTiss > (int)(faThresholdForOrientationFeature*255) 
          && ObjImg[z][x][y].faTiss > (int)((faThresholdForOrientationFeature-0.08)*255) )
      {
          //printf("case 3: before counting orientation feature - %f\n", Degree);
          Degree *=computeSimilarityOfColorPDMaps(i, j, k, x, y, z);
          //printf("case 3: after counting orientation feature - %f\n", Degree); 
      }       
      
      
  return Degree ;
}


/* smoothing ... */

void SmoothDeformationField(Fvector3d ***DeformFld, int image_size, int z_size,  DTIImgAttribute ***MdlImg, int SmoothingTimes)
{
  int i,j,k,n,t ;
  int x,y,z ;
  Ivector3d  *nb_pnt ;
  int        nb_size, count, half, temp_num ;
  Fvector3d  nbr_tot, direct ;
  float      LocalRatio, mag ;

  printf("\n\nsmoothing ...\n") ;
  nb_size = 3 ;
  nb_pnt= Ivector3dalloc1d(nb_size*nb_size*nb_size) ;
  calculate_hood_byIncreasingRadius(nb_pnt, nb_size, &count, 1., 1., 1.) ; /* 0.5, 0.5, 0.5, Feb 28, 2002 */

  half=nb_size/2;  
  LocalRatio = DfmSmthCoeff ; /* LocalRatio = 0.5 ; before Dec 19, 2003 */
  printf("smoothing factor = %f\n", LocalRatio) ;
  for(t=0; t<DfmSmthTimes; t++ ) /* DfmSmthTimes=1; before Dec 19, 2003*/
    {
      printf("The %d-th time of smoothing deformation field, without edge-preserving!\n", t) ;

      for(k=0;k<z_size;k++)
	for(i=0;i<image_size;i++)
	  for(j=0;j<image_size;j++)
	    {
	      nbr_tot.x=0; nbr_tot.y=0; nbr_tot.z=0; temp_num=0;
	      for(n=1;n<27;n++) /* 26 neighbor */
		{
		  x = i + nb_pnt[n].x ;
		  y = j + nb_pnt[n].y ;
		  z = k + nb_pnt[n].z ;
		  
		  if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
		    {
		      nbr_tot.x += DeformFld[z][x][y].x;
		      nbr_tot.y += DeformFld[z][x][y].y;
		      nbr_tot.z += DeformFld[z][x][y].z;
		      
		      temp_num++ ;
		    }
		}
	      if(temp_num>0)
		{
		  nbr_tot.x /= temp_num ;
		  nbr_tot.y /= temp_num ;
		  nbr_tot.z /= temp_num ;
		}
	      
	  /* Jun 21, 2004 */
	  if( SmoothingTimes<=1 || (MdlImg[k][i][j].faEdge<=MdlThreshold.CannyEdge_UP && 
          (IsFAFeature || (TraceEdgeConfidence*MdlImg[k][i][j].traceEdge<=MdlThreshold.CannyEdge_UP)) && 
          MdlImg[k][i][j].prolateVar<=MdlThreshold.Variance_UP) ) 
	    {
	      DeformFld[k][i][j].x += (nbr_tot.x - DeformFld[k][i][j].x)*LocalRatio ;
	      DeformFld[k][i][j].y += (nbr_tot.y - DeformFld[k][i][j].y)*LocalRatio ; 
	      DeformFld[k][i][j].z += (nbr_tot.z - DeformFld[k][i][j].z)*LocalRatio ; 	  
	    }
	  else
	   {
	     direct.x = (nbr_tot.x - DeformFld[k][i][j].x)*LocalRatio ;
	     direct.y = (nbr_tot.y - DeformFld[k][i][j].y)*LocalRatio ;
	     direct.z = (nbr_tot.z - DeformFld[k][i][j].z)*LocalRatio ;
	     mag = Fvector3dmag(direct) ;
	     if( mag>3.0 ) /* 2.0 before June 22 2004 */
		{
		  direct.x /= mag/3.0 ; 
		  direct.y /= mag/3.0 ;
		  direct.z /= mag/3.0 ;
		}

	     DeformFld[k][i][j].x += direct.x ;
	     DeformFld[k][i][j].y += direct.y ;
	     DeformFld[k][i][j].z += direct.z ;
	    }
	      
	      /* boundary constraints */
	      if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
	      if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
	      if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;
	      
	      if(DeformFld[k][i][j].x+i>=(image_size-1)) DeformFld[k][i][j].x=(image_size-1)-i ;
	      if(DeformFld[k][i][j].y+j>=(image_size-1)) DeformFld[k][i][j].y=(image_size-1)-j ;
	      if(DeformFld[k][i][j].z+k>=(z_size-1))     DeformFld[k][i][j].z=(z_size-1)-k ;
	    }
    }
 
  printf("End\n") ;

  /* free */
  free(nb_pnt) ;
}


void SmoothDeformationFieldWithEdgePreserving( Ivector3d *FixedMdlVer, int MdlVer_Num, Fvector3d ***DeformFld, DTIImgAttribute ***MdlImg, int image_size, int z_size, float ratio_iteration, int SmoothingTimes )
{
  int i,j,k,n, t,s ;
  int x,y,z, sx,sy,sz ;
  Ivector3d  *nb_pnt ;
  int        nb_size, count, half, temp_num ;
  Fvector3d  nbr_tot ;
  float      LocalRatio ;
  unsigned char ***FixedImg;


  printf("\nEdge preserving smoothing ...\n") ;
  nb_size = 3 ;
  nb_pnt= Ivector3dalloc1d(nb_size*nb_size*nb_size) ;
  calculate_hood_byIncreasingRadius(nb_pnt, nb_size, &count, 1., 1., 1.) ; /* 0.5, 0.5, 0.5, Feb 28, 2002 */


  /* insert the MdelVer poistions into an image, for the computation convenience. */
  FixedImg = UCalloc3d(image_size,image_size,z_size);
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	FixedImg[k][i][j]=0 ;
  for(s=0; s<MdlVer_Num; s++)
    {
      sx = FixedMdlVer[s].x ;
      sy = FixedMdlVer[s].y ;
      sz = FixedMdlVer[s].z ;
      
      FixedImg[sz][sx][sy]=255 ;
    }

  /* smoothing */
  LocalRatio = 0.75-0.25*ratio_iteration*ratio_iteration ;   /* 0.75, 0.25 */
  printf("Hierarchical ratio in edge-preserving smoothing : %f\n", LocalRatio) ;
  for( t=0; t<SmoothingTimes; t++)
    {
      printf("The %dth time ...\n", t) ;
      for(k=0;k<z_size;k++)
	for(i=0;i<image_size;i++)
	  for(j=0;j<image_size;j++)
	    if( /*FixedImg[k][i][j]<255 &&*/ MdlImg[k][i][j].faTiss>0 ) /* for saving time, smooth on brain region only, no background. Feb 26, 2002 */ 
	      {
		nbr_tot.x=0; nbr_tot.y=0; nbr_tot.z=0; temp_num=0;
		for(n=1;n<27;n++) /* 26 neighbor */
		  {
		    x = i + nb_pnt[n].x ;
		    y = j + nb_pnt[n].y ;
		    z = k + nb_pnt[n].z ;
		    
		    if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
		      {
			nbr_tot.x += DeformFld[z][x][y].x;
			nbr_tot.y += DeformFld[z][x][y].y;
			nbr_tot.z += DeformFld[z][x][y].z;
			
			temp_num++ ;
			 
		      }
		  }
		if(temp_num>0)
		  {
		    nbr_tot.x /= temp_num ;
		    nbr_tot.y /= temp_num ;
		    nbr_tot.z /= temp_num ;
		  }

		if( /*FixedImg[k][i][j]<255  -Jun 17 2004*/  SmoothingTimes<=1 || (MdlImg[k][i][j].faEdge<=MdlThreshold.CannyEdge_UP &&
            (IsFAFeature || (TraceEdgeConfidence*MdlImg[k][i][j].traceEdge<=MdlThreshold.CannyEdge_UP)) 
            && MdlImg[k][i][j].prolateVar<=MdlThreshold.Variance_UP)  ) 
		  {		
		    DeformFld[k][i][j].x += (nbr_tot.x - DeformFld[k][i][j].x)*LocalRatio ;
		    DeformFld[k][i][j].y += (nbr_tot.y - DeformFld[k][i][j].y)*LocalRatio ; 
		    DeformFld[k][i][j].z += (nbr_tot.z - DeformFld[k][i][j].z)*LocalRatio ; 
		  }
		else
		  {
		    DeformFld[k][i][j].x += (nbr_tot.x - DeformFld[k][i][j].x)*LocalRatio/100 ; /*1000*/ /*/2.0 ; */ 
		    DeformFld[k][i][j].y += (nbr_tot.y - DeformFld[k][i][j].y)*LocalRatio/100 ;          /*/2.0 ; */ 
		    DeformFld[k][i][j].z += (nbr_tot.z - DeformFld[k][i][j].z)*LocalRatio/100 ;          /*/2.0 ; */  
		  }
		
		/* boundary constraints */
		if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
		if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
		if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;
		
		if(DeformFld[k][i][j].x+i>=(image_size-1)) DeformFld[k][i][j].x=(image_size-1)-i ;
		if(DeformFld[k][i][j].y+j>=(image_size-1)) DeformFld[k][i][j].y=(image_size-1)-j ;
		if(DeformFld[k][i][j].z+k>=(z_size-1))     DeformFld[k][i][j].z=(z_size-1)-k ;
	      }
    }
  printf("End\n") ;

  /* free */
  free(nb_pnt) ;
  UCfree3d(FixedImg, z_size, image_size) ;
}



/*float   SMR_THR=0.8;*/  /* 0.8 */

#define BUBBNUM   10    /* 8 10*/
#define STEPPNT   3     /* 3 (May2001), 20 */
int STEPNBR=12 ;        /* 6 (May2001), 8 10*/
#define ITER_THRD 0.85   /* iteration threshold for switching focus */
void AffinDeform3D(Fvector3d *MdlVer, Ivector3d *FixedMdlVer, int MdlVer_Num, DTIImgAttribute *MdlVer_Fea, Fvector3d ***DeformFld, DTIImgAttribute ***MdlImg, Ivector3d *ObjVer, int ObjVer_Num, DTIImgAttribute *ObjVer_Fea, DTIImgAttribute ***ObjImg, int image_size, int z_size, int resolution) 
{
  int   i,j,s,k,g,l,m,n,old_count,count,num,how_many,max,size,t, SHIFT, incre, LocalM, scale, beforeN, SelectVerNum, vv, RealVV ;
  float weight, CurrentDegree, OverlapDegree, MaxDegree, AdaptiveFactor, DistSeg, MinDist, DrivingVoxelSimilarity ;
  Fvector3d Search, Crnt, Tmp, dfm, parallel, globl;
  float mag, factor, Delta, level, magDfm ;
  int   x,y,z, sx,sy,sz ;

  /* Bubble for checking similarity */
  Ivector3d  *nb_pnt, Grid, StudiedPoint;
  int      nb_size ;
  /* Bubble for transforming subvolume */
  Ivector3d  *SubV_pnt;
  int        SubVNum, SubVsize, UsedVNum ;
  float      UsedGuassianSigma ;


  /* for estimating the overlapping degree */
  int bubble_size[BUBBNUM];
  int real_size ;

  /* for local geometric features of 3D shape */
  float     CenterRate, LocalRatio ;
  Fvector3d *verModelLast, nbr_tot ;
  int       numTEMP, Last_searched_point, temp_num ;


  /* to speed up, the neighboring driving voxels are not necessary to deform for multiple times. */
  unsigned char ***SearchingStatus ;
  int           FilterSize ;


  /* June 5, 2001 */
  Fvector3d  ***DeformFld_Last ;
  DeformFld_Last=Fvector3dalloc3d(image_size,image_size,z_size);


  /* global constraints  */
  verModelLast      = Fvector3dalloc1d(MdlVer_Num); /* used as model */
  /* remember */
  for(numTEMP=0; numTEMP<MdlVer_Num; numTEMP++)
    {
      verModelLast[numTEMP].x=MdlVer[numTEMP].x; verModelLast[numTEMP].y=MdlVer[numTEMP].y; verModelLast[numTEMP].z=MdlVer[numTEMP].z;
    }
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	{
	  DeformFld_Last[k][i][j].x=DeformFld[k][i][j].x ;
	  DeformFld_Last[k][i][j].y=DeformFld[k][i][j].y ;
	  DeformFld_Last[k][i][j].z=DeformFld[k][i][j].z ;
	}


  /* shift assigned */
  SHIFT = resolution/2 ;  if(SHIFT<1) SHIFT=1;  printf("Shift=%d\n", SHIFT) ;


  /* get the forces from the landmarks */
  SMR_THR = 0.6*(1-ratio_iteration)+0.2 ; /* (.5,.3)(0.8 0.001) */ printf("SMR_THR=%f\n", SMR_THR) ;
  //SMR_THR = 0.8*(1-ratio_iteration)+0.001 ; /* (.5,.3)(0.8 0.001) */ printf("SMR_THR=%f\n", SMR_THR) ;
  DIF_THR = (1.-SMR_THR)*255.0 ;
  printf("before ObjVer_Num=%d\n", ObjVer_Num) ;
  if(ratio_iteration<=ITER_THRD && UsingForcesFromObject==YYES) 
    SearchTheNearestVertexForEachLandmark_March2003(ObjVer, ObjVer_Num, ObjVer_Fea, ObjImg,    MdlVer, MdlVer_Num, MdlVer_Fea, FixedMdlVer, MdlImg, DeformFld,  image_size, z_size,  SHIFT+1) ;


  incre = 2*SHIFT/6 /*8, 20, May 2001*/ ; if(incre<1) incre=1 ;
  max = SHIFT+1 ;
  nb_size = max*2+1; /*nb_size should be odd number */ 
  nb_pnt= Ivector3dalloc1d(nb_size*nb_size*nb_size) ;
  calculate_hood_byIncreasingRadius(nb_pnt, nb_size, &count, 1., 1., 1.) ; 
  printf("nbr num:%d with rad %d\n", nb_size, count) ;
    /* for transformation in Subvolume */
    SubVsize = 41 ; /* nb_size+2*0 */
    SubV_pnt = Ivector3dalloc1d(SubVsize*SubVsize*SubVsize) ;
    calculate_hood_byIncreasingRadius(SubV_pnt, SubVsize, &SubVNum, 1., 1., 1.) ; 
    printf("Subvolume num:%d with rad %d\n", SubVsize, SubVNum) ;
  t = 0;   /* 3 8 */ /* 0 ~ 9*/
  t = resolution/10 ;if(t>2) t=2 ;
  SelectVerNum = MdlVer_Num ;
  printf("incre=%d  max=%d   searched Num=%d    t=%d\n", incre, max, SelectVerNum, t ) ;



  /* Feb 27, 2002: To speed up, the neighboring driving voxels are not necessary to deform for multiple times. */
  SearchingStatus = UCalloc3d(image_size,image_size,z_size);
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	SearchingStatus[k][i][j]=OOK ;
  RealVV = 0 ;
  for(vv=0; vv<MdlVer_Num; vv+=STEPPNT)  /*for(s=0; s<MdlVer_Num; s+=STEPPNT)*/
  {
    s = (vv+Start_Search_Point)%MdlVer_Num ; 

    if( (RealVV<SelectVerNum || Fvector3dmag(forceByFocusedPntInObj[s])!=0) && SearchingStatus[FixedMdlVer[s].z][FixedMdlVer[s].x][FixedMdlVer[s].y]==OOK )
    {
      RealVV ++ ;
      Last_searched_point = s ;
      if( s%100==0 ) { printf("\ns=%d  Last_searched_point= %d", s, Last_searched_point) ;
                       printf("percent finished = %f\n", (float)RealVV/SelectVerNum) ; 
		     }

      /* May 24, 2001 */
      STEPNBR = count/20 ;  if(STEPNBR<1) STEPNBR=1 ;

      if( Fvector3dmag(forceByFocusedPntInObj[s])==0 || ratio_iteration>ITER_THRD)
	{
	  /* begin to search by greedy algorithm: external part */
	  dfm.x = 0 ; dfm.y = 0 ; dfm.z = 0 ;
	  MaxDegree = 0 ;
	  MinDist = 100000.0 ;
	  /*printf("VerNumInTheSameRegionOfCurrentVer[%d]=%d\n", s,VerNumInTheSameRegionOfCurrentVer[s]) ;*/
	  sx = FixedMdlVer[s].x ;
	  sy = FixedMdlVer[s].y ;
	  sz = FixedMdlVer[s].z ;
	  if( sx>=image_size || sy>=image_size || sz>=z_size || sx<0 || sy<0 || sz<0 ) { printf("out\n"); exit(0);}
	  for(Search.x=-SHIFT; Search.x<=SHIFT; Search.x+=incre)
	    for(Search.y=-SHIFT; Search.y<=SHIFT; Search.y+=incre)
	      for(Search.z=-SHIFT; Search.z<=SHIFT; Search.z+=incre)
		{
/*		  x = (int)(MdlVer[s].x+Search.x) ;
		  y = (int)(MdlVer[s].y+Search.y) ;
		  z = (int)(MdlVer[s].z+Search.z) ;
*/
		  /* MdlVer[s]'s current position */
		  x = (int)(DeformFld[sz][sx][sy].x + sx + Search.x + 0.5) ;
		  y = (int)(DeformFld[sz][sx][sy].y + sy + Search.y + 0.5) ;
		  z = (int)(DeformFld[sz][sx][sy].z + sz + Search.z + 0.5) ;

		  /*printf("1:(%d) xyz=%d,%d,%d   %f,%f,%f\n", SHIFT, x,y,z, Search.x, Search.y, Search.z) ;*/

		  if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 && (MdlVer_Fea[s].faEdge>0 && ObjImg[z][x][y].faEdge>0 || MdlVer_Fea[s].faEdge==0 && ObjImg[z][x][y].faEdge==0) && (MdlVer_Fea[s].traceEdge>0 && ObjImg[z][x][y].traceEdge>0 || MdlVer_Fea[s].traceEdge==0 && ObjImg[z][x][y].traceEdge==0) /*08-2003*/ && (DrivingVoxelSimilarity=SimilarityBetweenFeaturesInImgAndFeaturesInMdl(ObjImg,x,y,z,MdlVer_Fea,s)>SMR_THR || (BigVN==YYES && MdlVer_Fea[s].prolateVar>0 && ObjImg[z][x][y].prolateVar>0)) ) 
		    {
		      /*printf("2: xyz=%d,%d,%d\n", x,y,z) ;*/

		      OverlapDegree = 0 ;
		      DistSeg = 0 ;
		      real_size = 0 ;
		      for(n=0; n<count; n+=STEPNBR)
			{
				/* Coordinate In Template */
			  Grid.x = sx+nb_pnt[n].x ;
			  Grid.y = sy+nb_pnt[n].y ;
			  Grid.z = sz+nb_pnt[n].z ;
			  
			  /*printf("3: Gxyz=%d,%d,%d\n", Grid.x,Grid.y,Grid.z) ;*/

			  if( Grid.x<image_size && Grid.y<image_size && Grid.z<z_size && Grid.x>=0 && Grid.y>=0 && Grid.z>=0 ) /* true */
			    {
			      /*printf("4: Gxyz=%d,%d,%d\n", Grid.x,Grid.y,Grid.z) ;*/

			      /*real_size ++ ;	Nov 15, 2001 */
			      Crnt.x = DeformFld[Grid.z][Grid.x][Grid.y].x + Grid.x ;
			      Crnt.y = DeformFld[Grid.z][Grid.x][Grid.y].y + Grid.y ;
			      Crnt.z = DeformFld[Grid.z][Grid.x][Grid.y].z + Grid.z ;

			      /* shift for every vertex in this set of neighborhood */
			      level = (fabs(nb_pnt[n].x)+fabs(nb_pnt[n].y)+fabs(nb_pnt[n].z))/3.0 ; 
			      /*level = sqrt( nb_pnt[n].x*nb_pnt[n].x + nb_pnt[n].y*nb_pnt[n].y + nb_pnt[n].z*nb_pnt[n].z ) ; */ /* tried before June 18, 2001 */
			      ShiftByGuassianWeight(Crnt, &Tmp, Search, level, nb_size) ;

			      x = (int)(Tmp.x + 0.5) ;
			      y = (int)(Tmp.y + 0.5) ;
			      z = (int)(Tmp.z + 0.5) ;

			      /*printf("5: xyz=%d,%d,%d\n", x,y,z) ;*/

			      if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
				{
				  CurrentDegree = SimilarityBetweenFeaturesInMdlAndObjImgs( ObjImg, x, y, z, MdlImg, Grid.x, Grid.y, Grid.z ) ;

				  /* March 27, 2003: This is added for considering   */
				  /* (1) binary tissue matching, (2) BG effect.      */    /*good for yng brain*/
				  if( YoungBrain==YYES  && MdlImg[Grid.z][Grid.x][Grid.y].prolateVar==0 && CurrentDegree<0.6 ) 
				    CurrentDegree=0;
				  /*if( MdlImg[Grid.z][Grid.x][Grid.y].Tiss==BG && CurrentDegree>0.6 )
				    {
				    CurrentDegree=0; 
				    real_size -- ; 
				    }*/
				     /* will not count current one. Notable size will be increased one nest*/

				  real_size ++ ;
                    
				  if( MdlImg[Grid.z][Grid.x][Grid.y].faEdge>MdlThreshold.CannyEdge_UP 
                      || (!IsFAFeature&&(TraceEdgeConfidence*MdlImg[Grid.z][Grid.x][Grid.y].traceEdge>MdlThreshold.CannyEdge_UP))) /* June 6, 2001*/
				    {
				      CurrentDegree *= 1.2 ;
				      real_size += (1.2-1.0) ;
				    }
				}
			      else
				CurrentDegree = 0 ;

			      /*if( MdlImg[Grid.z][Grid.x][Grid.y].Edge>0 ) 
				{
				CurrentDegree *= 1.2 ;
				real_size += (1.2-1.0) ;
				} Nov 15, 2001 */
			      
			      DistSeg += (1-CurrentDegree)/(count/STEPNBR);
			      if( DistSeg>MinDist ) break ; /* no need to continue on this selected deformation */
			      OverlapDegree += CurrentDegree ; 
			    }
			}
		      if(real_size>0) 
			OverlapDegree = OverlapDegree/real_size ; 
		      
		      if( OverlapDegree>MaxDegree )
			{
			  MinDist = DistSeg ;
			  MaxDegree = OverlapDegree ;
			  dfm.x = Search.x ;
			  dfm.y = Search.y ;	    
			  dfm.z = Search.z ;
			}
		    }
		  /*printf("1: end\n") ;*/
		}
	}
      else
	{
	  MaxDegree = 10000.0 ;

	  x = FixedMdlVer[s].x ;
	  y = FixedMdlVer[s].y ;
	  z = FixedMdlVer[s].z ;

	  dfm.x = (DeformFld_Last[z][x][y].x-DeformFld[z][x][y].x+forceByFocusedPntInObj[s].x)*(1.0+(1+ratio_iteration))/2. ;  /*5.*/
	  dfm.y = (DeformFld_Last[z][x][y].y-DeformFld[z][x][y].y+forceByFocusedPntInObj[s].y)*(1.0+(1+ratio_iteration))/2. ;	    
	  dfm.z = (DeformFld_Last[z][x][y].z-DeformFld[z][x][y].z+forceByFocusedPntInObj[s].z)*(1.0+(1+ratio_iteration))/2. ; /* May 2001 */
	  /*printf("dfm=(%f,%f,%f)\n", dfm.x, dfm.y, dfm.z) ;*/

	  /* scale it */
	  magDfm = Fvector3dmag(dfm) ;
	  if( magDfm>nb_size/2 ) 
	    {
	      dfm.x = dfm.x/magDfm*(nb_size/2) ;
	      dfm.y = dfm.y/magDfm*(nb_size/2) ;
	      dfm.z = dfm.z/magDfm*(nb_size/2) ;
	    }
	}
      /*printf("\ns=%d  MaxDegree=%f  (%f,%f,%f)\n", s, MaxDegree, dfm.x, dfm.y, dfm.z) ;*/


      if( s%100==0 )      printf("s=%d  MaxDegree=%f  DrivingVoxelSimilarity=%f\n", s, MaxDegree, DrivingVoxelSimilarity) ;
      if( MaxDegree>MatchingDegreeOfMdlOnImgEdge ) /*&& (dfm.x!=0 || dfm.y!=0 || dfm.z!=0) )*/   /* deform it*/
	/* MatchingDegreeOfMdlOnImgEdge=0.1 */
	{
	  if( s%100==0 )
	    {
	      printf("%d->   OL(%d)=%f: (%f,%f,%f)\n", SelectVerNum, s, MaxDegree, dfm.x, dfm.y, dfm.z) ;
	    }
	  
	  /* get the sizes of real subvolume and Gaussian Sigma */
	  /*UsedGuassianSigma = sqrt(dfm.x*dfm.x + dfm.y*dfm.y + dfm.z*dfm.z)*0.8 ;
	  if( UsedGuassianSigma<1.0 ) UsedGuassianSigma=1.0 ;  
	  UsedVNum = pow( 2*((int)(UsedGuassianSigma*1.0))+1, 3.0) ; 
	  if( UsedVNum>SubVNum )  UsedVNum = SubVNum ; */
	  UsedVNum=count ;
	  UsedGuassianSigma = nb_size ;
	   if( s%100==0 ) printf("            Current Sigma =%f  volNum=%d\n", UsedGuassianSigma, UsedVNum) ;

	  /* change the status around this model driving voxel */	
	  /*FilterSize = UsedGuassianSigma/2/3*2+1 ;  if( FilterSize>3 ) FilterSize=3 ;
	  FilterSize = FilterSize*FilterSize*FilterSize ; 
	  for(n=0; n<FilterSize; n++) 
	    {  
	      x = FixedMdlVer[s].x+nb_pnt[n].x ;  
	      y = FixedMdlVer[s].y+nb_pnt[n].y ;  
	      z = FixedMdlVer[s].z+nb_pnt[n].z ; 
	      if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 ) 
		SearchingStatus[z][x][y]=FFAIL ; 
	    }*/ 


	  /* Begin to deform the subvolume at Gaussian way */
	  for(n=0; n<UsedVNum; n++)
	    {
	      StudiedPoint.x = FixedMdlVer[s].x+SubV_pnt[n].x ;
	      StudiedPoint.y = FixedMdlVer[s].y+SubV_pnt[n].y ;
	      StudiedPoint.z = FixedMdlVer[s].z+SubV_pnt[n].z ;
	      
	      if( StudiedPoint.x<image_size && StudiedPoint.y<image_size && StudiedPoint.z<z_size && StudiedPoint.x>=0 && StudiedPoint.y>=0 && StudiedPoint.z>=0 )
		{

		  /* shift for every vertex in this set of neighborhood */
		  level = (fabs(SubV_pnt[n].x)+fabs(SubV_pnt[n].y)+fabs(SubV_pnt[n].z))/3.0 ;
		  /*level = sqrt( SubV_pnt[n].x*SubV_pnt[n].x + SubV_pnt[n].y*SubV_pnt[n].y + SubV_pnt[n].z*SubV_pnt[n].z ) ; */ /* tried before June 18, 2001 */
		  weight = GuassianWeight( level, UsedGuassianSigma /*nb_size*/ ) ;
		  /*weight = exp(-level*level/(2.*nb_size*nb_size)) ;*/
		  

		  /* converge to its center */
		  nbr_tot.x = 0 ;  nbr_tot.y = 0 ;  nbr_tot.z = 0 ;
		  temp_num = 0 ;
		  for(i=1;i<27;i++) /* 26 neighbor */
		    {
		      Grid.x = StudiedPoint.x+nb_pnt[i].x ;
		      Grid.y = StudiedPoint.y+nb_pnt[i].y ;
		      Grid.z = StudiedPoint.z+nb_pnt[i].z ;
		      
		      if( Grid.x<image_size && Grid.y<image_size && Grid.z<z_size && Grid.x>=0 && Grid.y>=0 && Grid.z>=0 )
			{
			  nbr_tot.x += DeformFld[Grid.z][Grid.x][Grid.y].x;
			  nbr_tot.y += DeformFld[Grid.z][Grid.x][Grid.y].y;
			  nbr_tot.z += DeformFld[Grid.z][Grid.x][Grid.y].z;
			  
			  temp_num++ ;
			}
		    }
		  if(temp_num>0)
		    {
		      nbr_tot.x /= temp_num ;
		      nbr_tot.y /= temp_num ;
		      nbr_tot.z /= temp_num ;
		    }
		  else
		    printf("(%d,%d,%d) temp_num=%d (total 8)\n", StudiedPoint.x, StudiedPoint.y, StudiedPoint.z, temp_num) ;
		  
		  CenterRate = smoothFactor ; /*0.05 */
		  nbr_tot.x =  (nbr_tot.x - DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].x)*CenterRate ; 
		  nbr_tot.y =  (nbr_tot.y - DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].y)*CenterRate ;
		  nbr_tot.z =  (nbr_tot.z - DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].z)*CenterRate ;
		  
		  Delta=0.005*ratio_iteration ; /* June 6, 2001*/
		  /*printf("weight=%f (%d)", weight, level[n]) ; getchar() ;*/
		  DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].x += (dfm.x*weight + nbr_tot.x)*(Deform_RATE+Delta) ;
		  DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].y += (dfm.y*weight + nbr_tot.y)*(Deform_RATE+Delta) ;  
		  DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].z += (dfm.z*weight + nbr_tot.z)*(Deform_RATE+Delta) ;  
		}
	    }
	}
      /*printf("good!\n") ;*/
    }
  }


  Start_Search_Point = Last_searched_point ;

  /* get update MdlVer's position from the DeformFld, for estimating global transformation*/
  for(s=0; s<MdlVer_Num; s++)
    {  
      x = FixedMdlVer[s].x ;
      y = FixedMdlVer[s].y ;
      z = FixedMdlVer[s].z ;
      
      MdlVer[s].x = DeformFld[z][x][y].x + x ;
      MdlVer[s].y = DeformFld[z][x][y].y + y ;
      MdlVer[s].z = DeformFld[z][x][y].z + z ;
    }

               /*0.3*//*0.3*/
               /*0.4*//*0.4*/
               /*0.6*//*0.4*/
               /*0.5*//*0.5*/   /* probably for EN** et al */
  LocalRatio = 0.25 + 0.65*exp(-(ratio_iteration-1.)*(ratio_iteration-1.)/2./.25/.25) ; /* (0.3,0.3),  directly 0.88 */
  /* deformation modified by global affine-transformation plus local deformation */
  GlobalAffineUpdatePlusLocalDeformation( MdlVer, verModelLast, MdlVer_Num ) ;
  Mat_Print(Transform) ;
  /* update deformation matrix */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	{
	  Crnt.x = DeformFld_Last[k][i][j].x + i ;
	  Crnt.y = DeformFld_Last[k][i][j].y + j ;
	  Crnt.z = DeformFld_Last[k][i][j].z + k ;
	  
	  globl.x = Transform->data[0][0]*Crnt.x + Transform->data[0][1]*Crnt.y + Transform->data[0][2]*Crnt.z + Transform->data[0][3] -i ;
	  globl.y = Transform->data[1][0]*Crnt.x + Transform->data[1][1]*Crnt.y + Transform->data[1][2]*Crnt.z + Transform->data[1][3] -j ;
	  globl.z = Transform->data[2][0]*Crnt.x + Transform->data[2][1]*Crnt.y + Transform->data[2][2]*Crnt.z + Transform->data[2][3] -k ;
	  
	  DeformFld[k][i][j].x = globl.x + (DeformFld[k][i][j].x-globl.x)*LocalRatio/1.2;
	  DeformFld[k][i][j].y = globl.y + (DeformFld[k][i][j].y-globl.y)*LocalRatio/1.2;
	  DeformFld[k][i][j].z = globl.z + (DeformFld[k][i][j].z-globl.z)*LocalRatio/1.2;

	  /* boundary constraints */
	  if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
	  if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
	  if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;

	  if(DeformFld[k][i][j].x+i>=(image_size-1)) DeformFld[k][i][j].x=(image_size-1)-i ;
	  if(DeformFld[k][i][j].y+j>=(image_size-1)) DeformFld[k][i][j].y=(image_size-1)-j ;
	  if(DeformFld[k][i][j].z+k>=(z_size-1))     DeformFld[k][i][j].z=(z_size-1)-k ;
	}
  SmoothDeformationField(DeformFld, image_size, z_size, MdlImg, SmoothingTimes) ;
  if(SmoothingTimes>0) 
    SmoothDeformationFieldWithEdgePreserving( FixedMdlVer, MdlVer_Num, DeformFld, MdlImg, image_size, z_size, ratio_iteration, SmoothingTimes ) ;
  /* focused edge points in the model */
  for(s=0; s<MdlVer_Num; s++)
    {  
      x = FixedMdlVer[s].x ;
      y = FixedMdlVer[s].y ;
      z = FixedMdlVer[s].z ;
      
      MdlVer[s].x = DeformFld[z][x][y].x + x ;
      MdlVer[s].y = DeformFld[z][x][y].y + y ;
      MdlVer[s].z = DeformFld[z][x][y].z + z ;
    }
  
  /* statistical correcting */
  if(StatisticsEmployedOrNot==ADD) 
    {
      StatisticalMapping(MdlVer, MdlVer_Num, Statistical_factor) ; /* Setp. 3, 1999 */
      printf("statistics information added!\n\n\n") ;
    }
  else 
    printf("statistics information not added!\n\n\n") ;
  

  /* free */
  free(nb_pnt) ;
  free(SubV_pnt) ;
  UCfree3d(SearchingStatus, z_size, image_size) ;
    
  /* geometric */
  free(verModelLast) ;
  Fvector3dfree3d(DeformFld_Last, z_size, image_size) ;
}


/* for model */
void Select3DEdgePointsInModelTO1D(int *MdlVer_Num, DTIImgAttribute ***MdlImg, Fvector3d ***DeformFld, int image_size, int z_size)
{
  int   i, j, k, c ;
  unsigned char ***FeaPoints ;
  FILE  *fp;


  /* To view which points are selected as landmarks. */
  FeaPoints = UCalloc3d(image_size,image_size,z_size);

  (*MdlVer_Num) = 0 ;
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	if( MdlImg[k][i][j].prolateVar>MdlThreshold.Variance_UP || MdlImg[k][i][j].faEdge>MdlThreshold.CannyEdge_UP || 
        (!IsFAFeature&&(TraceEdgeConfidence*MdlImg[k][i][j].traceEdge>MdlThreshold.CannyEdge_UP)) ) 
	  (*MdlVer_Num)+=1 ;
  printf("There are %d focused edge points in model image!\n", (*MdlVer_Num));

  MdlVer     = Fvector3dalloc1d(*MdlVer_Num);
  FixedMdlVer= Ivector3dalloc1d(*MdlVer_Num);  /* for connection */
  MdlVer_Fea = DTIImgAttributealloc1d(*MdlVer_Num) ; /* attributes for each selected point */

  c = 0 ;
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	if( MdlImg[k][i][j].prolateVar>MdlThreshold.Variance_UP || MdlImg[k][i][j].faEdge>MdlThreshold.CannyEdge_UP || 
        (!IsFAFeature&&(TraceEdgeConfidence*MdlImg[k][i][j].traceEdge>MdlThreshold.CannyEdge_UP)) )
	  {
	    /* position */
	    MdlVer[c].x = i + DeformFld[k][i][j].x ;
	    MdlVer[c].y = j + DeformFld[k][i][j].y ;
	    MdlVer[c].z = k + DeformFld[k][i][j].z ;
   		  FixedMdlVer[c].x = i ;
		  FixedMdlVer[c].y = j ;
		  FixedMdlVer[c].z = k ;

	    /* attributes  */
        
	    MdlVer_Fea[c].prolateVar  = MdlImg[k][i][j].prolateVar;   /* edge */
	    MdlVer_Fea[c].prolateMean  = MdlImg[k][i][j].prolateMean;   /* geometric */

	    MdlVer_Fea[c].oblateVar  = MdlImg[k][i][j].oblateVar;   /* edge */
	    MdlVer_Fea[c].oblateMean  = MdlImg[k][i][j].oblateMean;   /* geometric */

	    MdlVer_Fea[c].sphereVar  = MdlImg[k][i][j].sphereVar;   /* edge */
	    MdlVer_Fea[c].sphereMean  = MdlImg[k][i][j].sphereMean;   /* geometric */
        MdlVer_Fea[c].faTiss  = MdlImg[k][i][j].faTiss;   /* intensity */
	    MdlVer_Fea[c].traceEdge = MdlImg[k][i][j].traceEdge;  /* VN volume */
	    MdlVer_Fea[c].faEdge = MdlImg[k][i][j].faEdge;  /* CSF/BG volume */

	    c ++ ;
	    
	    FeaPoints[k][i][j] = 255 ;
	  }
	else
	  FeaPoints[k][i][j] = 0 ;

  /* saving ... */
    fp=myopen("MdlImg_FocuedEdges","w"); 
      for(k=0;k<z_size;k++)  
      for(i=0;i<image_size;i++)  
      fwrite(FeaPoints[k][i],1,image_size,fp);  
      fclose(fp);

  /* free */
  UCfree3d(FeaPoints, z_size, image_size) ;
}


/* for object */
void Select3DEdgePointsInObjectTO1D(int *ObjVer_Num, DTIImgAttribute ***ObjImg, int image_size, int z_size)
{
  int   i, j, k, c ;
  unsigned char ***FeaPoints ;
  FILE  *fp;

  /* To view which points are selected as landmarks. */
  FeaPoints = UCalloc3d(image_size,image_size,z_size);


  (*ObjVer_Num) = 0 ;
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	if( ObjImg[k][i][j].prolateVar>ObjThreshold.Variance_UP || ObjImg[k][i][j].faEdge>ObjThreshold.CannyEdge_UP || 
        (!IsFAFeature && (TraceEdgeConfidence*ObjImg[k][i][j].traceEdge>ObjThreshold.CannyEdge_UP )) )
	  (*ObjVer_Num)+=1 ;
  printf("There are %d focued edge points in the object image!\n", *ObjVer_Num);

  ObjVer     = Ivector3dalloc1d(*ObjVer_Num);
  ObjVer_Fea = DTIImgAttributealloc1d(*ObjVer_Num) ; /* attributes for each selected point */

  c = 0 ;
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	if( ObjImg[k][i][j].prolateVar>ObjThreshold.Variance_UP || ObjImg[k][i][j].faEdge>ObjThreshold.CannyEdge_UP || 
        (!IsFAFeature&&(TraceEdgeConfidence*ObjImg[k][i][j].traceEdge>ObjThreshold.CannyEdge_UP)) )
	  {
		/* position */
	    ObjVer[c].x = i ;
	    ObjVer[c].y = j ;
	    ObjVer[c].z = k ;

		/* attributes  */
        
	    ObjVer_Fea[c].prolateVar  = ObjImg[k][i][j].prolateVar;   /* edge */	    
	    ObjVer_Fea[c].prolateMean  = ObjImg[k][i][j].prolateMean;   /* geometric */

	    ObjVer_Fea[c].oblateVar  = ObjImg[k][i][j].oblateVar;   /* edge */	    
	    ObjVer_Fea[c].oblateMean  = ObjImg[k][i][j].oblateMean;   /* geometric */

	    ObjVer_Fea[c].sphereVar  = ObjImg[k][i][j].sphereVar;   /* edge */	    
	    ObjVer_Fea[c].sphereMean  = ObjImg[k][i][j].sphereMean;   /* geometric */
        ObjVer_Fea[c].faTiss  = ObjImg[k][i][j].faTiss;   /* intensity */
	    ObjVer_Fea[c].traceEdge = ObjImg[k][i][j].traceEdge;  /* VN volume */
	    ObjVer_Fea[c].faEdge = ObjImg[k][i][j].faEdge;  /* CSF/BG volume */

	    c ++ ;

	    FeaPoints[k][i][j] = 255 ;
	  }
	else
	  FeaPoints[k][i][j] = 0 ;

  /* saving ... */
    fp=myopen("ObjImg_FocuedEdges","w"); 
      for(k=0;k<z_size;k++)  
      for(i=0;i<image_size;i++)  
      fwrite(FeaPoints[k][i],1,image_size,fp);  
      fclose(fp);

  /* free */
  UCfree3d(FeaPoints, z_size, image_size) ;
}

void SearchTheNearestVertexForEachLandmark_March2003(Ivector3d *ObjVer, int ObjVer_Num, DTIImgAttribute *ObjVer_Fea, DTIImgAttribute ***ObjImg, Fvector3d *MdlVer, int MdlVer_Num, DTIImgAttribute *MdlVer_Fea, Ivector3d *FixedMdlVer, DTIImgAttribute ***MdlImg, Fvector3d ***DeformFld,   int image_size, int z_size,  int max) 
{
  int        i, j, k, l ;
  int        ***VerticesImg, x, y, z, r, t, px, py, pz, vx, vy, vz, s ;
  int        TotalMatchingPoints, TotalMdlPntMatched, TotalSamples ;
  float      degree, StepDesign, Pointer ;

  /* for subvolume matching*/
   float level, CurrentDegree, OverlapDegree, MaxDegree ;
   Fvector3d Crnt, Tmp, Search, SubjNb, dfm;
   int   sx,sy,sz, SmplStep, incre, dfm_s ;
   int real_size;
   
   /* subvolume */
   Ivector3d  *nb_pnt, Grid;
   int        nb_size, n,count ;
  
   nb_size = max*2+1; /*nb_size should be odd number */ 
   nb_pnt= Ivector3dalloc1d(nb_size*nb_size*nb_size) ;
   calculate_hood_byIncreasingRadius(nb_pnt, nb_size, &count, 1., 1., 1.) ; 
   SmplStep = count/20 ;  if(SmplStep<1) SmplStep=1 ;
   printf("   nbr num:%d with rad %d <- sampling step=%d\n", count,nb_size, SmplStep) ;



  /* For saving searching time */
  VerticesImg = Ialloc3d(image_size,image_size,z_size);
  if (!VerticesImg) printf("   memeoy application in VerticesImg\n") ;
  /* transfering surface's vertices */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	VerticesImg[k][i][j] = -1 ;
  for( i=0; i<MdlVer_Num; i++)
    {
      x = (int)(MdlVer[i].x) ;
      y = (int)(MdlVer[i].y) ;
      z = (int)(MdlVer[i].z) ;
      
      if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
	  VerticesImg[z][x][y] = i ;
    }
  printf("   image setting\n") ;

  /* reset */
  for( i=0; i<MdlVer_Num; i++)
    {
      forceByFocusedPntInObj[i].x = 0 ;
      forceByFocusedPntInObj[i].y = 0 ;
      forceByFocusedPntInObj[i].z = 0 ;
      multiple[i] = 0 ;
    }


  TotalSamples = MdlVer_Num/2;
          if(TotalSamples>ObjVer_Num) TotalSamples = ObjVer_Num ;
          if(TotalSamples>20000)      TotalSamples = 20000 ;
  StepDesign = (float)ObjVer_Num/(float)TotalSamples ; printf("   StepDesign=%f\n", StepDesign) ;
  incre = 2*max/6 ; if(incre<1) incre=1 ;              printf("   incre=%d\n", incre);
    TotalMatchingPoints = 0;
  for(Pointer=0, l=0; l<ObjVer_Num; Pointer += StepDesign, l=(int)Pointer)
    {
      px = ObjVer[l].x ;
      py = ObjVer[l].y ;
      pz = ObjVer[l].z ;	  

      dfm.x = 0 ; dfm.y = 0 ; dfm.z = 0 ;
      MaxDegree = 0 ;
      for(SubjNb.x=-max; SubjNb.x<=max; SubjNb.x+=incre)
	for(SubjNb.y=-max; SubjNb.y<=max; SubjNb.y+=incre)
	  for(SubjNb.z=-max; SubjNb.z<=max; SubjNb.z+=incre)
      /*for(t=0; t<pixelNumIn_MyHood && Status==FFAIL; t++)*/
	{
	  vx = (int)(px + SubjNb.x) ; /*+ MyHood[t].x ;*/
	  vy = (int)(py + SubjNb.y) ; /*+ MyHood[t].y ;*/
	  vz = (int)(pz + SubjNb.z) ; /*+ MyHood[t].z ;*/

	  if( vx<image_size && vy<image_size && vz<z_size && vx>=0 && vy>=0 && vz>=0)
	    {
	      s = VerticesImg[vz][vx][vy] ;
	      
	      if( s>=0 && px<image_size && py<image_size && pz<z_size && px>=0 && py>=0 && pz>=0 && (MdlVer_Fea[s].faEdge>0 && ObjVer_Fea[l].faEdge>0 || MdlVer_Fea[s].faEdge==0 && ObjVer_Fea[l].faEdge==0) && (MdlVer_Fea[s].traceEdge>0 && ObjVer_Fea[l].traceEdge>0 || MdlVer_Fea[s].traceEdge==0 && ObjVer_Fea[l].traceEdge==0) )
		{
		  degree = SimilarityBetweenFocusedPointsInMdlObjImgs(ObjVer_Fea,l, MdlVer_Fea,s) ;
		  
		  if( degree>SMR_THR )
		    {
		      /* get subvolume similarity */
		      /*****************************************/
		        sx = FixedMdlVer[s].x ;
			sy = FixedMdlVer[s].y ;
			sz = FixedMdlVer[s].z ;
			
			Search.x = -SubjNb.x ;  /*+ MyHood[t].x ;*/
			Search.y = -SubjNb.y ;  /*+ MyHood[t].y ;*/
			Search.z = -SubjNb.z ;  /*+ MyHood[t].z ;*/			
			
			OverlapDegree = 0 ;
			real_size = 0 ;
			for(n=0; n<count; n+=SmplStep)
			  {
			    /* Coordinate In Template */
			    Grid.x = sx+nb_pnt[n].x ;
			    Grid.y = sy+nb_pnt[n].y ;
			    Grid.z = sz+nb_pnt[n].z ;
			    
			    if( Grid.x<image_size && Grid.y<image_size && Grid.z<z_size && Grid.x>=0 && Grid.y>=0 && Grid.z>=0 ) /* true */
			      {
				Crnt.x = DeformFld[Grid.z][Grid.x][Grid.y].x + Grid.x ;
				Crnt.y = DeformFld[Grid.z][Grid.x][Grid.y].y + Grid.y ;
				Crnt.z = DeformFld[Grid.z][Grid.x][Grid.y].z + Grid.z ;
				
				/* shift for every vertex in this set of neighborhood */
				level = (fabs(nb_pnt[n].x)+fabs(nb_pnt[n].y)+fabs(nb_pnt[n].z))/3.0 ; 
				ShiftByGuassianWeight(Crnt, &Tmp, Search, level, nb_size) ;

				x = (int)(Tmp.x + 0.5) ;
				y = (int)(Tmp.y + 0.5) ;
				z = (int)(Tmp.z + 0.5) ;
				
				if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
				  {
				    CurrentDegree = SimilarityBetweenFeaturesInMdlAndObjImgs( ObjImg, x, y, z, MdlImg, Grid.x, Grid.y, Grid.z ) ;

				    /* March 27, 2003: This is added for considering   */
				    /* (1) binary tissue matching, (2) BG effect.      */  /*good for yng brain*/
				    if( YoungBrain==YYES &&  MdlImg[Grid.z][Grid.x][Grid.y].prolateVar==0 && CurrentDegree<0.6 ) 
				      CurrentDegree=0;
				    /*if(  MdlImg[Grid.z][Grid.x][Grid.y].Tiss==BG && CurrentDegree>0.6 )
				      {
				      CurrentDegree=0; 
				      real_size -- ; 
				      }*/
				       /* will not count current one. Notable size will be increased one nest*/
			    
				    real_size ++ ;
                     
				    if( MdlImg[Grid.z][Grid.x][Grid.y].faEdge>MdlThreshold.CannyEdge_UP 
                      || (!IsFAFeature&&(TraceEdgeConfidence*MdlImg[Grid.z][Grid.x][Grid.y].traceEdge>MdlThreshold.CannyEdge_UP)) ) /* June 6, 2001*/
				    {
					CurrentDegree *= 1.2 ;
					real_size += (1.2-1.0) ;
				    }
				  }
				else
				  CurrentDegree = 0 ;
				
				OverlapDegree += CurrentDegree ; 
			      }
			  }
			if(real_size>0) 
			  OverlapDegree = OverlapDegree/real_size ;
                      /*****************************************/

			if( OverlapDegree>MaxDegree )
			  {
			    MaxDegree = OverlapDegree ;
			    dfm.x = Search.x ;
			    dfm.y = Search.y ;	    
			    dfm.z = Search.z ;
			    dfm_s = s ;
			  }
		    }
		}
	    }
	}

      /* get a maximal one */
      if( MaxDegree>MatchingDegreeOfMdlOnImgEdge )
	{
	    TotalMatchingPoints ++;
	  forceByFocusedPntInObj[dfm_s].x += dfm.x ;
	  forceByFocusedPntInObj[dfm_s].y += dfm.y ;
	  forceByFocusedPntInObj[dfm_s].z += dfm.z ;
	  multiple[dfm_s] += 1 ;
	}
    }
  printf("   TotalMatchingPoints=%d (%f)\n", TotalMatchingPoints, (float)TotalMatchingPoints/(float)ObjVer_Num*100.0) ;


  TotalMdlPntMatched = 0 ;
  for( i=0; i<MdlVer_Num; i++)
    if( multiple[i]>0 )
      {
	/*printf("i=%d num=%d\n", i,multiple[i]) ;*/
	forceByFocusedPntInObj[i].x /= multiple[i] ;
	forceByFocusedPntInObj[i].y /= multiple[i] ;
	forceByFocusedPntInObj[i].z /= multiple[i] ;
	TotalMdlPntMatched ++ ;
      }
  printf("   TotalMdlPntMatched=%d (%f)\n", TotalMdlPntMatched, (float)TotalMdlPntMatched/(float)MdlVer_Num*100.0) ;

  /* free */
  Ifree3d(VerticesImg, z_size, image_size) ;
}




/*scaling and shifting*/
void EstimatingScalingAndShiftOf3DSurfaceFromImg(Fvector3d *MdlVer, Ivector3d *FixedMdlVer, DTIImgAttribute *MdlVer_Fea, int MdlVer_Num, Fvector3d ***DeformFld, DTIImgAttribute ***ObjImg, int image_size, int z_size )
{
  int   i, j, k, s, t, real_size ;
  float CurrentDegree, OverlapDegree, MaxDegree ;
  int x, y, z ;
  float max_shift;
  Fvector3d Crnt, center, shift, estimated_shift ;
  Fvector3d scaling, estimated_scaling, dfm ;

  /* for estimating the overlapping degree */
  int bubble_size[BUBBNUM] ;
  Fvector3d **MultiBubble, cBubble ;

  MultiBubble = Fvector3dalloc2d(BUBBNUM,10000);
  /* prepare for overlapping estimation */
  calculate_hood(MultiBubble, bubble_size, 1.0, 1.0, 1.0);


  printf("Estimating the scaling and the shifting\n") ;

  /* calculate center of the surface */
  center.x=center.y=center.z=0 ;
  for(s=0; s<MdlVer_Num; s++)
    {
      center.x += MdlVer[s].x/MdlVer_Num ;
      center.y += MdlVer[s].y/MdlVer_Num ;
      center.z += MdlVer[s].z/MdlVer_Num ;
    }


  t=2; /*0, 3 */ MaxDegree=-1.0 ;
  max_shift = 0.0 ; /* 0, 6.0 */
  for(scaling.x=0.85;  scaling.x<1.35;  scaling.x+=0.05)
    for(scaling.y=0.85;  scaling.y<1.35;  scaling.y+=0.05)
      for(scaling.z=0.85; scaling.z<1.35; scaling.z+=0.05)
    {
      printf("scaling.x=%f  scaling.y=%f  scaling.z=%f\n", scaling.x, scaling.y, scaling.z) ;
      for(shift.x=-max_shift; shift.x<=max_shift; shift.x+=2.0)
	for(shift.y=-max_shift; shift.y<=max_shift; shift.y+=2.0)
	  for(shift.z=-max_shift; shift.z<=max_shift; shift.z+=2.0)
	    {
	      /*printf("shift.x=%f shift.y=%f shift.z=%f\n", shift.x, shift.y, shift.z) ;*/

	      /* on surface */
	      OverlapDegree = 0 ;
	      for(s=0; s<MdlVer_Num; s+=3) /*3, 30*/
		{
		  Crnt.x = scaling.x*(MdlVer[s].x-center.x) + center.x + shift.x ;
		  Crnt.y = scaling.y*(MdlVer[s].y-center.y) + center.y + shift.y ;
		  Crnt.z = scaling.z*(MdlVer[s].z-center.z) + center.z + shift.z ;
		  
		  real_size = 0 ;  CurrentDegree = 0 ;
		  cBubble.x=cBubble.y=cBubble.z=0;
		  for(j=0;j<bubble_size[t];j++)
		    {
		      x = (int)(Crnt.x+MultiBubble[t][j].x) ;
		      y = (int)(Crnt.y+MultiBubble[t][j].y) ;
		      z = (int)(Crnt.z+MultiBubble[t][j].z) ;
		      
		      if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 && (ObjImg[z][x][y].faEdge-MdlVer_Fea[s].faEdge)<21
                  && (IsFAFeature || (ObjImg[z][x][y].traceEdge-MdlVer_Fea[s].traceEdge)<21) )
			{
			  real_size ++ ;
			  
			  cBubble.x += (MultiBubble[t][j].x) ;
			  cBubble.y += (MultiBubble[t][j].y) ;
			  cBubble.z += (MultiBubble[t][j].z) ;
			}
		    }
		  if( real_size>0 )   
		    {
		      cBubble.x /= (real_size*(t+3)) ;  
		      cBubble.y /= (real_size*(t+3)) ;  
		      cBubble.z /= (real_size*(t+3)) ;  
		      
		      CurrentDegree = 1.0 - sqrt(cBubble.x*cBubble.x + cBubble.y*cBubble.y + cBubble.z*cBubble.z) ;
		    }
		  else  	      
		    CurrentDegree = 0 ;
		  
		  if( CurrentDegree>0.1 )  CurrentDegree=1.0 ;
		  else                     CurrentDegree=0.  ; 
		  
		  OverlapDegree += CurrentDegree/MdlVer_Num ; 
		}
	      
	      if( OverlapDegree>MaxDegree )
		{
		  MaxDegree = OverlapDegree ;
		  estimated_shift.x  = shift.x ;
		  estimated_shift.y  = shift.y ;
		  estimated_shift.z  = shift.z ;
		  estimated_scaling.x = scaling.x ;
		  estimated_scaling.y = scaling.y ;
		  estimated_scaling.z = scaling.z ;
		}
	    }
      printf("      MaxDegree=%f\n", MaxDegree) ;	      
    }
  printf(" estimated_shift.x=%f\n estimated_shift.y=%f\n estimated_shift.z=%f\n estimated_scaling.x=%f\n estimated_scaling.y=%f\n estimated_scaling.z=%f\n", estimated_shift.x, estimated_shift.y, estimated_shift.z, estimated_scaling.x, estimated_scaling.y, estimated_scaling.z) ;

  /* update by estimation */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	{
	  Crnt.x = DeformFld[k][i][j].x + i ;
	  Crnt.y = DeformFld[k][i][j].y + j ;
	  Crnt.z = DeformFld[k][i][j].z + k ;
	  
	  DeformFld[k][i][j].x = estimated_scaling.x*(Crnt.x-center.x) + center.x + estimated_shift.x -i ;
	  DeformFld[k][i][j].y = estimated_scaling.y*(Crnt.y-center.y) + center.y + estimated_shift.y -j ;
	  DeformFld[k][i][j].z = estimated_scaling.z*(Crnt.z-center.z) + center.z + estimated_shift.z -k ;

	  /* boundary constraints */
	  if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
	  if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
	  if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;

	  if(DeformFld[k][i][j].x+i>=image_size) DeformFld[k][i][j].x=image_size-i ;
	  if(DeformFld[k][i][j].y+j>=image_size) DeformFld[k][i][j].y=image_size-j ;
	  if(DeformFld[k][i][j].z+k>=z_size)     DeformFld[k][i][j].z=z_size-k ;
	}
  printf("Estimation finished!\n") ;

  for(s=0; s<MdlVer_Num; s++)
    { 
      x = FixedMdlVer[s].x ;
      y = FixedMdlVer[s].y ;
      z = FixedMdlVer[s].z ;

      if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
	{
	  MdlVer[s].x = DeformFld[z][x][y].x + x ;
	  MdlVer[s].y = DeformFld[z][x][y].y + y ;
	  MdlVer[s].z = DeformFld[z][x][y].z + z ;
	}
      else
	printf("see here: (%d,%d,%d)\n", x,y,z) ;
    }
  printf("Estimation finished!\n") ;

  /* free */
  Fvector3dfree2d(MultiBubble, BUBBNUM) ;
}
/*scaling and shifting*/


void GlobalAffineUpdatePlusLocalDeformation( Fvector3d *verObject, Fvector3d *verModel, int MdlVer_Num )
{
   int i, j, k ;
   Matrix *ModelMatrix, *ObjectMatrix, *FittingMatrix, *TmpM1, *TmpM2, *InversedMtrx/*, *Transform*/ ;



   /* create  metrices*/
   CreateMatrix(&ModelMatrix,  4, MdlVer_Num);
   CreateMatrix(&ObjectMatrix, 4, MdlVer_Num);
   CreateMatrix(&FittingMatrix,4, MdlVer_Num);
   CreateMatrix(&TmpM1,        4, 4);
   CreateMatrix(&TmpM2,        4, 4);
   CreateMatrix(&InversedMtrx, 4, 4);
/*   CreateMatrix(&Transform,    4, 4);*/


   /* get ModelMatrix->data[4][MdlVer_Num] */
   for(j=0; j<MdlVer_Num; j++)
     {
       ModelMatrix->data[0][j] = verModel[j].x ;
       ModelMatrix->data[1][j] = verModel[j].y ;
       ModelMatrix->data[2][j] = verModel[j].z ;
       ModelMatrix->data[3][j] = 1 ;
     }
     
   /* get ObjectMatrix->data[4][MdlVer_Num] */
   for(j=0; j<MdlVer_Num; j++)
     {
       ObjectMatrix->data[0][j] = verObject[j].x ;
       ObjectMatrix->data[1][j] = verObject[j].y ;
       ObjectMatrix->data[2][j] = verObject[j].z ;
       ObjectMatrix->data[3][j] = 1 ;
     }
     

   /*get matrix 4X4 TmpM1*/
   for(i=0; i<4; i++)
     for(j=0; j<4; j++)
       {
	 TmpM1->data[i][j] = 0 ;
	 
	 for(k=0; k<MdlVer_Num; k++)
	   TmpM1->data[i][j] += (ModelMatrix->data[i][k]*ModelMatrix->data[j][k]) ;
       }

   
   /*get matrix 4X4 TmpM2 */
   for(i=0; i<4; i++)
     for(j=0; j<4; j++)
       {
	 TmpM2->data[i][j] = 0 ;
	 
	 for(k=0; k<MdlVer_Num; k++)
	   TmpM2->data[i][j] += (ObjectMatrix->data[i][k]*ModelMatrix->data[j][k]) ;
       }


   Mat_Inverse( TmpM1, InversedMtrx ) ;

   /* get affine transformation */
   Mat_A_equal_BxC(Transform, TmpM2, InversedMtrx) ;             /*A=BxC*/


   /* use model to best fit the object */
   Mat_A_equal_BxC(FittingMatrix, Transform, ModelMatrix) ;    


   /* back to model variables */
   for(j=0; j<MdlVer_Num; j++)
     {
       verModel[j].x = FittingMatrix->data[0][j] ;
       verModel[j].y = FittingMatrix->data[1][j] ;
       verModel[j].z = FittingMatrix->data[2][j] ;
     }


   /* free */ 
   FreeMatrix(ModelMatrix) ;
   FreeMatrix(ObjectMatrix) ;
   FreeMatrix(FittingMatrix) ;
   FreeMatrix(TmpM1) ;
   FreeMatrix(TmpM2) ;
   FreeMatrix(InversedMtrx) ;
/*   FreeMatrix(Transform) ;*/
}



/* To speed up the algorithm */
/*
void ShiftByGuassianWeight(Fvector3d In, Fvector3d *Tmp, Fvector3d Search, float level, int max)
{
  float weight ;
  int l,m ;

  weight = exp(-level*level/(2.*max*max)) ;
  
  (*Tmp).x = In.x + Search.x*weight ;
  (*Tmp).y = In.y + Search.y*weight ;
  (*Tmp).z = In.z + Search.z*weight ;
}
*/
/* To speed up the algorithm */
#define MAX_SIGMA 100
#define MAX_LEVEL 500
float GuassianAtLevelSigma[MAX_SIGMA][MAX_LEVEL] ;
int   regist_CompGuassianOnce = OOK ;
void ShiftByGuassianWeight(Fvector3d In, Fvector3d *Tmp, Fvector3d Search, float level, float sigma)
{
  float weight ;
  int   s, l ;

  /* calculate for one time */
  if(regist_CompGuassianOnce == OOK )
    {
      regist_CompGuassianOnce = FFAIL ;

      for(l=0; l<MAX_LEVEL; l++)
	GuassianAtLevelSigma[0][l] = 0 ;

      for(s=1; s<MAX_SIGMA; s++)
	for(l=0; l<MAX_LEVEL; l++)
	  GuassianAtLevelSigma[s][l] = exp(-l*l/(2.0*s*s)) ;
    }

  if( sigma<MAX_SIGMA && level<MAX_LEVEL )
    weight = GuassianAtLevelSigma[(int)sigma][(int)level] ;
  else 
    {
      weight = exp(-level*level/(2.0*sigma*sigma)) ;
      printf("\n\n\n\nWatch here!!!\n\n\n") ;
    }
  (*Tmp).x = In.x + Search.x*weight ;
  (*Tmp).y = In.y + Search.y*weight ;
  (*Tmp).z = In.z + Search.z*weight ;
}


float GuassianWeight(float level, float sigma)
{
  float weight ;

  if( sigma<MAX_SIGMA && level<MAX_LEVEL )
    weight = GuassianAtLevelSigma[(int)sigma][(int)level] ;
  else 
    {
      weight = exp(-level*level/(2.0*sigma*sigma)) ;
      printf("\n\n\n\nWatch here!!!\n\n\n") ;
    }

  return(weight) ;
}





/****************************************************************************************************************/
/****************************************************************************************************************/
/*********************************     Statistics     ***********************************************************/
/****************************************************************************************************************/
/****************************************************************************************************************/
#define CHNEIGENNUM   3 /*3*/




/* part 0 */
void AlignmentObjectToModel( Fvector3d *verMdl, Fvector3d *verObj, Matrix *tMdl2Obj, int MdlVer_Num ) ;
void AffineTransform3Dsurface( Fvector3d *ver, Matrix *tMdl2Obj, int  MdlVer_Num) ;

/* part 1 */
void Open_TrainingResults( Matrix *T1, Matrix *T2, float *T3, int SizeOfT3 ) ;

/* part 2 */
/*void StatisticalMapping(Fvector3d *ver) ;*/
void Correct_3DsurfacebyT1T2T3(Fvector3d *ver, int MdlVer_Num, Matrix *T1, Matrix *T2, float  *T3, float alpha) ;




void AlignmentObjectToModel( Fvector3d *verMdl, Fvector3d *verObj, Matrix *tMdl2Obj, int MdlVer_Num )
{
   int i, j, k ;
   Matrix *ObjMatrix, *MdlMatrix, *FittingMatrix, *TmpM1, *TmpM2, *InversedMtrx, *Transform ;



   /* create  metrices*/
   CreateMatrix(&ObjMatrix,    4, MdlVer_Num);
   CreateMatrix(&MdlMatrix,    4, MdlVer_Num);
   CreateMatrix(&FittingMatrix,4, MdlVer_Num);
   CreateMatrix(&TmpM1,        4, 4);
   CreateMatrix(&TmpM2,        4, 4);
   CreateMatrix(&InversedMtrx, 4, 4);
   CreateMatrix(&Transform,    4, 4);


   /* get ObjMatrix->data[4][MdlVer_Num] */
   for(j=0; j<MdlVer_Num; j++)
     {
       ObjMatrix->data[0][j] = verObj[j].x ;
       ObjMatrix->data[1][j] = verObj[j].y ;
       ObjMatrix->data[2][j] = verObj[j].z ;
       ObjMatrix->data[3][j] = 1 ;
     }
     
   /* get MdlMatrix->data[4][MdlVer_Num] */
   for(j=0; j<MdlVer_Num; j++)
     {
       MdlMatrix->data[0][j] = verMdl[j].x ;
       MdlMatrix->data[1][j] = verMdl[j].y ;
       MdlMatrix->data[2][j] = verMdl[j].z ;
       MdlMatrix->data[3][j] = 1 ;
     }
     

   /*get matrix 4X4 TmpM1*/
   for(i=0; i<4; i++)
     for(j=0; j<4; j++)
       {
	 TmpM1->data[i][j] = 0 ;
	 
	 for(k=0; k<MdlVer_Num; k++)
	   TmpM1->data[i][j] += (ObjMatrix->data[i][k]*ObjMatrix->data[j][k]) ;
       }

   
   /*get matrix 4X4 TmpM2 */
   for(i=0; i<4; i++)
     for(j=0; j<4; j++)
       {
	 TmpM2->data[i][j] = 0 ;
	 
	 for(k=0; k<MdlVer_Num; k++)
	   TmpM2->data[i][j] += (MdlMatrix->data[i][k]*ObjMatrix->data[j][k]) ;
       }


   Mat_Inverse( TmpM1, InversedMtrx ) ;

   /* get affine transformation */
   Mat_A_equal_BxC(Transform, TmpM2, InversedMtrx) ;             /*A=BxC*/


   /* use model to best fit the object */
   Mat_A_equal_BxC(FittingMatrix, Transform, ObjMatrix) ;    


   /* back to model variables */
   for(j=0; j<MdlVer_Num; j++)
     {
       verObj[j].x = FittingMatrix->data[0][j] ;
       verObj[j].y = FittingMatrix->data[1][j] ;
       verObj[j].z = FittingMatrix->data[2][j] ;
     }
 
   /* To return the transform from model to object */
   Mat_Inverse( Transform, tMdl2Obj ) ;


   /* free */ 
   FreeMatrix(ObjMatrix) ;
   FreeMatrix(MdlMatrix) ;
   FreeMatrix(FittingMatrix) ;
   FreeMatrix(TmpM1) ;
   FreeMatrix(TmpM2) ;
   FreeMatrix(InversedMtrx) ;
   FreeMatrix(Transform) ;
}


void AffineTransform3Dsurface( Fvector3d *ver, Matrix *tMdl2Obj,  int  MdlVer_Num )
{
   int i,k,j ;
   Matrix *verMatrix, *resMatrix ;


   /* create  metrices*/
   CreateMatrix(&verMatrix,    4, MdlVer_Num);
   CreateMatrix(&resMatrix,    4, MdlVer_Num);


   /* get verMatrix->data[4][MdlVer_Num] */
   for(j=0; j<MdlVer_Num; j++)
     {
       verMatrix->data[0][j] = ver[j].x ;
       verMatrix->data[1][j] = ver[j].y ;
       verMatrix->data[2][j] = ver[j].z ;
       verMatrix->data[3][j] = 1 ;
     }


   /* affine transformation */
   Mat_A_equal_BxC(resMatrix, tMdl2Obj, verMatrix) ;             /*A=BxC*/

   /* back to model variables */
   for(j=0; j<MdlVer_Num; j++)
     {
       ver[j].x = resMatrix->data[0][j] ;
       ver[j].y = resMatrix->data[1][j] ;
       ver[j].z = resMatrix->data[2][j] ;
     }
 

   /* free */ 
   FreeMatrix(verMatrix) ;
   FreeMatrix(resMatrix) ;
}


void Open_TrainingResults( Matrix *T1, Matrix *T2, float *T3, int SizeOfT3 )
{
  FILE *fp;
  int i, j ;
  double *TempT1, *TempT2 ;
  int T1m, T1n, T2m, T2n ;
  char filename[180] ;

  T1m = T1->height ;
  T1n = T1->width ;
   T2m = T2->height ;
   T2n = T2->width ;

  /* allocate space */
  TempT1 = (double *)calloc( T1m*T1n, sizeof(double) );
  TempT2 = (double *)calloc( T2m*T2n, sizeof(double) );


  
  sprintf(filename, "%sTraningResults.3D", MdlDirtory);    printf("training resulted file: %s\n", filename) ;  
  if((fp=fopen(filename,"rb"))==NULL)  
  {
   printf("cannot open file\n");
   return ;
  }

  fseek(fp,0L,SEEK_SET);

  fread( TempT1,  sizeof(double), T1m*T1n,  fp ) ; 
  fread( TempT2,  sizeof(double), T2m*T2n,  fp ) ;
  fread( T3,      sizeof(float),  SizeOfT3, fp ) ;

  fclose(fp);


  /* for T1 */
  for(i=0; i<T1m; i++)
   for(j=0; j<T1n; j++)
    T1->data[i][j] = TempT1[i*T1n+j] ; 
  /* for T2 */
  for(i=0; i<T2m; i++)
   for(j=0; j<T2n; j++)
    T2->data[i][j] = TempT2[i*T2n+j] ; 


  /* free */
  free(TempT1) ;
  free(TempT2) ;
}

/*int max_nbr=1 ;*/
void StatisticalMapping(Fvector3d *ver,  int  MdlVer_Num, float alpha)
{
  /* for statistcs*/
  Matrix *T1, *T2 ;
  float  *T3 ;
  int    k, i, s ;

  /* for object */
  char   filename[180];
  FILE  *fp;
  Fvector3d  *verMdl, *verObj ;
  /* sizes: */
  int PntNUMx3 ;
  float sum ;

  Matrix *tMdl2Obj ;
  /* create  metrices*/
  CreateMatrix(&tMdl2Obj,  4, 4);

  
/* To get model */
  sprintf(filename, "%sMDL.ver.norm.nbr", MdlDirtory);    printf("model file name: %s\n", filename) ;
  fp=myopen(filename,"r");
  fseek(fp,0,SEEK_END);
  MdlVer_Num = ftell(fp)/(6*4+(max_nbr+1)*4); /*48*/ 
  fclose(fp);
  
  verMdl = Fvector3dalloc1d(MdlVer_Num);

  fp=myopen(filename,"r");
   fread(verMdl, sizeof(Fvector3d), MdlVer_Num,fp);
  fclose(fp);


  /* detecmine the sizes */
  PntNUMx3 = 3*(int)(MdlVer_Num) ;


  /* apply for mem */
  CreateMatrix(&T1,   PntNUMx3,     CHNEIGENNUM);
  CreateMatrix(&T2,   CHNEIGENNUM, PntNUMx3);
  T3 = vectorSHEN(0, PntNUMx3-1) ;
  
  /* Get the trained data */
  Open_TrainingResults( T1, T2, T3, PntNUMx3 ) ;
  

  
  /* copy imput object, April 27, 2000 */
  verObj = Fvector3dalloc1d(MdlVer_Num);
  for(s=0; s<MdlVer_Num; s++)
    {
      verObj[s].x = ver[s].x ;  verObj[s].y = ver[s].y ;  verObj[s].z = ver[s].z ;
    }
  
  /* correcting */
  AlignmentObjectToModel( verMdl, ver, tMdl2Obj, MdlVer_Num ) ;
  Correct_3DsurfacebyT1T2T3(ver, MdlVer_Num, T1, T2, T3, alpha) ;
  AffineTransform3Dsurface( ver, tMdl2Obj,  MdlVer_Num ) ;
  
  /* This part is added on April 27, 2000, trying to fixing energy deduction problem in shape statistics method */
  AlignmentObjectToModel( verObj, ver, tMdl2Obj, MdlVer_Num ) ;



  /* statistics free */
  free(verMdl) ;
  
  FreeMatrix(T1) ; 
  FreeMatrix(T2) ;
  free_vectorSHEN(T3, 0, PntNUMx3-1) ; 
  
  FreeMatrix(tMdl2Obj) ;
}


void Correct_3DsurfacebyT1T2T3(Fvector3d *ver, int MdlVer_Num, Matrix *T1, Matrix *T2, float  *T3, float alpha)
{
  float  *Pout, *Pin, Rad, *TmpP ;
  int    i,k ;
  int    PntNUMx3=3*(int)(MdlVer_Num) ;

  /* to apply for vectors */ 
  Pout = vectorSHEN(0, PntNUMx3-1) ;
  Pin  = vectorSHEN(0, PntNUMx3-1) ;
  TmpP = vectorSHEN(0, PntNUMx3-1) ; /* for temporary use */

  
  for(i=0; i<(int)(MdlVer_Num); i++)
    {   
      Pin[i*3]   = (float)ver[i].x;        
      Pin[i*3+1] = (float)ver[i].y;
      Pin[i*3+2] = (float)ver[i].z;
    }
  
  /* correcting */
  Mat_times_Vector(TmpP, T2, Pin) ; 
  Mat_times_Vector(Pout, T1, TmpP) ; 
  for(k=0; k<PntNUMx3; k++)
    Pout[k] += T3[k] ;
  
  for(i=0; i<(int)(MdlVer_Num); i++)
    {   
      ver[i].x = (alpha*Pout[i*3]  + (1-alpha)*Pin[i*3] );        
      ver[i].y = (alpha*Pout[i*3+1]+ (1-alpha)*Pin[i*3+1]);
      ver[i].z = (alpha*Pout[i*3+2]+ (1-alpha)*Pin[i*3+2]);
    }
  
  /* free */
  free_vectorSHEN(Pout, 0, PntNUMx3-1) ; 
  free_vectorSHEN(Pin,  0, PntNUMx3-1) ; 
  free_vectorSHEN(TmpP, 0, PntNUMx3-1) ;
}
/****************************************************************************************************************/
/*********************************     Statistics     ***********************************************************/
/****************************************************************************************************************/



