/**
 * This program is based in the AdaptiveBrainMappingUsingGirdmesh_V2.5, however
 * the intention is to start eliminating redundant implementations. In particular
 * if a method has already been implemented in adaptivebrain.c then this should
 * just include the definitions.
 * 
 * At the moment however is a work in progress becuase it is still to be 
 * determined what are the real important differences, and wich ones are 
 * unnecessary.
 */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <strings.h>
#include "mvcd.h"
#include "cres.h"
#include "matrixSHEN.h"  /*by SHEN*/

#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    used before June 22 2004 */
unsigned char GMVN_EDGE=100;
#define GMCSFBG_EDGE  120  /* added on June 4, 2001*/
#define WMVN_EDGE     180
#define WMGM_EDGE     255

/*
typedef struct 
{ 
  unsigned char Edge; 
  unsigned char Tiss; 
  unsigned char Geom; 
  unsigned char VNvlm; 
  unsigned char CSFBG; 
} ImgAttribute ; 
*/ 

typedef struct 
{ 
  unsigned char Geom_UP; 
  unsigned char Geom_DN; 
  unsigned char VNvlm_UP; 
  unsigned char VNvlm_DN; 
  unsigned char CSFBG_UP; 
  unsigned char CSFBG_DN; 
} Threshold ; 


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 */
ImgAttribute *MdlVer_Fea, *ObjVer_Fea ;
Threshold  MdlThreshold, ObjThreshold ;
int        SmoothingTimes ; /* for edge preserving smoothing */
int        OverSmoothTimes=3 ; /* smoothing times, making deformation field oversmooth */ /* June 22 2004 */
int        BigVN ;        /* This is used to act "pull force" from the subject's ventrical, on the model's ventricle. */


/* 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 ;
float  XYZres ;

void MarcMain(int,char *[]);


void ShenMain(int argc,char *argv[]) ;
void show_usage_SHEN() ;
void AffinDeform3D(Fvector3d *MdlVer, Ivector3d *FixedMdlVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, Fvector3d ***DeformFld, ImgAttribute ***MdlImg, \
		   Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, ImgAttribute ***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( ImgAttribute ***ObjImg, Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder ) ;
void SaveCurrentWarpingResultOnOriginalObjImg( unsigned char ***ObjOriginalImg, Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder ) ;
void EstimatingScalingAndShiftOf3DSurfaceFromImg(Fvector3d *MdlVer, Ivector3d *FixedMdlVer, ImgAttribute *MdlVer_Fea, int MdlVer_Num, Fvector3d ***DeformFld, ImgAttribute ***ObjImg, int image_size, int z_size );
void SmoothDeformationField(Fvector3d ***DeformFld, int image_size, int z_size,  ImgAttribute ***MdlImg, int SmoothingTimes) ;
void SmoothDeformationFieldWithEdgePreserving( Ivector3d *FixedMdlVer, int MdlVer_Num, Fvector3d ***DeformFld, ImgAttribute ***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 ;
static int   search_resolution, last_order, AddMthdOfAvoidingContraction=NOTADD, EstimateTransformOrNot=NOTADD, StatisticsEmployedOrNot=NOTADD, YoungBrain=NNO, OutputTemporayResults=YYES, DfmSmthTimes,WrongSegVNasGM ;
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, ImgAttribute ***ObjImg, int image_size, int z_size) ;
void Select3DEdgePointsInModelTO1D(int *MdlVer_Num, ImgAttribute ***MdlImg, Fvector3d ***DeformFld, int image_size, int z_size) ;
void SearchTheNearestVertexForEachLandmark( Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, \
	 Fvector3d *MdlVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, int image_size, int z_size ) ;
void SearchTheNearestVertexForEachLandmark_NewFeb2002(Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, ImgAttribute ***ObjImg, \
Fvector3d *MdlVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, ImgAttribute ***MdlImg, Fvector3d ***DeformFld, \
int image_size, int z_size) ;
void SearchTheNearestVertexForEachLandmark_March2003(Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, ImgAttribute ***ObjImg, \
						     Fvector3d *MdlVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, Ivector3d *FixedMdlVer, ImgAttribute ***MdlImg, Fvector3d ***DeformFld,   int image_size, int z_size,  int max) ;




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,"wb");
  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], ImgAttribute ***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);
	
  /* 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].Tiss ;
  sprintf(fn, "%s_Tissue", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ;  

  /* 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].Edge ;
  sprintf(fn, "%s_Edge", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 

  /* Geometric */
  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].Geom ;
  sprintf(fn, "%s_Geometric", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 

  /* VN volume */
  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].VNvlm ;
  sprintf(fn, "%s_VNvolume", filename);
  WriteImg(fn,   TempImg,   image_size, z_size) ; 

  /* CSF/BG volume */
  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].CSFBG ;
  sprintf(fn, "%s_CSFBG", 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,"wb");
  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,"rb");
  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 ; /* it was directly 1.0 at June 18 2001 ~ Oct 23 2002; theoretically it should be 0. */
  if( SmoothingTimes>OverSmoothTimes ) alpha = 0.5 ; /* June 22, 2004 */
  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 = x/xres ;
	      Hood[t].y = y/yres ;
	      Hood[t].z = 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 = x/xres ;
	      Hood[t].y = y/yres ;
	      Hood[t].z = 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 = x/xres ;
	      Hood[t].y = y/yres ;
	      Hood[t].z = 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) ;
}


