void* radix_tree_tag_set(struct radix_s* root, unsigned long index, unsigned int tag)
{
if (root->root_node == NULL)
return NULL;
if (index > root->max_key)
return NULL;
if (tag >= NB_TAGS)
return NULL;
// find the item, go down tree
int current_height = 0;
struct radix_node_s* current_node = root->root_node;
int next_child;
while (current_height < root->height-1)
{
next_child = NEXT_CHILD(root, current_height, index);
current_node = (struct radix_node_s*) current_node->children[next_child];
if (current_node == NULL)
return NULL;
current_height++;
}
next_child = NEXT_CHILD(root, current_height, index);
if (current_node->children[next_child] == NULL)
return NULL;
TAG_SET(current_node, tag, next_child);
struct radix_node_s* out = current_node->children[next_child];
//going back up, updating tags
current_height--;
current_node = current_node->parent;
while (current_node != NULL)
{
next_child = NEXT_CHILD(root, current_height, index);
TAG_SET(current_node, tag, next_child);
current_node = current_node->parent;
current_height--;
}
return out;
}
/**
* Remove given item & update the tree
*
* @root radix root
* @info item info
* @return error code
*/
error_t radix_tree_remove_item(struct radix_s *root, radix_item_info_t *info)
{
kmem_req_t req;
struct radix_node_s *current_node;
struct radix_node_s *current_parent;
uint_t current_height;
uint_t next_child;
uint_t deleted;
uint_t key;
uint_t i;
req.type = KMEM_RADIX_NODE;
current_node = info->node;
current_node->children[info->index] = NULL;
bitmap_clear(current_node->bitmap, info->index);
current_node->count--;
current_height = info->height;
next_child = info->index;
key = info->key;
// updating local tag
for (i = 0; i < NB_TAGS; ++i)
TAG_CLEAR(current_node, i, next_child);
if (current_node->count == 0)
{
req.ptr = current_node;
kmem_free(&req);
deleted = 1;
}
current_node = current_node->parent;
current_height--;
// going back up the tree, up till we reach the root
// updating tags on the way up
// deleting childless nodes on the way up
while (current_node)
{
next_child = NEXT_CHILD(root, current_height, key);
current_parent = current_node->parent;
if (deleted) // child node was deleted
{
current_node->children[next_child] = NULL;
bitmap_clear(current_node->bitmap, next_child);
current_node->count--;
for (i = 0; i < NB_TAGS; ++i)
TAG_CLEAR(current_node, i, next_child);
}
else
{
struct radix_node_s* temp_node = current_node->children[next_child];
for (i = 0; i < NB_TAGS; ++i)
{
if (temp_node->tags[i])
TAG_SET(current_node, i, next_child);
else
TAG_CLEAR(current_node, i, next_child);
}
}
deleted = 0;
if (current_node->count == 0)
{
req.ptr = current_node;
kmem_free(&req);
deleted = 1;
}
current_node = current_parent;
current_height--;
}
// check if we can shrink the tree
shrink_tree(root);
return 0;
}
int main( int argc , char * argv[] )
{
THD_dfvec3 *xx , *yy , dv ;
int nvec=0 , ii,jj, iarg ;
THD_dvecmat rt , rtinv ;
THD_dmat33 pp,ppt , rr ;
THD_dfvec3 tt ;
THD_3dim_dataset *mset=NULL , *dset=NULL ;
double *ww=NULL ;
int nww=0 ;
int keeptags=1 , wtval=0 , verb=0 , dummy=0 ;
char * prefix = "tagalign" , *mfile=NULL ;
float *fvol , cbot,ctop , dsum ;
int nval , nvox , clipit , ival, RMETH=MRI_CUBIC;
float matar[12] ;
int use_3dWarp=1 , matrix_type=ROTATION ;
mainENTRY("3dTagalign main");
/*--- help? ---*/
/*- scan args -*/
iarg = 1 ; RMETH=MRI_CUBIC;
while( iarg < argc && argv[iarg][0] == '-' ){
/*-----*/
if( strcmp(argv[iarg],"-h") == 0 ||
strcmp(argv[iarg],"-help") == 0){ /* 22 Apr 2003 */
usage_3dTagalign(strlen(argv[iarg]) > 3 ? 2:1);
exit(0);
}
/*-----*/
if( strcmp(argv[iarg],"-NN") == 0 ){
RMETH = MRI_NN ; iarg++ ; continue ;
}
if( strcmp(argv[iarg],"-linear") == 0 ){
RMETH = MRI_LINEAR ; iarg++ ; continue ;
}
if( strcmp(argv[iarg],"-cubic") == 0 ){
RMETH = MRI_CUBIC ; iarg++ ; continue ;
}
if( strcmp(argv[iarg],"-quintic") == 0 ){
RMETH = MRI_QUINTIC ; iarg++ ; continue ;
}
/*-----*/
if( strcmp(argv[iarg],"-rotate") == 0 ){ /* 22 Apr 2003 */
matrix_type = ROTATION ; use_3dWarp = 1 ;
iarg++ ; continue ;
}
/*-----*/
if( strcmp(argv[iarg],"-affine") == 0 ){ /* 21 Apr 2003 */
matrix_type = AFFINE ; use_3dWarp = 1 ;
iarg++ ; continue ;
}
/*-----*/
if( strcmp(argv[iarg],"-rotscl") == 0 ){ /* 22 Apr 2003 */
matrix_type = ROTSCL ; use_3dWarp = 1 ;
iarg++ ; continue ;
}
#if 0
/*-----*/
if( strcmp(argv[iarg],"-3dWarp") == 0 ){ /* 21 Apr 2003 */
use_3dWarp = 1 ;
iarg++ ; continue ;
}
#endif
/*-----*/
if( strcmp(argv[iarg],"-master") == 0 ){
if( mset != NULL ) ERREX("Can only have one -master option") ;
if( ++iarg >= argc ) ERREX("Need an argument after -master") ;
mset = THD_open_dataset( argv[iarg] ) ;
if( mset == NULL ) ERREX("Can't open -master dataset") ;
if( mset->tagset == NULL ) ERREX("No tags in -master dataset") ;
if( TAGLIST_COUNT(mset->tagset) < 3 ) ERREX("Not enough tags in -master dataset") ;
for( nvec=ii=0 ; ii < TAGLIST_COUNT(mset->tagset) ; ii++ )
if( TAG_SET(TAGLIST_SUBTAG(mset->tagset,ii)) ) nvec++ ;
if( nvec < 3 ) ERREX("Not enough tags set in -master dataset") ;
if( nvec < TAGLIST_COUNT(mset->tagset) )
fprintf(stderr,"++ WARNING: not all tags are set in -master dataset\n") ;
if( verb ) fprintf(stderr,"++ Found %d tags in -master dataset\n",nvec) ;
iarg++ ; continue ;
}
#if 0
/*-----*/
if( strcmp(argv[iarg],"-wtval") == 0 ){
if( ww != NULL ) ERREX("Can't have -wtval after -wt1D") ;
wtval++ ;
iarg++ ; continue ;
}
/*-----*/
if( strcmp(argv[iarg],"-wt1D") == 0 ){
MRI_IMAGE * wtim ; float * wtar ;
if( wtval ) ERREX("Can't have -wt1D after -wtval") ;
if( ww != NULL ) ERREX("Can't have two -wt1D options!") ;
if( ++iarg >= argc ) ERREX("Need an argument after -wt1D") ;
wtim = mri_read_1D( argv[iarg] ) ;
if( wtim == NULL ) ERREX("Can't read -wtim file") ;
if( wtim->ny > 1 ) ERREX("-wtim file has more than one columm") ;
wtar = MRI_FLOAT_PTR(wtim) ;
ww = (double *) malloc(sizeof(double)*wtim->nx) ; nww = wtim->nx ;
for( ii=0 ; ii < nww ; ii++ ){
ww[ii] = (double) wtar[ii] ;
if( ww[ii] < 0.0 ) ERREX("Negative value found in -wt1D file") ;
}
mri_free(wtim) ;
iarg++ ; continue ;
}
#endif
/*-----*/
if( strcmp(argv[iarg],"-nokeeptags") == 0 ){
keeptags = 0 ;
iarg++ ; continue ;
}
/*-----*/
if( strncmp(argv[iarg],"-verb",5) == 0 ){
verb++ ;
iarg++ ; continue ;
}
/*-----*/
if( strcmp(argv[iarg],"-dummy") == 0 ){
dummy++ ;
iarg++ ; continue ;
}
/*-----*/
if( strcmp(argv[iarg],"-prefix") == 0 ){
if( ++iarg >= argc ) ERREX("Need an argument after -prefix") ;
prefix = argv[iarg] ;
if( !THD_filename_ok(prefix) ) ERREX("-prefix string is illegal") ;
iarg++ ; continue ;
}
/*-----*/
if( strcmp(argv[iarg],"-matvec") == 0 ){
if( ++iarg >= argc ) ERREX("Need an argument after -matvec") ;
mfile = argv[iarg] ;
if( !THD_filename_ok(mfile) ) ERREX("-matvec string is illegal") ;
iarg++ ; continue ;
}
/*-----*/
fprintf(stderr,"** Unknown option: %s\n",argv[iarg]) ;
suggest_best_prog_option(argv[0], argv[iarg]);
exit(1) ;
} /* end of scanning command line for options */
if( argc < 2 ){
ERROR_message("Too few options");
usage_3dTagalign(0);
exit(1) ;
}
if( mset == NULL ) ERREX("No -master option found on command line") ;
#if 0
if( ww != NULL && nww < nvec ) ERREX("Not enough weights found in -wt1D file") ;
/*-- if -wtval, setup weights from master tag values --*/
if( wtval ){
ww = (double *) malloc(sizeof(double)*nvec) ; nww = nvec ;
for( ii=jj=0 ; ii < TAGLIST_COUNT(mset->tagset) ; ii++ ){
if( TAG_SET(TAGLIST_SUBTAG(mset->tagset,ii)) ){
ww[jj] = (double) TAG_VAL(TAGLIST_SUBTAG(mset->tagset,ii)) ;
if( ww[jj] < 0.0 ) ERREX("Negative value found in -master tag values") ;
jj++ ;
}
}
}
#endif
/*-- read input dataset (to match to master dataset) --*/
if( iarg >= argc ) ERREX("No input dataset?") ;
dset = THD_open_dataset( argv[iarg] ) ;
if( dset == NULL ) ERREX("Can't open input dataset") ;
if( dset->tagset == NULL ) ERREX("No tags in input dataset") ;
if( TAGLIST_COUNT(dset->tagset) !=
TAGLIST_COUNT(mset->tagset) ) ERREX("Tag counts don't match in -master and input") ;
/* check if set tags match exactly */
for( ii=0 ; ii < TAGLIST_COUNT(mset->tagset) ; ii++ ){
if( TAG_SET(TAGLIST_SUBTAG(mset->tagset,ii)) !=
TAG_SET(TAGLIST_SUBTAG(dset->tagset,ii)) )
ERREX("Set tags don't match in -master and input") ;
}
/*-- load vector lists: xx=master, yy=input --*/
xx = (THD_dfvec3 *) malloc( sizeof(THD_dfvec3) * nvec ) ;
yy = (THD_dfvec3 *) malloc( sizeof(THD_dfvec3) * nvec ) ;
dsum = 0.0 ;
for( ii=jj=0 ; ii < nvec ; ii++ ){
if( TAG_SET(TAGLIST_SUBTAG(mset->tagset,ii)) ){
LOAD_DFVEC3( xx[jj] , /* N.B.: */
TAG_X( TAGLIST_SUBTAG(mset->tagset,ii) ) , /* these are */
TAG_Y( TAGLIST_SUBTAG(mset->tagset,ii) ) , /* in Dicom */
TAG_Z( TAGLIST_SUBTAG(mset->tagset,ii) ) ) ; /* order now */
LOAD_DFVEC3( yy[jj] ,
TAG_X( TAGLIST_SUBTAG(dset->tagset,ii) ) ,
TAG_Y( TAGLIST_SUBTAG(dset->tagset,ii) ) ,
TAG_Z( TAGLIST_SUBTAG(dset->tagset,ii) ) ) ;
dv = SUB_DFVEC3( xx[jj] , yy[jj] ) ;
dsum += dv.xyz[0]*dv.xyz[0] + dv.xyz[1]*dv.xyz[1] + dv.xyz[2]*dv.xyz[2] ;
jj++ ;
}
}
dsum = sqrt(dsum/nvec) ;
fprintf(stderr,"++ RMS distance between tags before = %.2f mm\n" , dsum ) ;
/*-- compute best transformation from mset to dset coords --*/
switch( matrix_type ){
default:
case ROTATION:
rt = DLSQ_rot_trans( nvec , yy , xx , ww ) ; /* in thd_rot3d.c */
break ;
case AFFINE:
rt = DLSQ_affine ( nvec , yy , xx ) ; /* 21 Apr 2003 */
break ;
case ROTSCL:
rt = DLSQ_rotscl ( nvec , yy , xx , (DSET_NZ(dset)==1) ? 2 : 3 ) ;
break ;
}
rtinv = INV_DVECMAT(rt) ;
/*-- check for floating point legality --*/
nval = 0 ;
for( ii=0 ; ii < 3 ; ii++ ){
dsum = rt.vv.xyz[ii] ; nval += thd_floatscan(1,&dsum) ;
for( jj=0 ; jj < 3 ; jj++ ){
dsum = rt.mm.mat[ii][jj] ; nval += thd_floatscan(1,&dsum) ;
}
}
if( nval > 0 ){
fprintf(stderr,"** Floating point errors during calculation\n"
"** of transform matrix and translation vector\n" ) ;
exit(1) ;
}
/*-- check for rotation matrix legality --*/
dsum = DMAT_DET(rt.mm) ;
if( dsum == 0.0 || (matrix_type == ROTATION && fabs(dsum-1.0) > 0.01) ){
fprintf(stderr,"** Invalid transform matrix computed: tags dependent?\n"
"** computed [matrix] and [vector] follow:\n" ) ;
for( ii=0 ; ii < 3 ; ii++ )
fprintf(stderr," [ %10.5f %10.5f %10.5f ] [ %10.5f ] \n",
rt.mm.mat[ii][0],rt.mm.mat[ii][1],rt.mm.mat[ii][2],rt.vv.xyz[ii] );
exit(1) ;
}
/*-- print summary --*/
if( verb ){
fprintf(stderr,"++ Matrix & Vector [Dicom: x=R-L; y=A-P; z=I-S]\n") ;
for( ii=0 ; ii < 3 ; ii++ )
fprintf(stderr," %10.5f %10.5f %10.5f %10.5f\n",
rt.mm.mat[ii][0],rt.mm.mat[ii][1],rt.mm.mat[ii][2],rt.vv.xyz[ii] );
}
if( matrix_type == ROTATION || matrix_type == ROTSCL ){
double theta, costheta , dist , fac=1.0 ;
if( matrix_type == ROTSCL ){
fac = DMAT_DET(rt.mm); fac = fabs(fac);
if( DSET_NZ(dset) == 1 ) fac = sqrt(fac) ;
else fac = cbrt(fac) ;
}
costheta = 0.5 * sqrt(1.0 + DMAT_TRACE(rt.mm)/fac ) ;
theta = 2.0 * acos(costheta) * 180/3.14159265 ;
dist = SIZE_DFVEC3(rt.vv) ;
fprintf(stderr,"++ Total rotation=%.2f degrees; translation=%.2f mm; scaling=%.2f\n",
theta,dist,fac) ;
}
if( mfile ){
FILE * mp ;
if( THD_is_file(mfile) )
fprintf(stderr,"++ Warning: -matvec will overwrite file %s\n",mfile) ;
mp = fopen(mfile,"w") ;
if( mp == NULL ){
fprintf(stderr,"** Can't write to -matvec %s\n",mfile) ;
} else {
for( ii=0 ; ii < 3 ; ii++ )
fprintf(mp," %10.5f %10.5f %10.5f %10.5f\n",
rt.mm.mat[ii][0],rt.mm.mat[ii][1],rt.mm.mat[ii][2],rt.vv.xyz[ii] );
fclose(mp) ;
if( verb ) fprintf(stderr,"++ Wrote matrix+vector to %s\n",mfile) ;
}
}
if( dummy ){
fprintf(stderr,"++ This was a -dummy run: no output dataset\n") ; exit(0) ;
}
/*-- 21 Apr 2003: transformation can be done the old way (a la 3drotate),
or the new way (a la 3dWarp). --*/
#if 0
if( !use_3dWarp ){ /**** the old way ****/
/*-- now must scramble the rotation matrix and translation
vector from Dicom coordinate order to dataset brick order --*/
pp = DBLE_mat_to_dicomm( dset ) ;
ppt = TRANSPOSE_DMAT(pp) ;
rr = DMAT_MUL(ppt,rt.mm) ; rr = DMAT_MUL(rr,pp) ; tt = DMATVEC(ppt,rt.vv) ;
/*-- now create the output dataset by screwing with the input dataset
(this code is adapted from 3drotate.c) --*/
DSET_mallocize(dset) ;
DSET_load( dset ) ; CHECK_LOAD_ERROR(dset) ;
dset->idcode = MCW_new_idcode() ;
dset->dblk->diskptr->storage_mode = STORAGE_BY_BRICK ; /* 14 Jan 2004 */
EDIT_dset_items( dset ,
ADN_prefix , prefix ,
ADN_label1 , prefix ,
ADN_none ) ;
if( !THD_ok_overwrite() &&
(THD_deathcon() && THD_is_file(dset->dblk->diskptr->header_name) )){
fprintf(stderr,
"** Output file %s already exists -- cannot continue!\n",
dset->dblk->diskptr->header_name ) ;
exit(1) ;
}
tross_Make_History( "3dTagalign" , argc,argv , dset ) ;
/*-- if desired, keep old tagset --*/
if( keeptags ){
THD_dfvec3 rv ;
dsum = 0.0 ;
for( jj=ii=0 ; ii < TAGLIST_COUNT(dset->tagset) ; ii++ ){
if( TAG_SET(TAGLIST_SUBTAG(dset->tagset,ii)) ){
rv = DMATVEC( rt.mm , yy[jj] ) ; /* operating on */
rv = ADD_DFVEC3( rt.vv , rv ) ; /* Dicom order */
dv = SUB_DFVEC3( xx[jj] , rv ) ;
dsum += dv.xyz[0]*dv.xyz[0] + dv.xyz[1]*dv.xyz[1]
+ dv.xyz[2]*dv.xyz[2] ;
UNLOAD_DFVEC3( rv , TAG_X( TAGLIST_SUBTAG(dset->tagset,ii) ) ,
TAG_Y( TAGLIST_SUBTAG(dset->tagset,ii) ) ,
TAG_Z( TAGLIST_SUBTAG(dset->tagset,ii) ) ) ;
jj++ ;
}
}
dsum = sqrt(dsum/nvec) ;
fprintf(stderr,"++ RMS distance between tags after = %.2f mm\n" , dsum ) ;
} else {
myXtFree(dset->tagset) ; /* send it to the dustbin */
}
/*-- rotate sub-bricks --*/
if( verb ) fprintf(stderr,"++ computing output BRIK") ;
nvox = DSET_NVOX(dset) ;
nval = DSET_NVALS(dset) ;
fvol = (float *) malloc( sizeof(float) * nvox ) ;
THD_rota_method( MRI_HEPTIC ) ;
clipit = 1 ;
for( ival=0 ; ival < nval ; ival++ ){
/*- get sub-brick out of dataset -*/
EDIT_coerce_type( nvox ,
DSET_BRICK_TYPE(dset,ival),DSET_ARRAY(dset,ival) ,
MRI_float,fvol ) ;
if( clipit ){
register int ii ; register float bb,tt ;
bb = tt = fvol[0] ;
for( ii=1 ; ii < nvox ; ii++ ){
if( fvol[ii] < bb ) bb = fvol[ii] ;
else if( fvol[ii] > tt ) tt = fvol[ii] ;
}
cbot = bb ; ctop = tt ;
}
if( verb && nval < 5 ) fprintf(stderr,".") ;
/*- rotate it -*/
THD_rota_vol_matvec( DSET_NX(dset) , DSET_NY(dset) , DSET_NZ(dset) ,
fabs(DSET_DX(dset)) , fabs(DSET_DY(dset)) ,
fabs(DSET_DZ(dset)) ,
fvol , rr , tt ) ;
if( verb ) fprintf(stderr,".") ;
if( clipit ){
register int ii ; register float bb,tt ;
bb = cbot ; tt = ctop ;
for( ii=0 ; ii < nvox ; ii++ ){
if( fvol[ii] < bb ) fvol[ii] = bb ;
else if( fvol[ii] > tt ) fvol[ii] = tt ;
}
}
if( verb && nval < 5 ) fprintf(stderr,".") ;
/*- put it back into dataset -*/
EDIT_coerce_type( nvox, MRI_float,fvol ,
DSET_BRICK_TYPE(dset,ival),DSET_ARRAY(dset,ival) );
} /* end of loop over sub-brick index */
if( verb ) fprintf(stderr,":") ;
/* save matrix+vector into dataset, too */
UNLOAD_DMAT(rt.mm,matar[0],matar[1],matar[2],
matar[4],matar[5],matar[6],
matar[8],matar[9],matar[10] ) ;
UNLOAD_DFVEC3(rt.vv,matar[3],matar[7],matar[11]) ;
THD_set_atr( dset->dblk, "TAGALIGN_MATVEC", ATR_FLOAT_TYPE, 12, matar ) ;
/* write dataset to disk */
dset->dblk->master_nvals = 0 ; /* in case this was a mastered dataset */
DSET_write(dset) ;
if( verb ) fprintf(stderr,"\n") ;
} else
#endif
{ /**** the new way: use 3dWarp type transformation ****/
THD_3dim_dataset *oset ;
THD_vecmat tran ;
#if 0
DFVEC3_TO_FVEC3( rt.vv , tran.vv ) ;
DMAT_TO_MAT ( rt.mm , tran.mm ) ;
#else
DFVEC3_TO_FVEC3( rtinv.vv , tran.vv ) ;
DMAT_TO_MAT ( rtinv.mm , tran.mm ) ;
#endif
mri_warp3D_method( RMETH ) ;
oset = THD_warp3D_affine( dset, tran, mset, prefix, 0, WARP3D_NEWDSET ) ;
if( oset == NULL ){
fprintf(stderr,"** ERROR: THD_warp3D() fails!\n"); exit(1);
}
tross_Copy_History( dset , oset ) ;
tross_Make_History( "3dTagalign" , argc,argv , oset ) ;
UNLOAD_DMAT(rt.mm,matar[0],matar[1],matar[2],
matar[4],matar[5],matar[6],
matar[8],matar[9],matar[10] ) ;
UNLOAD_DFVEC3(rt.vv,matar[3],matar[7],matar[11]) ;
THD_set_atr( oset->dblk, "TAGALIGN_MATVEC", ATR_FLOAT_TYPE, 12, matar ) ;
/*-- if desired, keep old tagset --*/
if( keeptags ){
THD_dfvec3 rv ;
oset->tagset = myXtNew(THD_usertaglist) ;
*(oset->tagset) = *(dset->tagset) ;
dsum = 0.0 ;
for( jj=ii=0 ; ii < TAGLIST_COUNT(oset->tagset) ; ii++ ){
if( TAG_SET(TAGLIST_SUBTAG(oset->tagset,ii)) ){
rv = DMATVEC( rt.mm , yy[jj] ) ;
rv = ADD_DFVEC3( rt.vv , rv ) ;
dv = SUB_DFVEC3( xx[jj] , rv ) ;
dsum += dv.xyz[0]*dv.xyz[0] + dv.xyz[1]*dv.xyz[1]
+ dv.xyz[2]*dv.xyz[2] ;
UNLOAD_DFVEC3( rv , TAG_X( TAGLIST_SUBTAG(oset->tagset,ii) ) ,
TAG_Y( TAGLIST_SUBTAG(oset->tagset,ii) ) ,
TAG_Z( TAGLIST_SUBTAG(oset->tagset,ii) ) ) ;
jj++ ;
}
}
dsum = sqrt(dsum/nvec) ;
fprintf(stderr,"++ RMS distance between tags after = %.2f mm\n" , dsum ) ;
}
DSET_write(oset) ;
} /* end of 3dWarp-like work */
exit(0) ;
}
