00001 #include "mrilib.h"
00002 
00003 /*---------------------------------------------------------------------------*/
00004 /*! - Input = matrix that transforms (i,j,k) indexes to RAI (x,y,z) coords.
00005     - Output = AFNI orientation codes for (i,j,k) axes.
00006     - The method is to find which permutation of (x,y,z) has the smallest
00007       angle to the (i,j,k) axes directions.
00008     - 27 Aug 2003 - RWCox
00009 -----------------------------------------------------------------------------*/
00010 
00011 THD_ivec3 THD_matrix_to_orientation( THD_mat33 R )
00012 {
00013    double xi,xj,xk , yi,yj,yk , zi,zj,zk , val,detQ,detP ;
00014    THD_dmat33 P , Q , M ;
00015    THD_ivec3 vor ;
00016    int i,j,k,p,q,r , ibest,jbest,kbest,pbest,qbest,rbest ;
00017    double vbest ;
00018 
00019    LOAD_IVEC3(vor,ORI_R2L_TYPE,ORI_A2P_TYPE,ORI_I2S_TYPE) ; /* default */
00020 
00021    UNLOAD_MAT(R,xi,xj,xk,yi,yj,yk,zi,zj,zk) ;
00022 
00023    /* normalize column vectors to get unit vectors along each ijk-axis */
00024 
00025    val = sqrt( xi*xi + yi*yi + zi*zi ) ;
00026    if( val == 0.0 ){ xi  = 1.0 ; yi  = 0.0 ; zi  = 0.0 ; }
00027    else            { xi /= val ; yi /= val ; zi /= val ; }
00028 
00029    val = sqrt( xj*xj + yj*yj + zj*zj ) ;
00030    if( val == 0.0 ){ xj  = 0.0 ; yj  = 1.0 ; zj  = 0.0 ; }
00031    else            { xj /= val ; yj /= val ; zj /= val ; }
00032 
00033    val = xi*xj + yi*yj + zi*zj ;    /* orthgonalize col #2 to col #1 */
00034    if( fabs(val) > 1.e-5 ){
00035      xj -= val*xi ; yj -= val*yi ; zj -= val*zi ;
00036      val = sqrt( xj*xj + yj*yj + zj*zj ) ;
00037      xj /= val ; yj /= val ; zj /= val ;
00038    }
00039 
00040    val = sqrt( xk*xk + yk*yk + zk*zk ) ;
00041    if( val == 0.0 ){ xk = yi*zj-zi*yj; yk = zi*xj-zj*xi ; zk=xi*yj-yi*xj ; }
00042    else            { xk /= val ; yk /= val ; zk /= val ; }
00043 
00044    val = xi*xk + yi*yk + zi*zk ;    /* orthogonalize col #3 to col #1 */
00045    if( fabs(val) > 1.e-5 ){
00046      xk -= val*xi ; yk -= val*yi ; zk -= val*zi ;
00047      val = sqrt( xk*xk + yk*yk + zk*zk ) ;
00048      xk /= val ; yk /= val ; zk /= val ;
00049    }
00050    val = xj*xk + yj*yk + zj*zk ;    /* and to col #2 */
00051    if( fabs(val) > 1.e-5 ){
00052      xk -= val*xj ; yk -= val*yj ; zk -= val*zj ;
00053      val = sqrt( xk*xk + yk*yk + zk*zk ) ;
00054      xk /= val ; yk /= val ; zk /= val ;
00055    }
00056 
00057    LOAD_DMAT(Q,xi,xj,xk,yi,yj,yk,zi,zj,zk) ;
00058 
00059    /* at this point, Q is the rotation matrix from the (i,j,k) to (x,y,z) axes */
00060 
00061    detQ = DMAT_DET(Q) ;
00062    if( detQ == 0.0 ) return vor ; /* shouldn't happen unless user is a dufis or oufis */
00063 
00064    /* build and test all possible +1/-1 permutation matrices */
00065 
00066    vbest = -666.0 ; ibest=pbest=qbest=rbest=1 ; jbest=2 ; kbest=3 ;
00067    for( i=1 ; i <= 3 ; i++ ){     /* i = column number to use for row #1 */
00068     for( j=1 ; j <= 3 ; j++ ){    /* j = column number to use for row #2 */
00069      if( i == j ) continue ;
00070       for( k=1 ; k <= 3 ; k++ ){  /* k = column number to use for row #3 */
00071        if( i == k || j == k ) continue ;
00072        LOAD_ZERO_DMAT(P) ;
00073        for( p=-1 ; p <= 1 ; p+=2 ){    /* p,q,r are -1 or +1      */
00074         for( q=-1 ; q <= 1 ; q+=2 ){   /* and go into rows #1,2,3 */
00075          for( r=-1 ; r <= 1 ; r+=2 ){
00076            P.mat[0][i-1] = p ; P.mat[1][j-1] = q ; P.mat[2][k-1] = r ;
00077            detP = DMAT_DET(P) ;                 /* sign of permutation */
00078            if( detP * detQ <= 0.0 ) continue ;  /* doesn't match sign of Q */
00079            M = DMAT_MUL(P,Q) ;
00080 
00081            /* we want the largest trace(M) == smallest angle to P axes */
00082            /* actual angle = 2.0*acos(0.5*sqrt(1.0+vbest)) (in radians) */
00083 
00084            val = M.mat[0][0] + M.mat[1][1] + M.mat[2][2] ; /* trace */
00085            if( val > vbest ){
00086              vbest = val ;
00087              ibest = i ; jbest = j ; kbest = k ;
00088              pbest = p ; qbest = q ; rbest = r ;
00089 #if 0
00090 fprintf(stderr,"mattor: vbest=%g (%g) i=%d j=%d k=%d p=%d q=%d r=%d\n",
00091         val, 2.0*acos(0.5*sqrt(1.0+vbest))*(180.0/3.14159265) , i,j,k,p,q,r) ;
00092 #endif
00093            }
00094    }}}}}}
00095 
00096    switch( ibest*pbest ){
00097      case  1: i = ORI_R2L_TYPE ; break ;  /* xbest = +x-DICOM */
00098      case -1: i = ORI_L2R_TYPE ; break ;  /* xbest = -x-DICOM */
00099      case  2: i = ORI_A2P_TYPE ; break ;  /* xbest = +y-DICOM */
00100      case -2: i = ORI_P2A_TYPE ; break ;  /* xbest = -y-DICOM */
00101      case  3: i = ORI_I2S_TYPE ; break ;  /* xbest = +z-DICOM */
00102      case -3: i = ORI_S2I_TYPE ; break ;  /* xbest = -z-DICOM */
00103    }
00104 
00105    switch( jbest*qbest ){
00106      case  1: j = ORI_R2L_TYPE ; break ;
00107      case -1: j = ORI_L2R_TYPE ; break ;
00108      case  2: j = ORI_A2P_TYPE ; break ;
00109      case -2: j = ORI_P2A_TYPE ; break ;
00110      case  3: j = ORI_I2S_TYPE ; break ;
00111      case -3: j = ORI_S2I_TYPE ; break ;
00112    }
00113 
00114    switch( kbest*rbest ){
00115      case  1: k = ORI_R2L_TYPE ; break ;
00116      case -1: k = ORI_L2R_TYPE ; break ;
00117      case  2: k = ORI_A2P_TYPE ; break ;
00118      case -2: k = ORI_P2A_TYPE ; break ;
00119      case  3: k = ORI_I2S_TYPE ; break ;
00120      case -3: k = ORI_S2I_TYPE ; break ;
00121    }
00122 
00123    LOAD_IVEC3(vor,i,j,k) ; return vor ;
00124 }