/* May 2001*/
void Get_WMedge_GmVnEdge_FromSegmentedImg(ImgAttribute ***Img)
{
 int i,j,k ;
 int x,y,z ;
 int flag, strength ;
 int flag_VN, flag_GM ;

 printf("\nfinding WM edges and GM+VN edges ...\n") ;


 strength = 1 ;
 
 /* reset edge map */
 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].Edge=0 ;

 /* WM edge map */
 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(Img[k][i][j].Tiss==WM)
	 {
	   flag_GM = 0 ;
	    if(Img[k+1][i][j].Tiss==GM) flag_GM++;
	    if(Img[k-1][i][j].Tiss==GM) flag_GM++;
	    if(Img[k][i+1][j].Tiss==GM) flag_GM++;
	    if(Img[k][i-1][j].Tiss==GM) flag_GM++;
	    if(Img[k][i][j+1].Tiss==GM) flag_GM++;
	    if(Img[k][i][j-1].Tiss==GM) flag_GM++;

	   flag_VN = 0 ;
	    if(Img[k+1][i][j].Tiss==VN) flag_VN++;
	    if(Img[k-1][i][j].Tiss==VN) flag_VN++;
	    if(Img[k][i+1][j].Tiss==VN) flag_VN++;
	    if(Img[k][i-1][j].Tiss==VN) flag_VN++;
	    if(Img[k][i][j+1].Tiss==VN) flag_VN++;
	    if(Img[k][i][j-1].Tiss==VN) flag_VN++;

	   /* determine, whether edge? If edge, which type */
	   if( flag_GM>flag_VN )
	     Img[k][i][j].Edge = WMGM_EDGE ;
	   if( flag_GM<=flag_VN && flag_VN!=0 )
	     Img[k][i][j].Edge = WMVN_EDGE ;
	 }

 /* June 4 2001: edge between GM and CSF/BG */
 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(Img[k][i][j].Tiss==GM)
	 {
	   flag = 0 ;
	    if(Img[k+1][i][j].Tiss<=CSF) flag++;
	    if(Img[k-1][i][j].Tiss<=CSF) flag++;
	    if(Img[k][i+1][j].Tiss<=CSF) flag++;
	    if(Img[k][i-1][j].Tiss<=CSF) flag++;
	    if(Img[k][i][j+1].Tiss<=CSF) flag++;
	    if(Img[k][i][j-1].Tiss<=CSF) flag++;
	   if( flag>=strength )
	     Img[k][i][j].Edge = GMCSFBG_EDGE ;
	 }

 /* edge between GM and VN */
 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(Img[k][i][j].Tiss==GM)
	 {
	   flag = 0 ;
	    if(Img[k+1][i][j].Tiss==VN) flag++;
	    if(Img[k-1][i][j].Tiss==VN) flag++;
	    if(Img[k][i+1][j].Tiss==VN) flag++;
	    if(Img[k][i-1][j].Tiss==VN) flag++;
	    if(Img[k][i][j+1].Tiss==VN) flag++;
	    if(Img[k][i][j-1].Tiss==VN) flag++;
	   if( flag>=strength )
	     Img[k][i][j].Edge = GMVN_EDGE ;
	 }

}


void Compute_GeometricFeatures(ImgAttribute ***Img, int scale, float Xres, float Yres, float Zres)
{
  int i,j,k, temp, tempk, tempi, tempj, t, l,m,n ;
  float degree ;
  int   NumDown, value, VN_value, CSFBG_value, XYscale, Zscale ;

  int       bubble_size, pixelNumInBubble;
  Fvector3d *bubble ;

  printf("Compute geometric features for each edge voxel.\n") ;
  
  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  = scale/Zres ;
  XYscale = 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].Edge==0)
	    {
	      Img[k][i][j].Geom  = 0 ; 
	      Img[k][i][j].VNvlm = 0 ; 
	      Img[k][i][j].CSFBG = 0 ; 
	    }

          /* limit computation candidates */
	  if(Img[k][i][j].Edge!=0 )
	    {
	      NumDown = 0 ;
	      VN_value = 0 ;
	      CSFBG_value = 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].Tiss!=WM )  NumDown++ ;
		  if( Img[k+n][i+l][j+m].Tiss==VN )  VN_value++ ;
		  if( Img[k+n][i+l][j+m].Tiss<=CSF)  CSFBG_value++ ;
		}
	      degree = ((float)NumDown/(float)pixelNumInBubble) ;
	      value = degree*255*1.2 ; /*degree*degree*255*/
	      if( value>255 ) value = 255 ;
	      Img[k][i][j].Geom = (unsigned char)value ;
	      /*printf("(%d,%d,%d)=%d\n", k, i, j, NumDown) ;*/

	      /* VN volume */
	      VN_value = VN_value*255*1.2/pixelNumInBubble ;
	      if( VN_value>255 ) VN_value=255 ;
	      Img[k][i][j].VNvlm = (unsigned char)VN_value ;

	      /* CSF/BG volume */
	      CSFBG_value = CSFBG_value*255*1.2/pixelNumInBubble ;
	      if( CSFBG_value>255 ) CSFBG_value=255 ;
	      Img[k][i][j].CSFBG = (unsigned char)CSFBG_value ;
	    }
	}

  /* free */
  free(bubble);
}


int EvaluationOfRegistration( ImgAttribute ***MdlImg, ImgAttribute ***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 ;
  int total ;

  total = 0 ;
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  x = i + DeformFld[k][i][j].x ;
	  y = j + DeformFld[k][i][j].y ;
	  z = k + DeformFld[k][i][j].z ;

	  if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
	    resImg = ObjImg[z][x][y].Tiss ;
	  else
	    resImg = 0 ;

	  if(resImg!=MdlImg[k][i][j].Tiss)
		  total ++ ;
	}

  /* save */
  sprintf(filename, "Diff.sample%d.%d", XYZres, fileOrder);    
  /* write the smoothed image */
  fp=myopen(filename,"wb");
  fprintf(fp, "Diff.sample%d.%d:  %d\n", XYZres, fileOrder, total);    
  fclose(fp);
}


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,"rb"))==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 = SurfVer[s].x + 0.5 ;
	y = SurfVer[s].y + 0.5 ;
	z = 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,"wb");
   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) ;
}




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







