00001 /*****************************************************************************
00002    Major portions of this software are copyrighted by the Medical College
00003    of Wisconsin, 1994-2000, and are released under the Gnu General Public
00004    License, Version 2.  See the file README.Copyright for details.
00005 ******************************************************************************/
00006 
00007 #include "thd_shear3d.h"
00008 
00009 #include "debugtrace.h"  /* 29 Jan 2001 */
00010 
00011 /*===========================================================================
00012   Routines to manipulate 3D shears, used for rotating 3D volumes - RWCox
00013 =============================================================================*/
00014 
00015 /*----------------------------------------------------------------------
00016    "Norm" of a set of shears [maximum stretch factor]
00017 ------------------------------------------------------------------------*/
00018 
00019 #define BIG_NORM 1.e+38
00020 
00021 double norm_3shear( MCW_3shear sh )
00022 {
00023    double top=0.0 , val ;
00024    int ii , jj ;
00025 
00026    if( ! ISVALID_3SHEAR(sh) ) return BIG_NORM ;
00027 
00028    for( ii=0 ; ii < 3 ; ii++ ){
00029      jj  = sh.ax[ii] ;
00030      val = fabs( sh.scl[ii][(jj+1)%3] ) ; if( val > top ) top = val ;
00031      val = fabs( sh.scl[ii][(jj+2)%3] ) ; if( val > top ) top = val ;
00032    }
00033 
00034    return top ;
00035 }
00036 
00037 /*----------------------------------------------------------------------
00038   Create a 3x3 matrix from one shear
00039 ------------------------------------------------------------------------*/
00040 
00041 THD_dmat33 make_shear_matrix( int ax , double scl[3] )
00042 {
00043    THD_dmat33 m ;
00044 
00045    switch( ax ){
00046       case 0:  LOAD_SHEARX_DMAT( m , scl[0],scl[1],scl[2] ) ; break ;
00047       case 1:  LOAD_SHEARY_DMAT( m , scl[1],scl[0],scl[2] ) ; break ;
00048       case 2:  LOAD_SHEARZ_DMAT( m , scl[2],scl[0],scl[1] ) ; break ;
00049       default: LOAD_ZERO_DMAT( m )                          ; break ;
00050    }
00051    return m ;
00052 }
00053 
00054 /*------------------------------------------------------------------------
00055   Permute the axes of a shear structure so that what was
00056   in order (0,1,2) is now in order (ox1,ox2,ox3).
00057   If P is the 3x3 matrix [ delta(i,ox{j+1}) ] (i,j=0,1,2), then
00058 
00059                                         T
00060       [output shear] = P [input shear] P
00061 --------------------------------------------------------------------------*/
00062 
00063 MCW_3shear permute_3shear( MCW_3shear shin , int ox1, int ox2, int ox3 )
00064 {
00065    MCW_3shear shout ;
00066    int ii , ain,aout , pi[3] ;
00067 
00068    /* sanity check */
00069 
00070    if( ! ISVALID_3SHEAR(shin) ){ INVALIDATE_3SHEAR(shout) ; return shout ; }
00071 
00072    pi[0] = ox1 ; pi[1] = ox2 ; pi[2] = ox3 ;
00073 
00074    for( ii=0 ; ii < 4 ; ii++ ){
00075 
00076       ain  = shin.ax[ii] ;   /* axis of input */
00077       aout = pi[ain] ;       /* axis of output */
00078 
00079       shout.ax[ii]         = aout ;             /* store new axis */
00080       shout.scl[ii][pi[0]] = shin.scl[ii][0] ;  /* permuted scalings */
00081       shout.scl[ii][pi[1]] = shin.scl[ii][1] ;
00082       shout.scl[ii][pi[2]] = shin.scl[ii][2] ;
00083       shout.sft[ii]        = shin.sft[ii] ;     /* copy shift */
00084    }
00085 
00086    shout.flip0 = shin.flip0 ; shout.flip1 = shin.flip1 ;
00087 
00088    return shout ;
00089 }
00090 
00091 /*------------------------------------------------------------------------
00092   Permute a 3x3 matrix:             T
00093                         [output] = P  [q] P
00094 --------------------------------------------------------------------------*/
00095 
00096 THD_dmat33 permute_dmat33( THD_dmat33 q , int ox1, int ox2, int ox3 )
00097 {
00098    THD_dmat33 m ;
00099    int ii , jj , pi[3] ;
00100 
00101    pi[0] = ox1 ; pi[1] = ox2 ; pi[2] = ox3 ;
00102 
00103    for( ii=0 ; ii < 3 ; ii++ )
00104       for( jj=0 ; jj < 3 ; jj++ )
00105          m.mat[ii][jj] = q.mat[ pi[ii] ] [ pi[jj] ] ;
00106 
00107    return m ;
00108 }
00109 
00110 /*------------------------------------------------------------------------
00111   Permute a 3vector:             T
00112                      [output] = P  [q]
00113 --------------------------------------------------------------------------*/
00114 
00115 THD_dfvec3 permute_dfvec3( THD_dfvec3 q , int ox1, int ox2, int ox3 )
00116 {
00117    THD_dfvec3 m ;
00118    int ii , pi[3] ;
00119 
00120    pi[0] = ox1 ; pi[1] = ox2 ; pi[2] = ox3 ;
00121 
00122    for( ii=0 ; ii < 3 ; ii++ )
00123       m.xyz[ii] = q.xyz[ pi[ii] ] ;
00124 
00125    return m ;
00126 }
00127 
00128 /*------------------------------------------------------------------------
00129    Function to compute the decomposition of a 3x3 matrix into a product
00130    of 4 shears:
00131 
00132      Q = Sx[bx2,cx2,f] Sz[az,bz,f] Sy[ay,cy,f] Sx[bx1,cx1,1]
00133 
00134    where             [ f b c ]              [ 1 0 0 ]              [ 1 0 0 ]
00135          Sx[b,c,f] = [ 0 1 0 ]  Sy[a,c,f] = [ a f b ]  Sz[a,b,f] = [ 0 f 0 ]
00136                      [ 0 0 0 ]              [ 0 0 1 ]              [ a b 1 ]
00137 
00138    "f" is a stretching/shrinking factor applied with the 1D shear.
00139    If det[Q] = 1 , then f = 1 and these are pure shears.
00140 
00141    In addition, a shift is allowed for, so that the total coordinate
00142    transformation being decomposed is really
00143 
00144        [ x ]          [ x ]     [ xdel ]
00145        [ y ]   =  [Q] [ y ]   + [ ydel ]
00146        [ z ]          [ z ]     [ zdel ]
00147             new            old
00148 
00149    The output shifts are applied to the last three transformations, so
00150    that the factoring produces this sequence of operations:
00151 
00152        [ x ]           [ x ]
00153        [ y ]   = [Sx1] [ y ]
00154        [ z ]           [ z ]
00155             1               old
00156 
00157        [ x ]           [ x ]    [ 0  ]
00158        [ y ]   = [Sy]  [ y ]  + [ dy ]
00159        [ z ]           [ z ]    [ 0  ]
00160             2               1
00161 
00162        [ x ]           [ x ]    [ 0  ]
00163        [ y ]   = [Sz]  [ y ]  + [ 0  ]
00164        [ z ]           [ z ]    [ dz ]
00165             3               2
00166 
00167        [ x ]           [ x ]    [ dx ]
00168        [ y ]   = [Sx2] [ y ]  + [ 0  ]
00169        [ z ]           [ z ]    [ 0  ]
00170             new             3
00171 
00172    The function returns a set of shears in the MCW_3shear struct type.
00173 
00174    The C code for generating the parameters (f,bx2,cx2,az,bz,ay,cy,bx1,cx1,dx,dy,cz)
00175    was generated by the following Maple V Release 4 script:
00176 
00177     with(linalg) : readlib(C) :
00178 
00179     Sx := (f,b,c,dx) -> matrix( [ [f,b,c,dx], [0,1,0,0 ], [0,0,1,0 ], [0,0,0,1] ] ) :
00180     Sy := (f,a,c,dy) -> matrix( [ [1,0,0,0 ], [a,f,c,dy], [0,0,1,0 ], [0,0,0,1] ] ) :
00181     Sz := (f,a,b,dz) -> matrix( [ [1,0,0,0 ], [0,1,0,0 ], [a,b,f,dz], [0,0,0,1] ] ) :
00182 
00183     SS := evalm( Sx(f,bx2,cx2,dx) &* Sz(f,az,bz,dz) &* Sy(f,ay,cy,dy) &* Sx(1,bx1,cx1,0) ) :
00184 
00185     QQ := matrix( [ [q11,q12,q13,xdel], [q21,q22,q23,ydel], [q31,q32,q33,zdel], [0,0,0,1] ] ) :
00186 
00187     ee := { seq( SS[i,1]-QQ[i,1] , i=1..3 ) ,
00188             seq( SS[i,2]-QQ[i,2] , i=1..3 ) ,
00189             seq( SS[i,3]-QQ[i,3] , i=1..3 ) ,
00190             seq( SS[i,4]-QQ[i,4] , i=1..3 )  } :
00191 
00192     vv := { f,bx2,cx2,az,bz,ay,cy,bx1,cx1,dx,dy,dz } :
00193 
00194     s1 := solve( ee ,vv ) :
00195     s2 := map( x -> convert(x,radical) , s1 ) :
00196     ss := [ op(s2) ] :
00197     C(ss,optimized) ;
00198 --------------------------------------------------------------------------*/
00199 
00200 MCW_3shear shear_xzyx( THD_dmat33 *q , THD_dfvec3 *xyzdel )
00201 {
00202    /* input variables */
00203 
00204    double q11,q12,q13,q21,q22,q23,q31,q32,q33 , xdel,ydel,zdel ;
00205 
00206    /* computed parameters */
00207 
00208    double f,bx2,cx2,az,bz,ay,cy,bx1,cx1 , dx,dy,dz ;
00209 
00210    /* output variable */
00211 
00212    MCW_3shear shr ;
00213 
00214    /* internals (created by Maple) */
00215 
00216    double t1, t3, t4, t5, t6, t7, t8, t9, t10, t11,
00217           t12, t13, t15, t16, t17, t18, t19, t20, t22, t23,
00218           t24, t25, t26, t27, t28, t29, t30, t32, t34, t35,
00219           t36, t37, t38, t44, t45, t47, t50, t51, t53, t54,
00220           t55, t57, t61, t62, t64, t66, t67, t68, t69, t70,
00221           t73, t75, t77, t78, t79, t80, t81, t84, t85, t86,
00222           t87, t89, t90, t92, t94, t96, t102, t107, t109, t113,
00223           t118, t119, t121, t123, t125, t127, t129, t131, t132, t134,
00224           t141, t145, t148, t150, t151, t157, t160, t163, t164, t167,
00225           t185, t190, t193, t194, t195, t203, t206, t207, t210, t220,
00226           t221, t224, t230, t233, t238, t240, t241, t252, t264, t267,
00227           t269, t275, t292;
00228 
00229    /* initialize output to an invalid result */
00230 
00231    INVALIDATE_3SHEAR(shr) ;
00232 
00233    /* load inputs into local variables */
00234 
00235    UNLOAD_DMAT( *q , q11,q12,q13,q21,q22,q23,q31,q32,q33 ) ;
00236    xdel = xyzdel->xyz[0] ; ydel = xyzdel->xyz[1] ; zdel = xyzdel->xyz[2] ;
00237 
00238    /* the code generated by Maple, slightly massaged */
00239 
00240       ay = q21;
00241       dy = ydel;
00242       t1 = q21*q12;
00243       t3 = q13*q22;
00244       t4 = t3*q31;
00245       t5 = q21*q13;
00246       t6 = t5*q32;
00247       t7 = q23*q11;
00248       t8 = t7*q32;
00249       t9 = q12*q23;
00250       t10 = t9*q31;
00251       t11 = q22*q11;
00252       t12 = t11*q33;
00253       t13 = t1*q33+t4-t6+t8-t10-t12;
00254       t15 = q32*q32;
00255       t16 = t15*q32;
00256       t17 = q21*q21;
00257       t18 = t17*q21;
00258       t19 = t16*t18;
00259       t20 = q22*q22;
00260       t22 = q31*q31;
00261       t23 = t22*q32;
00262       t24 = q21*t20*t23;
00263       t25 = t20*q22;
00264       t26 = t22*q31;
00265       t27 = t25*t26;
00266       t28 = t15*t17;
00267       t29 = q22*q31;
00268       t30 = t28*t29;
00269       t32 = t13*t13;
00270 
00271       t34 = (-t19-3.0*t24+t27+3.0*t30)*t32 ;
00272            if( t34 > 0.0 ) t34 =   pow(  t34 , 0.333333333333333 ) ;
00273       else if( t34 < 0.0 ) t34 = - pow( -t34 , 0.333333333333333 ) ;
00274       else                 t34 = 0.0 ;
00275 
00276       if( t13 == 0.0 ) return shr ;
00277 
00278       t35 = 1/t13*t34;
00279       t36 = t35+q31;
00280       t37 = t36*q21;
00281       t38 = q12*q33;
00282       t44 = t36*q23;
00283       t45 = q11*q32;
00284       t47 = t36*q12;
00285       t50 = t36*q22;
00286       t51 = q11*q33;
00287       t53 = q32*t17;
00288       t54 = t53*q12;
00289       t55 = q32*q21;
00290       t57 = q32*q31;
00291       t61 = q32*q23*q11*q31;
00292       t62 = q31*q21;
00293       t64 = q22*q12;
00294       t66 = t22*q23;
00295       t67 = t66*q12;
00296       t68 = t22*q13;
00297       t69 = t68*q22;
00298       t70 = t29*t51;
00299       t73 = -t37*t38-t36*q13*t29+t37*q13*q32-t44*t45+t47*q23*q31+t50*t51+t54-
00300              t55*t11-t57*t5+t61+t62*t38-t62*t64-t67+t69-t70+q31*t20*q11;
00301       t75 = t20*t22;
00302 
00303       t77 = (t28-2.0*t29*t55+t75) ;
00304       if( t77 == 0.0 ) return shr ;
00305       t77 = 1/t77 ;
00306 
00307       cx2 = t73*t77;
00308       t78 = t44*q31;
00309       t79 = t62*q22;
00310       t80 = t62*q33;
00311       t81 = t37*q33;
00312       t84 = t34*t34;
00313 
00314       if( t84 == 0.0 ) return shr ;
00315       t85 = 1/t84;
00316 
00317       cy = (-t78+t79-t80+t81-t53+t66)*t32*t85;
00318       t86 = q21*t22;
00319       t87 = t64*t36;
00320       t89 = t17*q12;
00321       t90 = t89*t36;
00322       t92 = t51*t22;
00323       t94 = t68*q32;
00324       t96 = t36*t36;
00325       t102 = t51*q31;
00326       t107 = t11*t36;
00327       t109 = t38*t22;
00328       t113 = t86*t87-t57*t90+2.0*t50*t92+2.0*t37*t94+t96*t22*t3+t96*q31*t8-3.0*
00329              t24-t96*q22*t102+3.0*t30+t27-2.0*t36*t26*t3-t19-t62*q32*t107-2.0*t37*t109-t26*
00330              q21*t64;
00331       t118 = q32*q22;
00332       t119 = t118*q11;
00333       t121 = q11*t36;
00334       t123 = t26*q13;
00335       t125 = t26*q23;
00336       t127 = q33*t26;
00337       t129 = t96*q12;
00338       t131 = t96*q21;
00339       t132 = t38*q31;
00340       t134 = t22*t22;
00341       t141 = q31*q13*q32;
00342       t145 = -q11*t15*t17*q31+t23*t89+t86*t119+t121*t28-t123*t55+t125*t45+t1*
00343              t127-t129*t66+t131*t132+t134*q13*q22-t9*t134-2.0*t36*t22*t8-t131*t141-t11*t127+
00344              2.0*t47*t125;
00345 
00346       if( t34 == 0.0 ) return shr ;
00347       t148 = 1/t34;
00348 
00349       if( q21 == 0.0 ) return shr ;
00350       t150 = 1/q21;
00351 
00352       t151 = t148*t77*t150;
00353       bx2 = (t113+t145)*t13*t151;
00354       az = -t35;
00355       f = (-t29+t55)*t13*t148;
00356       t157 = ydel*q12;
00357       t160 = zdel*t17;
00358       t163 = ydel*t22;
00359       t164 = t163*q21;
00360       t167 = ydel*t26;
00361       t185 = xdel*q21;
00362       t190 = ydel*q11;
00363       t193 = -ydel*q22*t51*t26-t157*q23*t134-t160*t129*q33+t164*t119+t163*t54-
00364              t167*q21*q22*q12+ydel*t134*t3+t157*q21*q33*t26-t167*t6-3.0*ydel*q21*t75*q32+
00365              t167*t8+3.0*ydel*t15*t17*q22*q31+t185*t20*t26-ydel*t16*t18-t190*t28*q31;
00366       t194 = zdel*q21;
00367       t195 = t125*q12;
00368       t203 = xdel*t18;
00369       t206 = xdel*t17;
00370       t207 = q22*t22;
00371       t210 = t160*q32;
00372       t220 = zdel*t18;
00373       t221 = q32*q12;
00374       t224 = t123*q22;
00375       t230 = t194*t96;
00376       t233 = t194*t195+t194*t22*t12+t160*t94-t194*q32*t7*t22+t203*q31*t15-2.0*
00377              t206*t207*q32-t210*t107-t194*t75*q11+t194*q31*t20*q11*t36+t210*t11*q31-t220*
00378              t221*q31-t194*t224+t160*t207*q12+ydel*t25*t26-t230*t8-t160*t109;
00379       t238 = t194*t36;
00380       t240 = ydel*t96;
00381       t241 = t240*q21;
00382       t252 = ydel*t36;
00383       t264 = -t203*t36*t15+t230*t12+2.0*t238*t61-t241*t141-t240*q22*t102+t220*
00384              t221*t36-t160*q31*t87+2.0*t206*t36*t29*q32-2.0*t252*t22*t8+t164*t87+t190*t36*
00385              t17*t15-t240*t67-2.0*t252*t224+2.0*t252*q22*t92+t241*t132;
00386       t267 = t160*t36;
00387       t269 = ydel*q31;
00388       t275 = t252*q21;
00389       t292 = -2.0*t238*t67+t240*t69+2.0*t267*t132-t269*q32*t90-t185*t36*t20*t22
00390              +2.0*t275*t94+t230*t10+2.0*t238*t69+2.0*t252*t195-t230*t4-2.0*t275*t109-2.0*
00391              t267*t141-2.0*t238*t70+t240*q31*t8-t269*q21*t118*t121+t160*t96*q13*q32;
00392       dx = -(t193+t233+t264+t292)*t13*t151;
00393       bz = t36*t150;
00394       cx1 = -(t78+t79-t80-t96*q23+t81-t53)*t150*t32*t85;
00395       dz = (-t252+t194)*t150;
00396       bx1 = -(-t50+t55)*t150*t13*t148;
00397 
00398    /* load computed values into output structure */
00399 
00400    shr.ax[3] = 0; shr.scl[3][0] = f; shr.scl[3][1] = bx2; shr.scl[3][2] = cx2; shr.sft[3] = dx;
00401    shr.ax[2] = 2; shr.scl[2][2] = f; shr.scl[2][0] = az ; shr.scl[2][1] = bz ; shr.sft[2] = dz;
00402    shr.ax[1] = 1; shr.scl[1][1] = f; shr.scl[1][0] = ay ; shr.scl[1][2] = cy ; shr.sft[1] = dy;
00403    shr.ax[0] = 0; shr.scl[0][0] = 1; shr.scl[0][1] = bx1; shr.scl[0][2] = cx1; shr.sft[0] = 0 ;
00404 
00405    shr.flip0 = shr.flip1 = -1 ;  /* no flips now */
00406 
00407    return shr ;
00408 }
00409 
00410 /*---------------------------------------------------------------------------------------
00411    Decompose transformation (q,xyzdel) into a set of 4 shears,
00412    whose axis order is ox1,ox2,ox3,ox1; that is,
00413      q = S    S    S    S
00414           ox1  ox3  ox2  ox1
00415 -----------------------------------------------------------------------------------------*/
00416 
00417 MCW_3shear shear_arb( THD_dmat33 *q , THD_dfvec3 *xyzdel , int ox1,int ox2,int ox3 )
00418 {
00419    THD_dmat33 qq ;
00420    THD_dfvec3 xx ;
00421    MCW_3shear sh_xzyx , shout ;
00422 
00423    /* permute the input matrix and vector to the desired order */
00424 
00425    qq = permute_dmat33( *q      , ox1,ox2,ox3 ) ;
00426    xx = permute_dfvec3( *xyzdel , ox1,ox2,ox3 ) ;
00427 
00428    /* compute the Sx Sz Sy Sx factorization */
00429 
00430    sh_xzyx = shear_xzyx( &qq , &xx ) ;
00431    if( ! ISVALID_3SHEAR(sh_xzyx) ) return sh_xzyx ;
00432 
00433    /* permute the shear factorization back */
00434 
00435    shout = permute_3shear( sh_xzyx , ox1,ox2,ox3 ) ;
00436 
00437    return shout ;
00438 }
00439 
00440 /*-----------------------------------------------------------------------------------
00441    Find the "best" shear decomposition (smallest stretching factors).
00442    Input matrix q should have det(q)=1.
00443 -------------------------------------------------------------------------------------*/
00444 
00445 MCW_3shear shear_best( THD_dmat33 *q , THD_dfvec3 *xyzdel )
00446 {
00447    MCW_3shear sh[6] ;
00448    int ii , jbest ;
00449    double val , best ;
00450 
00451    double dsum,esum ;   /* 29 Feb 2004 */
00452 
00453    dsum = DMAT_TRACE(*q) ;
00454    esum = fabs(q->mat[0][1]) + fabs(q->mat[0][2])
00455          +fabs(q->mat[1][0]) + fabs(q->mat[1][2])
00456          +fabs(q->mat[2][0]) + fabs(q->mat[2][1]) ;
00457 
00458    if( dsum >= 2.99999 && esum/dsum < 1.e-6 ){
00459      MCW_3shear shr ; double dx=xyzdel->xyz[0], dy=xyzdel->xyz[1], dz=xyzdel->xyz[2] ;
00460 #if 0
00461      if( MRILIB_verbose ){
00462        fprintf(stderr,"shear_best: matrix=I?\n"); DUMP_DMAT33("matrix",*q);
00463      }
00464 #endif
00465      shr.ax[3]=0; shr.scl[3][0]=1.0; shr.scl[3][1]=0.0; shr.scl[3][2]=0.0; shr.sft[3]=dx;
00466      shr.ax[2]=2; shr.scl[2][2]=1.0; shr.scl[2][0]=0.0; shr.scl[2][1]=0.0; shr.sft[2]=dz;
00467      shr.ax[1]=1; shr.scl[1][1]=1.0; shr.scl[1][0]=0.0; shr.scl[1][2]=0.0; shr.sft[1]=dy;
00468      shr.ax[0]=0; shr.scl[0][0]=1.0; shr.scl[0][1]=0.0; shr.scl[0][2]=0.0; shr.sft[0]=0.0;
00469      shr.flip0 = shr.flip1 = -1 ;  /* no flips */
00470      return shr ;
00471    }
00472 
00473    /* compute all 6 possible factorizations (the brute force approach) */
00474 
00475    sh[0] = shear_arb( q , xyzdel , 0,1,2 ) ;
00476    sh[1] = shear_arb( q , xyzdel , 0,2,1 ) ;
00477    sh[2] = shear_arb( q , xyzdel , 1,0,2 ) ;
00478    sh[3] = shear_arb( q , xyzdel , 1,2,0 ) ;
00479    sh[4] = shear_arb( q , xyzdel , 2,0,1 ) ;
00480    sh[5] = shear_arb( q , xyzdel , 2,1,0 ) ;
00481 
00482    /* find the one with the smallest "norm" */
00483 
00484    jbest = 0 ; best = BIG_NORM ;
00485    for( ii=0 ; ii < 6 ; ii++ ){
00486      val = norm_3shear( sh[ii] ) ;
00487      if( val < best ){ best = val ; jbest = ii ; }
00488    }
00489 
00490    return sh[jbest] ;
00491 }
00492 
00493 /*--------------------------------------------------------------------------
00494    Compute a rotation matrix specified by 3 angles:
00495       Q = R3 R2 R1, where Ri is rotation about axis axi by angle thi.
00496 ----------------------------------------------------------------------------*/
00497 
00498 THD_dmat33 rot_to_matrix( int ax1 , double th1 ,
00499                           int ax2 , double th2 , int ax3 , double th3  )
00500 {
00501    THD_dmat33 q , p ;
00502 
00503    LOAD_ROT_DMAT( q , th1 , ax1 ) ;
00504    LOAD_ROT_DMAT( p , th2 , ax2 ) ; q = DMAT_MUL( p , q ) ;
00505    LOAD_ROT_DMAT( p , th3 , ax3 ) ; q = DMAT_MUL( p , q ) ;
00506 
00507    return q ;
00508 }
00509 
00510 /*--------------------------------------------------------------------------
00511    Compute a set of shears to carry out a rotation + shift.
00512    The rotation is specified as the composition of elementary
00513    rotations about 3 axes.
00514 ----------------------------------------------------------------------------*/
00515 
00516 MCW_3shear rot_to_shear( int ax1 , double th1 ,
00517                          int ax2 , double th2 ,
00518                          int ax3 , double th3 ,
00519                          int dcode , double dx , double dy , double dz ,
00520                          double xdel , double ydel , double zdel )
00521 {
00522    int flip0=-1 , flip1=-1 ;  /* no flips */
00523    THD_dmat33 q , p ;
00524    THD_dfvec3 d , c ;
00525 
00526    static MCW_3shear shr ;
00527    static int old_ax1=-99 , old_ax2=-99 , old_ax3=-99 , old_dcode=-99 ;
00528    static double old_th1 , old_th2 , old_th3 , old_dx , old_dy , old_dz ,
00529                  old_xdel, old_ydel, old_zdel ;
00530 
00531    /* check if this is a duplicate call */
00532 
00533    if( ax1 == old_ax1  &&  ax2 == old_ax2  &&  ax3 == old_ax3  && dcode == old_dcode &&
00534        th1 == old_th1  &&  th2 == old_th2  &&  th3 == old_th3  &&
00535        dx  == old_dx   &&  dy  == old_dy   &&  dz  == old_dz   &&
00536       xdel == old_xdel && ydel == old_ydel && zdel == old_zdel    ) return shr ;
00537 
00538    old_ax1   = ax1 ; old_ax2  = ax2 ; old_ax3  = ax3 ;
00539    old_th1   = th1 ; old_th2  = th2 ; old_th3  = th3 ;
00540    old_dx    = dx  ; old_dy   = dy  ; old_dz   = dz  ;
00541    old_xdel  = xdel; old_ydel = ydel; old_zdel = zdel; old_dcode = dcode;
00542 
00543    /* compute rotation matrix */
00544 
00545    q = rot_to_matrix( ax1,th1 , ax2,th2 , ax3,th3 ) ;
00546 
00547    /* if trace too small, maybe we should flip a couple axes */
00548 
00549    if( DMAT_TRACE(q) < 1.0 ){
00550       double top=q.mat[0][0] ; int itop=0 , i1,i2 ;
00551       if( top < q.mat[1][1] ){ top = q.mat[1][1] ; itop = 1 ; }
00552       if( top < q.mat[2][2] ){ top = q.mat[2][2] ; itop = 2 ; }
00553       switch(itop){
00554          case 0: i1 = 1 ; i2 = 2 ; LOAD_DIAG_DMAT(p, 1,-1,-1) ; break ;
00555          case 1: i1 = 0 ; i2 = 2 ; LOAD_DIAG_DMAT(p,-1, 1,-1) ; break ;
00556          case 2: i1 = 0 ; i2 = 1 ; LOAD_DIAG_DMAT(p,-1,-1, 1) ; break ;
00557       }
00558       if( q.mat[i1][i1] + q.mat[i2][i2] < -0.02 ){
00559          q = DMAT_MUL( q , p ) ;
00560          flip0 = i1 ; flip1 = i2 ;  /* yes flips */
00561       }
00562    }
00563 
00564    LOAD_DFVEC3( d , dx,dy,dz ) ;
00565 
00566    switch( dcode ){
00567       default: break ;   /* nothing */
00568 
00569       case DELTA_BEFORE:
00570          d = DMATVEC(q,d) ; break ;
00571 
00572       case DELTA_FIXED:
00573          c = DMATVEC(q,d) ; d = SUB_DFVEC3(d,c) ; break ;
00574    }
00575 
00576    /* scale q and d by the voxel dimensions */
00577 
00578    d.xyz[0] = d.xyz[0] / xdel ;  /* d <- inv[D] d, where D = diag[xdel,ydel,zdel] */
00579    d.xyz[1] = d.xyz[1] / ydel ;
00580    d.xyz[2] = d.xyz[2] / zdel ;
00581 
00582    q.mat[0][1] *= (ydel/xdel) ;  /* q <- inv[D] q D */
00583    q.mat[0][2] *= (zdel/xdel) ;
00584    q.mat[1][0] *= (xdel/ydel) ;  /* q still has det[q]=1 after this */
00585    q.mat[1][2] *= (zdel/ydel) ;  /* and the diagonal is unaffected */
00586    q.mat[2][0] *= (xdel/zdel) ;
00587    q.mat[2][1] *= (ydel/zdel) ;
00588 
00589    /* compute the "best" shear for this q matrix */
00590 
00591    shr = shear_best( &q , &d ) ;
00592 
00593    /* if cannot compute shear, try perturbing the matrix a little */
00594 
00595 #undef PER0
00596 #undef PER1
00597 #undef PER2
00598 #define PER0 1.09e-6
00599 #define PER1 1.22e-6
00600 #define PER2 1.37e-6
00601 
00602    if( ! ISVALID_3SHEAR(shr) ){
00603       THD_dmat33 pt ;
00604       p  = rot_to_matrix( 0,PER0 , 1,PER1 , 2,PER2 ) ;
00605       q  = DMAT_MUL( q , p ) ;
00606       pt = TRANSPOSE_DMAT( p ) ;
00607       q  = DMAT_MUL( pt , q ) ;
00608 
00609       shr = shear_best( &q , &d ) ;
00610       if( ! ISVALID_3SHEAR(shr) ) return shr ;  /* give up */
00611    }
00612 
00613    shr.flip0 = flip0 ; shr.flip1 = flip1 ;
00614 
00615 #if 0
00616    if( MRILIB_verbose ) DUMP_3SHEAR("rot_to_shear",shr) ;
00617 #endif
00618 
00619    return shr ;
00620 }
00621 
00622 /*--------------------------------------------------------------------------
00623    Compute a set of shears to carry out a rotation + shift
00624    (note the shift is always DELTA_AFTER in this code).
00625 ----------------------------------------------------------------------------*/
00626 
00627 MCW_3shear rot_to_shear_matvec( THD_dmat33 rmat , THD_dfvec3 tvec ,
00628                                 double xdel , double ydel , double zdel )
00629 {
00630    int flip0=-1 , flip1=-1 ;  /* no flips */
00631    THD_dmat33 q , p ;
00632    THD_dfvec3 d , c ;
00633    MCW_3shear shr ;
00634 
00635 #if 0
00636    /* compute rotation matrix */
00637 
00638    q = rmat ;
00639 #else
00640    /* 13 Feb 2001: make sure it is orthogonal;
00641                    even slightly off produces bad shears */
00642 
00643    p = DMAT_svdrot_old( rmat ) ;
00644    q = TRANSPOSE_DMAT( p ) ;
00645 #endif
00646 
00647    /* if trace too small, maybe we should flip a couple axes */
00648 
00649    if( DMAT_TRACE(q) < 1.0 ){
00650       double top=q.mat[0][0] ; int itop=0 , i1,i2 ;
00651       if( top < q.mat[1][1] ){ top = q.mat[1][1] ; itop = 1 ; }
00652       if( top < q.mat[2][2] ){ top = q.mat[2][2] ; itop = 2 ; }
00653       switch(itop){
00654          case 0: i1 = 1 ; i2 = 2 ; LOAD_DIAG_DMAT(p, 1,-1,-1) ; break ;
00655          case 1: i1 = 0 ; i2 = 2 ; LOAD_DIAG_DMAT(p,-1, 1,-1) ; break ;
00656          case 2: i1 = 0 ; i2 = 1 ; LOAD_DIAG_DMAT(p,-1,-1, 1) ; break ;
00657       }
00658       if( q.mat[i1][i1] + q.mat[i2][i2] < -0.02 ){
00659          q = DMAT_MUL( q , p ) ;
00660          flip0 = i1 ; flip1 = i2 ;  /* yes flips */
00661       }
00662    }
00663 
00664    d = tvec ;
00665 
00666    /* scale q and d by the voxel dimensions */
00667 
00668    d.xyz[0] = d.xyz[0] / xdel ;  /* d <- inv[D] d, where D = diag[xdel,ydel,zdel] */
00669    d.xyz[1] = d.xyz[1] / ydel ;
00670    d.xyz[2] = d.xyz[2] / zdel ;
00671 
00672    q.mat[0][1] *= (ydel/xdel) ;  /* q <- inv[D] q D */
00673    q.mat[0][2] *= (zdel/xdel) ;
00674    q.mat[1][0] *= (xdel/ydel) ;  /* q still has det[q]=1 after this */
00675    q.mat[1][2] *= (zdel/ydel) ;
00676    q.mat[2][0] *= (xdel/zdel) ;
00677    q.mat[2][1] *= (ydel/zdel) ;
00678 
00679    /* compute the "best" shear for this q matrix */
00680 
00681    shr = shear_best( &q , &d ) ;
00682 
00683    /* if cannot compute shear, try perturbing the matrix a little */
00684 
00685    if( ! ISVALID_3SHEAR(shr) ){
00686       THD_dmat33 pt ;
00687       p  = rot_to_matrix( 0,PER0 , 1,PER1 , 2,PER2 ) ;
00688       q  = DMAT_MUL( q , p ) ;
00689       pt = TRANSPOSE_DMAT( p ) ;
00690       q  = DMAT_MUL( pt , q ) ;
00691 
00692       shr = shear_best( &q , &d ) ;
00693       if( ! ISVALID_3SHEAR(shr) ) return shr ;  /* give up */
00694    }
00695 
00696    shr.flip0 = flip0 ; shr.flip1 = flip1 ;
00697 
00698 #if 0
00699    if( MRILIB_verbose ) DUMP_3SHEAR("rot_to_shear",shr) ;
00700 #endif
00701 
00702    return shr ;
00703 }
00704 
00705 /*=========================================================================
00706   Routines to compute the rotation+translation to best align a set
00707   of 3D points to one another -- RWCox - 16 Jul 2000.
00708 ===========================================================================*/
00709 
00710 /*---------------------------------------------------------------------
00711    Compute        T
00712            [inmat]  [inmat]
00713 -----------------------------------------------------------------------*/
00714 
00715 THD_dmat33 DMAT_xt_x( THD_dmat33 inmat )
00716 {
00717    THD_dmat33 tt,mm ;
00718    tt = TRANSPOSE_DMAT(inmat) ;
00719    mm = DMAT_MUL(tt,inmat) ;
00720    return mm ;
00721 }
00722 
00723 /*--------------------------------------------------------------------*/
00724                                            /*                T  */
00725 THD_dmat33 DMAT_x_xt( THD_dmat33 inmat )   /* [inmat] [inmat]   */
00726 {                                          /* 09 Apr 2003 - RWC */
00727    THD_dmat33 tt,mm ;
00728    tt = TRANSPOSE_DMAT(inmat) ;
00729    mm = DMAT_MUL(inmat,tt) ;
00730    return mm ;
00731 }
00732 
00733 /*--------------------------------------------------------------------
00734    Compute the eigensolution of the symmetric matrix inmat; that is,
00735    orthogonal [X] and diagonal [D] such that
00736 
00737         [inmat] [X] = [X] [D]
00738 ----------------------------------------------------------------------*/
00739 
00740 THD_dvecmat DMAT_symeig( THD_dmat33 inmat )
00741 {
00742    THD_dvecmat out ;
00743    double a[9] , e[3] ;
00744    int ii,jj ;
00745 
00746    /* load matrix from inmat into simple array */
00747 
00748    for( jj=0 ; jj < 3 ; jj++ )
00749       for( ii=0 ; ii < 3 ; ii++ ) a[ii+3*jj] = inmat.mat[ii][jj] ;
00750 
00751    symeig_double( 3 , a , e ) ;     /* eigensolution of array */
00752 
00753    /* load eigenvectors and eigenvalues into output */
00754 
00755    for( jj=0 ; jj < 3 ; jj++ ){
00756       out.vv.xyz[jj] = e[jj] ;                 /* eigenvalues */
00757       for( ii=0 ; ii < 3 ; ii++ )
00758          out.mm.mat[ii][jj] = a[ii+3*jj] ;     /* eigenvectors */
00759    }
00760 
00761    return out ;
00762 }
00763 
00764 /*---------------------------------------------------------------------
00765    Compute the SVD of matrix inmat.
00766 -----------------------------------------------------------------------*/
00767 
00768 typedef struct { THD_dmat33 u,v ; THD_dfvec3 d ; } THD_udv33 ;
00769 
00770 THD_udv33 DMAT_svd( THD_dmat33 inmat )
00771 {
00772    THD_udv33 out ;
00773    double a[9] , e[3] , u[9] , v[9] ;
00774    int ii , jj ;
00775 
00776    /* load matrix from inmat into simple array */
00777 
00778    for( jj=0 ; jj < 3 ; jj++ )
00779       for( ii=0 ; ii < 3 ; ii++ ) a[ii+3*jj] = inmat.mat[ii][jj] ;
00780 
00781    svd_double( 3,3 , a , e,u,v ) ;
00782 
00783    /* load eigenvectors and eigenvalues into output */
00784 
00785    for( jj=0 ; jj < 3 ; jj++ ){
00786       out.d.xyz[jj] = e[jj] ;                 /* eigenvalues */
00787       for( ii=0 ; ii < 3 ; ii++ ){
00788          out.u.mat[ii][jj] = u[ii+3*jj] ;     /* eigenvectors */
00789          out.v.mat[ii][jj] = v[ii+3*jj] ;     /* eigenvectors */
00790       }
00791    }
00792 
00793    return out ;
00794 }
00795 
00796 /*---------------------------------------------------------------------
00797    Compute                  pp
00798             [      T       ]
00799             [ inmat  inmat ]   for some power pp
00800 -----------------------------------------------------------------------*/
00801 
00802 THD_dmat33 DMAT_pow( THD_dmat33 inmat , double pp )
00803 {
00804    THD_dmat33 out , mm , dd ;
00805    THD_dvecmat vm ;
00806    int ii ;
00807 
00808    /* special case */
00809 
00810    if( pp == 0.0 ){ LOAD_DIAG_DMAT(out,1,1,1) ; return out ; }
00811 
00812    mm = DMAT_xt_x( inmat ) ;
00813    vm = DMAT_symeig( mm ) ;  /* get eigensolution [X] and [D] */
00814 
00815    /* raise [D] to the pp power */
00816 
00817    for( ii=0 ; ii < 3 ; ii++ )
00818       vm.vv.xyz[ii] = (vm.vv.xyz[ii] <= 0.0) ? 0.0
00819                                              : pow(vm.vv.xyz[ii],pp) ;
00820 
00821    /*                  pp    T  */
00822    /* result is [X] [D]   [X]   */
00823 
00824    LOAD_DIAG_DMAT( dd , vm.vv.xyz[0],vm.vv.xyz[1],vm.vv.xyz[2] ) ;
00825 
00826    mm  = DMAT_MUL( vm.mm , dd ) ;
00827    dd  = TRANSPOSE_DMAT( vm.mm ) ;
00828    out = DMAT_MUL( mm , dd ) ;
00829    return out ;
00830 }
00831 
00832 /*---------------------------------------------------------------------
00833    Compute the rotation         T
00834                          [V] [U]
00835    from the matrix, where                     T
00836                          [inmat] = [U] [D] [V]
00837    is the SVD of the input matrix.  We actually calculate it as
00838 
00839                      -1/2
00840        [     T      ]           T
00841        [inmat  inmat]    [inmat]
00842 
00843    which is the same thing (do your linear algebra, dude).
00844 -----------------------------------------------------------------------*/
00845 
00846 THD_dmat33 DMAT_svdrot_old( THD_dmat33 inmat )
00847 {
00848    THD_dmat33 sq , out , tt ;
00849 
00850    sq  = DMAT_pow( inmat , -0.5 ) ;
00851    tt  = TRANSPOSE_DMAT(inmat) ;
00852    out = DMAT_MUL( sq , tt ) ;
00853    return out ;
00854 }
00855 
00856 /*---------------------------------------------------------------------*/
00857 /*  Alternative calculation to above, computing U and V directly.
00858     This avoids a problem that arises when inmat is singular.
00859 -----------------------------------------------------------------------*/
00860 
00861 THD_dmat33 DMAT_svdrot_new( THD_dmat33 inmat )
00862 {
00863    THD_dmat33 mm , nn ;
00864    THD_dvecmat vm , um ;
00865 
00866    mm = DMAT_xt_x( inmat ) ;
00867    vm = DMAT_symeig( mm ) ;  /* vm.mm matrix is now V */
00868 
00869    mm = DMAT_x_xt( inmat ) ;
00870    um = DMAT_symeig( mm ) ;  /* um.mm matrix is now U */
00871 
00872    mm = TRANSPOSE_DMAT(um.mm) ;
00873    nn = DMAT_MUL( vm.mm , mm ) ;
00874    return nn ;
00875 }
00876 
00877 /*---------------------------------------------------------------------*/
00878 /*  Yet another alternative calculation to above, computing U and V
00879     even more directly.  [22 Oct 2004]
00880 -----------------------------------------------------------------------*/
00881 
00882 THD_dmat33 DMAT_svdrot_newer( THD_dmat33 inmat )
00883 {
00884    THD_udv33 udv ;
00885    THD_dmat33 vm , um , nn ;
00886 
00887    udv = DMAT_svd( inmat ) ;
00888    vm = udv.v ;
00889    um = TRANSPOSE_DMAT( udv.u );
00890    nn = DMAT_MUL( vm , um ) ;
00891    return nn ;
00892 }
00893 
00894 /*---------------------------------------------------------------------
00895   Compute proper orthgonal matrix R and vector V to make
00896 
00897      yy = [R] xx + V   (k=0..n-1)
00898        k        k
00899 
00900   true in the (weighted) least squares sense.  If ww == NULL, then
00901   weights of all 1 are used.  Method follows
00902     KS Arun, TS Huang, and SD Blostein, IEEE PAMI, 9:698-700, 1987
00903   and uses the routines above to compute the matrix [R].
00904 -----------------------------------------------------------------------*/
00905 
00906 THD_dvecmat DLSQ_rot_trans( int n, THD_dfvec3 *xx, THD_dfvec3 *yy, double *ww )
00907 {
00908    THD_dvecmat out ;
00909    THD_dfvec3  cx,cy , tx,ty ;
00910    THD_dmat33  cov ;
00911    double *wt , wsum , dd ;
00912    int ii,jj,kk ;
00913 
00914    /*- check for bad inputs -*/
00915 
00916    if( n < 3 || xx == NULL || yy == NULL ){ LOAD_ZERO_DMAT(out.mm); return out; }
00917 
00918    /*- make a fake weight array, if none supplied -*/
00919 
00920    if( ww == NULL ){
00921       wt = (double *) malloc(sizeof(double)*n) ;
00922       for( kk=0 ; kk < n ; kk++ ) wt[kk] = 1.0 ;
00923    } else {
00924       wt = ww ;
00925    }
00926 
00927    /*- compute centroids of each set of vectors -*/
00928 
00929    LOAD_DFVEC3(cx,0,0,0) ; LOAD_DFVEC3(cy,0,0,0) ; wsum = 0.0 ;
00930    for( kk=0 ; kk < n ; kk++ ){
00931       cx = SCLADD_DFVEC3(1,cx,wt[kk],xx[kk]) ;  /* weighted sums of vectors */
00932       cy = SCLADD_DFVEC3(1,cy,wt[kk],yy[kk]) ;
00933       wsum += wt[kk] ;                          /* sum of weights */
00934    }
00935    wsum = 1.0 / wsum ;
00936    cx.xyz[0] *= wsum ; cx.xyz[1] *= wsum ; cx.xyz[2] *= wsum ;  /* centroids */
00937    cy.xyz[0] *= wsum ; cy.xyz[1] *= wsum ; cy.xyz[2] *= wsum ;
00938 
00939    /*- compute covariance matrix -*/
00940 
00941    LOAD_DIAG_DMAT(cov,1.e-10,1.e-10,1.e-10) ;
00942    for( kk=0 ; kk < n ; kk++ ){
00943       tx = SUB_DFVEC3( xx[kk] , cx ) ;  /* remove centroids */
00944       ty = SUB_DFVEC3( yy[kk] , cy ) ;
00945       for( jj=0 ; jj < 3 ; jj++ )
00946          for( ii=0 ; ii < 3 ; ii++ )
00947             cov.mat[ii][jj] += wt[kk]*tx.xyz[ii]*ty.xyz[jj] ;
00948    }
00949    dd = ( fabs(cov.mat[0][0]) + fabs(cov.mat[1][1]) + fabs(cov.mat[2][2]) ) / 3.0 ;
00950 #if 0
00951 fprintf(stderr,"dd=%g  diag=%g %g %g\n",dd,cov.mat[0][0],cov.mat[1][1],cov.mat[2][2] ) ;
00952 #endif
00953    dd = dd / 1.e9 ;
00954 #if 0
00955 fprintf(stderr,"dd=%g\n",dd) ;
00956 #endif
00957    if( cov.mat[0][0] < dd ) cov.mat[0][0] = dd ;
00958    if( cov.mat[1][1] < dd ) cov.mat[1][1] = dd ;
00959    if( cov.mat[2][2] < dd ) cov.mat[2][2] = dd ;
00960 
00961 #if 0
00962 DUMP_DMAT33( "cov" , cov ) ;
00963 #endif
00964 
00965    out.mm = DMAT_svdrot_newer( cov ) ;  /* compute rotation matrix [R] */
00966 
00967 #if 0
00968 DUMP_DMAT33( "out.mm" , out.mm ) ;
00969 #endif
00970 
00971    tx = DMATVEC( out.mm , cx ) ;     /* compute translation vector V */
00972    out.vv = SUB_DFVEC3( cy , tx ) ;
00973 
00974    if( wt != ww ) free(wt) ;               /* toss the trash, if any */
00975 
00976    return out ;
00977 }
00978 
00979 /*--------------------------------------------------------------------------*/
00980 /*! Compute an affine transformation to take one set of vectors into another.
00981     That is, find general matrix R and vector B so that
00982 
00983      yy = [R] xx + V   (k=0..n-1)
00984        k        k
00985 
00986     is true in the unweighted least squares sense.
00987 ----------------------------------------------------------------------------*/
00988 
00989 THD_dvecmat DLSQ_affine( int n, THD_dfvec3 *xx, THD_dfvec3 *yy )
00990 {
00991    THD_dvecmat out ;
00992    THD_dfvec3  cx,cy , tx,ty ;
00993    THD_dmat33  yxt , xtx , xtxinv ;
00994    int ii,jj,kk ;
00995    double wsum ;
00996 
00997    /*- check for bad inputs -*/
00998 
00999    if( n < 3 || xx == NULL || yy == NULL ){ LOAD_ZERO_DMAT(out.mm); return out; }
01000 
01001    /*- compute centroids of each set of vectors -*/
01002 
01003    LOAD_DFVEC3(cx,0,0,0) ; LOAD_DFVEC3(cy,0,0,0) ; wsum = 0.0 ;
01004    for( kk=0 ; kk < n ; kk++ ){
01005      cx = ADD_DFVEC3(cx,xx[kk]) ;  /* sums of vectors */
01006      cy = ADD_DFVEC3(cy,yy[kk]) ;
01007    }
01008    wsum = 1.0 / n ;
01009    cx.xyz[0] *= wsum ; cx.xyz[1] *= wsum ; cx.xyz[2] *= wsum ;  /* centroids */
01010    cy.xyz[0] *= wsum ; cy.xyz[1] *= wsum ; cy.xyz[2] *= wsum ;
01011 
01012    /*- compute products of data matrices -*/
01013 
01014    LOAD_DIAG_DMAT(yxt,1.e-9,1.e-9,1.e-9) ;
01015    LOAD_DIAG_DMAT(xtx,1.e-9,1.e-9,1.e-9) ;
01016    for( kk=0 ; kk < n ; kk++ ){
01017      tx = SUB_DFVEC3( xx[kk] , cx ) ;  /* remove centroids */
01018      ty = SUB_DFVEC3( yy[kk] , cy ) ;
01019      for( jj=0 ; jj < 3 ; jj++ ){
01020        for( ii=0 ; ii < 3 ; ii++ ){
01021          yxt.mat[ii][jj] += ty.xyz[ii]*tx.xyz[jj] ;
01022          xtx.mat[ii][jj] += tx.xyz[ii]*tx.xyz[jj] ;
01023        }
01024      }
01025    }
01026    xtxinv = DMAT_INV( xtx ) ;
01027    out.mm = DMAT_MUL( yxt , xtxinv ) ;
01028    tx = DMATVEC( out.mm , cx ) ;
01029    out.vv = SUB_DFVEC3( cy , tx ) ;
01030    return out ;
01031 }
01032 
01033 /*--------------------------------------------------------------------------*/
01034 /*! Compute a transformation to take one set of vectors into another.
01035     That is, find matrix R and vector B so that
01036 
01037      yy = [R] xx + V   (k=0..n-1)
01038        k        k
01039 
01040     is true in the unweighted least squares sense.  Here, we restrict R
01041     to be a scalar multiple of an orthgonal matrix.
01042 ----------------------------------------------------------------------------*/
01043 
01044 THD_dvecmat DLSQ_rotscl( int n, THD_dfvec3 *xx, THD_dfvec3 *yy , int ndim )
01045 {
01046    THD_dvecmat out ;
01047    THD_dfvec3  cx,cy , tx,ty ;
01048    THD_dmat33  yxt , xtx , xtxinv , aa,bb,cc ;
01049    int ii,jj,kk ;
01050    double wsum ;
01051 
01052    /*- check for bad inputs -*/
01053 
01054    if( n < 3 || xx == NULL || yy == NULL ){ LOAD_ZERO_DMAT(out.mm); return out; }
01055 
01056    /*- compute centroids of each set of vectors -*/
01057 
01058    LOAD_DFVEC3(cx,0,0,0) ; LOAD_DFVEC3(cy,0,0,0) ; wsum = 0.0 ;
01059    for( kk=0 ; kk < n ; kk++ ){
01060      cx = ADD_DFVEC3(cx,xx[kk]) ;  /* sums of vectors */
01061      cy = ADD_DFVEC3(cy,yy[kk]) ;
01062    }
01063    wsum = 1.0 / n ;
01064    cx.xyz[0] *= wsum ; cx.xyz[1] *= wsum ; cx.xyz[2] *= wsum ;  /* centroids */
01065    cy.xyz[0] *= wsum ; cy.xyz[1] *= wsum ; cy.xyz[2] *= wsum ;
01066 
01067    /*- compute products of data matrices -*/
01068 
01069    LOAD_DIAG_DMAT(yxt,1.e-9,1.e-9,1.e-9) ;
01070    LOAD_DIAG_DMAT(xtx,1.e-9,1.e-9,1.e-9) ;
01071    for( kk=0 ; kk < n ; kk++ ){
01072      tx = SUB_DFVEC3( xx[kk] , cx ) ;  /* remove centroids */
01073      ty = SUB_DFVEC3( yy[kk] , cy ) ;
01074      for( jj=0 ; jj < 3 ; jj++ ){
01075        for( ii=0 ; ii < 3 ; ii++ ){
01076          yxt.mat[ii][jj] += ty.xyz[ii]*tx.xyz[jj] ;
01077          xtx.mat[ii][jj] += tx.xyz[ii]*tx.xyz[jj] ;
01078        }
01079      }
01080    }
01081    xtxinv = DMAT_INV( xtx ) ;
01082    aa = DMAT_MUL( yxt , xtxinv ) ;
01083    bb = DMAT_pow( aa , -0.5 ) ;
01084    cc = DMAT_MUL( aa , bb) ;
01085    wsum = DMAT_DET(aa); wsum = fabs(wsum);
01086    switch( ndim ){
01087      default:
01088      case 3: wsum = pow(wsum,0.333333333333) ; break ;  /* 3D rotation */
01089      case 2: wsum = sqrt(wsum)               ; break ;  /* 2D rotation */
01090    }
01091    out.mm = DMAT_SCALAR( cc , wsum ) ;
01092 
01093    tx = DMATVEC( out.mm , cx ) ;
01094    out.vv = SUB_DFVEC3( cy , tx ) ;
01095    return out ;
01096 }