/* the following is edited by SHEN */
void ShenMain(int argc,char *argv[])
{
  unsigned char ***Img;
  int           Img_XY, Img_Z ;
  ImgAttribute  ***MdlImg ;
  ImgAttribute  ***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, TempVNvlm_DN ;


  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*/
  OutputTemporayResults = YYES ; /* Dec 5, 2003 */
  DfmSmthCoeff = 0.5;  /* Dec 19, 2003 */
  DfmSmthTimes = 1 ;   /* Dec 19, 2003 */
  ConfidenceOnLastDeformation = 0.0 ; /* Aug 2004 */
  WrongSegVNasGM = NNO ; /* Dec 2006 */

  char fullVersion[] = "$Rev: 95 $"; //This line will be automatically 
                                       //populated by svn and should be
                                       //of the form "$Rev: 95 $" else it could
                                       //cause a segmentation fault
  char *shortVersion = strchr(fullVersion,' ')+1; //Eliminate te word "$Rev: " 
  char *secondSpaceInVersion = strchr(shortVersion,' '); //Find the space after the number
  *secondSpaceInVersion ='\0';//Eliminate anything after the number, by zeroing the space
  printf("\n\nHammer version 1.0.%s!\n\n",shortVersion) ;

  if(argc<num)
    show_usage_SHEN() ;

  while((c=getopt_long(argc-2,argv+2,"R:l:r:i:s:a:b:m:tedc:D:FO:S:h:E:NBUX:YTp:C:w")) != -1)
    {
      switch(c)
	{
	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=atof(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);
	  if( SmoothingTimes>OverSmoothTimes )  /* June 22 2004 */
	    {
	      GMVN_EDGE = WMVN_EDGE;
	    }
	  printf("SmoothingTimes=%d, OverSmoothTimes=%d, GMVN_EDGE=%d, WMVN_EDGE=%d\n", SmoothingTimes, OverSmoothTimes, GMVN_EDGE, WMVN_EDGE) ;
	  break;
	case 'N':
	  UsingForcesFromObject=NNO ;
	  break;
	case 'B':
	  BigVN = YYES ;
	  break;

	case 'U':
	  InitialUpdate = YYES ;
	  break;
	case 'X':
	  Img_XY=atoi(optarg);
	  break;

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

	case 'T':
	  OutputTemporayResults = NNO ;   /*Dec 5, 2003*/
	  break;

	case 'p':
	  sscanf(optarg, "%f,%d", &DfmSmthCoeff, &DfmSmthTimes) ;   /*Dec 19, 2003*/
	  break;

	case 'C':
	  ConfidenceOnLastDeformation=atof(optarg) ;
	  break ;

	case 'w':
	  WrongSegVNasGM = YYES ;   /*Dec 2006*/
	  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*/
  if( OutputTemporayResults==NNO ) printf(" Will not output temporary warping results.\n") ;
  printf(" DfmSmthCoeff=%f, DfmSmthTimes=%d\n\n", DfmSmthCoeff, DfmSmthTimes) ;
  printf(" ConfidenceOnLastDeformation=%f\n", ConfidenceOnLastDeformation) ; 
  printf(" WrongSegVNasGM=%d\n", WrongSegVNasGM) ;


  /***** Model image, segmented *****/
     /*Img_XY = 256 ;*/	 
     //sprintf(filename, "%s%s", MdlDirtory, argv[1]);    printf("\n\nmodel image : %s\n", filename) ;  
     sprintf(filename, "%s", argv[1]);    printf("\n\nmodel image : %s\n", filename) ;  
     fp=myopen(filename,"rb");
     fseek(fp,0,SEEK_END);
     Img_Z=ftell(fp)/(Img_XY*Img_XY);
     rewind(fp);
     
     Img = UCalloc3d(Img_XY,Img_XY,Img_Z);
     
     for(k=0;k<Img_Z;k++)
       for(i=0;i<Img_XY;i++)
	 fread(Img[k][i],1,Img_XY,fp);
     fclose(fp);
     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) ;

     MdlImg    = ImgAttributealloc3d(image_size,image_size,z_size); 
     DeformFld = Fvector3dalloc3d(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++)
	 {
	   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) ;
     
     /* get its edge map, geometric features */
     Get_WMedge_GmVnEdge_FromSegmentedImg(MdlImg) ;
     scale = 7*(1./XYZres) ; if(scale<3) scale=3; printf("scale=%d\n", scale) ;
     Compute_GeometricFeatures(MdlImg, scale, 1., 1., 1.5) ;

     WriteImgAttribute("MdlImg", MdlImg, image_size, z_size) ;
     
     /* Select edge points in the model image to be focused */
     MdlThreshold.Geom_UP = 0.9*255 ;
     MdlThreshold.Geom_DN = 0.4*255 ;
     MdlThreshold.VNvlm_UP = 255/12 ;
     MdlThreshold.VNvlm_DN = 170 ; /* ~65% added on Dec 2006 */
     MdlThreshold.CSFBG_UP = 255/4 ;
     Select3DEdgePointsInModelTO1D( &MdlVer_Num, MdlImg, DeformFld, image_size, z_size) ; /* get  MdlVer, FixedMdlVer, MdlVer_Fea*/

     
     
  /***** Object image, segmented  *****/
     /*Img_XY = 256 ;	 */
     fp=myopen(argv[2],"rb");
     fseek(fp,0,SEEK_END);
     Img_Z=ftell(fp)/(Img_XY*Img_XY);
     rewind(fp);
     
     Img = UCalloc3d(Img_XY,Img_XY,Img_Z);
     
     for(k=0;k<Img_Z;k++)
       for(i=0;i<Img_XY;i++)
	 fread(Img[k][i],1,Img_XY,fp);
     fclose(fp);
     printf("\n\n^^^^ Segmented object image input!\n") ;
     
     /*image_size = Img_XY/XYZres ;
       z_size = Img_Z/XYZres ; */
     
     ObjImg = ImgAttributealloc3d(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++)
	       ObjImg[k][i][j].Tiss=Img[(int)(k*XYZres)][(int)(i*XYZres)][(int)(j*XYZres)] ;
	 else
	   for(i=0; i<image_size; i++)
	     for(j=0; j<image_size; j++)
	       ObjImg[k][i][j].Tiss=0;
       }
     UCfree3d(Img, Img_Z, Img_XY) ;
     
     /* get its edge map, geometric features */
     Get_WMedge_GmVnEdge_FromSegmentedImg(ObjImg) ;
     scale = 7*(1./XYZres) ; if(scale<3) scale=3; printf("scale=%d\n", scale) ;
     Compute_GeometricFeatures(ObjImg, scale, 1., 1., 1.5) ;

     WriteImgAttribute("ObjImg", ObjImg, image_size, z_size) ;
     
     /* Select edge points in the object image to be used as landmarks */
     Select3DEdgePointsInObjectTO1D(&ObjVer_Num, ObjImg, image_size, z_size) ; /* got ObjVer, ObjVer_Fea*/
     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,"rb");
         //fp=myopen(argv[3],"rb");
	 fseek(fp,0,SEEK_END);
	 Img_Z=ftell(fp)/(Img_XY*Img_XY);
	 rewind(fp);
            Img = UCalloc3d(Img_XY,Img_XY,Img_Z);
	 for(k=0;k<Img_Z;k++)
	   for(i=0;i<Img_XY;i++)
	     fread(Img[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]=Img[(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(Img, 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 */
     ratio_iteration=0;
     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) ;
	     
	     MdlThreshold.Geom_UP = 0.9*(1-ratio_iteration)*255 ;
	     MdlThreshold.Geom_DN = 0.4*(2.*ratio_iteration)*255 ;
	     MdlThreshold.VNvlm_UP = 255/4 ;
               TempVNvlm_DN = 170+(255-170)*ratio_iteration*1.5 ; /* ~65% added on Dec 2006 */
               if(TempVNvlm_DN>255)  MdlThreshold.VNvlm_DN = 255 ;
               else                  MdlThreshold.VNvlm_DN = TempVNvlm_DN ;
	     MdlThreshold.CSFBG_UP = 255/4 ;
	     Select3DEdgePointsInModelTO1D( &MdlVer_Num, MdlImg, DeformFld, image_size, z_size) ; /* get  MdlVer, FixedMdlVer, MdlVer_Fea*/

	     forceByFocusedPntInObj = Fvector3dalloc1d(MdlVer_Num); 
	     multiple               = Ialloc1d(MdlVer_Num); 
	   }

	 /* for search match from object to Mdl */
	 pixelNumIn_MyHood = 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 = 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) ;
	 
         printf("Iteration %d out of %d     ", i+1, iter);

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

	 /* save temporary results */
	 if( OutputTemporayResults==YYES || i>=iter-1 )
	   {
	     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) ;
	 
	 /*EvaluationOfRegistration( MdlImg, ObjImg, DeformFld, image_size, z_size, (int)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: nucleus3 <model.segmented.image> <obj.segmented.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 -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 -T                     : Don't output temporary warping results\n\
\t -p <float,int>         : smoothing factor for deformation fields, range [0,1], 1->smoothest, (default: 0.50)), \n\
\t                          times of smoothing deformation fields (default: 1), \n\
\t -C <float>             : Confidence on deformation field calculated in the last resolution (0~1.0); default: 0.0\n\
\t -w                     : wrong tissue segmentation, segment VN as GM (or GM inside VN)\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 = 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 ;
int INTERPseg=NNO ;
void SaveCurrentWarpingResultOnSegmentedObj( ImgAttribute ***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].Tiss ;


  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  if(INTERPseg==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) ;

	      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 = i + DeformFld[k][i][j].x + 0.5 ;
	      y = j + DeformFld[k][i][j].y + 0.5 ;
	      z = 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 = i + DeformFld[k][i][j].x + 0.5 ;
	      y = j + DeformFld[k][i][j].y + 0.5 ;
	      z = 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 SimilarityBetweenFocusedPointsInMdlObjImgs( ImgAttribute *ObjVer_Fea, int l, ImgAttribute *MdlVer_Fea, int s)
{
  float Degree ;

  Degree = 0 ;
  if( ObjVer_Fea[l].Edge==MdlVer_Fea[s].Edge )
  {
	Degree  =  1.0-fabs(MdlVer_Fea[s].Geom -ObjVer_Fea[l].Geom)/255.0 ;
	Degree *= (1.0-fabs(MdlVer_Fea[s].VNvlm-ObjVer_Fea[l].VNvlm)/255.0) ;
	Degree *= (1.0-fabs(MdlVer_Fea[s].CSFBG-ObjVer_Fea[l].CSFBG)/255.0) ;
	Degree *= (1.0-fabs(MdlVer_Fea[s].Tiss -ObjVer_Fea[l].Tiss)/255.0) ;
  }

  return Degree ;
}

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

  Degree = 0 ;
  if( ObjImg[z][x][y].Edge==MdlVer_Fea[s].Edge )
  {
        Degree  =  1.0-fabs(MdlVer_Fea[s].Geom -ObjImg[z][x][y].Geom)/255.0 ;
	Degree *= (1.0-fabs(MdlVer_Fea[s].VNvlm-ObjImg[z][x][y].VNvlm)/255.0) ;
	Degree *= (1.0-fabs(MdlVer_Fea[s].CSFBG-ObjImg[z][x][y].CSFBG)/255.0) ;
	Degree *= (1.0-fabs(MdlVer_Fea[s].Tiss -ObjImg[z][x][y].Tiss)/255.0) ;
  }

  return Degree ;
}

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

  Degree = 0 ;
  if( ObjImg[z][x][y].Edge==MdlImg[k][i][j].Edge )
  {
        Degree  =  1.0-fabs(MdlImg[k][i][j].Geom -ObjImg[z][x][y].Geom)/255.0 ;
	Degree *= (1.0-fabs(MdlImg[k][i][j].VNvlm-ObjImg[z][x][y].VNvlm)/255.0) ;
	Degree *= (1.0-fabs(MdlImg[k][i][j].CSFBG-ObjImg[z][x][y].CSFBG)/255.0) ;
	Degree *= (1.0-fabs(MdlImg[k][i][j].Tiss -ObjImg[z][x][y].Tiss)/255.0) ;
  }

  return Degree ;
}


/* smoothing ... */
void SmoothDeformationField_FastImplementation(Fvector3d ***DeformFld, int image_size, int z_size)
{
  int i,j,k,n ;
  int x,y,z ;
  int        smth_size, temp_num ;
  Fvector3d  nbr_tot, nbr_average ;
  float      LocalRatio ;


  printf("smoothing ...\n") ;
  smth_size = 3 ; /* 3 */


  LocalRatio = 0.5 ;
  /* 1-D smoothing on y dimension */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      {
	for(j=0;j<image_size;j++)
	  {
	    if( j==0 )
	      {
		/* need to calcaulate total values around the first point */
		nbr_tot.x=0; nbr_tot.y=0; nbr_tot.z=0; temp_num=0;
		for(n=-smth_size;n<=smth_size;n++)
		  {
		    y = j + n ;
		    if( y<image_size && y>=0 )
		      {
			nbr_tot.x += DeformFld[k][i][y].x;
			nbr_tot.y += DeformFld[k][i][y].y;
			nbr_tot.z += DeformFld[k][i][y].z;
			
			temp_num++ ;
		      }
		  }
		if(temp_num>0)
		  {
		    nbr_average.x = nbr_tot.x/temp_num ;
		    nbr_average.y = nbr_tot.y/temp_num ;
		    nbr_average.x = nbr_tot.z/temp_num ;
		  }
	      }
	    else
	      {
		/* total values around the current point is obtained ...  */
		y = j-1 - smth_size ;
		if( y<image_size && y>=0 )
		  {
		    nbr_tot.x -= DeformFld[k][i][y].x;
		    nbr_tot.y -= DeformFld[k][i][y].y;
		    nbr_tot.z -= DeformFld[k][i][y].z;
		    
		    temp_num-- ;
		  }
		
		y = j + smth_size ;
		if( y<image_size && y>=0 )
		  {
		    nbr_tot.x += DeformFld[k][i][y].x;
		    nbr_tot.y += DeformFld[k][i][y].y;
		    nbr_tot.z += DeformFld[k][i][y].z;
		    
		    temp_num++ ;
		  }
		
		if(temp_num>0)
		  {
		    nbr_average.x = nbr_tot.x/temp_num ;
		    nbr_average.y = nbr_tot.y/temp_num ;
		    nbr_average.x = nbr_tot.z/temp_num ;
		  }
	      }  
	    	    
	    DeformFld[k][i][j].x += (nbr_average.x - DeformFld[k][i][j].x)*LocalRatio ;
	    DeformFld[k][i][j].y += (nbr_average.y - DeformFld[k][i][j].y)*LocalRatio ; 
	    DeformFld[k][i][j].z += (nbr_average.z - DeformFld[k][i][j].z)*LocalRatio ; 
	  }
      }
  
  
  /* 1-D smoothing on x dimension */
  for(k=0;k<z_size;k++)
    for(j=0;j<image_size;j++)
      {
	for(i=0;i<image_size;i++)
	  {
	    if( i==0 )
	      {
		/* need to calcaulate total values around the first point */
		nbr_tot.x=0; nbr_tot.y=0; nbr_tot.z=0; temp_num=0;
		for(n=-smth_size;n<=smth_size;n++)
		  {
		    x = i + n ;
		    if( x<image_size && x>=0 )
		      {
			nbr_tot.x += DeformFld[k][x][j].x;
			nbr_tot.y += DeformFld[k][x][j].y;
			nbr_tot.z += DeformFld[k][x][j].z;
			
			temp_num++ ;
		      }
		  }
		if(temp_num>0)
		  {
		    nbr_average.x = nbr_tot.x/temp_num ;
		    nbr_average.y = nbr_tot.y/temp_num ;
		    nbr_average.x = nbr_tot.z/temp_num ;
		  }
	      }
	    else
	      {
		/* total values around the current point is obtained ...  */
		x = i-1 - smth_size ;
		if( x<image_size && x>=0 )
		  {
		    nbr_tot.x -= DeformFld[k][x][j].x;
		    nbr_tot.y -= DeformFld[k][x][j].y;
		    nbr_tot.z -= DeformFld[k][x][j].z;
		    
		    temp_num-- ;
		  }
		
		x = i + smth_size ;
		if( x<image_size && x>=0 )
		  {
		    nbr_tot.x += DeformFld[k][x][j].x;
		    nbr_tot.y += DeformFld[k][x][j].y;
		    nbr_tot.z += DeformFld[k][x][j].z;
		    
		    temp_num++ ;
		  }
		
		if(temp_num>0)
		  {
		    nbr_average.x = nbr_tot.x/temp_num ;
		    nbr_average.y = nbr_tot.y/temp_num ;
		    nbr_average.x = nbr_tot.z/temp_num ;
		  }
	      }
	    
	    DeformFld[k][i][j].x += (nbr_average.x - DeformFld[k][i][j].x)*LocalRatio ;
	    DeformFld[k][i][j].y += (nbr_average.y - DeformFld[k][i][j].y)*LocalRatio ; 
	    DeformFld[k][i][j].z += (nbr_average.z - DeformFld[k][i][j].z)*LocalRatio ; 
	  }
      }
  
  
  /* 1-D smoothing on z dimension */
  for(i=0;i<image_size;i++)
    for(j=0;j<image_size;j++)
      {
	for(k=0;k<z_size;k++)
	  {
	    if( k==0 )
	      {
		/* need to calcaulate total values around the first point */
		nbr_tot.x=0; nbr_tot.y=0; nbr_tot.z=0; temp_num=0;
		for(n=-smth_size;n<=smth_size;n++)
		  {
		    z = k + n ;
		    if( z<z_size && z>=0 )
		      {
			nbr_tot.x += DeformFld[z][i][j].x;
			nbr_tot.y += DeformFld[z][i][j].y;
			nbr_tot.z += DeformFld[z][i][j].z;
			
			temp_num++ ;
		      }
		  }
		if(temp_num>0)
		  {
		    nbr_average.x = nbr_tot.x/temp_num ;
		    nbr_average.y = nbr_tot.y/temp_num ;
		    nbr_average.x = nbr_tot.z/temp_num ;
		  }
	      }
	    else
	      {
		/* total values around the current point is obtained ...  */
		z = k-1 - smth_size ;
		if( z<z_size && z>=0 )
		  {
		    nbr_tot.x -= DeformFld[z][i][j].x;
		    nbr_tot.y -= DeformFld[z][i][j].y;
		    nbr_tot.z -= DeformFld[z][i][j].z;
		    
		    temp_num-- ;
		  }
		
		z = k + smth_size ;
		if( z<z_size && z>=0 )
		  {
		    nbr_tot.x += DeformFld[z][i][j].x;
		    nbr_tot.y += DeformFld[z][i][j].y;
		    nbr_tot.z += DeformFld[z][i][j].z;
		    
		    temp_num++ ;
		  }
		
		if(temp_num>0)
		  {
		    nbr_average.x = nbr_tot.x/temp_num ;
		    nbr_average.y = nbr_tot.y/temp_num ;
		    nbr_average.x = nbr_tot.z/temp_num ;
		  }
	      }
	    
	    DeformFld[k][i][j].x += (nbr_average.x - DeformFld[k][i][j].x)*LocalRatio ;
	    DeformFld[k][i][j].y += (nbr_average.y - DeformFld[k][i][j].y)*LocalRatio ; 
	    DeformFld[k][i][j].z += (nbr_average.z - DeformFld[k][i][j].z)*LocalRatio ; 
	  }
      }

  /* assign ... */  
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	{
	  /* 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") ;
}

void SmoothDeformationField(Fvector3d ***DeformFld, int image_size, int z_size,  ImgAttribute ***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].Edge!=GMVN_EDGE && MdlImg[k][i][j].Edge!=WMVN_EDGE  ) 
	    {
	      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, ImgAttribute ***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].Tiss>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++ ;

			/*Jun 17 2004*/ /* to increase the weights of VN boundary points */
			if( SmoothingTimes>1 && (MdlImg[z][x][y].Edge==GMVN_EDGE || MdlImg[z][x][y].Edge==WMVN_EDGE) ) 
			  {
			    nbr_tot.x += DeformFld[z][x][y].x ; /*+ DeformFld[z][x][y].x ; */
			    nbr_tot.y += DeformFld[z][x][y].y ; /*+ DeformFld[z][x][y].y ; */
			    nbr_tot.z += DeformFld[z][x][y].z ; /*+ DeformFld[z][x][y].z ; */
			    
			    temp_num++ ; /*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].Edge!=GMVN_EDGE && MdlImg[k][i][j].Edge!=WMVN_EDGE  ) 
		  {		
		    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, ImgAttribute *MdlVer_Fea, Fvector3d ***DeformFld, ImgAttribute ***MdlImg, \
		   Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, ImgAttribute ***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 ;
  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], 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.8*(1-ratio_iteration)+0.001 ; /* 0.8 */ printf("SMR_THR=%f\n", SMR_THR) ;
  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) ;
    /*SearchTheNearestVertexForEachLandmark_NewFeb2002(ObjVer, ObjVer_Num, ObjVer_Fea, ObjImg,   MdlVer, MdlVer_Num, MdlVer_Fea, MdlImg, DeformFld, image_size, z_size) ; used before March 2003*/
    /*SearchTheNearestVertexForEachLandmark( ObjVer, ObjVer_Num, ObjVer_Fea, MdlVer, MdlVer_Num, MdlVer_Fea, image_size, z_size ) ;*/


  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 && (SimilarityBetweenFeaturesInImgAndFeaturesInMdl(ObjImg,x,y,z,MdlVer_Fea,s)>SMR_THR || (BigVN==YYES && MdlVer_Fea[s].VNvlm>0 && ObjImg[z][x][y].VNvlm>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].Edge==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].Edge>0 ) /* 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\n", s, MaxDegree) ;
      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, ImgAttribute ***MdlImg, Fvector3d ***DeformFld, int image_size, int z_size)
{
  int   i, j, k, c ;
  int   Geom_DownsUp ;
  unsigned char ***FeaPoints ;
  FILE  *fp;


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

  Geom_DownsUp = MdlThreshold.Geom_DN/1.1 ; /*1.5*/

  (*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].Edge!=0 && (MdlImg[k][i][j].Geom>MdlThreshold.Geom_UP || (MdlImg[k][i][j].Geom<MdlThreshold.Geom_DN && MdlImg[k][i][j].Geom>Geom_DownsUp) || MdlImg[k][i][j].VNvlm>MdlThreshold.VNvlm_UP || MdlImg[k][i][j].CSFBG>MdlThreshold.CSFBG_UP) && (MdlImg[k][i][j].VNvlm<=MdlThreshold.VNvlm_DN || WrongSegVNasGM==NNO) ) 
	  (*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 = ImgAttributealloc1d(*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].Edge!=0 && (MdlImg[k][i][j].Geom>MdlThreshold.Geom_UP || (MdlImg[k][i][j].Geom<MdlThreshold.Geom_DN && MdlImg[k][i][j].Geom>Geom_DownsUp) || MdlImg[k][i][j].VNvlm>MdlThreshold.VNvlm_UP || MdlImg[k][i][j].CSFBG>MdlThreshold.CSFBG_UP) && (MdlImg[k][i][j].VNvlm<=MdlThreshold.VNvlm_DN || WrongSegVNasGM==NNO) ) 
	  {
	    /* 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].Edge  = MdlImg[k][i][j].Edge;   /* edge */
	    MdlVer_Fea[c].Tiss  = MdlImg[k][i][j].Tiss;   /* intensity */
	    MdlVer_Fea[c].Geom  = MdlImg[k][i][j].Geom;   /* geometric */
	    MdlVer_Fea[c].VNvlm = MdlImg[k][i][j].VNvlm;  /* VN volume */
	    MdlVer_Fea[c].CSFBG = MdlImg[k][i][j].CSFBG;  /* CSF/BG volume */

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

  /* saving ... */
  //  fp=myopen("MdlImg_FocuedEdges","wb"); 
  //    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, ImgAttribute ***ObjImg, int image_size, int z_size)
{
  int   i, j, k, c ;
  float alpha, beta;
  int   Geom_DownsUp  ;
  unsigned char ***FeaPoints ;
  FILE  *fp;

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

  alpha = 0.9 ; /* */
  beta  = 0.4;

  ObjThreshold.Geom_UP = alpha*255 ;
  ObjThreshold.Geom_DN = beta*255 ;
  Geom_DownsUp = ObjThreshold.Geom_DN/1.1 ; /*1.5*/
  ObjThreshold.VNvlm_UP = 255/4 ;
  ObjThreshold.CSFBG_UP = 255/4 ;
  ObjThreshold.VNvlm_DN = 170 ; /*about 65% Dec 2006 */


  (*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].Edge!=0 && (ObjImg[k][i][j].Geom>ObjThreshold.Geom_UP || (ObjImg[k][i][j].Geom<ObjThreshold.Geom_DN && ObjImg[k][i][j].Geom>Geom_DownsUp) || ObjImg[k][i][j].VNvlm>ObjThreshold.VNvlm_UP || ObjImg[k][i][j].CSFBG>ObjThreshold.CSFBG_UP) && (ObjImg[k][i][j].VNvlm<=ObjThreshold.VNvlm_DN || WrongSegVNasGM==NNO) ) 
	  (*ObjVer_Num)+=1 ;
  printf("There are %d focued edge points in the object image!\n", *ObjVer_Num);

  ObjVer     = Ivector3dalloc1d(*ObjVer_Num);
  ObjVer_Fea = ImgAttributealloc1d(*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].Edge!=0 && (ObjImg[k][i][j].Geom>ObjThreshold.Geom_UP || (ObjImg[k][i][j].Geom<ObjThreshold.Geom_DN && ObjImg[k][i][j].Geom>Geom_DownsUp) || ObjImg[k][i][j].VNvlm>ObjThreshold.VNvlm_UP || ObjImg[k][i][j].CSFBG>ObjThreshold.CSFBG_UP) && (ObjImg[k][i][j].VNvlm<=ObjThreshold.VNvlm_DN || WrongSegVNasGM==NNO) ) 
	  {
		/* position */
	    ObjVer[c].x = i ;
	    ObjVer[c].y = j ;
	    ObjVer[c].z = k ;

		/* attributes  */
	    ObjVer_Fea[c].Edge  = ObjImg[k][i][j].Edge;   /* edge */
	    ObjVer_Fea[c].Tiss  = ObjImg[k][i][j].Tiss;   /* intensity */
	    ObjVer_Fea[c].Geom  = ObjImg[k][i][j].Geom;   /* geometric */
	    ObjVer_Fea[c].VNvlm = ObjImg[k][i][j].VNvlm;  /* VN volume */
	    ObjVer_Fea[c].CSFBG = ObjImg[k][i][j].CSFBG;  /* CSF/BG volume */

	    c ++ ;

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

  /* saving ... */
  //  fp=myopen("ObjImg_FocuedEdges","wb"); 
  //    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, ImgAttribute *ObjVer_Fea, ImgAttribute ***ObjImg, \
Fvector3d *MdlVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, Ivector3d *FixedMdlVer, ImgAttribute ***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, real_size, SmplStep, incre, dfm_s ;

   /* 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 = px + SubjNb.x ; /*+ MyHood[t].x ;*/
	  vy = py + SubjNb.y ; /*+ MyHood[t].y ;*/
	  vz = 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)
		{
		  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].Edge==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].Edge>0 )
				      {
					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 */
  free(nb_pnt) ;
  Ifree3d(VerticesImg, z_size, image_size) ;
}


void SearchTheNearestVertexForEachLandmark_NewFeb2002(Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, ImgAttribute ***ObjImg, \
Fvector3d *MdlVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, ImgAttribute ***MdlImg, Fvector3d ***DeformFld, \
int image_size, int z_size) 
{
  int        i, j, k, l ;
  int        ***VerticesImg, x, y, z, r, t, s ;
  Ivector3d  ObjCurrentVer, MdlCurrentVer ;
  int        Status, StepDesign ;
  float      Verdegree ;

  /* new */
  ImgAttribute ***DeformedMdlImg ;
  int        ObjSubVRad, ObjSubVNum ;
  Ivector3d  *ObjSubV ;
  int        real_size, n ;
  float      SubVdegree, CurrentDegree, BestSimilarity ;
  Ivector3d  Obj_index, Dfm_index ;


  /* subvolume used to compare the similarity of attribute vectors */ 
  ObjSubVRad = 1 ; /* 2*/
  ObjSubV = Ivector3dalloc1d((ObjSubVRad*2+1)*(ObjSubVRad*2+1)*(ObjSubVRad*2+1)) ;
  calculate_hood_byIncreasingRadius(ObjSubV, (ObjSubVRad*2+1), &ObjSubVNum, 1., 1., 1.) ; 


  /************************* model image ********************/
  /* get memory for saving attributes in the deformed image */
  DeformedMdlImg  = ImgAttributealloc3d(image_size,image_size,z_size); 

  /* get attributed for the deformed model image */
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      for(j=0;j<image_size;j++)
	{
         x = i + DeformFld[k][i][j].x + 0.5 ;
         y = j + DeformFld[k][i][j].y + 0.5 ;
         z = k + DeformFld[k][i][j].z + 0.5 ;

         if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
         {
	    DeformedMdlImg[z][x][y].Edge  = MdlImg[k][i][j].Edge;  /* edge */
	    DeformedMdlImg[z][x][y].Tiss  = MdlImg[k][i][j].Tiss;  /* intensity */
	    DeformedMdlImg[z][x][y].Geom  = MdlImg[k][i][j].Geom;  /* geometric */
	    DeformedMdlImg[z][x][y].VNvlm = MdlImg[k][i][j].VNvlm; /* VN volume */
	    DeformedMdlImg[z][x][y].CSFBG = MdlImg[k][i][j].CSFBG; /* CSF/BG volume */
          }
       }

  /************************* Currently Focused Vertices ********************/
  /* For saving searching time, we insert current MdlVer to an image */
  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 ;
    }


  /* begin */
  printf("Current pixelNumIn_MyHood=%d\n", pixelNumIn_MyHood) ;
  StepDesign = ObjVer_Num/20000+1 ; printf("StepDesign=%d\n", StepDesign) ; 
  for(l=0; l<ObjVer_Num; l+=StepDesign)
  {
      if( l%2000==0 ) printf("finished: %f\n", (float)l/(float)ObjVer_Num) ; 

      ObjCurrentVer.x = ObjVer[l].x ;
      ObjCurrentVer.y = ObjVer[l].y ;
      ObjCurrentVer.z = ObjVer[l].z ;	  

      Status = FFAIL ;    
      for( t=0; t<pixelNumIn_MyHood && Status==FFAIL; t+=(1+ObjSubVRad) )
	{
	  MdlCurrentVer.x = ObjCurrentVer.x + MyHood[t].x ;
	  MdlCurrentVer.y = ObjCurrentVer.y + MyHood[t].y ;
	  MdlCurrentVer.z = ObjCurrentVer.z + MyHood[t].z ;	  

	  if( MdlCurrentVer.x<image_size && MdlCurrentVer.y<image_size && MdlCurrentVer.z<z_size && MdlCurrentVer.x>=0 && MdlCurrentVer.y>=0 && MdlCurrentVer.z>=0)
	    {
	      s = VerticesImg[MdlCurrentVer.z][MdlCurrentVer.x][MdlCurrentVer.y] ;
	      
	      if( s>=0 /*&& ObjCurrentVer.x<image_size && ObjCurrentVer.y<image_size && ObjCurrentVer.z<z_size && ObjCurrentVer.x>=0 && ObjCurrentVer.y>=0 && ObjCurrentVer.z>=0  Feb 26, 2002*/ )
		{
		  Verdegree = SimilarityBetweenFocusedPointsInMdlObjImgs(ObjVer_Fea,l, MdlVer_Fea,s) ;
	
              /* To see whether Obj Ver finds a similar Mdl Ver */	  
		  if( Verdegree>SMR_THR || (BigVN==YYES && MdlVer_Fea[s].VNvlm>0 && ObjVer_Fea[l].VNvlm>0))
		    {
		      /* Then check whether their subvolumes are similar */
		      SubVdegree = 0 ;
		      real_size = 0 ;
		      for(n=0; n<ObjSubVNum; n+=2) /* n++, Feb 26, 2002 */
			{ 
   			  /* Coordinate In Object */
			  Obj_index.x = ObjCurrentVer.x + ObjSubV[n].x ;
			  Obj_index.y = ObjCurrentVer.y + ObjSubV[n].y ;
			  Obj_index.z = ObjCurrentVer.z + ObjSubV[n].z ;
			  
   			  /* Coordinate In Deformed Mdl */
			  Dfm_index.x = MdlCurrentVer.x + ObjSubV[n].x ;
			  Dfm_index.y = MdlCurrentVer.y + ObjSubV[n].y ;
			  Dfm_index.z = MdlCurrentVer.z + ObjSubV[n].z ;

			  if(Obj_index.x<image_size && Obj_index.y<image_size && Obj_index.z<z_size && Obj_index.x>=0 && Obj_index.y>=0 && Obj_index.z>=0 && Dfm_index.x<image_size && Dfm_index.y<image_size && Dfm_index.z<z_size && Dfm_index.x>=0 && Dfm_index.y>=0 && Dfm_index.z>=0)
			    {
			      CurrentDegree = SimilarityBetweenFeaturesInMdlAndObjImgs( ObjImg, Obj_index.x, Obj_index.y, Obj_index.z, DeformedMdlImg, Dfm_index.x, Dfm_index.y, Dfm_index.z ) ;
			      
			      real_size ++ ;
			      if( DeformedMdlImg[Dfm_index.z][ Dfm_index.x][ Dfm_index.y].Edge>0 ) /* June 6, 2001*/
				{
				  CurrentDegree *= 1.2 ;
				  real_size += (1.2-1.0) ;
				}
			    }
			  else
			    CurrentDegree = 0 ;
			  
			  SubVdegree += CurrentDegree ; 
			  
			  /* to predict whether */
			  BestSimilarity = (SubVdegree + (ObjSubVNum-1-n))/( real_size + (ObjSubVNum-1-n)) ;
			  if( BestSimilarity<SMR_THR )  break ;
			}
		      if(real_size>0) 
			SubVdegree = SubVdegree/real_size ; 
		      
		      if( SubVdegree>SMR_THR || (SubVdegree>0 && BigVN==YYES && ObjVer_Fea[l].VNvlm>0))
			{
			  forceByFocusedPntInObj[s].x -= MyHood[t].x ;
			  forceByFocusedPntInObj[s].y -= MyHood[t].y ;
			  forceByFocusedPntInObj[s].z -= MyHood[t].z ;
			  multiple[s] += 1 ;
			  
			  Status = OOK ;
			}
		      if( l%100==0 && Status==OOK ) printf("voxel searched in the %d-th neighbor\n", t) ;
		    }
		}
	    }
	}
  }


  /* normalization */
  real_size = 0 ;
  for( i=0; i<MdlVer_Num; i++)
    if( multiple[i]>0 )
      {
	real_size ++ ;	
	/*printf("i=%d num=%d\n", i,multiple[i]) ;*/
	forceByFocusedPntInObj[i].x /= multiple[i] ;
	forceByFocusedPntInObj[i].y /= multiple[i] ;
	forceByFocusedPntInObj[i].z /= multiple[i] ;
      }
  printf("\n\nTotal %f percent model vertices having forces from object.\n\n",  (float)real_size/(float)MdlVer_Num*100.0 ) ;
  
  /* free */
  free(ObjSubV) ;
  Ifree3d(VerticesImg, z_size, image_size) ;
  ImgAttributefree3d(DeformedMdlImg, z_size, image_size) ;
}


void SearchTheNearestVertexForEachLandmark( Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, \
	 Fvector3d *MdlVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, int image_size, int z_size )
{
  int        i, j, k, l ;
  int        ***VerticesImg, x, y, z, r, t, px, py, pz, vx, vy, vz, s ;
  int        Status, StepDesign ;
  float      degree ;


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

  StepDesign = ObjVer_Num/20000+1 ; printf("StepDesign=%d\n", StepDesign) ; 
  for(l=0; l<ObjVer_Num; l+=StepDesign)
    {
      px = ObjVer[l].x ;
      py = ObjVer[l].y ;
      pz = ObjVer[l].z ;	  

      Status = FFAIL ;    
      for(t=0; t<pixelNumIn_MyHood && Status==FFAIL; t++)
	{
	  vx = px + MyHood[t].x ;
	  vy = py + MyHood[t].y ;
	  vz = pz + 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)
		{
		  degree = SimilarityBetweenFocusedPointsInMdlObjImgs(ObjVer_Fea,l, MdlVer_Fea,s) ;
		  
		  if( degree>SMR_THR )
		    {
		      forceByFocusedPntInObj[s].x -= MyHood[t].x ;
		      forceByFocusedPntInObj[s].y -= MyHood[t].y ;
		      forceByFocusedPntInObj[s].z -= MyHood[t].z ;
		      multiple[s] += 1 ;

		      Status = OOK ;
		    }
		}
	    }
	}
    }

  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] ;
      }

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




/*scaling and shifting*/
void EstimatingScalingAndShiftOf3DSurfaceFromImg(Fvector3d *MdlVer, Ivector3d *FixedMdlVer, ImgAttribute *MdlVer_Fea, int MdlVer_Num, Fvector3d ***DeformFld, ImgAttribute ***ObjImg, int image_size, int z_size )
{
  int   i, j, k, s, t, real_size, max_shift ;
  float CurrentDegree, OverlapDegree, MaxDegree ;
  int x, y, z ;
  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 = Crnt.x+MultiBubble[t][j].x ;
		      y = Crnt.y+MultiBubble[t][j].y ;
		      z = 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].Edge==MdlVer_Fea[s].Edge )
			{
			  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,"rb");
  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,"rb");
   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     ***********************************************************/
/****************************************************************************************************************/



